+104 -78

src/xrt/auxiliary/tracking/t_euroc_recorder.cpp

reviewed

··· 35 35 using std::ofstream; 36 36 using std::queue; 37 37 using std::string; 38 38 + using std::to_string; 38 39 using std::vector; 39 40 using std::filesystem::create_directories; 40 41 ··· 42 43 { 43 44 struct xrt_frame_node node; 44 45 string path; //!< Destination path for the dataset 46 46 + int cam_count = -1; 45 47 46 48 bool recording; //!< Whether samples are being recorded 47 49 bool files_created; //!< Whether the dataset directory structure has been created ··· 53 55 struct xrt_slam_sinks cloner_queues; //!< Queue sinks that write into cloner sinks 54 56 struct xrt_imu_sink cloner_imu_sink; 55 57 struct xrt_pose_sink cloner_gt_sink; 56 56 - struct xrt_frame_sink cloner_left_sink; 57 57 - struct xrt_frame_sink cloner_right_sink; 58 58 + struct xrt_frame_sink cloner_sinks[XRT_TRACKING_MAX_SLAM_CAMS]; 58 59 59 60 // Writer sinks: write copied frame to disk 60 61 struct xrt_slam_sinks writer_queues; //!< Queue sinks that write into writer sinks 61 62 struct xrt_imu_sink writer_imu_sink; 62 63 struct xrt_pose_sink writer_gt_sink; 63 63 - struct xrt_frame_sink writer_left_sink; 64 64 - struct xrt_frame_sink writer_right_sink; 64 64 + struct xrt_frame_sink writer_sinks[XRT_TRACKING_MAX_SLAM_CAMS]; 65 65 66 66 queue<xrt_imu_sample> imu_queue{}; //!< IMU pushes get saved here and are delayed until left_frame pushes 67 67 mutex imu_queue_lock{}; //!< Lock for imu_queue ··· 71 71 72 72 // CSV file handles, ofstream implementation is already buffered. 73 73 // Using pointers because of `container_of` 74 74 - ofstream *imu_csv; 75 75 - ofstream *gt_csv; 76 76 - ofstream *left_cam_csv; 77 77 - ofstream *right_cam_csv; 74 74 + ofstream *imu_csv = nullptr; 75 75 + ofstream *gt_csv = nullptr; 76 76 + ofstream *cams_csv[XRT_TRACKING_MAX_SLAM_CAMS] = {}; 78 77 }; 79 78 80 79 ··· 107 106 *er->gt_csv << "#timestamp [ns],p_RS_R_x [m],p_RS_R_y [m],p_RS_R_z [m]," 108 107 "q_RS_w [],q_RS_x [],q_RS_y [],q_RS_z []" CSV_EOL; 109 108 110 110 - create_directories(path + "/mav0/cam0/data"); 111 111 - er->left_cam_csv = new ofstream{path + "/mav0/cam0/data.csv"}; 112 112 - *er->left_cam_csv << "#timestamp [ns],filename" CSV_EOL; 113 113 - 114 114 - create_directories(path + "/mav0/cam1/data"); 115 115 - er->right_cam_csv = new ofstream{path + "/mav0/cam1/data.csv"}; 116 116 - *er->right_cam_csv << "#timestamp [ns],filename" CSV_EOL; 109 109 + for (int i = 0; i < er->cam_count; i++) { 110 110 + string data_path = path + "/mav0/cam" + to_string(i) + "/data"; 111 111 + create_directories(data_path); 112 112 + er->cams_csv[i] = new ofstream{data_path + ".csv"}; 113 113 + *er->cams_csv[i] << "#timestamp [ns],filename" CSV_EOL; 114 114 + } 117 115 } 118 116 119 117 static void ··· 156 154 // Flush csv streams. Not necessary, doing it only to increase flush frequency 157 155 er->imu_csv->flush(); 158 156 er->gt_csv->flush(); 159 159 - er->left_cam_csv->flush(); 160 160 - er->right_cam_csv->flush(); 157 157 + for (int i = 0; i < er->cam_count; i++) { 158 158 + er->cams_csv[i]->flush(); 159 159 + } 161 160 } 162 161 163 162 extern "C" void ··· 189 188 } 190 189 191 190 static void 192 192 - euroc_recorder_save_frame(euroc_recorder *er, struct xrt_frame *frame, bool is_left) 191 191 + euroc_recorder_save_frame(euroc_recorder *er, struct xrt_frame *frame, int cam_index) 193 192 { 194 194 - string cam_name = is_left ? "cam0" : "cam1"; 193 193 + string cam_name = "cam" + to_string(cam_index); 195 194 uint64_t ts = frame->timestamp; 196 195 197 196 assert(frame->format == XRT_FORMAT_L8 || frame->format == XRT_FORMAT_R8G8B8); // Only formats supported ··· 202 201 cv::Mat img{(int)frame->height, (int)frame->width, img_type, frame->data, frame->stride}; 203 202 cv::imwrite(img_path, img); 204 203 205 205 - ofstream *cam_csv = is_left ? er->left_cam_csv : er->right_cam_csv; 206 206 - *cam_csv << ts << "," << filename << CSV_EOL; 204 204 + *er->cams_csv[cam_index] << ts << "," << filename << CSV_EOL; 207 205 } 208 206 209 209 - extern "C" void 210 210 - euroc_recorder_save_left(struct xrt_frame_sink *sink, struct xrt_frame *frame) 211 211 - { 212 212 - euroc_recorder *er = container_of(sink, euroc_recorder, writer_left_sink); 213 213 - euroc_recorder_flush(er); 214 214 - euroc_recorder_save_frame(er, frame, true); 215 215 - } 207 207 + #define DEFINE_SAVE_CAM(cam_id) \ 208 208 + extern "C" void euroc_recorder_save_cam##cam_id(struct xrt_frame_sink *sink, struct xrt_frame *frame) \ 209 209 + { \ 210 210 + euroc_recorder *er = container_of(sink, euroc_recorder, writer_sinks[cam_id]); \ 211 211 + euroc_recorder_flush(er); \ 212 212 + euroc_recorder_save_frame(er, frame, cam_id); \ 213 213 + } 216 214 217 217 - extern "C" void 218 218 - euroc_recorder_save_right(struct xrt_frame_sink *sink, struct xrt_frame *frame) 219 219 - { 220 220 - euroc_recorder *er = container_of(sink, euroc_recorder, writer_right_sink); 221 221 - euroc_recorder_save_frame(er, frame, false); 222 222 - } 215 215 + DEFINE_SAVE_CAM(0) 216 216 + DEFINE_SAVE_CAM(1) 217 217 + DEFINE_SAVE_CAM(2) 218 218 + DEFINE_SAVE_CAM(3) 219 219 + DEFINE_SAVE_CAM(4) 220 220 + 221 221 + //! Be sure to define the same number of defined functions as 222 222 + //! XRT_TRACKING_MAX_SLAM_CAMS and to add them to to euroc_recorder_save_cam 223 223 + static void (*euroc_recorder_save_cam[XRT_TRACKING_MAX_SLAM_CAMS])(struct xrt_frame_sink *sink, 224 224 + struct xrt_frame *frame) = { 225 225 + euroc_recorder_save_cam0, // 226 226 + euroc_recorder_save_cam1, // 227 227 + euroc_recorder_save_cam2, // 228 228 + euroc_recorder_save_cam3, // 229 229 + euroc_recorder_save_cam4, // 230 230 + }; 223 231 224 232 225 233 /* ··· 264 272 265 273 266 274 static void 267 267 - euroc_recorder_receive_frame(euroc_recorder *er, struct xrt_frame *src_frame, bool is_left) 275 275 + euroc_recorder_receive_frame(euroc_recorder *er, struct xrt_frame *src_frame, int cam_index) 268 276 { 269 277 if (!er->recording) { 270 278 return; ··· 274 282 xrt_frame *copy = nullptr; 275 283 u_frame_clone(src_frame, &copy); 276 284 277 277 - xrt_sink_push_frame(is_left ? er->writer_queues.cams[0] : er->writer_queues.cams[1], copy); 285 285 + xrt_sink_push_frame(er->writer_queues.cams[cam_index], copy); 278 286 279 287 xrt_frame_reference(&copy, NULL); 280 288 } 281 289 282 282 - extern "C" void 283 283 - euroc_recorder_receive_left(struct xrt_frame_sink *sink, struct xrt_frame *frame) 284 284 - { 285 285 - euroc_recorder *er = container_of(sink, euroc_recorder, cloner_left_sink); 286 286 - euroc_recorder_receive_frame(er, frame, true); 287 287 - } 290 290 + #define DEFINE_RECEIVE_CAM(cam_id) \ 291 291 + extern "C" void euroc_recorder_receive_cam##cam_id(struct xrt_frame_sink *sink, struct xrt_frame *frame) \ 292 292 + { \ 293 293 + euroc_recorder *er = container_of(sink, euroc_recorder, cloner_sinks[cam_id]); \ 294 294 + euroc_recorder_receive_frame(er, frame, cam_id); \ 295 295 + } 288 296 289 289 - extern "C" void 290 290 - euroc_recorder_receive_right(struct xrt_frame_sink *sink, struct xrt_frame *frame) 291 291 - { 292 292 - euroc_recorder *er = container_of(sink, euroc_recorder, cloner_right_sink); 293 293 - euroc_recorder_receive_frame(er, frame, false); 294 294 - } 297 297 + DEFINE_RECEIVE_CAM(0) 298 298 + DEFINE_RECEIVE_CAM(1) 299 299 + DEFINE_RECEIVE_CAM(2) 300 300 + DEFINE_RECEIVE_CAM(3) 301 301 + DEFINE_RECEIVE_CAM(4) 302 302 + 303 303 + //! Be sure to define the same number of defined functions as 304 304 + //! XRT_TRACKING_MAX_SLAM_CAMS and to add them to to euroc_recorder_receive_cam 305 305 + static void (*euroc_recorder_receive_cam[XRT_TRACKING_MAX_SLAM_CAMS])(struct xrt_frame_sink *, 306 306 + struct xrt_frame *) = { 307 307 + euroc_recorder_receive_cam0, // 308 308 + euroc_recorder_receive_cam1, // 309 309 + euroc_recorder_receive_cam2, // 310 310 + euroc_recorder_receive_cam3, // 311 311 + euroc_recorder_receive_cam4, // 312 312 + }; 295 313 296 314 297 315 /* ··· 310 328 struct euroc_recorder *er = container_of(node, struct euroc_recorder, node); 311 329 delete er->imu_csv; 312 330 delete er->gt_csv; 313 313 - delete er->left_cam_csv; 314 314 - delete er->right_cam_csv; 331 331 + for (int i = 0; i < er->cam_count; i++) { 332 332 + delete er->cams_csv[i]; 333 333 + } 315 334 delete er; 316 335 } 317 336 ··· 323 342 */ 324 343 325 344 extern "C" xrt_slam_sinks * 326 326 - euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path, bool record_from_start) 345 345 + euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path, int cam_count, bool record_from_start) 327 346 { 328 347 struct euroc_recorder *er = new euroc_recorder{}; 329 348 330 349 er->recording = record_from_start; 350 350 + er->cam_count = cam_count; 331 351 332 352 struct xrt_frame_node *xfn = &er->node; 333 353 xfn->break_apart = euroc_recorder_node_break_apart; ··· 341 361 time_t seconds = os_realtime_get_ns() / U_1_000_000_000; 342 362 constexpr size_t size = sizeof("YYYYMMDDHHmmss"); 343 363 char datetime[size] = {0}; 344 344 - strftime(datetime, size, "%Y%m%d%H%M%S", localtime(&seconds)); 364 364 + (void)strftime(datetime, size, "%Y%m%d%H%M%S", localtime(&seconds)); 345 365 string default_path = string{"euroc_recording_"} + datetime; 346 366 er->path = default_path; 347 367 } ··· 354 374 355 375 // Setup sink pipeline 356 376 357 357 - // First, make the public queues that will clone frames in memory so that 358 358 - // original frames can be released as soon as possible. Not doing this could 359 359 - // result in frame queues from the user being filled up. 360 360 - er->cloner_queues.cam_count = 2; 361 361 - u_sink_queue_create(xfctx, 0, &er->cloner_left_sink, &er->cloner_queues.cams[0]); 362 362 - u_sink_queue_create(xfctx, 0, &er->cloner_right_sink, &er->cloner_queues.cams[1]); 363 363 - er->cloner_queues.imu = &er->cloner_imu_sink; 364 364 - er->cloner_queues.gt = &er->cloner_gt_sink; 377 377 + // We expose a "cloner" sink that will clone frames in memory so that original 378 378 + // frames can be released as soon as possible. Not doing this could result in 379 379 + // frame queues from the user being filled up. After that we write to disk. We 380 380 + // also put queues between these to support sensors streaming on different 381 381 + // threads. 382 382 + // cloner_queue -> cloner_sink (clone ) -> writer_queue -> writer_sink (write to disk) 383 383 + 384 384 + er->cloner_queues.cam_count = er->cam_count; 385 385 + er->writer_queues.cam_count = er->cam_count; 386 386 + for (int i = 0; i < er->cam_count; i++) { 365 387 366 366 - // Clone samples into heap and release original samples right after 367 367 - er->cloner_imu_sink.push_imu = euroc_recorder_receive_imu; 368 368 - er->cloner_gt_sink.push_pose = euroc_recorder_receive_gt; 369 369 - er->cloner_left_sink.push_frame = euroc_recorder_receive_left; 370 370 - er->cloner_right_sink.push_frame = euroc_recorder_receive_right; 388 388 + // If any of these asserts failed see docs on euroc_recorder_receive/save_cam 389 389 + assert(euroc_recorder_receive_cam[ARRAY_SIZE(euroc_recorder_receive_cam) - 1] != nullptr); 390 390 + assert(euroc_recorder_save_cam[ARRAY_SIZE(euroc_recorder_save_cam) - 1] != nullptr); 371 391 372 372 - // Then, make a queue to save frame sinks to disk in a separate thread 373 373 - er->writer_queues.cam_count = 2; 374 374 - u_sink_queue_create(xfctx, 0, &er->writer_left_sink, &er->writer_queues.cams[0]); 375 375 - u_sink_queue_create(xfctx, 0, &er->writer_right_sink, &er->writer_queues.cams[1]); 376 376 - er->writer_queues.imu = nullptr; 377 377 - er->writer_queues.gt = nullptr; 392 392 + u_sink_queue_create(xfctx, 0, &er->cloner_sinks[i], &er->cloner_queues.cams[i]); 393 393 + er->cloner_sinks[i].push_frame = euroc_recorder_receive_cam[i]; 394 394 + u_sink_queue_create(xfctx, 0, &er->writer_sinks[i], &er->writer_queues.cams[i]); 395 395 + er->writer_sinks[i].push_frame = euroc_recorder_save_cam[i]; 396 396 + } 378 397 379 379 - // Write cloned samples to disk with these 398 398 + er->cloner_queues.imu = &er->cloner_imu_sink; 399 399 + er->cloner_imu_sink.push_imu = euroc_recorder_receive_imu; 400 400 + er->writer_queues.imu = nullptr; // We use a std::queue instead 380 401 er->writer_imu_sink.push_imu = euroc_recorder_save_imu; 402 402 + 403 403 + er->cloner_queues.gt = &er->cloner_gt_sink; 404 404 + er->cloner_gt_sink.push_pose = euroc_recorder_receive_gt; 405 405 + er->writer_queues.gt = nullptr; // We use a std::queue instead 381 406 er->writer_gt_sink.push_pose = euroc_recorder_save_gt; 382 382 - er->writer_left_sink.push_frame = euroc_recorder_save_left; 383 383 - er->writer_right_sink.push_frame = euroc_recorder_save_right; 384 407 385 408 xrt_slam_sinks *public_sinks = &er->cloner_queues; 386 409 return public_sinks; ··· 392 415 euroc_recorder *er = (euroc_recorder *)ptr; 393 416 euroc_recorder_try_mkfiles(er); 394 417 er->recording = !er->recording; 395 395 - snprintf(er->recording_btn.label, sizeof(er->recording_btn.label), 396 396 - er->recording ? "Stop recording" : "Record EuRoC dataset"); 418 418 + (void)snprintf(er->recording_btn.label, sizeof(er->recording_btn.label), 419 419 + er->recording ? "Stop recording" : "Record EuRoC dataset"); 397 420 } 398 421 399 422 extern "C" void 400 400 - euroc_recorder_add_ui(struct xrt_slam_sinks *public_sinks, void *root) 423 423 + euroc_recorder_add_ui(struct xrt_slam_sinks *public_sinks, void *root, const char *prefix) 401 424 { 402 425 euroc_recorder *er = container_of(public_sinks, euroc_recorder, cloner_queues); 403 426 er->recording_btn.cb = euroc_recorder_btn_cb; 404 427 er->recording_btn.ptr = er; 405 405 - u_var_add_button(root, &er->recording_btn, er->recording ? "Stop recording" : "Record EuRoC dataset"); 428 428 + 429 429 + char tmp[256]; 430 430 + (void)snprintf(tmp, sizeof(tmp), "%s%s", prefix, er->recording ? "Stop recording" : "Record EuRoC dataset"); 431 431 + u_var_add_button(root, &er->recording_btn, tmp); 406 432 }

+4 -2

src/xrt/auxiliary/tracking/t_euroc_recorder.h

reviewed

··· 24 24 * 25 25 * @param xfctx Frame context for the sinks. 26 26 * @param record_path Directory name to save the dataset or NULL for a default based on the current datetime. 27 27 + * @param cam_count Number of cameras to record 27 28 * @param record_from_start Whether to start recording immediately on creation. 28 29 * @return struct xrt_slam_sinks* Sinks to push samples to for recording. 29 30 * 30 31 * @ingroup aux_tracking 31 32 */ 32 33 struct xrt_slam_sinks * 33 33 - euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path, bool record_from_start); 34 34 + euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path, int cam_count, bool record_from_start); 34 35 35 36 /*! 36 37 * Add EuRoC recorder UI button to start recording after creation. 37 38 * 38 39 * @param er The sinks returned by @ref euroc_recorder_create 39 40 * @param root The pointer to add UI button to 41 41 + * @param prefix Prefix in case you have multiple recorders, otherwise pass an empty string 40 42 */ 41 43 void 42 42 - euroc_recorder_add_ui(struct xrt_slam_sinks *er, void *root); 44 44 + euroc_recorder_add_ui(struct xrt_slam_sinks *er, void *root, const char *prefix); 43 45 44 46 #ifdef __cplusplus 45 47 }

+2 -2

src/xrt/auxiliary/tracking/t_tracker_slam.cpp

reviewed

··· 968 968 u_var_add_log_level(&t, &t.log_level, "Log Level"); 969 969 u_var_add_bool(&t, &t.submit, "Submit data to SLAM"); 970 970 u_var_add_bool(&t, &t.gt.override_tracking, "Track with ground truth (if available)"); 971 971 - euroc_recorder_add_ui(t.euroc_recorder, &t); 971 971 + euroc_recorder_add_ui(t.euroc_recorder, &t, ""); 972 972 973 973 u_var_add_gui_header(&t, NULL, "Trajectory Filter"); 974 974 u_var_add_bool(&t, &t.filter.use_moving_average_filter, "Enable moving average filter"); ··· 1398 1398 1399 1399 xrt_frame_context_add(xfctx, &t.node); 1400 1400 1401 1401 - t.euroc_recorder = euroc_recorder_create(xfctx, NULL, false); 1401 1401 + t.euroc_recorder = euroc_recorder_create(xfctx, NULL, t.cam_count, false); 1402 1402 1403 1403 t.last_imu_ts = INT64_MIN; 1404 1404 t.last_cam_ts = vector<timepoint_ns>(t.cam_count, INT64_MIN);