目录
- initFramePoseByPnP(frame_count, Ps, Rs, tic, ric)
- triangulate(frame_count, Ps, Rs, tic, ric)
initFramePoseByPnP(frame_count, Ps, Rs, tic, ric)
其中,输出在Ps,Rs
中,tic
为tbc ,ric
为Rbc
1、将本帧所有的 已知深度的点,利用它第一次出现的帧的位姿和当时三角化出的xyz,来计算出其世界系坐标并存入pts3D
,相应的当前帧的归一化平面坐标存入pts2D
,步骤为:
- points先由在相机系的位置–>IMU系
- 由points在IMU系的位置->point在世界系 (Rs,Ps为imu系相对世界系的位姿)
P
i
m
u
=
R
b
c
∗
P
c
a
m
+
t
b
c
P_{imu} = R_{bc} * P_{cam} + t_{bc}
Pimu=Rbc∗Pcam+tbc
P
b
=
R
w
b
∗
P
i
m
u
+
t
w
b
P_b = R_{wb} * P_{imu}~ + t_{wb}
Pb=Rwb∗Pimu +twb
Vector3d ptsInIMU = ric[0] * (point * estimated_depth) + tic[0];//归一化了, 乘逆深度才是真实坐标
Vector3d ptsInWorld = Rs[ ] * ptsInCam + Ps[ ];
- 转换完放入pts3D即可,同时在当前帧的平面坐标直接放pts2D
由上一帧位姿(相对IMU系)和外参,得到上一帧的Cam位姿 :
Rwc = Rwb * Rbc
Pwc = Rwb * Pbc + twb
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系)和外参Rbc计算左右相机位姿Twc0Twc1 (即[R, t]) leftPose, rightPose
利用svd方法对双目point0, point1
进行三角化,输出point3d
为相对世界系的Pw ,
triangulatePoint(leftPose, rightPose, point0, point1, point3d);
变为相对左相机坐标系下的坐标 Rcw T * Pw + tcw,
最后把深度提取出来,用feature.estimated_depth = depth
放到FeaturePerId
的逆深度属性中
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)