【SLAM】VINS-Fusion解析——流程

VINS-Fusion分析 因为时间原因，没有像vins-mono看的和写的那么具体。有时间的话我会补充完整版。 vins-fusion不像mono那样有三个node，它只有一个node，在rosNodeTest.cpp里。我推测之所以这样做，是为了方便非ros版的c++项目使用。先上流程图。
  vins-fusion和vins-mono关于单目和双目的区别，主要体现在特征点的三角化，初始化和重投影误差上。  

mono的三角化，是对前一帧和后一帧进行的， fusion的三角化，是对左目和双目进行的。 mono的初始化，非常复杂，先SfM，再与IMU进行松耦合相互标定； fusion的初始化，是在三角化后，再进行一次非线性优化就完成了。 mono的重投影误差，是左目的i，j时刻关于共视点的重投影； fusion的重投影误差，是左目的i时刻和右目的j时候关于共视点的重投影。

  基本上所有的内容都在estimator.cpp。 几个比较印象深刻的的地方。   feature_tracker里面，  

并没有对imageConstPtr的时间/频率校准工作，而只是放在img_buf里面； 单目光流跟踪加上了反向光流追踪。

  数据预处理，  

没有采用messurements这个大的数据结构，而是用了几个小的vec来存放着对齐的数据； 对于边界位置的IMU采用，放弃了被相邻两帧共享的线性插值处理；

  processMeasurements：  

对于状态量Rs，mono里面初始化是Identity，而在fusion里面，初始化是用acc的平均值与gw对比就获得了相对于先验w系的旋转，通过这个获得Rs[0]；

  然后初始化区别非常大，  

没有construct和IMUvisual align那一些复杂的操作。此时PVQ都是w系的，那么左右双目的位姿也都是w系的，可以直接三角化双目的特征点获得特征点在w系上的坐标和逆深度； 之后就可以通过PnP进一步优化Ps和Rs。 最后非线性优化一下，就结束了。

  TODO 而mono里面，能不能也采用一样的方式呢？一开始PVQ都是w系的，scaler也是，然后三角化不同pose，得到一些3D点，然后PnP优化？这样是不是精度就不行了？这是不是fusion双目版比mono版精度低的理由？   optimization:  

重投影误差用的是不同camera之间的。

<code class="prism language-cpp has-numbering">  <span class="token comment">//左相机在i时刻和j时刻分别观测到路标点</span>
    ProjectionTwoFrameOneCamFactor <span class="token operator">*</span>f_td <span class="token operator">=</span> <span class="token keyword">new</span> <span class="token function">ProjectionTwoFrameOneCamFactor</span><span class="token punctuation">(</span>pts_i<span class="token punctuation">,</span> pts_j<span class="token punctuation">,</span> it_per_id<span class="token punctuation">.</span>feature_per_frame<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">.</span>velocity<span class="token punctuation">,</span> it_per_frame<span class="token punctuation">.</span>velocity<span class="token punctuation">,</span>
                                                     it_per_id<span class="token punctuation">.</span>feature_per_frame<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">.</span>cur_td<span class="token punctuation">,</span> it_per_frame<span class="token punctuation">.</span>cur_td<span class="token punctuation">)</span><span class="token punctuation">;</span>
    problem<span class="token punctuation">.</span><span class="token function">AddResidualBlock</span><span class="token punctuation">(</span>f_td<span class="token punctuation">,</span> loss_function<span class="token punctuation">,</span> para_Pose<span class="token punctuation">[</span>imu_i<span class="token punctuation">]</span><span class="token punctuation">,</span> para_Pose<span class="token punctuation">[</span>imu_j<span class="token punctuation">]</span><span class="token punctuation">,</span> para_Ex_Pose<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">,</span> para_Feature<span class="token punctuation">[</span>feature_index<span class="token punctuation">]</span><span class="token punctuation">,</span> para_Td<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>

<span class="token keyword">if</span><span class="token punctuation">(</span>STEREO <span class="token operator">&&</span> it_per_frame<span class="token punctuation">.</span>is_stereo<span class="token punctuation">)</span>
<span class="token punctuation">{</span>                
    Vector3d pts_j_right <span class="token operator">=</span> it_per_frame<span class="token punctuation">.</span>pointRight<span class="token punctuation">;</span>
    <span class="token keyword">if</span><span class="token punctuation">(</span>imu_i <span class="token operator">!=</span> imu_j<span class="token punctuation">)</span>
    <span class="token punctuation">{</span>   <span class="token comment">//左相机在i时刻、右相机在j时刻分别观测到路标点</span>
        ProjectionTwoFrameTwoCamFactor <span class="token operator">*</span>f <span class="token operator">=</span> <span class="token keyword">new</span> <span class="token function">ProjectionTwoFrameTwoCamFactor</span><span class="token punctuation">(</span>pts_i<span class="token punctuation">,</span> pts_j_right<span class="token punctuation">,</span> it_per_id<span class="token punctuation">.</span>feature_per_frame<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">.</span>velocity<span class="token punctuation">,</span> it_per_frame<span class="token punctuation">.</span>velocityRight<span class="token punctuation">,</span>
                                                     it_per_id<span class="token punctuation">.</span>feature_per_frame<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">.</span>cur_td<span class="token punctuation">,</span> it_per_frame<span class="token punctuation">.</span>cur_td<span class="token punctuation">)</span><span class="token punctuation">;</span>
        problem<span class="token punctuation">.</span><span class="token function">AddResidualBlock</span><span class="token punctuation">(</span>f<span class="token punctuation">,</span> loss_function<span class="token punctuation">,</span> para_Pose<span class="token punctuation">[</span>imu_i<span class="token punctuation">]</span><span class="token punctuation">,</span> para_Pose<span class="token punctuation">[</span>imu_j<span class="token punctuation">]</span><span class="token punctuation">,</span> para_Ex_Pose<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">,</span> para_Ex_Pose<span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span><span class="token punctuation">,</span> para_Feature<span class="token punctuation">[</span>feature_index<span class="token punctuation">]</span><span class="token punctuation">,</span> para_Td<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    <span class="token punctuation">}</span>
    <span class="token keyword">else</span>
    <span class="token punctuation">{</span>   <span class="token comment">//左相机和右相机在i时刻分别观测到路标点</span>
        ProjectionOneFrameTwoCamFactor <span class="token operator">*</span>f <span class="token operator">=</span> <span class="token keyword">new</span> <span class="token function">ProjectionOneFrameTwoCamFactor</span><span class="token punctuation">(</span>pts_i<span class="token punctuation">,</span> pts_j_right<span class="token punctuation">,</span> it_per_id<span class="token punctuation">.</span>feature_per_frame<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">.</span>velocity<span class="token punctuation">,</span> it_per_frame<span class="token punctuation">.</span>velocityRight<span class="token punctuation">,</span>
                                                     it_per_id<span class="token punctuation">.</span>feature_per_frame<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">.</span>cur_td<span class="token punctuation">,</span> it_per_frame<span class="token punctuation">.</span>cur_td<span class="token punctuation">)</span><span class="token punctuation">;</span>
        problem<span class="token punctuation">.</span><span class="token function">AddResidualBlock</span><span class="token punctuation">(</span>f<span class="token punctuation">,</span> loss_function<span class="token punctuation">,</span> para_Ex_Pose<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">,</span> para_Ex_Pose<span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span><span class="token punctuation">,</span> para_Feature<span class="token punctuation">[</span>feature_index<span class="token punctuation">]</span><span class="token punctuation">,</span> para_Td<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    <span class="token punctuation">}</span>
   
<span class="token punctuation">}</span>


</code>