LiLi

admin管理员组

文章数量:1406766

LiLi

源码阅读，能力有限，如有某处理解错误，请指出，谢谢。

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 rEigen::Matrix<double, 15, 15> sqrt_info =Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration_->covariance_.inverse()).matrixL().transpose();residual = sqrt_info * residualif (jacobians) {//获取预积分的误差递推函数中pqv关于ba、bg的Jacobiandouble 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、bgiif (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、bgjif (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位姿优化

后端优化 | VINS-Mono 论文公式推导与代码解析分讲_qq_43525734的博客-CSDN博客

vins中紧耦合优化模型（状态量状态方程观测方程）_火星机器人life的博客-CSDN博客

本文标签： LiLi