本篇代碼閱讀筆記接着上一篇VINS-Mono代碼閱讀筆記（七）：vins_estimator中相機與IMU的外參標定，來學習一下VINS-Mono中初始化部分的代碼。單目緊耦合的VIO系統是一個高非線性系統，在系統開始運行的時候需要準確的初始化估計。VINS-Mono系統通過將IMU預積分值與視覺估計結果進行鬆散的對齊來得到必要的初始化。在VINS論文的第V. ESTIMATOR INITIALIZATION部分將初始化分爲兩部分來描述：A.在滑動窗口中僅視覺的SFM估計；B.視覺和慣導的對齊。
初始化部分是從processImage函數中開始的，所以先從processImage中初始化部分的代碼開始來分析。
1.processImage中初始化代碼
processImage函數中，在做完相機-IMU之間的外參標定後會進行初始化是否完成的判斷，也就是下面的代碼。
if (solver_flag == INITIAL)
{
//WINDOW_SIZE==10 frame_count達到了WINDOW_SIZE，這樣能確保有足夠的frame參與初始化
if (frame_count == WINDOW_SIZE)
{
bool result = false;
//外參初始化成功且當前幀時間戳距離上次初始化時間戳大於0.1秒，就進行初始化操作
if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
{
//對視覺和慣性單元進行初始化
result = initialStructure();
//初始化時間戳更新
initial_timestamp = header.stamp.toSec();
}
if(result)//初始化成功
{
//先進行一次滑動窗口非線性優化，得到當前幀與第一幀的位姿
solver_flag = NON_LINEAR;
solveOdometry();
slideWindow();
f_manager.removeFailures();
ROS_INFO("Initialization finish!");
last_R = Rs[WINDOW_SIZE];
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
else
slideWindow();//初始化失敗則直接滑動窗口
}
else
frame_count++;//圖像幀數量+1
}
solver_flag用於判斷初始化是否完成，當其值爲INITIAL的時候表示初始化未完成，初始化成功後solver_flag的值會修改爲NON_LINEAR。
從上邊代碼可以看到，只有當滑動窗口中的圖像幀個數達到WINDOW_SIZE=10後纔會進行初始化操作。這裏滑動窗口的大小設置爲10幀，是爲了限制計算複雜度，因爲後邊的旋轉和平移計算是需要遍歷滑動窗口中最後一幀之前的所有幀的。可以看到，調用initialStructure函數後，如果判斷初始化失敗了，就直接去滑動窗口（也就是根據策略刪除窗口中的某一幀）。
2.初始化結構體函數initialStructure代碼分析
initialStructure代碼如下：
/**
* 視覺慣導聯合初始化函數
*/
bool Estimator::initialStructure()
{
TicToc t_sfm;
//1.1.check imu observibility 通過計算標準差來進行判斷
{
map<double, ImageFrame>::iterator frame_it;
Vector3d sum_g;
//遍歷除了第一幀圖像外的所有圖像幀
for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
{
double dt = frame_it->second.pre_integration->sum_dt;
//計算每一幀圖像對應的加速度
Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
//圖像的加速度之累加和
sum_g += tmp_g;
}
Vector3d aver_g;
//計算平均加速度。因爲上邊計算的是除了第一幀圖像之外的其他所有圖像幀對應的加速度之和，所以這裏要減去1
aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);
double var = 0;
for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
{
double dt = frame_it->second.pre_integration->sum_dt;
//計算獲得每一幀的加速度
Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
//加速度減去平均加速度的差值的平方的累加
var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);
//cout << "frame g " << tmp_g.transpose() << endl;
}
/**
* 簡單來說，標準差是一組數值自平均值分散開來的程度的一種測量觀念。
* 一個較大的標準差，代表大部分的數值和其平均值之間差異較大；一個較小的標準差，代表這些數值較接近平均值。
* 方差公式：s^2=[(x1-x)^2 +...(xn-x)^2]/n
* 標準差等於方差的算數平方根，標準差公式：s=sqrt(s^2)
*/
var = sqrt(var / ((int)all_image_frame.size() - 1));
//ROS_WARN("IMU variation %f!", var);
//通過加速度標準差判斷IMU是否有充分運動，標準差必須大於等於0.25
if(var < 0.25)
{
ROS_INFO("IMU excitation not enouth!");
//return false;
}
}
// global sfm
Quaterniond Q[frame_count + 1];//旋轉量，四元數數組
Vector3d T[frame_count + 1];//平移量數組
map<int, Vector3d> sfm_tracked_points;//用於存儲sfm重建出來的特徵點的座標（所有觀測到該特徵點的圖像幀ID和圖像座標）
vector<SFMFeature> sfm_f;
/**
* 1.2.將f_manager.feature中的feature存儲到sfm_f中
*/
for (auto &it_per_id : f_manager.feature)
{
int imu_j = it_per_id.start_frame - 1;
SFMFeature tmp_feature;
tmp_feature.state = false;
tmp_feature.id = it_per_id.feature_id;
//遍歷每一個能觀察到該feature的frame
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
Vector3d pts_j = it_per_frame.point;
//每個特徵點能夠被哪些幀觀測到以及特徵點在這些幀中的座標
tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()}));
}
sfm_f.push_back(tmp_feature);
}
/**
* 1.3.位姿求解
*/
Matrix3d relative_R;
Vector3d relative_T;
int l;
//通過求取本質矩陣來求解出位姿
/**
* 這裏的l表示滑動窗口中第l幀是從第一幀開始到滑動窗口中第一個滿足與當前幀的平均視差足夠大的幀，
* 會作爲參考幀到下面的全局sfm使用，得到的Rt爲當前幀到第l幀的座標系變換Rt，存儲在relative_R和relative_T當中
* */
if (!relativePose(relative_R, relative_T, l))
{
ROS_INFO("Not enough features or parallax; Move device around");
return false;
}
GlobalSFM sfm;
/**
* 1.4.三角化特徵點，對滑窗每一幀求解sfm問題
*/
if(!sfm.construct(frame_count + 1, Q, T, l,
relative_R, relative_T,
sfm_f, sfm_tracked_points))
{
ROS_DEBUG("global SFM failed!");
marginalization_flag = MARGIN_OLD;
return false;
}
/**
* 1.5.對於所有的圖像幀，包括不在滑動窗口中的，提供初始的RT估計，然後solvePnP進行優化
*/
//solve pnp for all frame
map<double, ImageFrame>::iterator frame_it;
map<int, Vector3d>::iterator it;
frame_it = all_image_frame.begin( );
//遍歷所有的圖像幀
for (int i = 0; frame_it != all_image_frame.end( ); frame_it++)
{
// provide initial guess
cv::Mat r, rvec, t, D, tmp_r;
if((frame_it->first) == Headers[i].stamp.toSec())
{
frame_it->second.is_key_frame = true;
//各幀相對於參考幀的旋轉和相機-IMU之間的旋轉做乘積，就將座標系轉換到了IMU座標系下
frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose();
//各幀相對於參考幀的平移
frame_it->second.T = T[i];
i++;
continue;
}
if((frame_it->first) > Headers[i].stamp.toSec())
{
i++;
}
//注意這裏的 Q和 T是圖像幀的位姿，而不是求解PNP時所用的座標系變換矩陣，兩者具有對稱關係
Matrix3d R_inital = (Q[i].inverse()).toRotationMatrix();
Vector3d P_inital = - R_inital * T[i];
cv::eigen2cv(R_inital, tmp_r);
//使用羅德里格斯公式將旋轉矩陣轉換成旋轉向量
cv::Rodrigues(tmp_r, rvec);
cv::eigen2cv(P_inital, t);
frame_it->second.is_key_frame = false;
//獲取 pnp需要用到的存儲每個特徵點三維點和圖像座標的 vector
vector<cv::Point3f> pts_3_vector;
vector<cv::Point2f> pts_2_vector;
for (auto &id_pts : frame_it->second.points)
{
int feature_id = id_pts.first;
for (auto &i_p : id_pts.second)
{
it = sfm_tracked_points.find(feature_id);
if(it != sfm_tracked_points.end())
{
Vector3d world_pts = it->second;
cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2));
pts_3_vector.push_back(pts_3);
Vector2d img_pts = i_p.second.head<2>();
cv::Point2f pts_2(img_pts(0), img_pts(1));
pts_2_vector.push_back(pts_2);
}
}
}
/**
* 構造相機內參，因爲已經是歸一化後坐標了，這裏的內參就是單位矩陣了
*/
cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
//保證特徵點數大於5，這樣就可以用五點法求解
if(pts_3_vector.size() < 6)
{
cout << "pts_3_vector size " << pts_3_vector.size() << endl;
ROS_DEBUG("Not enough points for solve pnp !");
return false;
}
/**
* PnP求解位姿
* bool cv::solvePnP (
* InputArray objectPoints, 世界座標系中的特徵點的座標
* InputArray imagePoints, 圖像中的像素點的座標
* InputArray cameraMatrix, 相機內參
* InputArray distCoeffs, 去畸變參數
* OutputArray rvec, 要求的旋轉量
* OutputArray tvec, 要求的平移量
* bool useExtrinsicGuess = false,
* int flags = SOLVEPNP_ITERATIVE
* )
*/
if (! cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1))
{
ROS_DEBUG("solve pnp fail!");
return false;
}
//旋轉向量到旋轉矩陣的轉換
cv::Rodrigues(rvec, r);
MatrixXd R_pnp,tmp_R_pnp;
//將矩陣從cv中的cv::Mat類型轉換爲eigen中的MatrixXd
cv::cv2eigen(r, tmp_R_pnp);
//這裏也同樣需要將座標變換矩陣轉變成圖像幀位姿，並轉換爲IMU座標系的位姿
R_pnp = tmp_R_pnp.transpose();
MatrixXd T_pnp;
//將位移矩陣轉換爲MatrixXd形式
cv::cv2eigen(t, T_pnp);
T_pnp = R_pnp * (-T_pnp);
//將求得的旋轉和平移存儲起來，這裏RIC[0].transpose()是相機到IMU的旋轉，這樣就把相機座標系中求出的位姿旋轉到IMU座標系中去了
frame_it->second.R = R_pnp * RIC[0].transpose();
frame_it->second.T = T_pnp;
}
//2.視覺慣性對齊求解
if (visualInitialAlign())
return true;
else
{
ROS_INFO("misalign visual structure with IMU");
return false;
}
}
按照作者論文中的描述，該函數中分爲以下兩部分：
2.1 滑動窗口中的視覺SFM
這部分代碼對應的是論文中V-A部分。通常遇到比較長比較複雜的代碼，我比較喜歡將代碼按照邏輯劃分成塊來分析。這部分也照此方法來劃分爲下面五個部分。
2.1.1 通過標準差判斷IMU的運動情況
這部分對應代碼如下：
//1.1.check imu observibility 通過計算標準差來進行判斷
{
map<double, ImageFrame>::iterator frame_it;
Vector3d sum_g;
//遍歷除了第一幀圖像外的所有圖像幀
for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
{
double dt = frame_it->second.pre_integration->sum_dt;
//計算每一幀圖像對應的加速度
Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
//圖像的加速度之累加和
sum_g += tmp_g;
}
Vector3d aver_g;
//計算平均加速度。因爲上邊計算的是除了第一幀圖像之外的其他所有圖像幀對應的加速度之和，所以這裏要減去1
aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);
double var = 0;
for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
{
double dt = frame_it->second.pre_integration->sum_dt;
//計算獲得每一幀的加速度
Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
//加速度減去平均加速度的差值的平方的累加
var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);
//cout << "frame g " << tmp_g.transpose() << endl;
}
/**
* 簡單來說，標準差是一組數值自平均值分散開來的程度的一種測量觀念。
* 一個較大的標準差，代表大部分的數值和其平均值之間差異較大；一個較小的標準差，代表這些數值較接近平均值。
* 方差公式：s^2=[(x1-x)^2 +...(xn-x)^2]/n
* 標準差等於方差的算數平方根，標準差公式：s=sqrt(s^2)
*/
var = sqrt(var / ((int)all_image_frame.size() - 1));
//ROS_WARN("IMU variation %f!", var);
//通過加速度標準差判斷IMU是否有充分運動，標準差必須大於等於0.25
if(var < 0.25)
{
ROS_INFO("IMU excitation not enouth!");
//return false;
}
}
以上代碼其實是通過IMU預積分得到的圖像幀對應的速度來計算所有圖像的加速度累加和，再通過加速度累加和計算平均加速度，進而求得加速度的標準差。我在代碼註釋中也寫明瞭標準差的物理意義：標準差是一組數值自平均值分散開來的程度的一種測量觀念。對應到VINS系統中來說，這裏計算的標準差可以反應IMU中加速度計運行的情況。勻速運動的加速度爲0，在這種情況下加速度計就要“失效”了。
但是不清楚的是，爲什麼這裏設定的標準差閾值0.25的情況下，在標準差小於0.25的時候分支代碼中的return false作者註釋掉了。這樣的話這一部分代碼的計算就是無用的了。爲什麼要註釋掉呢？？？
2.1.2 將全局feature當中的特徵存儲到sfm_f中
這部分對應代碼如下：
/**
* 1.2.將f_manager.feature中的feature存儲到sfm_f中
*/
for (auto &it_per_id : f_manager.feature)
{
int imu_j = it_per_id.start_frame - 1;
SFMFeature tmp_feature;
tmp_feature.state = false;
tmp_feature.id = it_per_id.feature_id;
//遍歷每一個能觀察到該feature的frame
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
Vector3d pts_j = it_per_frame.point;
tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()}));
}
sfm_f.push_back(tmp_feature);
}
遍歷feature，獲得裏邊每一個特徵點對應的圖像幀中點的x,y座標存入tmp_feature中，最後將tmp_feature存入到sfm_f中。後邊就可以通過sfm_f獲取到feature的id以及有哪些幀可以觀測到該feature以及在圖像幀中的座標。
2.1.3 求解最新幀和滑動窗口中第一個滿足條件的幀之間的位姿關係
這部分對應代碼如下：
/**
* 1.3.位姿求解
*/
Matrix3d relative_R;
Vector3d relative_T;
int l;
//通過求取本質矩陣來求解出位姿
/**
* 這裏的l表示滑動窗口中第l幀是從第一幀開始到滑動窗口中第一個滿足與當前幀的平均視差足夠大的幀，
* 會作爲參考幀到下面的全局sfm使用，得到的Rt爲當前幀到第l幀的座標系變換Rt，存儲在relative_R和relative_T當中
* */
if (!relativePose(relative_R, relative_T, l))
{
ROS_INFO("Not enough features or parallax; Move device around");
return false;
}
這裏是要通過relativePose函數計算出滑動窗口中第l幀和最新圖像幀之間的旋轉和平移的。至於這個l是怎麼計算出來的，看下面的代碼就知道了。
/**
* 這裏的第l幀是從第一幀開始到滑動窗口中第一個滿足與當前幀的平均視差足夠大的幀，
* 會作爲參考幀到下面的全局sfm使用，得到的Rt爲當前幀到第l幀的座標系變換Rt
* 該函數判斷滑動窗口中第一個到窗口最後一幀對應特徵點的平均視差大於30，且內點數目大於12的幀，此時可進行初始化，同時返回當前幀到第l幀的座標系變換R和T
* */
bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)
{
// find previous frame which contians enough correspondance and parallex with newest frame
for (int i = 0; i < WINDOW_SIZE; i++)
{
vector<pair<Vector3d, Vector3d>> corres;
//尋找第i幀到窗口最後一幀的對應特徵點，存放在corres中
corres = f_manager.getCorresponding(i, WINDOW_SIZE);
//匹配的特徵點對要大於20
if (corres.size() > 20)
{
//計算平均視差
double sum_parallax = 0;
double average_parallax;
for (int j = 0; j < int(corres.size()); j++)
{
//第j個對應點在第i幀和最後一幀的(x,y)
Vector2d pts_0(corres[j].first(0), corres[j].first(1));
Vector2d pts_1(corres[j].second(0), corres[j].second(1));
//計算視差
double parallax = (pts_0 - pts_1).norm();
//計算視差的總和
sum_parallax = sum_parallax + parallax;
}
//計算平均視差
average_parallax = 1.0 * sum_parallax / int(corres.size());
//判斷是否滿足初始化條件：視差>30和內點數滿足要求(大於12)
//solveRelativeRT()通過基礎矩陣計算當前幀與第l幀之間的R和T,並判斷內點數目是否足夠
//同時返回窗口最後一幀（當前幀）到第l幀（參考幀）的relative_R，relative_T
if(average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres, relative_R, relative_T))
{
//l中存放的是窗口中從最前邊開始遍歷和最後一幀滿足視差和RT估計的第一個幀的id
l = i;
ROS_DEBUG("average_parallax %f choose l %d and newest frame to triangulate the whole structure", average_parallax * 460, l);
return true;
}
}
}
return false;
}
relativePose函數中遍歷滑動窗口中的圖像幀，計算每一幀和最新一幀圖像之間對應的匹配特徵點對。在遍歷滑動窗口過程中，如果匹配的特徵點對數大於20則計算這些特徵點對的平均視差。如果平均視差大於30，則繼續根據匹配的特徵點對調用solveRelativeRT計算旋轉和平移。
solveRelativeRT代碼如下所示：
/**
* 根據提供的特徵點對計算旋轉矩陣和平移向量
*/
bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)
{
if (corres.size() >= 15)
{
vector<cv::Point2f> ll, rr;
for (int i = 0; i < int(corres.size()); i++)
{
ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
}
cv::Mat mask;
/**
* 求得本質矩陣
* Mat cv::findFundamentalMat(
* InputArray points1, //第一幅圖像中的特徵點數組
* InputArray points2, //第二幅圖像中的特徵點數組
* int method = FM_RANSAC, //計算fundamental matrix的方法，這裏使用了cv::FM_RANSAC，說明使用了8點法進行求解的
* CV_FM_7POINT for a 7-point algorithm. N=7
* CV_FM_8POINT for an 8-point algorithm. N≥8
* CV_FM_RANSAC for the RANSAC algorithm. N≥8
* CV_FM_LMEDS for the LMedS algorithm. N≥8
* double ransacReprojThreshold = 3., //點到對極線的最大距離，超過這個值的點將被捨棄
* double confidence = 0.99, //矩陣正確的可信度
* OutputArray mask = noArray() //輸出在計算過程中沒有被捨棄的點
* )
* 該函數對應鏈接：https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#gae420abc34eaa03d0c6a67359609d8429
*/
cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);
//cameraMatrix初始化爲單位矩陣
cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
cv::Mat rot, trans;
//由本質矩陣恢復出位姿
/**
* int cv::recoverPose ( InputArray E, //本質矩陣
* InputArray points1, //第一幅圖像點的數組
* InputArray points2, //第二幅圖像點的數組
* InputArray cameraMatrix, //相機內參
* OutputArray R, //第一幀座標系到第二幀座標系的旋轉矩陣
* OutputArray t, //第一幀座標系到第二幀座標系的平移向量
* InputOutputArray mask = noArray() //在findFundamentalMat()中沒有被捨棄的點
* )
*/
int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);
//cout << "inlier_cnt " << inlier_cnt << endl;
Eigen::Matrix3d R;
Eigen::Vector3d T;
for (int i = 0; i < 3; i++)
{
T(i) = trans.at<double>(i, 0);
for (int j = 0; j < 3; j++)
R(i, j) = rot.at<double>(i, j);
}
//得到旋轉平移
Rotation = R.transpose();
Translation = -R.transpose() * T;
//內點數必須大於12
if(inlier_cnt > 12)
return true;
else
return false;
}
return false;
}
這裏計算出滑動窗口中第一個滿足條件的幀和最新幀之間的旋轉和平移之後，還要判斷內點數是否大於12，大於12才認爲計算出的旋轉和平移量是有效的。
2.1.4 三角化求解地圖點的深度
這部分代碼如下：
GlobalSFM sfm;
/**
* 1.4.三角化特徵點，對滑窗每一幀求解sfm問題
*/
if(!sfm.construct(frame_count + 1, Q, T, l,
relative_R, relative_T,
sfm_f, sfm_tracked_points))
{
ROS_DEBUG("global SFM failed!");
marginalization_flag = MARGIN_OLD;
return false;
}
construct函數代碼如下，具體步驟見代碼中註釋部分，需要注意的是這裏求解的pose[i]表示第l幀到第i幀的變換矩陣T=[R|T]。
/**
* 對窗口中每個圖像幀求解sfm問題，得到所有圖像幀相對於參考幀的旋轉四元數Q、平移向量T和特徵點座標sfm_tracked_points
* 參數frame_num的值爲frame_count + 1
* 傳入的參數l就是參考幀的index
* */
bool GlobalSFM::construct(int frame_num, Quaterniond* q, Vector3d* T, int l,
const Matrix3d relative_R, const Vector3d relative_T,
vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points)
{
feature_num = sfm_f.size();
//cout << "set 0 and " << l << " as known " << endl;
// have relative_r relative_t
// intial two view
//q爲四元數數組，大小爲frame_count+1
q[l].w() = 1;
q[l].x() = 0;
q[l].y() = 0;
q[l].z() = 0;
//T爲平移量數組，大小爲frame_count+1
T[l].setZero();
//往q中存入最新幀的旋轉，該旋轉等於第l幀的旋轉和相對旋轉的四元數乘積
q[frame_num - 1] = q[l] * Quaterniond(relative_R);
T[frame_num - 1] = relative_T;
//cout << "init q_l " << q[l].w() << " " << q[l].vec().transpose() << endl;
//cout << "init t_l " << T[l].transpose() << endl;
//rotate to cam frame
Matrix3d c_Rotation[frame_num];
Vector3d c_Translation[frame_num];
Quaterniond c_Quat[frame_num];
double c_rotation[frame_num][4];
double c_translation[frame_num][3];
Eigen::Matrix<double, 3, 4> Pose[frame_num];
c_Quat[l] = q[l].inverse();//c_Quat[l]中存放q[l]的逆
c_Rotation[l] = c_Quat[l].toRotationMatrix();//四元數轉爲旋轉矩陣
c_Translation[l] = -1 * (c_Rotation[l] * T[l]);
/**
* matrix.block(i,j, p, q) : 表示返回從矩陣(i, j)開始，每行取p個元素，每列取q個元素所組成的臨時新矩陣對象，原矩陣的元素不變；
* matrix.block<p,q>(i, j) :<p, q>可理解爲一個p行q列的子矩陣，該定義表示從原矩陣中第(i, j)開始，獲取一個p行q列的子矩陣，
* 返回該子矩陣組成的臨時矩陣對象，原矩陣的元素不變；
*/
//將第l幀的旋轉和平移量存入到Pose當中
Pose[l].block<3, 3>(0, 0) = c_Rotation[l];
Pose[l].block<3, 1>(0, 3) = c_Translation[l];
c_Quat[frame_num - 1] = q[frame_num - 1].inverse();
c_Rotation[frame_num - 1] = c_Quat[frame_num - 1].toRotationMatrix();
c_Translation[frame_num - 1] = -1 * (c_Rotation[frame_num - 1] * T[frame_num - 1]);
Pose[frame_num - 1].block<3, 3>(0, 0) = c_Rotation[frame_num - 1];
Pose[frame_num - 1].block<3, 1>(0, 3) = c_Translation[frame_num - 1];
//至此，計算出了滑動窗口中第l幀的位姿Pose[l]和最新幀的位姿Pose[frame_num-1]，下面就是三角化處理
//1: trangulate between l ----- frame_num - 1
//2: solve pnp l + 1; trangulate l + 1 ------- frame_num - 1;
for (int i = l; i < frame_num - 1 ; i++)//對於參考幀和當前幀之間的某一幀，三角化該幀和當前幀的路標點
{
// solve pnp
if (i > l)//不是參考幀
{
Matrix3d R_initial = c_Rotation[i - 1];
Vector3d P_initial = c_Translation[i - 1];
if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
return false;
c_Rotation[i] = R_initial;
c_Translation[i] = P_initial;
c_Quat[i] = c_Rotation[i];
Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
Pose[i].block<3, 1>(0, 3) = c_Translation[i];
}
// triangulate point based on the solve pnp result
triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);
}
//3: triangulate l-----l+1 l+2 ... frame_num -2
for (int i = l + 1; i < frame_num - 1; i++)//對於參考幀和當前幀之間的某一幀，三角化參考幀和該幀的路標點
triangulateTwoFrames(l, Pose[l], i, Pose[i], sfm_f);
//4: solve pnp l-1; triangulate l-1 ----- l
// l-2 l-2 ----- l
for (int i = l - 1; i >= 0; i--)//對於第一幀和參考幀之間的某一幀,先用PnP求解該幀位姿,然後三角化該幀到參考幀的路標點
{
//solve pnp
Matrix3d R_initial = c_Rotation[i + 1];
Vector3d P_initial = c_Translation[i + 1];
if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
return false;
c_Rotation[i] = R_initial;
c_Translation[i] = P_initial;
c_Quat[i] = c_Rotation[i];
Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
Pose[i].block<3, 1>(0, 3) = c_Translation[i];
//triangulate
triangulateTwoFrames(i, Pose[i], l, Pose[l], sfm_f);
}
//5: triangulate all other points
for (int j = 0; j < feature_num; j++)//三角化其他未恢復的路標點
{
if (sfm_f[j].state == true)
continue;
if ((int)sfm_f[j].observation.size() >= 2)
{
Vector2d point0, point1;
int frame_0 = sfm_f[j].observation[0].first;
point0 = sfm_f[j].observation[0].second;
int frame_1 = sfm_f[j].observation.back().first;
point1 = sfm_f[j].observation.back().second;
Vector3d point_3d;
triangulatePoint(Pose[frame_0], Pose[frame_1], point0, point1, point_3d);
sfm_f[j].state = true;
sfm_f[j].position[0] = point_3d(0);
sfm_f[j].position[1] = point_3d(1);
sfm_f[j].position[2] = point_3d(2);
//cout << "trangulated : " << frame_0 << " " << frame_1 << " 3d point : " << j << " " << point_3d.transpose() << endl;
}
}
/*
for (int i = 0; i < frame_num; i++)
{
q[i] = c_Rotation[i].transpose();
cout << "solvePnP q" << " i " << i <<" " <<q[i].w() << " " << q[i].vec().transpose() << endl;
}
for (int i = 0; i < frame_num; i++)
{
Vector3d t_tmp;
t_tmp = -1 * (q[i] * c_Translation[i]);
cout << "solvePnP t" << " i " << i <<" " << t_tmp.x() <<" "<< t_tmp.y() <<" "<< t_tmp.z() << endl;
}
*/
//full BA
ceres::Problem problem;
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
//cout << " begin full BA " << endl;
for (int i = 0; i < frame_num; i++)
{
//double array for ceres
c_translation[i][0] = c_Translation[i].x();
c_translation[i][1] = c_Translation[i].y();
c_translation[i][2] = c_Translation[i].z();
c_rotation[i][0] = c_Quat[i].w();
c_rotation[i][1] = c_Quat[i].x();
c_rotation[i][2] = c_Quat[i].y();
c_rotation[i][3] = c_Quat[i].z();
problem.AddParameterBlock(c_rotation[i], 4, local_parameterization);
problem.AddParameterBlock(c_translation[i], 3);
if (i == l)
{
problem.SetParameterBlockConstant(c_rotation[i]);
}
if (i == l || i == frame_num - 1)
{
problem.SetParameterBlockConstant(c_translation[i]);
}
}
for (int i = 0; i < feature_num; i++)
{
if (sfm_f[i].state != true)
continue;
for (int j = 0; j < int(sfm_f[i].observation.size()); j++)
{
int l = sfm_f[i].observation[j].first;
ceres::CostFunction* cost_function = ReprojectionError3D::Create(
sfm_f[i].observation[j].second.x(),
sfm_f[i].observation[j].second.y());
problem.AddResidualBlock(cost_function, NULL, c_rotation[l], c_translation[l],
sfm_f[i].position);
}
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.minimizer_progress_to_stdout = true;
options.max_solver_time_in_seconds = 0.2;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";
if (summary.termination_type == ceres::CONVERGENCE || summary.final_cost < 5e-03)
{
//cout << "vision only BA converge" << endl;
}
else
{
//cout << "vision only BA not converge " << endl;
return false;
}
for (int i = 0; i < frame_num; i++)
{
q[i].w() = c_rotation[i][0];
q[i].x() = c_rotation[i][1];
q[i].y() = c_rotation[i][2];
q[i].z() = c_rotation[i][3];
q[i] = q[i].inverse();
//cout << "final q" << " i " << i <<" " <<q[i].w() << " " << q[i].vec().transpose() << endl;
}
for (int i = 0; i < frame_num; i++)
{
T[i] = -1 * (q[i] * Vector3d(c_translation[i][0], c_translation[i][1], c_translation[i][2]));
//cout << "final t" << " i " << i <<" " << T[i](0) <<" "<< T[i](1) <<" "<< T[i](2) << endl;
}
for (int i = 0; i < (int)sfm_f.size(); i++)
{
if(sfm_f[i].state)
sfm_tracked_points[sfm_f[i].id] = Vector3d(sfm_f[i].position[0], sfm_f[i].position[1], sfm_f[i].position[2]);
}
return true;
}
2.1.5 PnP求解位姿
/**
* 1.5.對於所有的圖像幀，包括不在滑動窗口中的，提供初始的RT估計，然後solvePnP進行優化
*/
//solve pnp for all frame
map<double, ImageFrame>::iterator frame_it;
map<int, Vector3d>::iterator it;
frame_it = all_image_frame.begin( );
for (int i = 0; frame_it != all_image_frame.end( ); frame_it++)
{
// provide initial guess
cv::Mat r, rvec, t, D, tmp_r;
if((frame_it->first) == Headers[i].stamp.toSec())
{
frame_it->second.is_key_frame = true;
frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose();
frame_it->second.T = T[i];
i++;
continue;
}
if((frame_it->first) > Headers[i].stamp.toSec())
{
i++;
}
//注意這裏的 Q和 T是圖像幀的位姿，而不是求解PNP時所用的座標系變換矩陣，兩者具有對稱關係
Matrix3d R_inital = (Q[i].inverse()).toRotationMatrix();
Vector3d P_inital = - R_inital * T[i];
cv::eigen2cv(R_inital, tmp_r);
//羅德里格斯公式將旋轉矩陣轉換成旋轉向量
cv::Rodrigues(tmp_r, rvec);
cv::eigen2cv(P_inital, t);
frame_it->second.is_key_frame = false;
//獲取 pnp需要用到的存儲每個特徵點三維點和圖像座標的 vector
vector<cv::Point3f> pts_3_vector;
vector<cv::Point2f> pts_2_vector;
for (auto &id_pts : frame_it->second.points)
{
int feature_id = id_pts.first;
for (auto &i_p : id_pts.second)
{
it = sfm_tracked_points.find(feature_id);
if(it != sfm_tracked_points.end())
{
Vector3d world_pts = it->second;
cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2));
pts_3_vector.push_back(pts_3);
Vector2d img_pts = i_p.second.head<2>();
cv::Point2f pts_2(img_pts(0), img_pts(1));
pts_2_vector.push_back(pts_2);
}
}
}
//保證特徵點數大於 5
cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
if(pts_3_vector.size() < 6)
{
cout << "pts_3_vector size " << pts_3_vector.size() << endl;
ROS_DEBUG("Not enough points for solve pnp !");
return false;
}
//PnP求解位姿
if (! cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1))
{
ROS_DEBUG("solve pnp fail!");
return false;
}
cv::Rodrigues(rvec, r);
MatrixXd R_pnp,tmp_R_pnp;
cv::cv2eigen(r, tmp_R_pnp);
//這裏也同樣需要將座標變換矩陣轉變成圖像幀位姿，並轉換爲IMU座標系的位姿
R_pnp = tmp_R_pnp.transpose();
MatrixXd T_pnp;
cv::cv2eigen(t, T_pnp);
T_pnp = R_pnp * (-T_pnp);
frame_it->second.R = R_pnp * RIC[0].transpose();
frame_it->second.T = T_pnp;
}
這裏需要重點了解一下solvePnP這個函數，該函數是opencv提供的在給出世界座標系下的3d點座標和與這些3d點座標對應的像素平面上的2d點座標的情況下求解旋轉和平移量的函數。代碼中求解出來的就是rvec旋轉量和t平移量。這個圖示過程可以參看下圖所示，該圖來自opencv中solvePnP函數的解釋中，路徑如下：solvePnP函數官方解釋
2.2 視覺慣導對齊
這部分代碼對應的是論文中V-B部分。