From d20c9012e911dcad1764708914a0c83cbb84f387 Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Tue, 9 Jul 2019 12:53:11 +0200 Subject: [PATCH] Added initial marker to IMU alignment --- doc/Calibration.md | 2 +- src/calibration/cam_imu_calib.cpp | 78 +++++++++++++++++++++++++++++++ 2 files changed, 79 insertions(+), 1 deletion(-) diff --git a/doc/Calibration.md b/doc/Calibration.md index 5f0e9ff..5e6badf 100644 --- a/doc/Calibration.md +++ b/doc/Calibration.md @@ -67,7 +67,7 @@ The buttons in the GUI are located in the order which you need to follow to cali * `init_cam_imu` initializes the rotation between camera and IMU by aligning rotation velocities of the camera to the gyro data. * `init_opt` initializes the optimization. Shows reprojected corners in magenta and the estimated values from the spline as solid lines below. * `optimize` runs an iteration of the optimization. You should press it several times until convergence before proceeding to next steps. -* `init_mocap` initializes the transformation from the Aprilgrid calibration pattern to the Mocap coordinate system. **Currently we assume that the orientation of the Mocap marker frame is approximately aligned with IMU axes**. +* `init_mocap` initializes the transformation from the Aprilgrid calibration pattern to the Mocap coordinate system. * `save_calib` save the current calibration as `calibration.json` in the result folder. * `save_mocap_calib` save the current Mocap to IMU calibration as `mocap_calibration.json` in the result folder. diff --git a/src/calibration/cam_imu_calib.cpp b/src/calibration/cam_imu_calib.cpp index 353d10f..6e5f960 100644 --- a/src/calibration/cam_imu_calib.cpp +++ b/src/calibration/cam_imu_calib.cpp @@ -484,6 +484,83 @@ void CamImuCalib::initMocap() { return; } + { + std::vector timestamps_cam; + Eigen::vector rot_vel_mocap; + Eigen::vector rot_vel_imu; + + Sophus::SO3d R_i_mark_init = calib_opt->mocap_calib->T_i_mark.so3(); + + for (size_t i = 1; i < vio_dataset->get_gt_timestamps().size(); i++) { + int64_t timestamp0_ns = vio_dataset->get_gt_timestamps()[i - 1]; + int64_t timestamp1_ns = vio_dataset->get_gt_timestamps()[i]; + + Sophus::SE3d T_a_mark0 = vio_dataset->get_gt_pose_data()[i - 1]; + Sophus::SE3d T_a_mark1 = vio_dataset->get_gt_pose_data()[i]; + + double dt = (timestamp1_ns - timestamp0_ns) * 1e-9; + + Eigen::Vector3d rot_vel_c0 = + R_i_mark_init * (T_a_mark0.so3().inverse() * T_a_mark1.so3()).log() / + dt; + + timestamps_cam.push_back(timestamp0_ns); + rot_vel_mocap.push_back(rot_vel_c0); + } + + for (size_t j = 0; j < timestamps_cam.size(); j++) { + int idx = -1; + int64_t min_dist = std::numeric_limits::max(); + + for (size_t i = 1; i < vio_dataset->get_gyro_data().size(); i++) { + int64_t dist = + vio_dataset->get_gyro_data()[i].timestamp_ns - timestamps_cam[j]; + if (std::abs(dist) < min_dist) { + min_dist = std::abs(dist); + idx = i; + } + } + + rot_vel_imu.push_back(vio_dataset->get_gyro_data()[idx].data); + } + + BASALT_ASSERT_STREAM(rot_vel_mocap.size() == rot_vel_imu.size(), + "rot_vel_cam.size() " << rot_vel_mocap.size() + << " rot_vel_imu.size() " + << rot_vel_imu.size()); + + // R_i_c * rot_vel_mocap = rot_vel_imu + // R_i_c * rot_vel_mocap * rot_vel_mocap.T = rot_vel_imu * rot_vel_mocap.T + // R_i_c = rot_vel_imu * rot_vel_mocap.T * (rot_vel_mocap * + // rot_vel_mocap.T)^-1; + + Eigen::Matrix rot_vel_mocap_m( + 3, rot_vel_mocap.size()), + rot_vel_imu_m(3, rot_vel_imu.size()); + + for (size_t i = 0; i < rot_vel_mocap.size(); i++) { + rot_vel_mocap_m.col(i) = rot_vel_mocap[i]; + rot_vel_imu_m.col(i) = rot_vel_imu[i]; + } + + Eigen::Matrix3d R_i_mark = + rot_vel_imu_m * rot_vel_mocap_m.transpose() * + (rot_vel_mocap_m * rot_vel_mocap_m.transpose()).inverse(); + + // std::cout << "raw R_i_c0\n" << R_i_c0 << std::endl; + + Eigen::AngleAxisd aa(R_i_mark); // RotationMatrix to AxisAngle + R_i_mark = aa.toRotationMatrix(); + + Sophus::SE3d T_i_mark_new(R_i_mark, Eigen::Vector3d::Zero()); + calib_opt->mocap_calib->T_i_mark = + T_i_mark_new * calib_opt->mocap_calib->T_i_mark; + + std::cout << "Initialized T_i_mark:\n" + << calib_opt->mocap_calib->T_i_mark.matrix() << std::endl; + } + + // Initialize T_w_moc; Sophus::SE3d T_w_moc; // TODO: check for failure cases.. @@ -491,6 +568,7 @@ void CamImuCalib::initMocap() { i < vio_dataset->get_gt_timestamps().size(); i++) { int64_t timestamp_ns = vio_dataset->get_gt_timestamps()[i]; T_w_moc = calib_opt->getT_w_i(timestamp_ns) * + calib_opt->mocap_calib->T_i_mark * vio_dataset->get_gt_pose_data()[i].inverse(); std::cout << "Initialized T_w_moc:\n" << T_w_moc.matrix() << std::endl;