Церера: Правильна ли моя локальная параметризация SO(3)+R³ для фактора предварительной интеграции IMU (особенно ComputeJ ⇐ C++

[Anonymous]

Я пытаюсь добавить фактор предварительной интеграции IMU в свою оптимизацию в VIO. Я просто хочу быть уверенным, все ли я делаю правильно, особенно часть LocalParameterization. Ниже приведен коэффициент предварительной интеграции IMU. Я только что вставил файл .h, чтобы вы могли иметь представление о параметризации, используемой в факторе.

Код: Выделить всё

#ifndef IMU_INTEG_FACTOR_H
#define IMU_INTEG_FACTOR_H
#include 
#include "IMU/basicTypes.hpp"
#include "IMU/imuPreintegrated.hpp"

namespace vio {

class ImuIntegFactor : public ceres::SizedCostFunction {
public:
ImuIntegFactor(IMUPreintegrated* pre_integration) : pre_integration_(pre_integration) {
Eigen::Matrix info = pre_integration_->get_information();
sqrt_info_ = Eigen::LLT(info.cast()).matrixL().transpose();
}

void ScaleSqrtInfo(double scale) {
// only scale the sub information matrix for rotation, velocity, pose.
sqrt_info_.block(0, 0) *= scale;
sqrt_info_.block(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 sqrt_info_;
IMUPreintegrated* pre_integration_;
};

class ImuIntegGdirFactor : public ceres::SizedCostFunction {
public:
ImuIntegGdirFactor(IMUPreintegrated* pre_integration) : pre_integration_(pre_integration) {
Eigen::Matrix info = pre_integration_->get_information();
sqrt_info_ = Eigen::LLT(info.cast()).matrixL().transpose();
}

void ScaleSqrtInfo(double scale) {
// only scale the sub information matrix for rotation, velocity, pose.
sqrt_info_.block(0, 0) *= scale;
sqrt_info_.block(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 sqrt_info_;
IMUPreintegrated* pre_integration_;
};

}
#endif /* IMU_INTEG_FACTOR_H */

Вот небольшая часть метода Evaluate:

Код: Выделить всё

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]);
...

И вот как я добавил это в задачу оптимизации ceres:

Код: Выделить всё

 // 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]);
}

Я создал локальную параметризацию для фактора предварительной интеграции IMU, поскольку операция дополнения для so3 должна обрабатываться по-другому.
Вот моя локальная реализация параметризации:

Код: Выделить всё

#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 omega(x);
Eigen::Map position(x + 3);

// delta = [ dtheta (3), dp (3) ]
Eigen::Map dtheta(delta);
Eigen::Map 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 omega_new(x_plus_delta);
Eigen::Map 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 J(jacobian);
J.setIdentity();
return true;
}

Меня больше всего беспокоит реализация локальной параметризации. Итак, я хотел бы узнать ваше мнение о том, правильно ли я это делаю или нет. Спасибо большое за внимание и время.

Подробнее здесь: https://stackoverflow.com/questions/798 ... tion-facto