本讲还是为了estimator类中最主要的函数processImage()做知识储备。
前面两讲知识储备主要讲了IMU预积分相关的integrationBase类以及图像特征点管理器feature_manager.cpp,本节将对processImage()的相机到IMU的外参矩阵的代码讲解。
当外参完全不知道的时候,VINS也可以在线对其进行估计(rotation),先在processImage内进行初步估计,然后在后续优化时,会在optimize函数中再次优化。
目录
2、SVD分解,Ax=0,对A填充。多帧组成A,一对点组成的A是4*4的。
3.至少迭代计算了WINDOW_SIZE次,且R的奇异值大于0.25才认为标定成功
我们首先来回顾以下整体的processImage()函数流程
processImage()中:
第一步为检测当前输入的image是否为关键帧,f_manager.addFeatureCheckParallax()
第二步为更新临时预积分初始值 tmp_pre_integration = new IntegrationBase()
第三步为:本文的贡献中相机和IMU外参的在线标定作为创新点。initial_ex_rotation.CalibrationExRotation()代码如下:
// 3. 如果没有外参则标定IMU到相机的外参
// 0表示外参优化完成;1表示粗略估计,后面作为初始值放进非线性优化;2表示需要进行标定。
if(ESTIMATE_EXTRINSIC == 2)
{
ROS_INFO("calibrating extrinsic param, rotation movement is needed");
if (frame_count != 0)
{
//得到corres传入的是当前帧和其之前一帧的对应特征点对的归一化坐标。
vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
Matrix3d calib_ric;
//标定从camera到IMU之间的外参数
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,目前单目情况下,ric内只有一个值
ric[0] = calib_ric;
RIC[0] = calib_ric;
ESTIMATE_EXTRINSIC = 1;// 完成外参标定
}
}
}
InitialEXRotation类
位于vins_estimator/src/initial/initial_ex_rotation.cpp,首先介绍一个这个类的成员变量以及成员函数:
/* 标定IMU和相机的外参数 */
class InitialEXRotation
{
public:
InitialEXRotation();
// !!!重要,主函数标定外参旋转矩阵
bool CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result);
private:
// 两帧之间相机系旋转矩阵
Matrix3d solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres);
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;// 相机之间旋转矩阵,对极几何得到
vector< Matrix3d > Rimu;// 两个IMU之间旋转矩阵,由IMU预积分得到
vector< Matrix3d > Rc_g; // 每个IMU相对于起始IMU的旋转矩阵
Matrix3d ric;// 相机到IMU的外参
};
1、CalibrationExRotation()函数
标定相机和IMU的外参数。
bool InitialEXRotation::CalibrationExRotation(vector<