vins博客的一部分7

initFramePoseByPnP(frame_count, Ps, Rs, tic, ric)

其中,输出在Ps,Rs中,tic为t_bc ,ric为R_bc

1、将本帧所有的 已知深度的点，利用它第一次出现的帧的位姿和当时三角化出的xyz，来计算出其世界系坐标并存入pts3D，相应的当前帧的归一化平面坐标存入pts2D,步骤为:

• points先由在相机系的位置–>IMU系
• 由points在IMU系的位置->point在世界系 (Rs,Ps为imu系相对世界系的位姿)

$P_{imu}=R_{bc}\ast P_{cam}+t_{bc}$

$P_b=R_{wb}\ast P_{imu}+t_{wb}$

Vector3d ptsInIMU = ric[0] * (point * estimated_depth) + tic[0];//归一化了, 乘逆深度才是真实坐标
Vector3d ptsInWorld = Rs[ ] * ptsInCam + Ps[ ];

• 转换完放入pts3D即可，同时在当前帧的平面坐标直接放pts2D

由上一帧位姿（相对IMU系）和外参,得到上一帧的Cam位姿 :

R_wc = R_wb * R_bc

P_wc = R_wb * P_bc + t_wb

RCam = Rs[frameCnt - 1] * ric[0];//Rwc，camera相对word位置
PCam = Rs[frameCnt - 1] * tic[0] + Ps[frameCnt - 1];

2、由3D点坐标、本帧对应2D点坐标，便可求解本帧位姿（imu积分得到的位姿 转到世界系下的RCam、PCam作为优化初值）

solvePoseByPnP(RCam, PCam, pts2D, pts3D)//结果更新在Cam和PCam

3、最后再将本本帧的RCam和PCam转到IMU系下，放入Rs[frameCnt], Ps[frameCnt]

triangulate(frame_count, Ps, Rs, tic, ric)

利用本帧的位姿Rs Ps（相对imu系）和外参R_bc计算左右相机位姿T_wc0T_wc1 (即[R, t]) leftPose, rightPose

利用svd方法对双目point0, point1进行三角化,输出point3d为相对世界系的P_w ,

triangulatePoint(leftPose, rightPose, point0, point1, point3d);

变为相对左相机坐标系下的坐标 R_cw ^T * P_w + t_cw,

最后把深度提取出来,用feature.estimated_depth = depth放到FeaturePerId的逆深度属性中