相机imu外参标定

1. 第一步初始化imu外参(可以从参数文档中读取，也可以计算出)，VINS中处理如下：

# Extrinsic parameter between IMU and Camera.

estimate_extrinsic:    #   Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.

                        #   Have an initial guess about extrinsic parameters. We will optimize around your initial guess.

                        #   Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.

#If you choose  or , you should write down the following matrix.

#Rotation from camera frame to imu frame, imu^R_cam

extrinsicRotation: !!opencv-matrix

   rows:

   cols:

   dt: d

   data: [0.0148655429818, -0.999880929698, 0.00414029679422,

           0.999557249008, 0.0149672133247, 0.025715529948,

           -0.0257744366974, 0.00375618835797, 0.999660727178]

#Translation from camera frame to imu frame, imu^T_cam

extrinsicTranslation: !!opencv-matrix

   rows:

   cols:

   dt: d

   data: [-0.0216401454975,-0.064676986768, 0.00981073058949]

2. 在优化中，每来一帧则对外参更新一次

Matrix3d ric[NUM_OF_CAM];

Vector3d tic[NUM_OF_CAM];

//添加参数块

ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();

problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);

if (!ESTIMATE_EXTRINSIC)

{

    ROS_DEBUG("fix extinsic param");

    problem.SetParameterBlockConstant(para_Ex_Pose[i]);

}

else

    ROS_DEBUG("estimate extinsic param");

//添加残差块

if (ESTIMATE_TD)

{

        ProjectionTdFactor *f_td = new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[].velocity, it_per_frame

                                                         it_per_id.feature_per_frame[].cur_td, it_per_frame.cur_td,

                                                         it_per_id.feature_per_frame[].uv.y(), it_per_frame.uv.y());

        problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[], para_Feature[feature_index], para_Td[]);

}

else

{

    ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);

    problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[], para_Feature[feature_inde

}

该残差块为视觉模型计算重投影误差 Vision Model

相机imu外参标定

空间上的一个3D特征点会被camera多次观测到，j^th图像帧的camera的残差值被定义为，考虑l^th特征点第一次在i^th图像帧中被观察到恢复的深度信息，投影到第j 帧图像帧的重投影误差。

3. 相机imu外参的重要性

外参是相机和imu之间的桥梁，后端优化是以imu的坐标系为优化基准，所以在进行重投影误差时需要使用外参将空间点转换到相机坐标系。
初始化的三角化时也会用到外参来求出两个相机之间的旋转平移。

f_manager.triangulate(Ps, &(TIC_TMP[]), &(RIC[]));

Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;

Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;

Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;

Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);

Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);

Eigen::Map<Eigen::Vector2d> residual(residuals);

qcb(qbjw(qwbi(qbcqcb(qbjw(qwbi(qbcqcb(qbjw(qwbi(qbcqcb(qbjw(qwbi(qbccjlcjl

秒客网

相机imu外参标定

相关文章