VINS外参在线标定估计

由于使用的数据相对参数提供的不够精确导致结果精度不高，因此需要研究一下外参的在线估计。
VINS外参标定指的是对相机坐标系到IMU坐标系的变换矩阵进行在线标定与优化。

1、参数配置

在参数配置文件yaml中，参数estimate_extrinsic反映了外参的情况：
1、等于0表示这外参已经是准确的了，之后不需要优化；
2、等于1表示外参只是一个估计值，后续还需要将其作为初始值放入非线性优化中；
3、等于2表示不知道外参，需要进行标定，从代码estimator.cpp中的processImage()中的以下代码进入，主要是标定外参的旋转矩阵。

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矩阵转化为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);//三角化验证Rtdouble 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的RMatrix3d 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的REigen::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;}elsereturn 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;
}

参考博客：
VINS-Mono理论学习——视觉惯性联合初始化与外参标定

VINS外参在线标定估计相关推荐

VINS 外参在线标定
在VINS中相机的外参RicR_{ic}Ric是可以在线动态标定的,实现函数为: /*** @brief 外参在线标定* * @param corres 两帧图像之间的共视特征点* @param d ...
IROS2020开源软硬件！多激光雷达的协同定位建图及在线外参自标定
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达本文由作者林家荣授权转载,二次转载请联系作者 https://zhuanlan.zhihu.com/p ...
针对高分辨率雷达和相机的无标定板的像素级外参自标定方法
介绍:固态激光雷达和相机的外参标定系统摘要这是今年的一篇针对高分辨率的固态激光雷达(非重复性扫描型)或者多线的激光雷达和相机在无标定板的环境中自动化外参标定的一篇文章.本文的方法不需要基于巧克力板 ...
无目标场景下高分辨率激光雷达和相机的像素级外参自标定
文章:Pixel-level Extrinsic Self Calibration of High Resolution LiDAR and Camera in Targetless Environm ...
一起做激光反光板（六）-基于滑窗的EKF-SLAM及外参自动标定公式推导
在第四篇中已经提到,如果场景中反光板不够多,容易造成EKF系统效果不好的问题,且我们还想用上其他的点云信息,保证在反光板不够的情况下仍能够正确的收敛. 我们考虑扩充观测信息: (1)角点和线段特征,加 ...
相机和雷达外参联合标定
内容: 关于雷达和相机外参联合标定的踩坑纪录. Date: 2023/03/19 硬件: 上位机: Jetson ORIN (Ubuntu 20.04, ROS noetic) 雷达: Ouster ...
imu相机标定_解放双手——相机与IMU外参的在线标定
本文作者沈玥伶,公众号:计算机视觉life,编辑部成员一.相机与IMU的融合在SLAM的众多传感器解决方案中,相机与IMU的融合被认为具有很大的潜力实现低成本且高精度的定位与建图.这是因为这两个 ...
SST-Calib：结合语义和VO进行时空同步校准的lidar-visual外参标定方法(ITSC 2022)
原文链接: SST-Calib:结合语义和VO进行时空同步校准的lidar-visual外参标定方法(ITSC 2022) 论文阅读:SST-Calib: Simultaneous Spatial-T ...
无标定物体环境下，高分辨率雷达与相机的像素级外参标定
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达标题:Pixel-level Extrinsic Self Calibration of High R ...

VINS外参在线标定估计

VINS外参在线标定估计相关推荐

最新文章

热门文章