VINS理論與代碼詳解4——初始化

VINS理論與代碼詳解4——初始化

1.      基於滑動窗口的純視覺單目初始化

    在介紹純視覺初始化前我們首先講一講為什麼要初始化？初始化要做什麼？以及初始化的作用？我們初始化的原因是單目慣性緊耦合系統是一個非線性程度很高的系統，首先單目是無法獲得空間中的絕對尺度，而IMU又必然存在偏置，在後面進行求解的時候還需要用到重力加速度（包括大小和方向），對於速度比較敏感的條件下，比如說無人機，又要精確的速度信息，因此，如何有效的在緊耦合系統處理之前計算出這些量，對整個緊耦合系統的魯棒性有著重大的意義（其實這裏就可以理解成相機標定一樣，沒有正確的標定好相機的內參，相機在進行定比特的時候必然不准，而且很有可能會掛掉）。所以初始化要做的事其實說起來很簡單，就是計算出絕對尺度s、陀螺儀偏置bg、加速度偏置ba、重力加速度G和每個IMU時刻的速度v，VINS中重點說明了加速度計偏置值一般都會和重力加速度耦合到一起（也就是被重力加速度給吸收掉），重力加速度的量級要遠大於其加速度偏置，而且在初始化時間內加速度計偏置比較小，很難真正的計算得到，因此忽略加速度計偏置的影響，在初始化中不再計算。初始化的作用是不言而喻的，直接影響整個緊耦合系統的魯棒性以及定比特精度，並且初始化一般都需要一個比較漫長的時間，VINS大概需要十秒左右，ORB_SLAM2結合IMU的時間設定在15秒完成初始化。話不多說，直接進入正題。

         純視覺初始化在第V點的A部分，首先構建一個滑動窗口，包含一組數據幀。論文中提及使用的是對極幾何模型的5點法求解單目相機的相對變換，包括相對旋轉和無尺度信息的比特移。其實基本上每個單目模型都是使用對極幾何在初始化中求解兩幀的相對變換，這裏需要注意的是旋轉是具有尺度不變性的（其實就是單比特旋轉，不會有尺度信息，你仔細想想是不是？）。然後三角化得到相應的3d點坐標，有這些3d點和滑動窗口中其他的幀的2d點就可以進行PNP求解獲得滑動窗口中的所有的比特姿和特征點3d坐標，至此，純視覺初始化就完成了。是不是很簡單？當然啊，畢竟只是簡單的視覺初始化，而真正複雜的是視覺慣性聯合初始化，也就是我們初始化的重點和難點，所以下面的知識點一定要打起精神學啦！

 

2.      視覺慣性聯合初始化

    視覺慣性聯合初始化在第V點的B部分，這裏作者給定義的名字叫Visual-Inertia Alignment，即視覺慣性聯合初始化（而在ORBSLAM2+IMU的論文裏，作者定義的名稱就叫IMU initialization，即IMU初始化），為什麼定義這樣一個名詞，我覺得有兩個意義，第一在進行陀螺儀偏置初始化的時候要同時使用到IMU測量的旋轉和視覺測量的旋轉，也就是要聯合視覺和慣性的數據。第二這裏求得的尺度S的值不僅僅是IMU的，還是視覺和IMU整個系統的尺度。在具體的講解初始化每個過程的時候，有必要來個總體的概括，初始化在物理意義上的定義其實就是固有參數的標定，在數學模型上的定義其實就是公式（6）的矩陣方程求解，而公式（6）其實就是來自於最原始的PVQ積分公式，其中Q旋轉對應著陀螺儀，而PV對應著加速度計，如果不明白的話，不要緊，看完下面的整體推導過程相信聰明的你一定會茅塞頓開。

（1）      陀螺儀偏置標定

旋轉我們可以通過兩種方式求得，一種是陀螺儀測量值，一種就是視覺觀測值。按照正常的理解兩者的大小一定是相等的（假設沒有誤差），但實際情况肯定有誤差，我們就來看看各自的誤差。陀螺儀的誤差有兩部分測量噪聲和陀螺儀偏置，噪聲暫時可以忽略（畢竟太小），而視覺的誤差就只有觀測噪聲（也可以忽略不管），因此兩者差值的絕對值就是陀螺儀偏置，將整個滑動窗口的所有的旋轉做差構成了一個最小化誤差模型：

    公式15中第一個式子的第一項和第二項作四元數旋轉的廣義乘積就可以得到相機從bk到bk+1下的相對旋轉（bk+1坐標系下），第三項是陀螺儀從bk+1到bk下的相對旋轉（bk坐標系下），兩者在做廣義乘積，就是首先從bk到bk+1旋轉，然後再從bk+1到bk旋轉，相當於做差（OA+AO=0），第二個式子就是前面預積分提到的一階線性近似。然後取最小二乘，當然也可以使用SVD分解等方法求解。注意在求得陀螺儀偏置之後要再次將陀螺儀偏置代入到預積分中再求一次預積分的值，會更加精確。

（1）      速度、重力加速度和尺度標定

作者在這裏將這三個狀態量統一到一個狀態向量中，如公式16所示：

速度的是在bk坐標系下的，重力加速度在初始相機坐標系下，就像前面提到的，求解著幾個量是由P、V數學模型求得，在滑動窗口中考慮到兩個連續關鍵幀bk和bk+1，下面進行論文中公式17和19的推導：

    公式推導之後就會得到論文中的公式17、18和19，我們重點關注下為什麼要這樣推導，以及推導得到的運動方程關系。首先為什麼要進行這樣的推導，這完全取决於狀態向量的定義方式，我們最終要得到的方程形式左邊一定是以狀態向量的形式來錶達的，而且還要滿足其他量都是已知的（從IMU預積分和視覺跟踪得到），因此就需要將方程進行如此的變化，才能滿足這樣的關系。然後是最後的形式我們可以看到狀態向量最終的形式維度是（n+1）*3+3+1，兩個連續幀產生的運動方程的維度是3+3+3+1（vbkbk，vbk+1bk+1，gc0，s），比較維度就可以看到最終得到的H矩陣一定是一個正定對稱矩陣，因此可以采用快速的Cholesky分解。

 

（1）      重力優化

    上面其實已經得到了重力加速度的大小和方向，這裏為什麼還需要對重力進行優化呢？理由很簡單，這裏計算的重力吸收了重力加速度計的偏置，雖然不需要計算重力加速度計的偏置，但重力還是需要優化的，說到優化重力加速度，肯定包含兩個量，大小和方向，也就是三個維度，但是一般來說大小是確定已知的（這裏設為9.8），因此其實我們要做的就是優化方向，是一個兩維的向量，下圖是優化重力的方法以及b1,b2單比特向量的方向確定模型。

 代碼詳解：

注：上接IMU預積分

步驟2：處理圖像特征數據

這裏進來的數據不是圖像數據哦，而是前面已經跟踪匹配好的歸一化平面坐標。將當前幀的特征存放在image中，image的第一個元素類型是相機的編號，代錶是第幾幀圖像（從0開始），第二個元素是歸一化特征點坐標和特征點編號（從1開始），然後直接進入到處理圖像特征數據的線程中estimator.processImage(image, img_msg->header)。

1）Estimator::processImage(constmap<int, vector<pair<int, Vector3d>>> &image, conststd_msgs::Header &header)處理圖像特征數據方法

         首先對進來的圖像特征數據根據視差判斷是否是關鍵幀，選擇丟弃當前幀（但保留IMU數據）或者丟弃滑動窗口中最老的一幀。

步驟1：將圖像數據和時間存到圖像幀類中：首先將數據和時間保存到圖像幀的對象imageframe中（ImageFrame對象中包含特征點，時間，比特姿R，t，預積分對象pre_integration，是否是關鍵幀）,同時將臨時的預積分值保存到此對象中（這裏的臨時預積分初值就是在前面IMU預積分的時候計算的），然後將圖像幀的對象imageframe保存到all_image_frame對象中（imageframe的容器），更新臨時預積分初始值。

步驟2：標定相機和IMU的外參數：接著如果沒有外部參數就標定外部參數，參數傳遞有的話就跳過這一步（默認有，如果是自己的設備，可以設置為2對外參進行在線標定）。

步驟3：初始化系統同時進行BA優化：當求解器處於可初始化狀態時（初始狀態是可初始化，初始化成功就設置為不可初始化狀態），判斷當前frame_count是否達到WINDOW_SIZE，確保有足够的frame參與初始化，這裏的frame_count是滑動窗口中圖像幀的數量，一開始被初始化為0，滑動窗口總幀數是10。有外部參數同時當前幀時間戳大於初始化時間戳0.1秒，就進行初始化操作。

步驟3.1：initialStructure()系統初始化，首先初始化Vision-only SFM，然後初始化Visual-Inertial Alignment，構成整個初始化過程。

①保證IMU充分運動，通過線加速度判斷，一開始通過線加速度的標准差（離散程度）判斷保證IMU充分運動，加速度標准差大於0.25則代錶imu充分激勵，足够初始化。

②純視覺初始化，對SlidingWindow中的圖像幀和相機姿態求解sfm問題，這裏解决的是關鍵幀的比特姿和特征點坐標。

步驟1.首先構建SFMFeature對象sfm_f，SFMFeature數組中包含了特征點狀態（是否被三角化），id，2d點，3d坐標以及深度，將特征管理器中的特征信息保存到SFMFeature對象sfm_f中sfm_f.push_back(tmp_feature)。

步驟2.接著由對極約束中的F矩陣恢複出R、t，主要調用方法relativePose(relative_R, relative_T, l)。relativePose方法中首先通過FeatureManeger獲取（滑動窗口中）第i幀和最後一幀的特征匹配corres，當corres匹配足够大時，考察最新的keyFrame和sliding window中某個keyFrame之間有足够feature匹配和足够大的視差（id為l=i），滿足這兩個條件，然後這兩幀之間通過五點法恢複出R，t並且三角化出3D的特征點feature point，這裏是使用solveRelativeRT(corres, relative_R, relative_T)，solveRelativeRT方法定義在solv_5pts.cpp類中，由對極約束中的F矩陣恢複出R、t，直接調用opencv中的方法，沒什麼好說的， 這裏值得注意的是，這種relativePose得到的比特姿是第l幀的，第l幀的篩選是從第一幀開始到滑動窗口所有幀中一開始滿足平均視差足够大的幀，這裏的第l幀會作為參考幀到下面的全局SFM使用。到這裏就已經得到圖像的特征點2d坐標的提取，相機第l幀和最後一幀之間的旋轉和平移（注意暫時還沒有得到特征的3d點坐標），有了這些信息就可以構建全局的SFM類GlobalSFM sfm，在這裏調用sfm.construct(frame_count + 1, Q, T,l,relative_R, relative_T,sfm_f, sfm_tracked_points)，這裏以第l幀作為參考幀，在進行PNP求解之前，需要判斷當前幀數要大於第l幀，這保證了第l幀直接跳過PNP步驟， 首先執行下面的第l幀和最後一幀的三角化，得到共視的特征點，供下面第l+1幀和最後一幀求解PNP，然後利用pnp求解l+1幀到最後一幀的比特姿R_initial, P_initial，最後的比特姿都保存在Pose中，一次循環，得到l+1，l+2…n-1幀的比特姿。跳出步驟2 的循環後，至此得到了l+1，l+2…n-1幀的比特姿以及l+1，l+2…幀與n-1幀的特征點三角化。然後再三角化l幀和i幀（在第l幀和最後一幀之間的幀）之間的3d坐標，（這裏不明白為什麼要做兩次，是可以三角化出更多的特征點嗎？？？？），接著PNP求解l-1，l-2…0幀和l幀之間的比特姿已經三角化相應的特征點坐標，最後三角化其他所有的特征點。至此得到了滑動窗口中所有相機的比特姿以及特征點的3d坐標。第6部就是進行BA優化，使用的是ceres優化比特姿和特征點，這裏可以參考視覺SLAM第十講中的內容，優化方式相同。

步驟4：visualInitialAlign中調用VisualIMUAlignment方法，真正的視覺慣性聯合初始化，imu與視覺對齊，獲取絕對尺度等。這個方法定義在initial/initial_alignment.h中。

步驟4.1：solveGyroscopeBias計算陀螺儀偏置，整個方法的計算模型由論文中給出，使用LTLD方法求解最小二乘問題，delta_bg = A.ldlt().solve(b);這裏A +=tmp_A.transpose() * tmp_A，b += tmp_A.transpose() * tmp_b，其實就是處理AT*A*x=AT*b問題，一般的最小二乘問題直接處理Ax=b也就是Ax-b=0即可，這裏是使用LDLT方法，兩邊同乘以A矩陣的轉置得到的AT*A一定是可逆的，因此就可以直接兩邊同乘以其逆即可，相應的說明詳見LDLT方法。得到陀螺儀偏置之後將其值保存到前面定義的Bgs[]中，最後在重新計算一次預積分。

步驟4.2：LinearAlignment計算尺度，重力加速度和速度。論文中給出的公式是相鄰兩個速度的模型，映射到整個n+1個速度模型中，A矩陣一定是一個正定矩陣（實對稱矩陣），代碼中定義的A和b即是最總的H和b，tmp_A和tmp_b相鄰速度間的臨時變量。最後的求解方法：x = A.ldlt().solve(b)；然後調用RefineGravity重新計算重力加速度方向，得到最優解。