101 {
102 if (frame->detected_objects.empty()) {
103 ADEBUG <<
"No object input to transformer.";
104 return true;
105 }
106
107 const auto &camera_k_matrix = frame->camera_k_matrix;
108 float k_mat[9] = {0};
109 for (size_t i = 0; i < 3; i++) {
110 size_t i3 = i * 3;
111 for (size_t j = 0; j < 3; j++) {
112 k_mat[i3 + j] = camera_k_matrix(i, j);
113 }
114 }
115 ADEBUG <<
"Camera k matrix input to transformer: \n"
116 << k_mat[0] << ", " << k_mat[1] << ", " << k_mat[2] << "\n"
117 << k_mat[3] << ", " << k_mat[4] << ", " << k_mat[5] << "\n"
118 << k_mat[6] << ", " << k_mat[7] << ", " << k_mat[8] << "\n";
119 const auto &camera2world_pose = frame->camera2world_pose;
120
121 int height = frame->data_provider->src_height();
122 int width = frame->data_provider->src_width();
123 int nr_transformed_obj = 0;
124 const float PI = algorithm::Constant<float>::PI();
125 for (auto &obj : frame->detected_objects) {
126 if (obj == nullptr) {
127 ADEBUG <<
"Empty object input to transformer.";
128 continue;
129 }
130
131
132 float theta_ray = atan2(obj->camera_supplement.local_center[0],
133 obj->camera_supplement.local_center[2]);
134
135
136 float object_center[3] = {obj->camera_supplement.local_center[0],
137 obj->camera_supplement.local_center[1],
138 obj->camera_supplement.local_center[2]};
139 float dimension_hwl[3] = {obj->size(2), obj->size(1), obj->size(0)};
140 float rotation_y =
141 theta_ray + static_cast<float>(obj->camera_supplement.alpha);
142 if (rotation_y < -
PI) {
143 rotation_y += 2 *
PI;
144 }
else if (rotation_y >=
PI) {
145 rotation_y -= 2 *
PI;
146 }
147
148
149 float bbox[4] = {0};
150 bbox[0] = obj->camera_supplement.box.xmin;
151 bbox[1] = obj->camera_supplement.box.ymin;
152 bbox[2] = obj->camera_supplement.box.xmax;
153 bbox[3] = obj->camera_supplement.box.ymax;
154 float center2d[3] = {0};
155
156 CenterPointFromBbox(bbox, dimension_hwl, rotation_y, object_center,
157 center2d, k_mat, height, width);
158
159 FillResults(object_center, dimension_hwl, rotation_y, camera2world_pose,
160 theta_ray, obj);
161
162 ++nr_transformed_obj;
163 }
164 return nr_transformed_obj > 0;
165}
uint32_t height
Height of point cloud
uint32_t width
Width of point cloud