diff --git a/mins/src/update/wheel/UpdaterWheel.cpp b/mins/src/update/wheel/UpdaterWheel.cpp index e1afa5a..cc972e7 100644 --- a/mins/src/update/wheel/UpdaterWheel.cpp +++ b/mins/src/update/wheel/UpdaterWheel.cpp @@ -326,101 +326,84 @@ void UpdaterWheel::compute_linear_system_2D(MatrixXd &H, VectorXd &res, double t } } -/** - * Given a measurement, this will compute the linear system of the new measurements in respect to the state - * This will return a "small" H, res, and R which are only of a single measurement and sub-set of the state - */ -void UpdaterWheel::compute_linear_system_3D(MatrixXd &H, VectorXd &res, double time0, double time1) { +Matrix UpdaterWheel::ComputeResidual3D(const Matrix3d &R_GtoI0, const Vector3d &p_I0inG, + const Matrix3d &R_GtoI1, const Vector3d &p_I1inG, + const Matrix3d &R_ItoO, const Vector3d &p_IinO, + const Matrix3d &R_3D, const Vector3d &p_3D) { + Vector3d pOinI = -R_ItoO.transpose() * p_IinO; + Matrix3d RO0toO1 = R_ItoO * R_GtoI1 * R_GtoI0.transpose() * R_ItoO.transpose(); + Matrix res = Matrix::Zero(); + res.head(3) = -log_so3(R_3D * RO0toO1.transpose()); + res.tail(3) = p_3D - R_ItoO * R_GtoI0 * (p_I1inG + R_GtoI1.transpose() * pOinI - p_I0inG - R_GtoI0.transpose() * pOinI); + return res; +} - // Load state values - shared_ptr pose0 = state->clones.at(time0); - shared_ptr pose1 = state->clones.at(time1); - Vector3d pI0inG = pose0->pos(); - Vector3d pI1inG = pose1->pos(); - Matrix3d RGtoI0 = pose0->Rot(); - Matrix3d RGtoI1 = pose1->Rot(); - Vector3d pIinO = state->wheel_extrinsic->pos(); - Matrix3d RItoO = state->wheel_extrinsic->Rot(); - Vector3d pOinI = -RItoO.transpose() * pIinO; - Matrix3d RO0toO1 = RItoO * RGtoI1 * RGtoI0.transpose() * RItoO.transpose(); +pair UpdaterWheel::ComputeJacobians3D(const Matrix3d &R_GtoI0, const Vector3d &p_I0inG, + const Matrix3d &R_GtoI1, const Vector3d &p_I1inG, + const Matrix3d &R_ItoO, const Vector3d &p_IinO) { + Vector3d pOinI = -R_ItoO.transpose() * p_IinO; + Matrix3d RO0toO1 = R_ItoO * R_GtoI1 * R_GtoI0.transpose() * R_ItoO.transpose(); Matrix3d RO1toO0 = RO0toO1.transpose(); - // Compute Orientation and position measurement residual - res = Matrix::Zero(); - // orientation - Matrix3d R_est = RO0toO1; - res.block(0, 0, 3, 1) = -log_so3(R_3D * R_est.transpose()); - // position - Vector3d p_est = RItoO * RGtoI0 * (pI1inG + RGtoI1.transpose() * pOinI - pI0inG - RGtoI0.transpose() * pOinI); - res.block(3, 0, 3, 1) = p_3D - p_est; + Matrix3d dzr_dth0 = -R_ItoO * R_GtoI1 * R_GtoI0.transpose(); + Matrix3d dzr_dth1 = R_ItoO; + Matrix3d dzp_dth0 = R_ItoO * skew_x(R_GtoI0 * (p_I1inG + R_GtoI1.transpose() * pOinI - p_I0inG)); + Matrix3d dzp_dp0 = -R_ItoO * R_GtoI0; + Matrix3d dzp_dth1 = -R_ItoO * R_GtoI0 * R_GtoI1.transpose() * skew_x(pOinI); + Matrix3d dzp_dp1 = R_ItoO * R_GtoI0; + + MatrixXd H_poses = MatrixXd::Zero(6, 12); + H_poses.block(0, 0, 3, 3) = dzr_dth0; + H_poses.block(0, 6, 3, 3) = dzr_dth1; + H_poses.block(3, 0, 3, 3) = dzp_dth0; + H_poses.block(3, 3, 3, 3) = dzp_dp0; + H_poses.block(3, 6, 3, 3) = dzp_dth1; + H_poses.block(3, 9, 3, 3) = dzp_dp1; + + Matrix3d dzr_dthcalib = Matrix3d::Identity() - RO0toO1; + Matrix3d dzp_dthcalib = skew_x(R_ItoO * R_GtoI0 * (p_I1inG - p_I0inG) - RO1toO0 * p_IinO) + RO1toO0 * skew_x(p_IinO); + Matrix3d dzp_dpcalib = -RO1toO0 + Matrix3d::Identity(); + + MatrixXd H_ext = MatrixXd::Zero(6, 6); + H_ext.block(0, 0, 3, 3) = dzr_dthcalib; + H_ext.block(3, 0, 3, 3) = dzp_dthcalib; + H_ext.block(3, 3, 3, 3) = dzp_dpcalib; - // Now compute Jacobians! + return {H_poses, H_ext}; +} - // compute the size of the Jacobian - int H_size = 12; // Default size for pose of clone 1 and 2 +void UpdaterWheel::compute_linear_system_3D(MatrixXd &H, VectorXd &res, double time0, double time1) { + shared_ptr pose0 = state->clones.at(time0); + shared_ptr pose1 = state->clones.at(time1); + Matrix3d RItoO = state->wheel_extrinsic->Rot(); + Vector3d pIinO = state->wheel_extrinsic->pos(); + // Residual at current state values + res = ComputeResidual3D(pose0->Rot(), pose0->pos(), pose1->Rot(), pose1->pos(), RItoO, pIinO, R_3D, p_3D); + // Jacobians at FEJ state values + int H_size = 12; int H_count = 12; H_size += (state->op->wheel->do_calib_ext) ? 6 : 0; H_size += (state->op->wheel->do_calib_dt) ? 1 : 0; H_size += (state->op->wheel->do_calib_int) ? 3 : 0; H = MatrixXd::Zero(6, H_size); - - // Overwrite FEJ - pI0inG = pose0->pos_fej(); - pI1inG = pose1->pos_fej(); - RGtoI0 = pose0->Rot_fej(); - RGtoI1 = pose1->Rot_fej(); - RO0toO1 = RItoO * RGtoI1 * RGtoI0.transpose() * RItoO.transpose(); - RO1toO0 = RO0toO1.transpose(); - - // Jacobians in respect to current state - // orientation - Matrix3d dzr_dth0 = -RItoO * RGtoI1 * RGtoI0.transpose(); - Matrix3d dzr_dth1 = RItoO; - // position - Matrix3d dzp_dth0 = RItoO * skew_x(RGtoI0 * pI1inG + RGtoI0 * RGtoI1.transpose() * pOinI - RGtoI0 * pI0inG); - Matrix3d dzp_dp0 = -RItoO * RGtoI0; - Matrix3d dzp_dth1 = -RItoO * RGtoI0 * RGtoI1.transpose() * skew_x(pOinI); - Matrix3d dzp_dp1 = RItoO * RGtoI0; - - // Derivative theta change wrt oldest pose0 and pose1 - H.block(0, 0, 3, 3) = dzr_dth0; - H.block(0, 6, 3, 3) = dzr_dth1; - // Derivative position change wrt oldest pose0 and pose1 - H.block(3, 0, 3, 3) = dzp_dth0; - H.block(3, 3, 3, 3) = dzp_dp0; - H.block(3, 6, 3, 3) = dzp_dth1; - H.block(3, 9, 3, 3) = dzp_dp1; - - // Jacobian wrt wheel to IMU extrinsics + const auto [H_poses, H_ext] = ComputeJacobians3D(pose0->Rot_fej(), pose0->pos_fej(), pose1->Rot_fej(), pose1->pos_fej(), RItoO, pIinO); + H.block(0, 0, 6, 12) = H_poses; if (state->op->wheel->do_calib_ext) { - Matrix3d dzr_dthcalib = (Matrix3d::Identity() - RO0toO1); - Matrix3d dzp_dpcalib = -RO1toO0 + Matrix3d::Identity(); - Matrix3d dzp_dthcalib = skew_x(RItoO * RGtoI0 * (pI1inG - pI0inG) - RO1toO0 * pIinO) + RO1toO0 * skew_x(pIinO); - H.block(0, H_count, 3, 3) = dzr_dthcalib; - H.block(3, H_count, 3, 3) = dzp_dthcalib; - H.block(3, H_count + 3, 3, 3) = dzp_dpcalib; + H.block(0, H_count, 6, 6) = H_ext; H_count += 6; } - - // Jacobian wrt wheel timeoffset. if (state->op->wheel->do_calib_dt) { - // should be able to find imu wv assert(state->cpis.find(time0) != state->cpis.end()); assert(state->cpis.find(time1) != state->cpis.end()); Vector3d w0 = state->cpis.at(time0).w; Vector3d v0 = state->cpis.at(time0).v; Vector3d w1 = state->cpis.at(time1).w; Vector3d v1 = state->cpis.at(time1).v; - - // Put it in the Jacobian matrix - H.block(0, H_count, 3, 1) = dzr_dth0 * w0 + dzr_dth1 * w1; - H.block(3, H_count, 3, 1) = dzp_dth0 * w0 + dzp_dp0 * v0 + dzp_dth1 * w1 + dzp_dp1 * v1; + H.block(0, H_count, 3, 1) = H_poses.block(0, 0, 3, 3) * w0 + H_poses.block(0, 6, 3, 3) * w1; + H.block(3, H_count, 3, 1) = H_poses.block(3, 0, 3, 3) * w0 + H_poses.block(3, 3, 3, 3) * v0 + H_poses.block(3, 6, 3, 3) * w1 + H_poses.block(3, 9, 3, 3) * v1; H_count += 1; } - - // Jacobian wrt wheel intrinsics. if (state->op->wheel->do_calib_int) { - // Note they are the opposite sign H.block(0, H_count, 3, 3) = -dR_di_3D; H.block(3, H_count, 3, 3) = -dp_di_3D; } diff --git a/mins/src/update/wheel/UpdaterWheel.h b/mins/src/update/wheel/UpdaterWheel.h index 05ec45b..eed154c 100644 --- a/mins/src/update/wheel/UpdaterWheel.h +++ b/mins/src/update/wheel/UpdaterWheel.h @@ -84,6 +84,40 @@ class UpdaterWheel { const Eigen::Matrix3d &R_GtoI1, const Eigen::Vector3d &p_I1inG, const Eigen::Matrix3d &R_ItoO, const Eigen::Vector3d &p_IinO); + /** + * \brief Compute the 6-DOF 3D wheel odometry residual. + * \param[in] R_GtoI0 Rotation from global to IMU frame at time 0. + * \param[in] p_I0inG Position of IMU at time 0 expressed in global frame. + * \param[in] R_GtoI1 Rotation from global to IMU frame at time 1. + * \param[in] p_I1inG Position of IMU at time 1 expressed in global frame. + * \param[in] R_ItoO Rotation from IMU frame to wheel odometry frame. + * \param[in] p_IinO Position of IMU expressed in wheel odometry frame. + * \param[in] R_3D Preintegrated 3D orientation from wheel odometry. + * \param[in] p_3D Preintegrated 3D position from wheel odometry. + * \return Residual 6-vector [orientation_error_3, position_error_3]. + */ + static Eigen::Matrix ComputeResidual3D(const Eigen::Matrix3d &R_GtoI0, const Eigen::Vector3d &p_I0inG, + const Eigen::Matrix3d &R_GtoI1, const Eigen::Vector3d &p_I1inG, + const Eigen::Matrix3d &R_ItoO, const Eigen::Vector3d &p_IinO, + const Eigen::Matrix3d &R_3D, const Eigen::Vector3d &p_3D); + + /** + * \brief Compute analytical Jacobians of the 3D wheel odometry residual. + * Returns Jacobians w.r.t. the two IMU poses (6x12) and the wheel extrinsic (6x6). + * Pose columns are ordered [delta_th0(3), delta_p0(3), delta_th1(3), delta_p1(3)]. + * Extrinsic columns are ordered [delta_thO(3), delta_pO(3)]. + * \param[in] R_GtoI0 Rotation from global to IMU frame at time 0. + * \param[in] p_I0inG Position of IMU at time 0 expressed in global frame. + * \param[in] R_GtoI1 Rotation from global to IMU frame at time 1. + * \param[in] p_I1inG Position of IMU at time 1 expressed in global frame. + * \param[in] R_ItoO Rotation from IMU frame to wheel odometry frame. + * \param[in] p_IinO Position of IMU expressed in wheel odometry frame. + * \return {H_poses (6x12), H_ext (6x6)} Jacobians w.r.t. IMU poses and extrinsic calibration. + */ + static std::pair ComputeJacobians3D(const Eigen::Matrix3d &R_GtoI0, const Eigen::Vector3d &p_I0inG, + const Eigen::Matrix3d &R_GtoI1, const Eigen::Vector3d &p_I1inG, + const Eigen::Matrix3d &R_ItoO, const Eigen::Vector3d &p_IinO); + private: friend class Initializer; friend class IW_Initializer; diff --git a/mins/tests/test_UpdaterWheel3D.cpp b/mins/tests/test_UpdaterWheel3D.cpp new file mode 100644 index 0000000..07d4175 --- /dev/null +++ b/mins/tests/test_UpdaterWheel3D.cpp @@ -0,0 +1,108 @@ +// Numerically verifies UpdaterWheel's 3D analytical Jacobians using central finite differences. +#include +#include +#include "update/wheel/UpdaterWheel.h" +#include "utils/quat_ops.h" + +using namespace mins; +using Eigen::Matrix3d; +using Eigen::MatrixXd; +using Eigen::Vector3d; + +TEST(WheelJacobian3D, AnalyticalMatchesNumerical) { + const double epsilon = 1e-6; + const double tolerance = 1e-6; + + Matrix3d R_GtoI0 = ov_core::exp_so3(Vector3d(0.1, -0.2, 0.15)); + Vector3d p_I0inG(1.0, 0.5, 0.0); + Matrix3d R_GtoI1 = ov_core::exp_so3(Vector3d(0.05, -0.15, 0.3)); + Vector3d p_I1inG(2.0, 1.0, 0.0); + Matrix3d R_ItoO = ov_core::exp_so3(Vector3d(0.0, 0.0, 0.2)); + Vector3d p_IinO(0.1, 0.0, -0.2); + + // Zero-residual nominal measurement. + Vector3d pOinI = -R_ItoO.transpose() * p_IinO; + Matrix3d R_3D = R_ItoO * R_GtoI1 * R_GtoI0.transpose() * R_ItoO.transpose(); + Vector3d p_3D = R_ItoO * R_GtoI0 * (p_I1inG + R_GtoI1.transpose() * pOinI - p_I0inG - R_GtoI0.transpose() * pOinI); + + const auto [H_poses, H_ext] = UpdaterWheel::ComputeJacobians3D(R_GtoI0, p_I0inG, R_GtoI1, p_I1inG, R_ItoO, p_IinO); + + // --- H_poses: perturb pose0 (cols 0-5) and pose1 (cols 6-11) --- + MatrixXd H_poses_numerical = MatrixXd::Zero(6, 12); + for (int i = 0; i < 6; i++) { + // pose0 + Matrix3d R0_plus = R_GtoI0, R0_minus = R_GtoI0; + Vector3d p0_plus = p_I0inG, p0_minus = p_I0inG; + if (i < 3) { + Vector3d axis = Vector3d::Zero(); + axis(i) = 1.0; + R0_plus = ov_core::exp_so3(epsilon * axis) * R_GtoI0; + R0_minus = ov_core::exp_so3(-epsilon * axis) * R_GtoI0; + } else { + p0_plus(i - 3) += epsilon; + p0_minus(i - 3) -= epsilon; + } + Eigen::Matrix res_plus = UpdaterWheel::ComputeResidual3D(R0_plus, p0_plus, R_GtoI1, p_I1inG, R_ItoO, p_IinO, R_3D, p_3D); + Eigen::Matrix res_minus = UpdaterWheel::ComputeResidual3D(R0_minus, p0_minus, R_GtoI1, p_I1inG, R_ItoO, p_IinO, R_3D, p_3D); + double sign = (i < 3) ? 1.0 : -1.0; + H_poses_numerical.col(i) = sign * (res_plus - res_minus) / (2.0 * epsilon); + + // pose1 + Matrix3d R1_plus = R_GtoI1, R1_minus = R_GtoI1; + Vector3d p1_plus = p_I1inG, p1_minus = p_I1inG; + if (i < 3) { + Vector3d axis = Vector3d::Zero(); + axis(i) = 1.0; + R1_plus = ov_core::exp_so3(epsilon * axis) * R_GtoI1; + R1_minus = ov_core::exp_so3(-epsilon * axis) * R_GtoI1; + } else { + p1_plus(i - 3) += epsilon; + p1_minus(i - 3) -= epsilon; + } + res_plus = UpdaterWheel::ComputeResidual3D(R_GtoI0, p_I0inG, R1_plus, p1_plus, R_ItoO, p_IinO, R_3D, p_3D); + res_minus = UpdaterWheel::ComputeResidual3D(R_GtoI0, p_I0inG, R1_minus, p1_minus, R_ItoO, p_IinO, R_3D, p_3D); + H_poses_numerical.col(6 + i) = sign * (res_plus - res_minus) / (2.0 * epsilon); + } + + for (int row = 0; row < 6; row++) { + for (int col = 0; col < 12; col++) { + EXPECT_NEAR(H_poses(row, col), H_poses_numerical(row, col), tolerance) + << "H_poses mismatch at row=" << row << " col=" << col; + if (std::abs(H_poses_numerical(row, col)) > tolerance) { + EXPECT_GT(H_poses(row, col) * H_poses_numerical(row, col), 0.0) + << "H_poses sign mismatch at row=" << row << " col=" << col; + } + } + } + + // --- H_ext: perturb extrinsic (R_ItoO, p_IinO) --- + MatrixXd H_ext_numerical = MatrixXd::Zero(6, 6); + for (int i = 0; i < 6; i++) { + Matrix3d R_plus = R_ItoO, R_minus = R_ItoO; + Vector3d p_plus = p_IinO, p_minus = p_IinO; + if (i < 3) { + Vector3d axis = Vector3d::Zero(); + axis(i) = 1.0; + R_plus = ov_core::exp_so3(epsilon * axis) * R_ItoO; + R_minus = ov_core::exp_so3(-epsilon * axis) * R_ItoO; + } else { + p_plus(i - 3) += epsilon; + p_minus(i - 3) -= epsilon; + } + Eigen::Matrix res_plus = UpdaterWheel::ComputeResidual3D(R_GtoI0, p_I0inG, R_GtoI1, p_I1inG, R_plus, p_plus, R_3D, p_3D); + Eigen::Matrix res_minus = UpdaterWheel::ComputeResidual3D(R_GtoI0, p_I0inG, R_GtoI1, p_I1inG, R_minus, p_minus, R_3D, p_3D); + double sign = (i < 3) ? 1.0 : -1.0; + H_ext_numerical.col(i) = sign * (res_plus - res_minus) / (2.0 * epsilon); + } + + for (int row = 0; row < 6; row++) { + for (int col = 0; col < 6; col++) { + EXPECT_NEAR(H_ext(row, col), H_ext_numerical(row, col), tolerance) + << "H_ext mismatch at row=" << row << " col=" << col; + if (std::abs(H_ext_numerical(row, col)) > tolerance) { + EXPECT_GT(H_ext(row, col) * H_ext_numerical(row, col), 0.0) + << "H_ext sign mismatch at row=" << row << " col=" << col; + } + } + } +}