源码阅读，能力有限，如有某处理解错误，请指出，谢谢。
LidarKeyframeFactor.h：通过搭建边缘约束、平面约束，利用ceres非线性优化求解，求得q和t（当前帧到上一帧的位姿变换），使得点线距离和地面距离最小
#ifndef LIDARFACTOR_H
#define LIDARFACTOR_H
#include <iostream>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <Eigen/Dense>
#include <assert.h>
#include <cmath>
#include "utils/math_tools.h"
struct LidarEdgeFactor//边缘约束，该优化会不断的迭代,找到一个合适的q和t,使得点到线的距离最小
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW//使用时，加上此句，等于采用eigen中约定的方式重载了该类的new delete等内存分配函数
//curr_point 当前点；last_point_a上一帧的点a；last_point_b上一帧的点b；s当前点的时间戳占比
LidarEdgeFactor(Eigen::Vector3d curr_point_,
Eigen::Vector3d last_point_a_,
Eigen::Vector3d last_point_b_,
Eigen::Quaterniond qlb_,
Eigen::Vector3d tlb_,
double s_)
: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_) {
qlb = qlb_;
tlb = tlb_;
s = s_;
}
template <typename T>
//q是四元数参数块，t是平移向量的参数块，residual就是残差模块
bool operator()(const T *t, const T *q, T *residual) const
{
// 将double数据转成eigen的数据结构,注意这里必须都写成模板
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};
//构建当前帧到上一帧的旋转四元数和平移向量
Eigen::Quaternion<T> q_last_curr{q[0], q[1], q[2], q[3]};
Eigen::Matrix<T, 3, 1> t_last_curr{t[0], t[1], t[2]};
//这里暂不考虑畸变,因此这里不做任何变换
//考虑匀速模型(这一帧所有的旋转和平移都是一样的),根据当前点的时间戳占比,来计算,这个点到上一帧的旋转和平移
Eigen::Quaternion<T> q_l_b{T(qlb.w()), T(qlb.x()), T(qlb.y()), T(qlb.z())};
Eigen::Matrix<T, 3, 1> t_l_b{T(tlb.x()), T(tlb.y()), T(tlb.z())};
//把当前点根据当前计算的帧间位姿变换到上一帧
Eigen::Matrix<T, 3, 1> lp;
lp = q_last_curr * cp + t_last_curr;
Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);//模是三角形面积
Eigen::Matrix<T, 3, 1> de = lpa - lpb;
residual[0] = nu.norm() / de.norm();//norm（）计算二范数，也就是模
residual[0] *= T(s);
return true;
}
//返回一个new AutoDiffCostFunction，<LidarEdgeFactor, 3, 4, 3>这里的第1个3是指的三维残差,然后的4是四元数的参数块维度,之后的3是平移向量的参数块维度.
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_,
const Eigen::Vector3d last_point_a_,
const Eigen::Vector3d last_point_b_,
Eigen::Quaterniond qlb_,
Eigen::Vector3d tlb_,
double s_)
{
return (new ceres::AutoDiffCostFunction<LidarEdgeFactor, 1, 3, 4>(
new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, qlb_, tlb_, s_)));
}
Eigen::Vector3d curr_point, last_point_a, last_point_b;
Eigen::Quaterniond qlb;
Eigen::Vector3d tlb;
double s;
};
struct LidarPlaneNormFactor//平面约束，该优化会不断的迭代,找到一个合适的q和t,使得点到面的距离最小
{
LidarPlaneNormFactor(Eigen::Vector3d curr_point_,
Eigen::Vector3d plane_unit_norm_,
Eigen::Quaterniond qlb_,
Eigen::Vector3d tlb_,
double negative_OA_dot_norm_,
double score_) : curr_point(curr_point_),
plane_unit_norm(plane_unit_norm_),
qlb(qlb_),
tlb(tlb_),
negative_OA_dot_norm(negative_OA_dot_norm_),
score(score_) {}
template <typename T>
//q是四元数参数块、t是平移向量的参数块、residual就是残差模块
bool operator()(const T *t, const T *q, T *residual) const
{
// 将double数据转成eigen的数据结构,注意这里必须都写成模板
//构建当前帧到上一帧的旋转四元数和一个单位四元数
Eigen::Quaternion<T> q_w_curr{q[0], q[1], q[2], q[3]};
Eigen::Matrix<T, 3, 1> t_w_curr{t[0], t[1], t[2]};
//获取当前点的坐标
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix<T, 3, 1> point_w;
Eigen::Quaternion<T> q_l_b{T(qlb.w()), T(qlb.x()), T(qlb.y()), T(qlb.z())};//当前帧到上一帧的四元数变换
Eigen::Matrix<T, 3, 1> t_l_b{T(tlb.x()), T(tlb.y()), T(tlb.z())};//当前帧到上一帧的位移变换
//得到当前帧点在上一帧中的位置投影
point_w = q_l_b.inverse() * (cp - t_l_b);
point_w = q_w_curr * point_w + t_w_curr;
//计算点到平面的垂直距离
Eigen::Matrix<T, 3, 1> norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z()));
residual[0] = T(score) * (norm.dot(point_w) + T(negative_OA_dot_norm));
return true;
}
//返回一个new AutoDiffCostFunction，<LidarEdgeFactor,1, 4, 3>这里的第1个1是指的一维残差,然后的4是四元数的参数块维度,之后的3是平移向量的参数块维度
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_,
const Eigen::Vector3d plane_unit_norm_,
const Eigen::Quaterniond qlb_,
const Eigen::Vector3d tlb_,
const double negative_OA_dot_norm_,
const double score_)
{
return (new ceres::AutoDiffCostFunction<
LidarPlaneNormFactor, 1, 3, 4>(
new LidarPlaneNormFactor(curr_point_, plane_unit_norm_, qlb_, tlb_, negative_OA_dot_norm_, score_)));
}
Eigen::Vector3d curr_point;
Eigen::Vector3d plane_unit_norm;
Eigen::Quaterniond qlb;
Eigen::Vector3d tlb;
double negative_OA_dot_norm, score;
};
struct LidarPlaneNormIncreFactor//有增量因子的平面约束，功能和没有的增量因子的平面约束相同
{
LidarPlaneNormIncreFactor(Eigen::Vector3d curr_point_,
Eigen::Vector3d plane_unit_norm_,
double negative_OA_dot_norm_)
: curr_point(curr_point_),
plane_unit_norm(plane_unit_norm_),
negative_OA_dot_norm(negative_OA_dot_norm_) {}
template <typename T>
bool operator()(const T *q, const T *t, T *residual) const
{
// 将double数据转成eigen的数据结构,注意这里必须都写成模板
//构建当前帧到上一帧的旋转四元数和一个单位四元数
Eigen::Quaternion<T> q_inc{q[0], q[1], q[2], q[3]};
Eigen::Matrix<T, 3, 1> t_inc{t[0], t[1], t[2]};
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
//得到当前帧点在上一帧中的位置投影
Eigen::Matrix<T, 3, 1> point_w;
point_w = q_inc * cp + t_inc;
//计算点到平面的垂直距离
Eigen::Matrix<T, 3, 1> norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z()));
residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm);
return true;
}
//返回一个new AutoDiffCostFunction，<LidarEdgeFactor,1, 4, 3>这里的第1个1是指的一维残差,然后的4是四元数的参数块维度,之后的3是平移向量的参数块维度
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_,
const Eigen::Vector3d plane_unit_norm_,
const double negative_OA_dot_norm_)
{
return (new ceres::AutoDiffCostFunction<LidarPlaneNormIncreFactor, 1, 4, 3>(
new LidarPlaneNormIncreFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_)));
}
Eigen::Vector3d curr_point;
Eigen::Vector3d plane_unit_norm;
double negative_OA_dot_norm;
};
#endif // LIDARFACTOR_H参考链接：
Eigen(2):使用eigen进行四元数与旋转矩阵转换(c++实现)_biter0088的博客-CSDN博客_eigen 四元数转旋转矩阵
3D激光SLAM:ALOAM---Ceres 优化部分及代码分析_月照银海似蛟龙的博客-CSDN博客
Eigen中 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF的使用方式_C/C++中的预编译简介_惊鸿一博的博客-CSDN博客
assert.h是什么及使用方法_Bing_Lee的博客-CSDN博客_#include <assert.h>
LidarPoseFactor.h：求解两点之间最优的位姿变换矩阵
#pragma once
#include <ros/assert.h>
#include <iostream>
#include <eigen3/Eigen/Dense>
#include "utils/math_tools.h"
#include <ceres/ceres.h>
struct LidarPoseFactorAutoDiff//激光雷达姿态因子自动差分，求解两点之间最优的位姿变换矩阵
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW//使用时，加上此句，等于采用eigen中约定的方式重载了该类的new delete等内存分配函数
LidarPoseFactorAutoDiff(Eigen::Quaterniond delta_q_, Eigen::Vector3d delta_p_): delta_q(delta_q_), delta_p(delta_p_)
{}
template <typename T>
bool operator()(const T *p1, const T *p2, const T *p3, const T *p4, T *residuals) const
{
// 将double数据转成eigen的数据结构,注意这里必须都写成模板
//得到两点的姿态信息
Eigen::Matrix<T, 3, 1> P1(p1[0], p1[1], p1[2]);
Eigen::Quaternion<T> Q1(p2[0], p2[1], p2[2], p2[3]);
Eigen::Matrix<T, 3, 1> P2(p3[0], p3[1], p3[2]);
Eigen::Quaternion<T> Q2(p4[0], p4[1], p4[2], p4[3]);
//构建两点之间变换的四元数和平移向量
Eigen::Quaternion<T> tmp_delta_q(T(delta_q.w()), T(delta_q.x()), T(delta_q.y()), T(delta_q.z()));
Eigen::Matrix<T, 3, 1> tmp_delta_p(T(delta_p.x()), T(delta_p.y()), T(delta_p.z()));
//求解残差
Eigen::Matrix<T, 3, 1> residual1 = T(2.0) * (tmp_delta_q.inverse() * Q1.inverse() * Q2).vec();//vec()表示求四元数的虚部
Eigen::Matrix<T, 3, 1> residual2 = Q1.inverse() * (P2 - P1) - tmp_delta_p;
residuals[0] = T(0.2) * residual1[0];
residuals[1] = T(0.2) * residual1[1];
residuals[2] = T(0.2) * residual1[2];
residuals[3] = T(0.2) * residual2[0];
residuals[4] = T(0.2) * residual2[1];
residuals[5] = T(0.2) * residual2[2];
return true;
}
//返回一个new AutoDiffCostFunction，<LidarPoseFactorAutoDiff, 6, 3, 4, 3, 4>这里的第1个6是指的六维残差,之后的3是平移向量的参数块维度，然后的4是四元数的参数块维度
static ceres::CostFunction *Create(Eigen::Quaterniond delta_q_, Eigen::Vector3d delta_p_)
{
return (new ceres::AutoDiffCostFunction<LidarPoseFactorAutoDiff, 6, 3, 4, 3, 4>(
new LidarPoseFactorAutoDiff(delta_q_, delta_p_)));
}
private:
Eigen::Quaterniond delta_q;
Eigen::Vector3d delta_p;
};
struct LidarPoseLeftFactorAutoDiff//激光雷达姿态左因子自动差分
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW//使用时，加上此句，等于采用eigen中约定的方式重载了该类的new delete等内存分配函数
LidarPoseLeftFactorAutoDiff(Eigen::Quaterniond delta_q_, Eigen::Vector3d delta_p_, Eigen::Quaterniond ql_, Eigen::Vector3d pl_): delta_q(delta_q_), delta_p(delta_p_), ql(ql_), pl(pl_)
{}
template <typename T>
bool operator()(const T *p1, const T *p2, T *residuals) const
{
// 将double数据转成eigen的数据结构,注意这里必须都写成模板
//得到两点的姿态信息
Eigen::Matrix<T, 3, 1> P1(T(pl.x()), T(pl.y()), T(pl.z()));
Eigen::Quaternion<T> Q1(T(ql.w()), T(ql.x()), T(ql.y()), T(ql.z()));
Eigen::Matrix<T, 3, 1> P2(p1[0], p1[1], p1[2]);
Eigen::Quaternion<T> Q2(p2[0], p2[1], p2[2], p2[3]);
//构建两点之间变换的四元数和平移向量
Eigen::Quaternion<T> tmp_delta_q(T(delta_q.w()), T(delta_q.x()), T(delta_q.y()), T(delta_q.z()));
Eigen::Matrix<T, 3, 1> tmp_delta_p(T(delta_p.x()), T(delta_p.y()), T(delta_p.z()));
//求解残差
Eigen::Matrix<T, 3, 1> residual1 = T(2.0) * (tmp_delta_q.inverse() * Q1.inverse() * Q2).vec();
Eigen::Matrix<T, 3, 1> residual2 = Q1.inverse() * (P2 - P1) - tmp_delta_p;
residuals[0] = T(0.2) * residual1[0];
residuals[1] = T(0.2) * residual1[1];
residuals[2] = T(0.2) * residual1[2];
residuals[3] = T(0.2) * residual2[0];
residuals[4] = T(0.2) * residual2[1];
residuals[5] = T(0.2) * residual2[2];
return true;
}
//返回一个new AutoDiffCostFunction，<LidarPoseLeftFactorAutoDiff, 6, 3, 4>这里的第1个6是指的六维残差,之后的3是平移向量的参数块维度，然后的4是四元数的参数块维度
static ceres::CostFunction *Create(Eigen::Quaterniond delta_q_, Eigen::Vector3d delta_p_, Eigen::Quaterniond ql_, Eigen::Vector3d pl_)
{
return (new ceres::AutoDiffCostFunction<LidarPoseLeftFactorAutoDiff, 6, 3, 4>(
new LidarPoseLeftFactorAutoDiff(delta_q_, delta_p_, ql_, pl_)));
}
private:
Eigen::Quaterniond delta_q;
Eigen::Vector3d delta_p;
Eigen::Quaterniond ql;
Eigen::Vector3d pl;
};
struct LidarPoseRightFactorAutoDiff//激光雷达姿态右因子自动差分
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW//使用时，加上此句，等于采用eigen中约定的方式重载了该类的new delete等内存分配函数
LidarPoseRightFactorAutoDiff(Eigen::Quaterniond delta_q_, Eigen::Vector3d delta_p_, Eigen::Quaterniond qr_, Eigen::Vector3d pr_): delta_q(delta_q_), delta_p(delta_p_), qr(qr_), pr(pr_)
{}
template <typename T>
bool operator()(const T *p1, const T *p2, T *residuals) const
{
// 将double数据转成eigen的数据结构,注意这里必须都写成模板
//得到两点的姿态信息
Eigen::Matrix<T, 3, 1> P2(T(pr.x()), T(pr.y()), T(pr.z()));
Eigen::Quaternion<T> Q2(T(qr.w()), T(qr.x()), T(qr.y()), T(qr.z()));
Eigen::Matrix<T, 3, 1> P1(p1[0], p1[1], p1[2]);
Eigen::Quaternion<T> Q1(p2[0], p2[1], p2[2], p2[3]);
//构建两点之间变换的四元数和平移向量
Eigen::Quaternion<T> tmp_delta_q(T(delta_q.w()), T(delta_q.x()), T(delta_q.y()), T(delta_q.z()));
Eigen::Matrix<T, 3, 1> tmp_delta_p(T(delta_p.x()), T(delta_p.y()), T(delta_p.z()));
//求解残差
Eigen::Matrix<T, 3, 1> residual1 = T(2.0) * (tmp_delta_q.inverse() * Q1.inverse() * Q2).vec();
Eigen::Matrix<T, 3, 1> residual2 = Q1.inverse() * (P2 - P1) - tmp_delta_p;
residuals[0] = T(0.2) * residual1[0];
residuals[1] = T(0.2) * residual1[1];
residuals[2] = T(0.2) * residual1[2];
residuals[3] = T(0.2) * residual2[0];
residuals[4] = T(0.2) * residual2[1];
residuals[5] = T(0.2) * residual2[2];
return true;
}
//返回一个new AutoDiffCostFunction，<LidarPoseRightFactorAutoDiff, 6, 3, 4>这里的第1个6是指的六维残差,之后的3是平移向量的参数块维度，然后的4是四元数的参数块维度，两个3、4分别是指上一帧和当前帧
static ceres::CostFunction *Create(Eigen::Quaterniond delta_q_, Eigen::Vector3d delta_p_, Eigen::Quaterniond qr_, Eigen::Vector3d pr_)
{
return (new ceres::AutoDiffCostFunction<LidarPoseRightFactorAutoDiff, 6, 3, 4>(
new LidarPoseRightFactorAutoDiff(delta_q_, delta_p_, qr_, pr_)));
}
private:
Eigen::Quaterniond delta_q;
Eigen::Vector3d delta_p;
Eigen::Quaterniond qr;
Eigen::Vector3d pr;
};ImuFactor.h：搭建imu的残差
#pragma once
#include <ros/assert.h>
#include <iostream>
#include <eigen3/Eigen/Dense>
#include "utils/math_tools.h"
#include "Preintegration.h"
#include <ceres/ceres.h>
//public ceres::SizedCostFunction<15, 3, 4, 9, 3, 4, 9>；15, 3，4, 9, 3，4, 9分别代表１５维残差，前一帧3维平移向量，4维四元数，前一帧９维（线速度３维，加速度计的线加速度偏移为３维，陀螺仪的角加速度偏移为３维），当前帧3维平移向量，4维四元数，当前帧９维（线速度３维，加速度计的线加速度偏移为３维，陀螺仪的角加速度偏移为３维）
class ImuFactor : public ceres::SizedCostFunction<15, 3, 4, 9, 3, 4, 9> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW使用时，加上此句，等于采用eigen中约定的方式重载了该类的new delete等内存分配函数
ImuFactor() = delete;
ImuFactor(Preintegration *pre_integration) : pre_integration_{
pre_integration} {
g_vec_ = pre_integration_->g_vec_;
}
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
//优化的第一个状态量位姿为７维，平移３维，旋转的四元数为４维，优化的第二个状态量为９维度，线速度为３维，加速度计的线加速度偏移为３维，陀螺仪的角加速度偏移为３维
Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
Eigen::Quaterniond Qi(parameters[1][0], parameters[1][1], parameters[1][2], parameters[1][3]);
Qi.normalize();//归一化操作
Eigen::Vector3d Vi(parameters[2][0], parameters[2][1], parameters[2][2]);
Eigen::Vector3d Bai(parameters[2][3], parameters[2][4], parameters[2][5]);
Eigen::Vector3d Bgi(parameters[2][6], parameters[2][7], parameters[2][8]);
Eigen::Vector3d Pj(parameters[3][0], parameters[3][1], parameters[3][2]);
Eigen::Quaterniond Qj(parameters[4][0], parameters[4][1], parameters[4][2], parameters[4][3]);
Qj.normalize();//归一化操作
Eigen::Vector3d Vj(parameters[5][0], parameters[5][1], parameters[5][2]);
Eigen::Vector3d Baj(parameters[5][3], parameters[5][4], parameters[5][5]);
Eigen::Vector3d Bgj(parameters[5][6], parameters[5][7], parameters[5][8]);
//调用Preintegration.h中的evaluate()函数得到15维残差，残差分别为位移为３维，旋转为３维，线速度为３维，加速度计的线加速度偏移为３维，陀螺仪的角加速度偏移为３维
Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
residual = pre_integration_->evaluate(Pi, Qi, Vi, Bai, Bgi,
Pj, Qj, Vj, Baj, Bgj);
// LLT分解，residual 还需乘以信息矩阵的sqrt_info
// 因为优化函数其实是d=r^T P^-1 r ，P表示协方差，而ceres只接受最小二乘优化
// 因此需要把P^-1做LLT分解，使d=(L^T r)^T (L^T r) = r'^T r
Eigen::Matrix<double, 15, 15> sqrt_info =
Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration_->covariance_.inverse()).matrixL().transpose();
residual = sqrt_info * residual
if (jacobians) {
//获取预积分的误差递推函数中pqv关于ba、bg的Jacobian
double sum_dt = pre_integration_->sum_dt_;
Eigen::Matrix3d dp_dba = pre_integration_->jacobian_.template block<3, 3>(O_P, O_BA);
Eigen::Matrix3d dp_dbg = pre_integration_->jacobian_.template block<3, 3>(O_P, O_BG);
Eigen::Matrix3d dq_dbg = pre_integration_->jacobian_.template block<3, 3>(O_R, O_BG);
Eigen::Matrix3d dv_dba = pre_integration_->jacobian_.template block<3, 3>(O_V, O_BA);
Eigen::Matrix3d dv_dbg = pre_integration_->jacobian_.template block<3, 3>(O_V, O_BG);
//判断预积分中的数值是否稳定
if (pre_integration_->jacobian_.maxCoeff() > 1e8 || pre_integration_->jacobian_.minCoeff() < -1e8) {
ROS_WARN("numerical unstable in preintegration");
}
//第i帧的IMU位姿 pbi、qbi中的3维平移向量
if (jacobians[0]) {
Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
jacobian_pose_i.setZero();
jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
jacobian_pose_i = sqrt_info * jacobian_pose_i;
if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8) {
ROS_WARN("numerical unstable in preintegration");
}
}
//第i帧的IMU位姿 pbi、qbi中的4维四元数
if (jacobians[1]) {
Eigen::Map<Eigen::Matrix<double, 15, 4, Eigen::RowMajor>> jacobian_pose_qi(jacobians[1]);
jacobian_pose_qi.setZero();
Eigen::Vector3d tmp = -0.5 * g_vec_ * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt;
jacobian_pose_qi.block<3, 1>(O_P, 0) = 2 * (Qi.w() * tmp + skewSymmetric(Qi.vec()) * tmp);
jacobian_pose_qi.block<3, 3>(O_P, 1) = 2 * (Qi.vec().dot(tmp) * Eigen::Matrix3d::Identity() + Qi.vec() * tmp.transpose() - tmp * Qi.vec().transpose() - Qi.w() * skewSymmetric(tmp));
Eigen::Quaterniond corrected_delta_q =
pre_integration_->delta_q_ * deltaQ(dq_dbg * (Bgi - pre_integration_->linearized_bg_));
jacobian_pose_qi.block<3, 4>(O_R, 0) =
-2 * (Qleft(Qj.inverse()) * Qright(corrected_delta_q)).bottomRightCorner<3, 4>();
Eigen::Vector3d tmp1 = -g_vec_ * sum_dt + Vj - Vi;
jacobian_pose_qi.block<3, 1>(O_V, 0) = 2 * (Qi.w() * tmp1 + skewSymmetric(Qi.vec()) * tmp1);
jacobian_pose_qi.block<3, 3>(O_V, 1) = 2 * (Qi.vec().dot(tmp1) * Eigen::Matrix3d::Identity() + Qi.vec() * tmp1.transpose() - tmp1 * Qi.vec().transpose() - Qi.w() * skewSymmetric(tmp1));
jacobian_pose_qi = sqrt_info * jacobian_pose_qi;
}
// 第i帧的imu速度vbi、bai、bgi
if (jacobians[2]) {
Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_i(jacobians[2]);
jacobian_speedbias_i.setZero();
jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt;
jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;
jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg;
Eigen::Quaterniond corrected_delta_q =
pre_integration_->delta_q_ * deltaQ(dq_dbg * (Bgi - pre_integration_->linearized_bg_));
jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) =
-LeftQuatMatrix(Qj.inverse() * Qi * corrected_delta_q).topLeftCorner<3, 3>() * dq_dbg;
jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();
jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;
jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;
jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();
jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();
jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;
}
//第j帧的IMU位姿 pbj、qbj中的3维平移向量
if (jacobians[3]) {
Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> jacobian_pose_j(jacobians[3]);
jacobian_pose_j.setZero();
jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix();
jacobian_pose_j = sqrt_info * jacobian_pose_j;
}
//第i帧的IMU位姿 pbj、qbj中的4维四元数
if (jacobians[4]) {
Eigen::Map<Eigen::Matrix<double, 15, 4, Eigen::RowMajor>> jacobian_pose_qj(jacobians[4]);
jacobian_pose_qj.setZero();
Eigen::Quaterniond corrected_delta_q =
pre_integration_->delta_q_ * deltaQ(dq_dbg * (Bgi - pre_integration_->linearized_bg_));
jacobian_pose_qj.block<3, 4>(O_R, 0) =
2 * Qleft(corrected_delta_q.inverse() * Qi.inverse()).bottomRightCorner<3, 4>();
jacobian_pose_qj = sqrt_info * jacobian_pose_qj;
}
// 第j帧的IMU速度vbj、baj、bgj
if (jacobians[5]) {
Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_j(jacobians[5]);
jacobian_speedbias_j.setZero();
jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix();
jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity();
jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity();
jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;
}
}
return true;
}
Preintegration *pre_integration_;
Eigen::Vector3d g_vec_;
};参考链接：
VINS-MONO代码解读---processIMU()+intergrationBase类+imu_factor.h_可即的博客-CSDN博客
Ceres用法及Ceres-Sophus在位姿图优化问题的应用_牛犇犇子木的博客-CSDN博客_ceres位姿优化
扫一扫
1757
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
