相機imu外參標定

本文轉載自查看原文 2018-08-03 17:20 2486

1. 第一步初始化imu外參(可以從參數文檔中讀取，也可以計算出)，VINS中處理如下：

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
                        # 2  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 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix
   rows: 3
   cols: 3
   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: 3
   cols: 1
   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[0].velocity, it_per_frame
                                                         it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,
                                                         it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
        problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);

}
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[0], para_Feature[feature_inde
}

該殘差塊為視覺模型計算重投影誤差 Vision Model

空間上的一個3D特征點會被camera多次觀測到，j^th 圖像幀的camera的殘差值被定義為，考慮l^th特征點第一次在i^th圖像幀中被觀察到恢復的深度信息，投影到第j 幀圖像幀的重投影誤差。

3. 相機imu外參的重要性

外參是相機和imu之間的橋梁，后端優化是以imu的坐標系為優化基准，所以在進行重投影誤差時需要使用外參將空間點轉換到相機坐標系。
初始化的三角化時也會用到外參來求出兩個相機之間的旋轉平移。

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

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);

$\begin{matrix} (1) & \begin{array}{rcl} r_{C} ({\hat{z}}_{l}^{c_{j}}, X) = {[\begin{matrix} b_{1} & b_{2} \end{matrix}]}^{T} \cdot ({\bar{P}}_{l}^{c_{j}} - \frac{P_{l}^{c_{j}}}{‖ P_{l}^{c_{j}} ‖}) \\ {\bar{P}}_{l}^{c_{j}} = π_{c}^{- 1} ([\begin{matrix} {\hat{u}}_{l}^{c_{j}} \\ {\hat{v}}_{l}^{c_{j}} \end{matrix}]) \\ P_{l}^{c_{j}} = q_{b}^{c} (q_{w}^{b_{j}} (q_{b_{i}}^{w} (q_{c}^{b} \frac{1}{λ_{l}} π_{c}^{- 1} ([\begin{matrix} {\hat{u}}_{l}^{c_{i}} \\ {\hat{v}}_{l}^{c_{i}} \end{matrix}]) + p_{c}^{b}) + p_{b_{i}}^{w} - p_{b_{j}}^{w}) - p_{b}^{c}) \end{array} \end{matrix}$ $\begin{matrix} (1) & \begin{array}{rcl} r_{C} ({\hat{z}}_{l}^{c_{j}}, X) = {[\begin{matrix} b_{1} & b_{2} \end{matrix}]}^{T} \cdot ({\bar{P}}_{l}^{c_{j}} - \frac{P_{l}^{c_{j}}}{‖ P_{l}^{c_{j}} ‖}) \\ {\bar{P}}_{l}^{c_{j}} = π_{c}^{- 1} ([\begin{matrix} {\hat{u}}_{l}^{c_{j}} \\ {\hat{v}}_{l}^{c_{j}} \end{matrix}]) \\ P_{l}^{c_{j}} = q_{b}^{c} (q_{w}^{b_{j}} (q_{b_{i}}^{w} (q_{c}^{b} \frac{1}{λ_{l}} π_{c}^{- 1} ([\begin{matrix} {\hat{u}}_{l}^{c_{i}} \\ {\hat{v}}_{l}^{c_{i}} \end{matrix}]) + p_{c}^{b}) + p_{b_{i}}^{w} - p_{b_{j}}^{w}) - p_{b}^{c}) \end{array} \end{matrix}$ $\begin{matrix} (1) & \begin{array}{rcl} r_{C} ({\hat{z}}_{l}^{c_{j}}, X) = {[\begin{matrix} b_{1} & b_{2} \end{matrix}]}^{T} \cdot ({\bar{P}}_{l}^{c_{j}} - \frac{P_{l}^{c_{j}}}{‖ P_{l}^{c_{j}} ‖}) \\ {\bar{P}}_{l}^{c_{j}} = π_{c}^{- 1} ([\begin{matrix} {\hat{u}}_{l}^{c_{j}} \\ {\hat{v}}_{l}^{c_{j}} \end{matrix}]) \\ P_{l}^{c_{j}} = q_{b}^{c} (q_{w}^{b_{j}} (q_{b_{i}}^{w} (q_{c}^{b} \frac{1}{λ_{l}} π_{c}^{- 1} ([\begin{matrix} {\hat{u}}_{l}^{c_{i}} \\ {\hat{v}}_{l}^{c_{i}} \end{matrix}]) + p_{c}^{b}) + p_{b_{i}}^{w} - p_{b_{j}}^{w}) - p_{b}^{c}) \end{array} \end{matrix}$ $\begin{matrix} (1) & \begin{array}{rcl} r_{C} ({\hat{z}}_{l}^{c_{j}}, X) = {[\begin{matrix} b_{1} & b_{2} \end{matrix}]}^{T} \cdot ({\bar{P}}_{l}^{c_{j}} - \frac{P_{l}^{c_{j}}}{‖ P_{l}^{c_{j}} ‖}) \\ {\bar{P}}_{l}^{c_{j}} = π_{c}^{- 1} ([\begin{matrix} {\hat{u}}_{l}^{c_{j}} \\ {\hat{v}}_{l}^{c_{j}} \end{matrix}]) \\ P_{l}^{c_{j}} = q_{b}^{c} (q_{w}^{b_{j}} (q_{b_{i}}^{w} (q_{c}^{b} \frac{1}{λ_{l}} π_{c}^{- 1} ([\begin{matrix} {\hat{u}}_{l}^{c_{i}} \\ {\hat{v}}_{l}^{c_{i}} \end{matrix}]) + p_{c}^{b}) + p_{b_{i}}^{w} - p_{b_{j}}^{w}) - p_{b}^{c}) \end{array} \end{matrix}$ $\begin{matrix} (1) & \begin{array}{rcl} r_{C} ({\hat{z}}_{l}^{c_{j}}, X) = {[\begin{matrix} b_{1} & b_{2} \end{matrix}]}^{T} \cdot ({\bar{P}}_{l}^{c_{j}} - \frac{P_{l}^{c_{j}}}{‖ P_{l}^{c_{j}} ‖}) \\ {\bar{P}}_{l}^{c_{j}} = π_{c}^{- 1} ([\begin{matrix} {\hat{u}}_{l}^{c_{j}} \\ {\hat{v}}_{l}^{c_{j}} \end{matrix}]) \\ P_{l}^{c_{j}} = q_{b}^{c} (q_{w}^{b_{j}} (q_{b_{i}}^{w} (q_{c}^{b} \frac{1}{λ_{l}} π_{c}^{- 1} ([\begin{matrix} {\hat{u}}_{l}^{c_{i}} \\ {\hat{v}}_{l}^{c_{i}} \end{matrix}]) + p_{c}^{b}) + p_{b_{i}}^{w} - p_{b_{j}}^{w}) - p_{b}^{c}) \end{array} \end{matrix}$ $\begin{matrix} (1) & \begin{array}{rcl} r_{C} ({\hat{z}}_{l}^{c_{j}}, X) = {[\begin{matrix} b_{1} & b_{2} \end{matrix}]}^{T} \cdot ({\bar{P}}_{l}^{c_{j}} - \frac{P_{l}^{c_{j}}}{‖ P_{l}^{c_{j}} ‖}) \\ {\bar{P}}_{l}^{c_{j}} = π_{c}^{- 1} ([\begin{matrix} {\hat{u}}_{l}^{c_{j}} \\ {\hat{v}}_{l}^{c_{j}} \end{matrix}]) \\ P_{l}^{c_{j}} = q_{b}^{c} (q_{w}^{b_{j}} (q_{b_{i}}^{w} (q_{c}^{b} \frac{1}{λ_{l}} π_{c}^{- 1} ([\begin{matrix} {\hat{u}}_{l}^{c_{i}} \\ {\hat{v}}_{l}^{c_{i}} \end{matrix}]) + p_{c}^{b}) + p_{b_{i}}^{w} - p_{b_{j}}^{w}) - p_{b}^{c}) \end{array} \end{matrix}$

免責聲明！

本站轉載的文章為個人學習借鑒使用，本站對版權不負任何法律責任。如果侵犯了您的隱私權益，請聯系本站郵箱yoyou2525@163.com刪除。

猜您在找 VINS（四）初始化與相機IMU外參標定解放雙手——相機與IMU外參的在線標定相機的內參外參標定 [摘抄]相機標定（內參/外參）概要 cartographer---第二篇（imu與lidar外參的自動標定）利用Kalibr標定雙目相機與IMU Autoware calibration toolkit激光雷達與相機外參聯合標定相機-imu外參校准總結 lidar、深度相機與imu的相對位姿標定 VIO系統的IMU與相機時間偏差標定