ARTICLE AD BOX
I am trying to add IMU preintegration factor to my optimization in VIO. I just want to be sure if I am doing everything correctly, especially LocalParameterization part. Below is given the IMU preintegration factor. I just pasted .h file, so you can have an idea on parametrization used in factor.
#ifndef IMU_INTEG_FACTOR_H #define IMU_INTEG_FACTOR_H #include <ceres/ceres.h> #include "IMU/basicTypes.hpp" #include "IMU/imuPreintegrated.hpp" namespace vio { class ImuIntegFactor : public ceres::SizedCostFunction<15, 6, 9, 6, 9> { public: ImuIntegFactor(IMUPreintegrated* pre_integration) : pre_integration_(pre_integration) { Eigen::Matrix<double, 15, 15> info = pre_integration_->get_information(); sqrt_info_ = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(info.cast<double>()).matrixL().transpose(); } void ScaleSqrtInfo(double scale) { // only scale the sub information matrix for rotation, velocity, pose. sqrt_info_.block<9, 9>(0, 0) *= scale; sqrt_info_.block<6, 6>(9, 9) *= 1e-1; // scale bias information. } virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const; void check(double** parameters); Eigen::Matrix<double, 15, 15> sqrt_info_; IMUPreintegrated* pre_integration_; }; class ImuIntegGdirFactor : public ceres::SizedCostFunction<15, 6, 9, 6, 9, 3> { public: ImuIntegGdirFactor(IMUPreintegrated* pre_integration) : pre_integration_(pre_integration) { Eigen::Matrix<double, 15, 15> info = pre_integration_->get_information(); sqrt_info_ = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(info.cast<double>()).matrixL().transpose(); } void ScaleSqrtInfo(double scale) { // only scale the sub information matrix for rotation, velocity, pose. sqrt_info_.block<9, 9>(0, 0) *= scale; sqrt_info_.block<6, 6>(9, 9) *= 1e-1; // scale bias information. } virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const; void check(double** parameters); Eigen::Matrix<double, 15, 15> sqrt_info_; IMUPreintegrated* pre_integration_; }; } #endif /* IMU_INTEG_FACTOR_H */Here is a small part from Evaluate method:
bool ImuIntegFactor::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { /// step 1: prepare data Eigen::Vector3d omegai(parameters[0][0], parameters[0][1], parameters[0][2]); Eigen::Vector3d Pi(parameters[0][3], parameters[0][4], parameters[0][5]); Sophus::SO3d Ri = Sophus::SO3d::exp(omegai); Sophus::SO3d invRi = Ri.inverse(); Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]); Eigen::Vector3d Bgi(parameters[1][3], parameters[1][4], parameters[1][5]); Eigen::Vector3d Bai(parameters[1][6], parameters[1][7], parameters[1][8]); Eigen::Vector3d omegaj(parameters[2][0], parameters[2][1], parameters[2][2]); Eigen::Vector3d Pj(parameters[2][3], parameters[2][4], parameters[2][5]); Sophus::SO3d Rj = Sophus::SO3d::exp(omegaj); Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]); Eigen::Vector3d Bgj(parameters[3][3], parameters[3][4], parameters[3][5]); Eigen::Vector3d Baj(parameters[3][6], parameters[3][7], parameters[3][8]); ...And this is how I added it into ceres optimization problem:
// Add preintegration factors for each keyframe inside sliding window for (size_t i = 0; i < drt_vio_init_ptr_->local_active_frames.size(); i++) { ceres::LocalParameterization* pose_param = new PoseSO3LocalParameterization(); problem.AddParameterBlock(para_pose[i], SIZE_PARAMETERIZATION::SIZE_POSE, pose_param); problem.AddParameterBlock(para_speed_bias[i], SIZE_PARAMETERIZATION::SIZE_SPEEDBIAS); } for (size_t i = 0; i < drt_vio_init_ptr_->local_active_frames.size() - 1; i++) { size_t j = i + 1; if (drt_vio_init_ptr_->imu_meas[i].sum_dt_ > 10) continue; ImuIntegFactor* imu_factor = new ImuIntegFactor(&drt_vio_init_ptr_->imu_meas[i]); problem.AddResidualBlock(imu_factor, NULL, para_pose[i], para_speed_bias[i], para_pose[j], para_speed_bias[j]); }I created local paramterization for IMU preintegration factor, as addition operation for so3 should be handled differently.
Here is my local parametrization implementation:
#include "factor/pose_so3_local_parametrization.h" bool PoseSO3LocalParameterization::Plus( const double* x, const double* delta, double* x_plus_delta ) const { // x = [ omega (3), position (3) ] Eigen::Map<const Eigen::Vector3d> omega(x); Eigen::Map<const Eigen::Vector3d> position(x + 3); // delta = [ dtheta (3), dp (3) ] Eigen::Map<const Eigen::Vector3d> dtheta(delta); Eigen::Map<const Eigen::Vector3d> dp(delta + 3); // Convert so3 -> SO3 Sophus::SO3d R = Sophus::SO3d::exp(omega); // Right-multiplicative update Sophus::SO3d R_new = R * Sophus::SO3d::exp(dtheta); // Write back Eigen::Map<Eigen::Vector3d> omega_new(x_plus_delta); Eigen::Map<Eigen::Vector3d> position_new(x_plus_delta + 3); omega_new = R_new.log(); // back to so3 position_new = position + dp; return true; } bool PoseSO3LocalParameterization::ComputeJacobian( const double* /*x*/, double* jacobian ) const { // Jacobian of Plus(x, delta) w.r.t delta at delta = 0 // For so3 ⊕ R3, this is identity Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>> J(jacobian); J.setIdentity(); return true; }My main concern is related implementation of local parametrization. So, I would like to get your thoughts on that if I am doing it correctly or not. Thank you very much for your attention and time.
