OpenCV (相机姿态还原) | EVA'S WONDERLAND

在已知物体三维结构的情况下，如何计算出相机的姿态。

三维姿态

solvePnP

官方解释：

http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#solvepnp

1	bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec,bool useExtrinsicGuess=false, int flags=ITERATIVE )

注意，OpenCV 中还提供了 SolvePnPRansac 函数。它使用 RANSAC 算法求解 PnP 问题。这个函数能识别出错误的物体点/图像点对，并将其标记为异常数据。

RANSAC（随机抽样一致性）

用于匹配图像的算法

https://zhuanlan.zhihu.com/p/45532306

https://blog.csdn.net/weixin_42990464/article/details/119254747

cvPOSIT

官方解释：

http://www.opencv.org.cn/index.php/Cv%E7%85%A7%E7%9B%B8%E6%9C%BA%E5%AE%9A%E6%A0%87%E5%92%8C%E4%B8%89%E7%BB%B4%E9%87%8D%E5%BB%BA#POSIT

1	void cvPOSIT( CvPOSITObject* posit_object, CvPoint2D32f* image_points, double focal_length, CvTermCriteria criteria, CvMatr32f rotation_matrix, CvVect32f translation_vector )

两种函数的同与异

同：

输入都是 3D 点集和对应的 2D 点集，其中 cvPOSIT 的 3D 点包含在 posit_object 结构中
输出包括旋转矩阵和位移向量

异：

solvePnP 调用的是 cvFindExtrinsicCameraParams2 通过已知的内参进行未知外参求解，是一个精确解；而 cvPOSIT 是用仿射投影模型近似透视投影模型下，不断迭代计算出来的估计值(在物体深度变化相对于物体到摄像机的距离比较大的时候,这种算法可能不收敛)
solvePnP 输出的 rvec 是旋转向量，可以通过 Rodrigues 转换成旋转矩阵，有需要可以再转到欧拉角

实现

公园里的长椅

用标定的相机进行拍照并标注8个点

测量长椅的物理尺寸

椅座：242.5cm53.5cm\9cm

靠背：242.5cm24cm\9cm

两者相距 12cm

推导八个点的三维坐标

设椅座与靠背的交叉线的左侧顶点作为坐标系原点

vector<Point3f> objectPoints;
objectPoints.push_back(Point3f(0, 45, 0));
objectPoints.push_back(Point3f(242.5, 45, 0));
objectPoints.push_back(Point3f(242.5, 21, 0));
objectPoints.push_back(Point3f(0, 32, 0));
objectPoints.push_back(Point3f(0, 9, -9));
objectPoints.push_back(Point3f(242.5, 9, -9));
objectPoints.push_back(Point3f(242.5, 9, 44.5));
objectPoints.push_back(Point3f(0, 9, 44.5));

在二维成像平面中，写出这些点的坐标

vector<Point2f> imagePoints;
objectPoints.push_back(Point2f(136,113));
objectPoints.push_back(Point2f(379,114));
objectPoints.push_back(Point2f(379,150));
objectPoints.push_back(Point2f(138,135));
objectPoints.push_back(Point2f(143,146));
objectPoints.push_back(Point2f(381,166));
objectPoints.push_back(Point2f(345,194));
objectPoints.push_back(Point2f(103,161));

调用 `solvePnP` 函数，计算拍照时相机与这些点之间的相对位置

此函数实际上是通过旋转和平移，把物体坐标转换到以相机为中心的坐标系上（焦点为坐标原点）

注意，该函数得到的旋转量是一个三维容器。表示物体绕着一个单位向量（旋转轴）转了某个角度。（轴 + 角度，罗德里格旋转公式 ）。在 OpenCV 中，旋转角度对应着输出的旋转向量的值，该向量与旋转轴一致，所以，投影公式中使用 Rodrigues 函数来获取旋转三维矩阵。

Mat rvec, tvec;
solvePnP(objectPoints, imagePoints, // 对应的三维点和二维点
         cameraMatrix, cameraDistCoeffs, // 标定（相机内参 和 相机畸变）
         rvec, tvec); // 输出
// 转换成三维旋转矩阵
Mat rotation;
Rodrigues(rvec, rotation);

检验

使用 cv::viz 模块可以显示三维信息

cv::viz:: ...

使用样例

cv::viz::Viz3d visualizer("Viz window"); // 创建窗口
visualizer.setBackgroundColor(cv::viz::Color::white()); // 白色背景

// 创建一个虚拟相机
cv::viz::WCameraPosition cam(cMatrix, // 内部参数矩阵 类型为Matx33d（Matx<double, 3, 3>
                            image, // 平面上显示的图像
                            30.0, // 缩放因子
                            cv::viz::Color::black());
// 在环境中添加虚拟相机
visualizer.showWidget("Camera", cam);
// 用长方形表示虚拟的长椅
cv::viz::Wcube plane1(Point3f(0.0, 45.0, 0.0),
                     Point3f(242.5, 21.0, -9.0),
                     true, // 显示线条框架
                     cv::viz::Color::blue());
cv::viz::Wcube plane2(Point3f(0.0, 9.0, -9.0),
                     Point3f(242.5, 0.0, 44.5),
                     true, // 显示线条框架
                     cv::viz::Color::blue());
// 把虚拟物体加入到环境中
visualizer.showWidget("top", plane1);
visualizer.showWidget("bottom", plane2);

// 虚拟长椅也放在坐标原点，然后用cv::solvePnP函数计算出以相机为中心的位置，并把长椅移动到该位置。这个过程在setWidgetPose方法中完成。这里只是根据估算值进行了旋转和平移

Mat rotation;
// 将rotation转换成3*3的旋转矩阵
Rodrigues(rvec, rotation);

// 移动长椅
Affine3d pose(rotation, tvec);
visualizer.setWidgetPose("top", pose);
visualizer.setWidgetPose("bottom", pose);
// 循环显示
while(waitKey(100) == -1 && !visualizer.wasStopped()){
    
    visualizer.spinOnce(1, // 暂停1s
                       true); // 重绘
}
// 关闭可视化窗口 或 在OpenCV图像窗口上输入任意键就可以结束循环。在循环内部移动物体（用setWidgetPose），即可产生动画。

相机姿态更新

https://blog.csdn.net/aptx704610875/article/details/48915149