| 51 | basalt::MocapCalibration<double> mocap_calib; |
| 52 | |
| 53 | void write_aligned_dataset(const std::string& output_gt_path, |
| 54 | const basalt::VioDatasetPtr& vio_dataset, |
| 55 | int64_t best_offset_refined_ns, bool use_calib) { |
| 56 | Sophus::SE3d T_mark_i; |
| 57 | if (use_calib) T_mark_i = mocap_calib.T_i_mark.inverse(); |
| 58 | |
| 59 | const basalt::fs::path output_path(output_gt_path); |
| 60 | const auto parent_path = output_path.parent_path(); |
| 61 | if (!parent_path.empty()) { |
| 62 | basalt::fs::create_directories(parent_path); |
| 63 | } |
| 64 | |
| 65 | std::ofstream gt_out_stream(output_gt_path); |
| 66 | gt_out_stream << "#timestamp [ns], p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], " |
| 67 | "q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z []\n"; |
| 68 | |
| 69 | for (size_t i = 0; i < vio_dataset->get_gt_timestamps().size(); i++) { |
| 70 | gt_out_stream << vio_dataset->get_gt_timestamps()[i] + |
| 71 | best_offset_refined_ns |
| 72 | << ","; |
| 73 | Sophus::SE3d pose_corrected = vio_dataset->get_gt_pose_data()[i] * T_mark_i; |
| 74 | gt_out_stream << pose_corrected.translation().x() << "," |
| 75 | << pose_corrected.translation().y() << "," |
| 76 | << pose_corrected.translation().z() << "," |
| 77 | << pose_corrected.unit_quaternion().w() << "," |
| 78 | << pose_corrected.unit_quaternion().x() << "," |
| 79 | << pose_corrected.unit_quaternion().y() << "," |
| 80 | << pose_corrected.unit_quaternion().z() << std::endl; |
| 81 | } |
| 82 | } |
| 83 | |
| 84 | // Linear time version |
| 85 | double compute_error( |