一、IMU与Camera外参旋转标定
// initial_ex_rotation.cpp
/**
* 在线标定外参旋转
* 利用两帧之间的Camera旋转和IMU积分旋转,构建最小二乘问题,SVD求解外参旋转
* 1、Camera系,两帧匹配点计算本质矩阵E,分解得到四个解,根据三角化成功点比例确定最终正确解R、t,得到两帧之间的旋转R
* 2、IMU系,积分计算两帧之间的旋转
* 3、根据旋转构建最小二乘问题,SVD求解外参旋转
* @param corres 前一帧与当前帧匹配点
* @param delta_q_imu 前一帧与当前帧IMU预积分得到的旋转
* @param calib_ric_result 在线标定IMU与Camera之间的外参(旋转)
*/
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result);
二、IMU角速度偏置标定
// IMU角速度偏置标定
void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{
Matrix3d A;
Vector3d b;
Vector3d delta_bg;
A.setZero();
b.setZero();
map<double, ImageFrame>::iterator frame_i;
map<double, ImageFrame>::iterator frame_j;
// 从滑窗第一帧遍历到倒数第二帧
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
{
frame_j = next(frame_i);
MatrixXd tmp_A(3, 3);
tmp_A.setZero();
VectorXd tmp_b(3);
tmp_b.setZero();
Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
A += tmp_A.transpose() * tmp_A;
b += tmp_A.transpose() * tmp_b;
}
delta_bg = A.ldlt().solve(b);
// 更新偏置
for (int i = 0; i <= WINDOW_SIZE; i++)
Bgs[i] += delta_bg;
// 更新偏置之后,重新计算积分
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
{
frame_j = next(frame_i);
frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
}
}
三、标定重力向量、单目尺度
VINS初始化流程
/**
* 系统初始化
* 1、计算滑窗内IMU加速度的标准差,用于判断移动快慢
* 2、在滑窗中找到与当前帧具有足够大的视差,同时匹配较为准确的一帧,计算相对位姿变换
* 1) 提取滑窗中每帧与当前帧之间的匹配点(要求点在两帧之间一直被跟踪到,属于稳定共视点),超过20个则计算视差
* 2) 两帧匹配点计算本质矩阵E,恢复R、t
* 3) 视差超过30像素,匹配内点数超过12个,则认为符合要求,返回当前帧
* 3、以上面找到的这一帧为参考系,Pnp计算滑窗每帧位姿,然后三角化所有特征点,构建BA(最小化点三角化前后误差)优化每帧位姿
* 1) 3d-2d Pnp求解每帧位姿
* 2) 对每帧与l帧、当前帧三角化
* 3) 构建BA,最小化点三角化前后误差,优化每帧位姿
* 4) 保存三角化点
* 4、对滑窗中所有帧执行Pnp优化位姿
* 5、Camera与IMU初始化,零偏、尺度、重力方向
*/
bool Estimator::initialStructure();
重磅!3DCVer-学术论文写作投稿 交流群已成立
扫码添加小助手微信,可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群,旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。
同时也可申请加入我们的细分方向交流群,目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。
一定要备注:研究方向+学校/公司+昵称,例如:”3D视觉 + 上海交大 + 静静“。请按照格式备注,可快速被通过且邀请进群。原创投稿也请联系。
▲长按加微信群或投稿 ▲长按关注公众号
▲长按关注公众号