VINS外參線上標定估計
由於使用的資料相對引數提供的不夠精確導致結果精度不高，因此需要研究一下外參的線上估計。
VINS外參標定指的是對相機座標系到IMU座標系的變換矩陣進行線上標定與優化。
1、引數配置
在引數配置檔案yaml中，引數estimate_extrinsic反映了外參的情況：
1、等於0表示這外參已經是準確的了，之後不需要優化；
2、等於1表示外參只是一個估計值，後續還需要將其作為初始值放入非線性優化中；
3、等於2表示不知道外參，需要進行標定，從程式碼estimator.cpp中的processImage()中的以下程式碼進入，主要是標定外參的旋轉矩陣。
if (ESTIMATE_EXTRINSIC == 如果需要線上估計引數且給出初始值時，則需要先讀取矩陣R，t，並將其從opencv矩陣轉化為eigen矩陣，得到容器RIC,TIC的第一個矩陣分量。
2、標定原理
利用矩陣之間的對應關係，相機與IMU的外參固定不變。
其中q_bk_bk+1是陀螺儀預積分得到的，q_ck_ck+1是用8點法對前後幀對應的特徵點進行計算得到的。
將多個幀之間的等式關係一起構建超定方程Ax = 0 。對A進行svd分解，其中最小奇異值對應的右奇異向量便為結果x，即旋轉四元數q_b_c。在程式碼中，L為相機旋轉四元數的左乘矩陣，R為IMU旋轉四元數的右乘矩陣，因此其實構建的是：
3、包含函式
線上估計外參的具體實現方式在InitialEXRotation類中，該類位於vins_estimator/src/initial/initial_ex_rotation.cpp/.h中，用於標定外參旋轉矩陣。首先簡要介紹一下InitialEXRotation類的成員：
class InitialEXRotation
{
public:
InitialEXRotation();//建構函式
//標定外參旋轉矩陣
bool CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result);
private:
//求解幀間cam座標系的旋轉矩陣
Matrix3d solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres);
//三角化驗證Rt
double testTriangulation(const vector<cv::Point2f> &l,
const vector<cv::Point2f> &r,
cv::Mat_<double> R, cv::Mat_<double> t);
//本質矩陣SVD分解計算4組Rt值
void decomposeE(cv::Mat E,
cv::Mat_<double> &R1, cv::Mat_<double> &R2,
cv::Mat_<double> &t1, cv::Mat_<double> &t2);
int frame_count;//幀計數器
vector< Matrix3d > Rc;//幀間cam的R，由對極幾何得到
vector< Matrix3d > Rimu;//幀間IMU的R，由IMU預積分得到
vector< Matrix3d > Rc_g;//每幀IMU相對於起始幀IMU的R
Matrix3d ric;//cam到IMU的外參
};
4、估計實現
CalibrationExRotation() 標定外參旋轉矩陣，該函式目的是標定外參的旋轉矩陣。函式的入參 corres 是包含 兩幀之間 的多個匹配點對的歸一化座標的vector陣列，入參 delta_q 是通過 兩幀間 陀螺儀積分得到相對旋轉；
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>>
corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
/*記錄第幾次進入這個函式，標定的過程中會多次進入這個函式，
直到標定成功，每進一次通過入參得到公式（6）這樣一個約束。*/
frame_count++;
/*過幀間的匹配點對計算得到本質矩陣，然後分解得到旋轉矩陣R_ck+1^ck*/
//獲取相鄰幀之間相機座標系和IMU座標系的旋轉矩陣，存入vector中
Rc.push_back(solveRelativeR(corres));
Rimu.push_back(delta_q_imu.toRotationMatrix());//幀間陀螺儀積分得到R_bk+1^bk
/*每次迭代前先用前一次估計的ric將R_bk+1^bk變換成R_ck+1^ck，即公式（1）*/
Rc_g.push_back(ric.inverse() * delta_q_imu * ric);//每幀IMU相對於起始幀IMU的R
Eigen::MatrixXd A(frame_count * 4, 4); //構建公式（7）中的A矩陣
A.setZero();
int sum_ok = 0;
for (int i = 1; i <= frame_count; i++)
{
Quaterniond r1(Rc[i]);
Quaterniond r2(Rc_g[i]);
/* angularDistance就是計算兩個座標系之間相對旋轉矩陣在做軸角變換後(u * theta)
的角度theta, theta越小說明兩個座標系的姿態越接近，這個角度距離用於後面計算權重，
這裡計算權重就是為了降低外點的干擾，意思就是為了防止出現誤差非常大的R_bk+1^bk和
R_ck+1^ck約束導致估計的結果偏差太大*/
double angular_distance = 180 / M_PI * r1.angularDistance(r2);
ROS_DEBUG(
"%d %f", i, angular_distance);
//核函式做加權
double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
++sum_ok;
Matrix4d L, R;
/*L R 分別為四元數左乘和四元數右乘矩陣
下面的這幾步就是從公式（3）到公式（6）的過程 ，每次得到4行約束填入A矩陣中*/
double w = Quaterniond(Rc[i]).w();
Vector3d q = Quaterniond(Rc[i]).vec();
L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
L.block<3, 1>(0, 3) = q;
L.block<1, 3>(3, 0) = -q.transpose();
L(3, 3) = w;
Quaterniond R_ij(Rimu[i]);
w = R_ij.w();
q = R_ij.vec();
R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
R.block<3, 1>(0, 3) = q;
R.block<1, 3>(3, 0) = -q.transpose();
R(3, 3) = w;
A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
}
//對A矩陣做SVD分解，最小奇異值對應的右向量就是四元數解
JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
//這裡的四元數儲存的順序是[x,y,z,w]',即[qv qw]'
Matrix<double, 4, 1> x = svd.matrixV().col(3);
Quaterniond estimated_R(x);
ric = estimated_R.toRotationMatrix().inverse();
Vector3d ric_cov;
ric_cov = svd.singularValues().tail<3>();
/* 至少會迭代WINDOW_SIZE次，並且第二小的奇異值大於0.25才認為標定成功
（singularValues拿到的奇異值是從大到小儲存的），
意思就是最小的奇異值要足夠接近於0，和第二小之間要有足夠差距才行,
這樣才算求出了最優解 */
if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
{
calib_ric_result = ric;
return true;
}
else
return false;
}
5、solveRelativeR(corres)函式得到相機幀間旋轉
solveRelativeR(corres)根據對極幾何計算相鄰幀間相機座標系的旋轉矩陣，這裡的corres傳入的是當前幀和其之前一幀的對應特徵點對的歸一化座標。
Matrix3d InitialEXRotation::solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres)
{
if (corres.size() >= 9)
{
vectorcv::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 E = cv::findFundamentalMat(ll, rr);
cv::Mat_ R1, R2, t1, t2;
decomposeE(E, R1, R2, t1, t2);
if (determinant(R1) + 1.0 < 1e-09)
{
E = -E;
decomposeE(E, R1, R2, t1, t2);
}
double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2));
double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2));
cv::Mat_ ans_R_cv = ratio1 > ratio2 ? R1 : R2;
Matrix3d ans_R_eigen;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
ans_R_eigen(j, i) = ans_R_cv(i, j);
return ans_R_eigen;
}
return Matrix3d::Identity();
}
①將corres中對應點的二維座標分別存入ll與rr中。
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 E = cv::findFundamentalMat(ll, rr);
③對本質矩陣進行svd分解得到4組Rt解
cv::Mat_<double> R1, R2, t1, t2;
decomposeE(E, R1, R2, t1, t2);
④採用三角化對每組Rt解進行驗證，選擇是正深度的Rt解
double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2));
double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2));
cv::Mat_<double> ans_R_cv = ratio1 > ratio2 ? R1 : R2;
⑤這裡的R是上一幀到當前幀的變換矩陣，對其求轉置為當前幀相對於上一幀的姿態。
Matrix3d ans_R_eigen;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
ans_R_eigen(j, i) = ans_R_cv(i, j);
return ans_R_eigen;
}