姿态估计答案

【问题标题】：pose estimation姿态估计
【发布时间】：2012-07-20 22:52:38
【问题描述】：

在您的帮助下，我已经能够使用 CvPosit 来估计相机姿势（使用此链接 http://www.aforgenet.com/articles/posit/）。我正在使用 MATLAB 代码。

现在我希望通过在图像坐标上投影 3D 点来验证该姿势。详情如下：

假设立方体的图像点和世界点是：

World Points=[28 28 -28;-28 28 -28;28 -28 -28, 28 28 28]
Image_Points=[-4 29;-180 86;-5 -102;76 137];

得到的Pose为：
```
Pose =[0.896 0.0101 0.4439 -13.9708;-0.3031 0.7127 0.6326 13.7039;-0.3100 -0.701 0.6416 164.5663;0 0 0 1];
```
在我的示例中，我使用 640 作为焦距。现在从这个姿势开始，我想使用一个 3D 点并将其投影回来以获得 2D 图像点。理想情况下，我应该得到同样的观点。但我无法得到它。我在 MATLAB 中使用以下方式：
假设P = [28;28;28;28;1] % 4 X 1 matrix for a 3D point
```
P_Camera= Pose *  P;  

Calibration Matrix (K)=[640 0 0 0;0 640 0 0;0 0 1 0;0 0 0 1];

Image Points= P_Camera*K;           
```
我得到x = 15251 和y = 27447。我不知道我在这里做错了什么。请帮帮我！！

【问题讨论】：

【解决方案1】：

相机位姿或相机外在参数由旋转R和平移t组成。我相信你的姿势矩阵实际上是[R t; [0 0 0 1]]。要将 3D 均匀点 (4x1) 转换为相机坐标，您需要：

Xcam = K*[R t]*Xworld

其中 K 是 3x3 camera matrix，包含相机内在（内部）参数。稍微玩一下你的数字，如果你将 K 设置为：

K =

3.3202         0   -0.0229
     0    3.3202    0.0153
     0         0    1.0000

你会得到一个接近的答案（这个 K 不适合你的 F=640 的数据，但它是具有输入点和姿势的 LS 解决方案）。不过还是有一些错误。尝试校准您的相机并考虑光学失真（Matlab here 有一个很好的相机校准工具箱。

【讨论】：