绘制相机轨迹答案

【问题标题】：Plot Camera Trajectory绘制相机轨迹
【发布时间】：2017-03-07 15:46:17
【问题描述】：

给定一组 4x4 位姿矩阵，可以如下推导出相机的欧几里得坐标系位置：

其中R 是 3x3 旋转矩阵，t 是姿势的平移向量，根据 this question。

当以顺序方式处理这组位姿时，例如当每个位姿都参考某个时间步长的相机位姿时，旋转和平移分量可以按如下方式累加：

和

两者都可以代入第一个方程，以得出相机在给定时间步的相对位置。

我的问题是如何使用 OpenCV 或类似工具绘制这些点。对于以圆周运动围绕物体移动的相机，输出图应该是圆形的，原点在轨迹的起点。

一个例子如下所示：-

虽然我的问题不是明确地关于绘制如上所示的轴，但这将是一个奖励。

TL;DR：给定一组姿势，我们如何使用 OpenCV、VTK、Matplotlib、MATLAB 等常用工具生成类似上述的图。

【问题讨论】：

【解决方案1】：

获取每个绘图点的坐标轴向量X,Y,Z 和位置O

只需从矩阵中提取它们。见Understanding 4x4 homogenous transform matrices。现在我不知道你的矩阵是否已经是逆矩阵。因此，如果您的矩阵表示相机坐标系（未倒置），则直接提取所需的信息。如果不是先反转矩阵再提取。

如果你得到齐次变换矩阵，那么你可以通过利用转置操作来做伪逆。欲了解更多信息，请参阅full pseudo inverse matrix。
渲染每个绘图点

所以首先将轴绘制为线：
```
  red_line(O,O+a*X);
green_line(O,O+a*Y);
 blue_line(O,O+a*Z);
```
其中a 是轴线尺寸。在此之后为该位置绘制一个点
```
black_circle(O,r);
```
r 是某个半径。您可以使用任何 gfx lib/engine 进行绘图。我会选择 GDI 或 OpenGL 但这完全取决于你熟悉什么。

顺便说一句。为了改善时间线的变化，您可以调节颜色强度（从深色开始，以亮色结束，这样您就可以看到运动开始和结束的位置......）

【讨论】：