vins-fusion学习笔记

自己写着玩的,很多地方没写全或者不对,别问,问就是不知道,俺确实不知道~~

---

estimator.cpp

getIMUInterval()

• double t0, double t1,
• vector> &accVector
• vector> &gyrVector
• 输入pre_time和cur_time, 获得这个时间段内的acc和gyr数据,并存入到vector中
• 主要是时间上的逻辑

initFirstIMUPose()

vector<pair<double, Eigen::Vector3d>> &accVector

• 初始化当前一段时间内的加速度计的数据
• 首先会将当前一段时间的加速度计的值取平均值,然后g2R(averACC)这个函数
  • g2R()
  • R2ypr()
  • ypr2R
  • 上面三个操作是把imu坐标系与重力坐标系对齐$g_{imu}=[-9.4;-2.3;0.4]$变到$g=[0;0;9.8]$, 并且保证坐标系中yaw的值为0
  • 参考博客
  • $atan2(y，x)$求的是y/x的反正切，其返回值为[-pi,+pi]之间的一个数,$atan2(y,x)$所表达的意思是坐标原点为起点，指向(y,x)的射线在坐标平面上与x轴正方向之间的角的角度度。当y>0时，射线与x轴正方向的所得的角的角度指的是x轴正方向绕逆时针方向到达射线旋转的角的角度；而当y<0时，射线与x轴正方向所得的角的角度指的是x轴正方向绕顺时针方向达到射线旋转的角的角度
• 之后会得到一个初始化的$R_0$, 这个$R_0$存入滑动窗口的世界坐标系下的旋转$Rs[0]$

processIMU()

• 这里是处理prev_time和cur_time中imu数据, 也是imu预积分公式处理的地方,这里是为了完成三件事:
  • 更新当前的预积分量
  • 更新IMU残差的协防差矩阵
  • 更新IMU残差对于bias的雅克比矩阵
• frame_count 代表当前处理的这一帧在滑动窗口中的第几个, 取值范围是在0到WINDOW_SIZE之间

pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};

• 对这个滑动窗口的起始帧进行赋值处理, new一个预积分, 其中初始化的参数包括:
  • acc_0, gyr_0,linearized_acc, linearized_gyr 初始预积分的值以及后面更新时每一帧的acc和gyr的值
  • linearized_ba,linearized_bg 加速度的偏置以及陀螺仪的偏置,初始化的时候都为$[0;0;0]$
  • jacobian 初始化为$15\times 15$维的单位矩阵
  • covariance 初始化为$15\times 15$维的零矩阵
  • sum_dt 初始化为0
  • delta_p 初始化为$3\times 1$维的单位矩阵
  • delta_q 初始化为单位四元数
  • delta_v 初始化为零矩阵
  • noise 初始化为$18\times 18$维的噪声矩阵, 噪声项的对角协方差矩阵,imu标定拿到的参数
    
    
    
    
• acc_0, gyr_0
  • 预积分开头的帧当前的加速度以及角速度,因此这里需要知道$ac{c}_0$和$gy{r}_0$,之后每次预积分都要更新$ac{c}_0$和$gy{r}_0$
  • 预积分计算的是两帧图像之间积分的值$P,v,q$，通过一帧帧imu帧的迭代求解来获得
• 通过对pre_integrations进行push_back（），可以迭代求解出最终两帧图像间预积分的值
• 通过IMU的这些数据，来更新三个状态量，Ps,Vs,Rs（这个是绝对坐标系下的位姿）。这时候不是用预积分，而是用正常普通的积分并且用上中值积分

processImage()

• 这个函数主要用来处理图像的特征
  processImage(feature.second, feature.first)
• first是时间,second是featureFrame
• featureFrame这个容器存储:
  • xyz_uv_velocity <<
    • x
    • y
    • z
    • p_u
    • p_v
    • velocity_x
    • velocity_y;

featureFrame[feature_id].emplace_back(camera_id, xyz_uv_velocity);
featureBuf.push(make_pair(t, featureFrame));

• 首先是这个逻辑, VINS里为了控制优化计算量，在实时情况下，只对当前帧之前某一部分帧进行优化，而不是全部历史帧。局部优化帧的数量就是窗口大小。为了维持窗口大小，需要去除旧的帧添加新的帧，也就是边缘化Marginalization。到底是删去最旧的帧（MARGIN_OLD）还是删去刚刚进来窗口倒数第二帧(MARGIN_SECOND_NEW)，就需要对当前帧与之前帧进行视差比较，如果是当前帧变化很小，就会删去倒数第二帧，如果变化很大，就删去最旧的帧

  if (f_manager.addFeatureCheckParallax(frame_count, image, td))
    {
        marginalization_flag = MARGIN_OLD;
    }
    else
    {
        marginalization_flag = MARGIN_SECOND_NEW;
    }

• 接着判断ESTIMATE_EXTRINSIC这个参数的值,也就是对imu和camera的外参是否需要标定
• 外参标定结束后,判断当前使用了哪些传感器,双目,单目还是imu
• 开始进行初始化,单目+imu, 双目+imu,纯双目
• 进入双目加imu的程序中:
  • 首先进行三角化,初始没有说3d point不能进行pnp求解
  • 之后回到pnp求解那里,解出相机的位姿
  • 这个时候判断帧数是否到达了滑动窗口的大小,如果到达了则进入solveGyroscopeBias函数

optimization()

• vector2double(), 确定需要优化的变量
• 开始构建最小二乘问题, 添加估计量

problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
problem.AddParameterBlock(para_Td[0], 1);

• 构建imu的约束项IMUfactor, 包括imu的待估计量以及残差雅克比

IMUFactor *imu_factor = new IMUFactor(pre_integrations[j]);
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);

• 构建image的约束项
• VINS中地图点的参数化形式是逆深度的形式, 残差的计算对于双目相机有多种情况, 分别是
  • ProjectionOneFrameTwoCamFactor
  • ProjectionTwoFrameOneCamFactor
  • ProjectionTwoFrameTwoCamFactor
• 残差的计算给出了两种形式:
  • 第一种是在归一化平面上的重投影误差
  • 第二种是把归一化平面上的重投影误差投影到Unit sphere的误差
  • 论文中给出的是第二种，而代码中两种误差都保留了，并用宏UNIT_SPHERE_ERROR进行控制。
• ceres进行求解
• 使用double2vector()函数将优化参数恢复到滑动窗口内
• 构建边缘化的约束项, 有两种情况
  • 一种是边缘化旧帧,
  • 一种是边缘化新帧
• 如何判断边缘化旧帧还是新帧在processIMage()函数中的视差计算中得到,只有在当前帧变化过大时才会选择去边缘化旧帧,如果当前帧的视差变化较小,可以选择边缘化新进入的帧

 if (marginalization_flag == MARGIN_OLD){...}
  else{...}

• 开始进行两种边缘化策略,一种是边缘化旧帧,一种是边缘化新帧

边缘化旧帧

• 首先new一个类

MarginalizationInfo *marginalization_info = new MarginalizationInfo();

• 包含一些相关的变量

//各个优化变量的长度: 存放地址和长度
    std::unordered_map<long, int> parameter_block_size;
//各个优化变量的id: 存放地址, value都为0
    std::unordered_map<long, int> parameter_block_idx;
//各个优化变量对应的double指针类型的数据
    std::unordered_map<long, double *> parameter_block_data;

• 进行边缘化之后保留下来的

//各个优化变量的长度
    std::vector<int> keep_block_size; 
//各个优化变量的id
    std::vector<int> keep_block_idx;
//各个优化变量对应的double指针类型的数据
    std::vector<double *> keep_block_data;

• 边缘化之后从信息矩阵恢复出来

//雅克比矩阵
    Eigen::MatrixXd linearized_jacobians;
//残差向量
    Eigen::VectorXd linearized_residuals;

• 执行vector2double()函数
• 判断是否存在先验信息以及是否有效,如果有效就添加先验残差项, addResidualBlockInfo()函数添加残差项目

if (last_marginalization_info && last_marginalization_info->valid)
{
...
}

• 如果需要添加先验残差项,则新建容器drop_set,在先验帧的size中进行循环,比如在先验帧看到68个路标点,那么size()应该为$$11Pose+11V+2Ex+68\lambda +td=11\ast 6+11\ast 9+2\ast 6+68+1$$
  • $P(x,y,z,{\theta }_x,{\theta }_y,{\theta }_z)$ 相机位姿6维
  • $V(v_x,v_y,v_z,b{a}_x,b{a}_y,b{a}_z,b{g}_x,b{g}_y,b{g}_z)$ 相机速度、加速度偏置、角速度偏置9维
  • $Ex(t_{ic},r_{ic})$ 相机IMU外参6维
  • $\lambda$ 特征点逆深度$1维
  • $td$ 标定同步时间1维
• para_Pose[0], para_SpeedBias[0]是要被marg的量, 也就是滑窗内的第一个量,因此把这里的ipush进去drop_set.push_back(i);
• 构建imu的约束项
  • 计算残差和雅克比,第0帧和第1帧之间的IMU预积分值以及第0帧和第1帧相关优化变量(需要marg),vector{0, 1} 添加imu残差addResidualBlockInfo()
• 构建image的约束项
• 计算残差和雅克比,这里需要注意双目相机的约束项会比单目多一些,同样添加残差addResidualBlockInfo(), 注意image会用到loss_function(核函数)

vector<int>{0, 3}    
vector<int>{0, 4}
vector<int>{2}

• preMarginalize() 预处理函数,对于变量和内存的关联
• marginalize()边缘化
• 滑窗预移动,对指针进行一次移动操作而不是真正的滑窗

边缘化新帧

• 边缘化新帧marginalization_flag = MARGIN_SECOND_NEW
• 与边缘化旧帧操作基本相同

vector2double()

• 拿到滑动窗口中的各项状态估计参数,这些参数主要有

  • i为滑动窗口的几帧

  • 世界坐标系下的位置和旋转

  • 世界坐标系下的位置para_Pose $(x,y,z)$ $[012]$

    • Ps[i].x()
    • Ps[i].y()
    • Ps[i].z()

  • 世界坐标系下的旋转para_Pose $q$ $[3456]$

    • q= Rs[i]
    • q.x
    • q.y
    • q.x
    • q.w
      

  • 对于imu而言需要优化的有速度,速度偏置,陀螺仪偏置

  • 速度para_SpeedBias $(x,y,z)$ $[012]$

  • Vs[i].x

  • Vs[i].y

  • Vs[i].z

  • 加速度偏置para_SpeedBias $(x,y,z)$ $[345]$

  • Bas[i].x

  • Bas[i].y

  • Bas[i].z

  • 陀螺仪偏置para_SpeedBias $(x,y,z)$ $[678]$

    • Bgs[i].x()
    • Bgs[i].y()
    • Bgs[i].z()
      

  • 两个相机与imu之间的平移和旋转

  • 两个相机对应的外参的平移部分para_Ex_Pose $[i][012]$, 这里是相机到imu的平移ric

    • tic[i].x
    • tic[i].y
    • tic[i].z

  • 两个相机对应的外参的旋转部分para_Ex_Pose $[i][3456]$, 这里是相机到imu的旋转ric

    • Quaterniond q{ric[i]}
    • q.x
    • q.y
    • q.z
    • q.w
      

  • 所有特征点的逆深度para_Feature[i][0]

    • dep[i], dep由getDepthVector函数拿到, 每个特征点对应一个逆深度,大小由特征点的个数决定
      

  • td

    • 滑动窗口内第一帧图像到imu的时间差
      

double2vector()

• 把优化完成的各项参数再恢复到现有的数据中,也就是从para…中取回优化完成的数据
• ric(2): camera $\to$ imu的旋转矩阵
• tic(2): camera $\to$ imu的平移矩阵
• Ps(11): 滑动窗口内各帧在世界坐标系下的位置
• Vs(11): 滑动窗口内各帧的速度
• Rs(11): 滑动窗口内各帧在世界坐标系下的旋转
• Bas(11): 滑动窗口内imu加速度计的偏置
• Bgs(11): 滑动窗口内imu陀螺仪的偏置
• td:滑动窗口内第一帧图像到imu的时间差

updateLatestStates()

• 更新滑窗里面的变量
  • latest_time
  • latest_P
  • latest_Q
  • latest_V
  • latest_Ba
  • latest_Bg
  • latest_acc_0
  • latest_gyr_0
• 使用fastPredictIMU()函数预测imu的P, V, Q

slideWindow()

• 滑窗会紧接着边缘化操作,通过上次边缘化是旧帧还是新帧进行滑窗
• 如果是边缘化旧帧,就会进入 slideWindowOld()函数
• 如果是边缘化新帧,则进入slideWindowNew()函数

---

feature_manager.cpp

addFeatureCheckParallax()

• 进行视差比较,返回true or false, 也就是视差是否大到需要边缘化旧的帧
• 参考博客
  • 滑窗内的关键帧个数小于3
  • 如果当前帧的Features在滑窗内被观测到的个数小于20
  • 计算滑窗内所有满足条件的Feature(至少有三帧观测到该Feature)的观测帧倒数第二帧和导数第三帧的视差(归一化平面上的距离)，如果最终的平均距离大于10个像素点
  • 满足上面条件的Feature的个数为0的时候

getCorresponding()

• 得到两帧之间对应的特征点corres

CalibrationExRotation()

• 外参标定
• ric[0] = calib_ric;
• RIC[0] = calib_ric;
• 外参初始化

initFramePoseByPnP()

• 这个函数只有在frame_count不为0的时候才会生效
• PnP求解位姿函数
• 已知相机中特征点的像素坐标, 通过三角化得到这些像素坐标再空间中的位置,再通过2d-3d信息求解出当前帧也就是相机的位姿
• 该代码断分为两部分，前部分先判断当前特征中那些已经三角化出深度的点，计算出世界系坐标存入pts3D，相应的当前帧的归一化平面坐标存入pts2D，之后由外参转化出上一阵的相机位姿，然后进行solvePoseByPnP运算，求解当前帧的位姿，以便后面的三角化，当然要转化成imu坐标系下的位姿。solvePoseByPnP函数代码我就不贴在这里了，主要就是进行一系列的坐标系转化后，利用opencv自带的solvePnP函数解算位姿

triangulate()

• 三角化法求深度

triangulate(frame_count, Ps, Rs, tic, ric);

• frame_count: 代表滑动窗口内的第几帧
• Ps:滑动窗口内各帧在世界坐标系下的位置
• Rs:滑动窗口内各帧再世界坐标系下的旋转
• tic: 相机到imu的平移
• ric: 相机到imu的旋转
• 1 用Ps, Rs, tic, ric推导出左右两个相机的位姿,注意这里的坐标系是在imu坐标系下,所以位姿需要用imu的外参转一下

Eigen::Matrix<double, 3, 4> leftPose;
Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
leftPose.leftCols<3>() = R0.transpose();
leftPose.rightCols<1>() = -R0.transpose() * t0;

• 2 拿到归一化平面坐标

point0:归一化像素平面坐标$[u \quad v]$
point1:right同上

• 3 开始三角化,拿到世界坐标系的位置point3d

triangulatePoint(leftPose, rightPose, point0, point1, point3d)

• 4 相机位姿和相机坐标系之间存在一个求逆的过程，并且左右视图三角化这样求得的localPoint是在imu坐标系下的位置，还需要转化到左相机坐标系下，得到左相机坐标系下的深度。

Eigen::Vector3d localPoint;
localPoint = leftPose.leftCols<3>() * point3d + leftPose.rightCols<1>();
double depth = localPoint.z();
if (depth > 0)
    it_per_id.estimated_depth = depth;
else
    it_per_id.estimated_depth = INIT_DEPTH;

triangulatePoint()

• 函数通过三角化求解空间点坐标
• triangulatePoint(leftPose, rightPose, point0, point1, point3d)
• 两个相机的位姿是知道的,leftPose, rightPose
• 特征点在两个相机对应帧的图片里的归一化像素坐标也是知道的, point0, point1
• 通过对极几何和svd分解,求出这个特征点再空间中的3d坐标,也就是point3d
• VINS-mono 学习之 三角化

getDepthVector()

• 计算所有特征点的深度,逆深度

getFeatureCount()

 int cnt = 0;
    for (auto &it : feature)
    {
        it.used_num = it.feature_per_frame.size();
        if (it.used_num >= 4)
        {
            cnt++;
        }
    }
    return cnt;

---

initial_alignment.cpp

solveGyroscopeBias()

• 为了得到陀螺仪的偏置Bgs
  
• VINS-Mono理论学习——视觉惯性联合初始化与外参标定

---

imu_factor.h

Evaluate()

virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const

• imu factor的评价函数,优化参数parameters<7,9,7,9>:
  
• 定义并计算imu残差项residuals<15,1>, 进入到预计分的evaluate函数, 输入的是两帧之间的$P,V,Q,Ba,Bg$, 输出的是残差residual

 residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
                                      Pj, Qj, Vj, Baj, Bgj);

• 最终得到:
  
• 定义并计算一个信息矩阵sqrt_info<15, 15>, 这个信息矩阵是协方差矩阵的逆做LLT分解得到的
  • 真正的优化项其实是马氏距离:$d=residua{l}^T\ast {\Sigma }^{-1}\ast residual$, $\Sigma$是协方差
  • Ceres 只接受最小二乘优化,也就是$min(e^{T}e)$, 所以把$\Sigma$做 LLT 分解,即$\Sigma =L{L}^T$
  • 最终得到$d=r^{T}L{L}^{T}r=(L^{T}r{)}^T(L^{T}r)$, 所以真正去优化的残差项$residua{l}^{{}^{\prime }}=L^{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;

• 判断
  • 要进行ceres自动求导还是手动求导
  • 判断是否要对$J(0),J(1),J(2),J(3)$进行手动求导

        if (jacobians)
        {
           ...
           if (jacobians[0])
            {
            ...
            }
            if (jacobians[1])
            {
            ...   
            }
            if (jacobians[2])
            {
            ...
            }
            if (jacobians[3])
            {
            ...
            }
        }

• 进行手动求导,依然要先从jacobian矩阵中拿到各个增量的关于bias的雅克比
  
• 利用公式开始求导,这里其实是分别对i时刻的pose和speed以及j时刻的pose和speed求雅克比
• 其实就是对15*1的residual矩阵的每一项求偏导,所以叫残差雅克比, 求偏导的顺序为:
  • $[p_{b_k}^w,q_{b_k}^w]$
  • $[v_{b_k}^w,b_{a_k},b_{w_k}]$
  • $[p_{b_{k+1}}^w,q_{b_{k+1}}^w]$
  • $[v_{b_{k+1}}^w,b_{a_{k+1}},b_{w_{k+1}}]$s
    
    
• 需要注意的是,由于要添加到ceres中,所以这里的每一个雅克比求出来都要再乘以信息矩阵sqrt_info

---

integration_base.h

evaluate()

• 两帧之间的$PVQ$和$bias$的变化量的差
• 从jacobian矩阵中拿到各个增量关于bias的雅克比
  

Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);
Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);
Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);

• jacobian矩阵是通过迭代得到的$J_{k+1}=F_k{J}_k$
• 相减拿到当前帧的$\delta ba$和$\delta bg$
• 计算三个参数$PVQ$
  • corrected_delta_q
  • corrected_delta_v
  • corrected_delta_p

Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;

• 计算残差矩阵<15,1>residuals
  
  • 代码和公式对应的参数:
    • $\delta b_a$: dba
    • $\delta b_g$: dbg
    • $b_{a_{b_{k+1}}}$: Baj
    • $b_{{\omega }_{b_{k+1}}}$: Bgj
    • $b_{a_{b_k}}$: Bai
    • $b_{{\omega }_{b_k}}$: Bgi
    • $R_W^{b_k}$: Qi.inverse()
    • $g^w$: G
    • $\Delta t_k$: sum_dt
    • $p_{b_{k+1}}^w$:Pj
    • $p_{b_k}^w$:Pi
    • $v_{b_k}^w$: Vi
    • ${\alpha }_{b_{k+1}^{b_k}}$: corrected_delta_p
    • $r_{b_{k+1}^{b_k}}^{-1}$: corrected_delta_q.inverse()
    • $q_{b_k}$: Qi
    • $q_{b_{k+1}}$: Qj
    • $v_{b_k+1}^w$: Vj
    • $v_{b_k}^w$: Vi
    • ${\beta }_{b_{k+1}}^{b_k}$: corrected_delta_v
  • corrected_delta_p也就是公式中的${\alpha }_{b_{k+1}}^k$,这里的计算用到了一阶泰勒展开
    • ${\alpha }_{b_{k+1}}^k=\delta P_i+J_{b_i^a}^P\ast \delta b_i^a+J_{b_i^g}^P\ast \delta b_i^g$
  • corrected_delta_v也就是公式中的${\beta }_{b_{k+1}}^{b_k}$,
    • ${\beta }_{b_{k+1}}^{b_k}=\delta V_i+J_{b_i^a}^V\ast \delta b_i^a+J_{b_i^g}^V\ast \delta b_i^g$
  • corrected_delta_q 是公式中的$r_{b_{k+1}}^{b_k}$, 这里的r代表角度信息
  • $q_{b_{k+1}}^{b_k}=\delta q_i⨂\left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_i}^q\delta b_i^q\end{array}\right]$
  • $J_{b_i^a}^P=\frac{\partial \delta P}{\partial \delta b_i^a}$
  • $J_{b_i^g}^P=\frac{\partial \delta P}{\partial \delta b_i^g}$
  • $J_{b_i^a}^V=\frac{\partial \delta V}{\partial \delta b_i^a}$
  • $J_{b_i^g}^V=\frac{\partial \delta V}{\partial \delta b_i^g}$
  • $J_{b_i^g}^q=\frac{\delta q_b^g}{\partial b_i^g}$
  • 旋转和加速度没有什么关系,和陀螺仪才有关系

---

pose_local_parameterization.cpp

Plus()

ComputeJacobian()

GlobalSize()

• return 7

LocalSize()

• return 6

---

ProjectionTwoFrameOneCamFactor.cpp

• 构造函数里只是把i时刻和j时刻的速度值进行赋值

Evaluate()

• 待优化量赋值<7, 7, 7, 1, 1>
  
• 残差计算(取前两个元素)

  residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>();
  residual = sqrt_info * residual;

• 优化变量: $[p_{b_i}^w,q_{b_i}^w]$, $[p_{b_j}^w,q_{b_j}^w]$, $[p_c^b,q_c^b]$, ${\lambda }_l$, $td$
• $[p_{b_i}^w,q_{b_i}^w]$, $[p_{b_j}^w,q_{b_j}^w]$第i帧和第j帧的位姿
• $[p_c^b,q_c^b]$ imu和相机的外参表示
• 第$l$个特征点的逆深度${\lambda }_l$
• 残差雅克比矩阵
• pts_i: $P_l^{c_j^{{}^{\prime }}}$ 第$l$个特征点在第i帧单位相机平面的投影
• pts_j: $P_l^{c_j^{{}^{\prime }}}$ 第$l$个特征点在第j帧单位相机平面的投影
• pts_camera_i$:P_l^{c_i^{{}^{\prime }}}$ pts_i在第i帧的重投影
• pts_camera_j: $P_l^{c_j^{{}^{\prime }}}$ pts_i在第j帧的重投影

---

ProjectionTwoFrameTwoCamFactor.cpp

• 构造函数里把i时刻和j时刻的速度值进行赋值
• 是否使用球面误差

#ifdef UNIT_SPHERE_ERROR
...
#endif

Evaluate()

• 待优化量赋值<7, 7, 7,7, 1, 1>

• 这边会比2Frame1Cam中多出一个相机和imu的坐标变换
  

• 残差计算(取前两个元素)

• 雅克比矩阵

---

ProjectionOneFrameTwoCamFactor.cpp

Evaluate()

• 待优化量赋值<7, 7, 1, 1>
• 这边会比2Frame2Cam中少去了i,j时刻的位姿
  
• 残差计算
• 残差雅克比

---

marginalization_factor.cpp

• 对于MarginalizationInfo, 主要是完成边缘化操作, 这一系列函数包括addResidualBlockInfo(), preMarginalize(),marginalize
• 而MarginalizationFactor函数是使用ceres对边缘化残差进行优化(包括先验残差, imu残差, 视觉残差)

preMarginalize()

• factors是在addResidualBlockInfo()函数中添加的残差的容器factors.emplace_back(residual_block_info)此时取出保存的残差
• Evaluate()计算所有状态变量构成的残差和雅克比矩阵
• addr拿到优化变量的地址
• 开辟新的一块内存空间进行数据关联

double *data = new double[size];
memcpy(data, it->parameter_blocks[i], sizeof(double) * size);
parameter_block_data[addr] = data;

marginalize()

• 边缘化操作函数,舒尔布边缘化,利用多线程进行计算

addResidualBlockInfo()

 factors.emplace_back(residual_block_info);

    std::vector<double *> &parameter_blocks = residual_block_info->parameter_blocks;
    std::vector<int> parameter_block_sizes = residual_block_info->cost_function->parameter_block_sizes();

    for (int i = 0; i < static_cast<int>(residual_block_info->parameter_blocks.size()); i++)
    {
        double *addr = parameter_blocks[i];
        int size = parameter_blocks.size();

        //reinterpret_cast指针转换为整数,重新解释
        parameter_block_sizes[reinterpret_cast<long>(addr)] = size;
    }

    for (int i = 0; i < static_cast<int>(residual_block_info->drop_set.size()); i++)
    {
        double *addr = parameter_blocks[residual_block_info->drop_set[i]];
        parameter_block_idx[reinterpret_cast<long>(addr)] = 0;
    }

• reinterpret_cast VS static_cast
• static_cast主要管：有继承关系类的指针和内置数据类型的转换（和C的内置类型转换规则一致，可能改变底层的位，也可能不改变）。
• reinterpret_cast主要管：所有指针(引用)之间的转换
• 在它们管理的交叉点处——有继承关系的指针的转换，处理方式有所不同。

---