3D到2D世界对象坐标转换

Question

我有以下（简化）代码，代表通过视觉测距法估算的机器人的3D姿态：

// (r11 r12 r13 tx)
// (r21 r22 r23 ty)
// (r31 r32 r33 tz)
// (0   0   0   1)
Eigen::Matrix4f pose_prev_to_cur;
Eigen::Matrix4f pose_cur_to_world;

while (1) {
    pose_prev_to_cur = ... /* from visual odometry */
    pose_cur_to_world = pose_cur_to_world * pose_prev_to_cur.inverse();
}

如您所见，姿势矩阵包含一个3x3 3D旋转矩阵和一个3x1平移矢量。 我正在通过在地图上为每一帧创建一个点来绘制机器人轨迹：

// x translation maps to 2D x axis
int x = WINDOW_SCALE * pose_cur_to_world(0, 3) + WINDOW_SIZE / 2;
// z translation maps to 2D y axis
int y = WINDOW_SCALE * pose_cur_to_world(2, 3) + WINDOW_SIZE / 2;
// plotting the point
cv::circle(traj, cv::Point(x, y), 1, cv::Scalar(0, 255, 0), 2);

这对我来说效果很好，并且绘制了轨迹，如您在图像中看到的： 轨迹为绿色，当前机器人位置为红色。

现在我也有每一帧的深度读数。对于从相机获得的每个图像像素，我都有一个以米为单位的深度测量值。我也想在地图上标出距离。 我目前的想法是将深度值“添加”到姿势矩阵的z平移值中，如下所示：

Eigen::Matrix4f pose_depth_reading = pose_cur_to_world;

// offset z location by depth reading in meters
pose_depth_reading(2, 3) += 2.0;

现在我的问题是：如何在地图上绘制这个新点？ 如果仅使用3D点的x / z坐标绘制点，则显然它不会正确旋转，因为它在Y轴上只有一个偏移量。

我想过要获取计算出的2D映射点，并通过深度读取偏移其y坐标，然后绕原点进行2D旋转。但是我的问题是我需要从pose_cur_to_world中的3x3 3D旋转矩阵中获取2D旋转角度，但我不确定如何做到这一点。

我期望的最终结果如下图所示： 深度读数标记为蓝色，为简洁起见，我添加了带有X和Y轴的灰色坐标框架。