VINS初始化之外参在线标定
文章目录
一、理论推导
估计旋转外参数Qbc具体公式推导:
二、代码实现
initial_ex_rotation.cpp
if (ESTIMATE_EXTRINSIC == 2)
{
cout << "calibrating extrinsic param, rotation movement is needed" << endl;
if (frame_count != 0)
{
vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
Matrix3d calib_ric;
if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
{
// ROS_WARN("initial extrinsic rotation calib success");
// ROS_WARN_STREAM("initial extrinsic rotation: " << endl
// << calib_ric);
ric[0] = calib_ric;
RIC[0] = calib_ric;
ESTIMATE_EXTRINSIC = 1;
}
}
}
/**
* @Description: 在线标定相机到imu之间的相对旋转Rbc
* @param:corres: 两帧之间多个匹配点的归一化坐标
* @param:delta_q_imu: 两帧陀螺仪积分得到的相对旋转
* @return:calib_ric_result: 标定结果
*/
int flag = 0;
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
frame_count++;
// std::cout << "frame_count"<<frame_count << std::endl;
Rc.push_back(solveRelativeR(corres));
Rimu.push_back(delta_q_imu.toRotationMatrix());
// std::cout << "ric" << ric << std::endl;
Rc_g.push_back(ric.inverse() * delta_q_imu * ric);
// std::cout << "Rc: " <<Rc.at(flag)<< std::endl;
// std::cout << "Rc_g: " << Rc_g.at(flag)<< std::endl;
flag++;
Eigen::MatrixXd A(frame_count * 4, 4);
A.setZero();
int sum_ok = 0;
for (int i = 1; i <= frame_count; i++)
{
Quaterniond r1(Rc[i]);
Quaterniond r2(Rc_g[i]);
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;
double w = Quaterniond(Rc[i]).w();
Vector3d q