SLAM | 双目重投影误差雅克比推导

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求导：