使用されるデータは、提供されたパラメーターに対して十分に正確ではないため、結果の精度は高くありません。したがって、外部パラメーターのオンライン推定を検討する必要があります。
VINS外部パラメータキャリブレーションとは、カメラ座標系からIMU座標系への変換行列のオンラインキャリブレーションと最適化を指します。
1.パラメータ設定
パラメーター構成ファイルyamlで、パラメーターestimate_extrinsicは外部パラメーターの状況を反映します。1。0に等しい場合は
、外部パラメーターがすでに正確であり、後で最適化する必要がないことを意味します。2。1に等しい場合は、外部パラメーターを
意味します。は推定値であり、後で必要になります。非線形最適化の初期値として入力し
ます。3。2に等しい場合は、外部パラメーターが不明であり、キャリブレーションが必要であることを意味します。次のコードからprocessImage（）に入力します。コードestimator.cppで、主に外部パラメーターの回転マトリックスをキャリブレーションします。
if (ESTIMATE_EXTRINSIC == 2) // 不知道相机外参
{
ROS_WARN("have no prior about extrinsic param, calibrate extrinsic param");
RIC.push_back(Eigen::Matrix3d::Identity());
TIC.push_back(Eigen::Vector3d::Zero());
EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv";
}
else // 知道相机外参
{
if ( ESTIMATE_EXTRINSIC == 1) // 虽然知道相机外参，但是在优化过程中还是去优化外参，这里的1只是标记了一种状态，并不是指优化的参数的数量
{
ROS_WARN(" Optimize extrinsic param around initial guess!");
EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv";
}
if (ESTIMATE_EXTRINSIC == 0) // 知道相机外参，而且在优化过程中该参数是固定的，不参与优化
ROS_WARN(" fix extrinsic param ");
cv::Mat cv_R, cv_T;
fsSettings["extrinsicRotation"] >> cv_R;
fsSettings["extrinsicTranslation"] >> cv_T;
Eigen::Matrix3d eigen_R;
Eigen::Vector3d eigen_T;
cv::cv2eigen(cv_R, eigen_R);
cv::cv2eigen(cv_T, eigen_T);
Eigen::Quaterniond Q(eigen_R);
eigen_R = Q.normalized();
RIC.push_back(eigen_R);
TIC.push_back(eigen_T);
ROS_INFO_STREAM("Extrinsic_R : " << std::endl << RIC[0]);
ROS_INFO_STREAM("Extrinsic_T : " << std::endl << TIC[0].transpose());
}
オンラインでパラメーターを推定して初期値を与える必要がある場合は、最初に行列R、tを読み取り、それをopencv行列から固有行列に変換して、コンテナーRIC、TICの最初の行列コンポーネントを取得する必要があります。
2.校正原理
行列間の対応を使用して、カメラとIMUの外部パラメータが固定されます。
その中で、q_bk_bk + 1はジャイロスコープの事前積分によって得られ、q_ck_ck + 1は、8点法を使用して前後のフレームの対応する特徴点を計算することによって得られます。
過剰決定方程式Ax = 0は、複数のフレーム間の方程式関係を組み合わせることによって構築されます。Aでsvd分解を実行します。ここで、最小の特異値に対応する右の特異ベクトルは、回転したクォータニオンq_b_cである結果xです。コードでは、Lはカメラ回転クォータニオンの左乗算行列であり、RはIMU回転クォータニオンの右乗算行列であるため、実際に構築されるのは次のとおりです。
3.機能が含まれています
オンライン外部パラメーター推定の特定の実装は、vins_estimator / src / initial / initial_ex_rotation.cpp / .hにあり、外部パラメーター回転行列を較正するために使用されるInitialEXRotationクラスにあります。まず、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は、2つのフレーム間の複数の一致するポイントペアの正規化された座標を含むベクトル配列であり、入力パラメーターdelta_qは、2つのフレーム間のジャイロ積分によって取得された相対回転です。
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. resolveRelativeR（corres）関数は、カメラのフレーム回転を取得します
resolveRelativeR（corres）は、エピポーラジオメトリに従って、隣接するフレーム間のカメラ座標系の回転行列を計算します。ここでの相関は、現在のフレームと前のフレームの対応する特徴点ペアの正規化された座標です。
Matrix3d InitialEXRotation :: resolveRelativeR（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; 分解E（E、R1、R2、t1、t2）; if（determinant（R1）+ 1.0 <1e-09）{ E = -E; 分解E（E、R1、R2、t1、t2）; }
ダブルレシオ1 = max（testTriangulation（ll、rr、R1、t1）、testTriangulation（ll、rr、R1、t2））;
ダブルレシオ2 = 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）;
ans_R_eigenを返します。
}
return Matrix3d :: Identity（）;
}
①対応する点の2次元座標をそれぞれ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)));
}
②エピポーラ幾何学に従って2つのフレームの本質的な行列を解きます
cv::Mat E = cv::findFundamentalMat(ll, rr);
③4セットのRt解を得るための必須行列のSvd分解
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;
}
リファレンスブログ：
VINS-モノ理論研究-視覚慣性関節の初期化と外部パラメータのキャリブレーション