SLAM | 雙目重投影誤差雅克比推導
阿新 • • 發佈：2019-01-31
1. 雙目重投影誤差項（給的是歸一化座標）
//i時刻相機座標系下的map point座標 Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i; //i時刻IMU座標系下的map point座標 Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; //世界座標系下的map point座標 Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; //在j時刻imu座標系下的map point座標 Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); //在j時刻相機座標系下的map point座標 Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); double dep_j = pts_camera_j.z(); Eigen::Vector3d res; res[0] = pts_camera_j[0] / dep_j - pts_j[0] ; res[1] = pts_camera_j[1] / dep_j - pts_j[1] ; //右座標的差值 res[2] = pts_camera_j[0] / dep_j - bf / dep_j - right_x_j ;
2.優化量（Qi、Pi、Qj、Pj、qic、tic、inv_dep_j）
以對Pi、Qi求導為例：
（1）誤差項對pts_camera_j求導：
（2）pts_camera_j 對 Pi 、 Ri 求雅克比
（3） pts_camera_j 對 Pj、 Rj 求雅克比類似（2）
（4） pts_camera_j 對 tic、qic 求雅克比
