Vins-mono是香港科技大學(xué)開源的一個(gè)VIO算法?是用緊耦合方法實(shí)現(xiàn)的，通過單目+IMU恢復(fù)出尺度，效果非常棒。

1.特征點(diǎn)跟蹤

首先用cv::goodFeaturesToTrack在第一幀圖像上面找最強(qiáng)的150個(gè)特征點(diǎn)，非極大值抑制半徑為30。新的特征點(diǎn)都有自己的新的對應(yīng)的id。然后在下一幀過來時(shí)，對這些特征點(diǎn)用光流法進(jìn)行跟蹤，在下一幀上找匹配點(diǎn)。然后對前后幀中這些匹配點(diǎn)進(jìn)行校正。先對特征點(diǎn)進(jìn)行畸變校正，再投影到以原點(diǎn)為球心，半徑為1的球面上，再延伸到深度歸一化平面上，獲得最終校正后的位置。對于每對匹配點(diǎn)，基于校正后的位置，用F矩陣加ransac來篩選。然后再在匹配上的特征點(diǎn)之外的區(qū)域，用cv::goodFeaturesToTrack搜索最強(qiáng)的新的特征點(diǎn)，把特征點(diǎn)數(shù)量補(bǔ)上150個(gè)。

最后，把剩下的這些特征點(diǎn)，把圖像點(diǎn)投影回深度歸一化平面上，再畸變校正，再投影到球面上，再延伸到深度歸一化平面上，得到校正后的位置。把校正后的位置發(fā)送出去。

特征點(diǎn)跟蹤和匹配，就是前一幀到這一幀的，一幀幀繼承下去?；蛘呱尚碌奶卣鼽c(diǎn)。

2.初始化

2.1外參中的旋轉(zhuǎn)

用機(jī)器人手眼標(biāo)定的方法，計(jì)算出外參中的旋轉(zhuǎn)。

特別的：手眼標(biāo)定：

其實(shí)就是AX=XB? X是手眼的外參（對于VINS問題，也就是IMU和Camera的外參）

A是IMU在相鄰時(shí)間的變換? ?B是攝像頭的變換

從而：