223 {
224 if (frame->detected_objects.empty()) {
225 ADEBUG <<
"No object input to transformer.";
226 return true;
227 }
228
229 const auto &camera_k_matrix = frame->camera_k_matrix;
230 float k_mat[9] = {0};
231 for (size_t i = 0; i < 3; ++i) {
232 for (size_t j = 0; j < 3; ++j) {
233 k_mat[i * 3 + j] = camera_k_matrix(i, j);
234 }
235 }
236 ADEBUG <<
"Camera k matrix input to transformer: \n"
237 << k_mat[0] << ", " << k_mat[1] << ", " << k_mat[2] << "\n"
238 << k_mat[3] << ", " << k_mat[4] << ", " << k_mat[5] << "\n"
239 << k_mat[6] << ", " << k_mat[7] << ", " << k_mat[8] << "\n";
240
241 const int width_image = frame->data_provider->src_width();
242 const int height_image = frame->data_provider->src_height();
243 const auto &camera2world_pose = frame->camera2world_pose;
244 mapper_->Init(k_mat, width_image, height_image);
245
246 ObjMapperOptions obj_mapper_options;
247 float object_center[3] = {0};
248 float dimension_hwl[3] = {0};
249 float rotation_y = 0.0f;
250 int nr_transformed_obj = 0;
251
252 for (auto &obj : frame->detected_objects) {
253 if (obj == nullptr) {
254 ADEBUG <<
"Empty object input to transformer.";
255 continue;
256 }
257
258
259 float theta_ray = 0.0f;
260 SetObjMapperOptions(obj, camera_k_matrix, width_image, height_image,
261 &obj_mapper_options, &theta_ray);
262
263
264 mapper_->Solve3dBbox(obj_mapper_options, object_center, dimension_hwl,
265 &rotation_y);
266
267
268 FillResults(object_center, dimension_hwl, rotation_y, camera2world_pose,
269 theta_ray, obj);
270
271 ++nr_transformed_obj;
272 }
273
274 return nr_transformed_obj > 0;
275}