- 操作系统：ubuntu22.04
- OpenCV版本：OpenCV4.9
- IDE:Visual Studio Code
- 编程语言：C++11
算法描述
从3D-2D点对应关系出发，并基于一个初始解，精细化姿态（将物体坐标系中的3D点变换到相机坐标系的旋转和平移。
cv::solvePnPRefineLM 是 OpenCV 中用于精细化优化由 cv::solvePnP 或 cv::solvePnPRansac 等函数得到的初始估计值的函数。它使用 Levenberg-Marquardt (LM) 算法来最小化重投影误差，从而获得更精确的姿态估计（旋转和平移向量）。
函数原型
void cv::solvePnPRefineLM
(
InputArray objectPoints,
InputArray imagePoints,
InputArray cameraMatrix,
InputArray distCoeffs,
InputOutputArray rvec,
InputOutputArray tvec,
TermCriteria criteria = TermCriteria(TermCriteria::EPS+TermCriteria::COUNT, 20, FLT_EPSILON)
)
参数
- 参数objectPoints 物体坐标空间中的物体点数组，格式为Nx3的单通道或1xN/Nx1的三通道，其中N是点的数量。也可以传递 vector 类型的数据。
- 参数imagePoints 对应的图像点数组，格式为Nx2的单通道或1xN/Nx1的双通道，其中N是点的数量。也可以传递 vector 类型的数据。
- 参数cameraMatrix输入的相机内参矩阵 A = [ f x 0 c x 0 f y c y 0 0 1 ] A =\begin{bmatrix}f_x & 0 & c_x \\0 & f_y & c_y \\0 & 0 & 1\end{bmatrix} A= fx000fy0cxcy1
- 参数distCoeffs 输入的畸变系数向量 (k1, k2, p1, p2 [,k3 [,k4, k5, k6 [,s1, s2, s3, s4 [,τx, τy]]]])，包含4、5、8、12或14个元素。如果该向量为空，则假设畸变为零。
- 参数rvec 输入/输出旋转向量（见 Rodrigues），与 tvec 一起将模型坐标系中的点变换到相机坐标系中。输入值用作初始解。
- 参数tvec 输入/输出平移向量。输入值用作初始解。
- 参数criteria 确定何时停止Levenberg-Marquardt迭代算法的标准。
该函数从至少3个物体点及其对应的图像投影出发，给定旋转和平移向量的初始解以及相机内参矩阵和畸变系数，精细化物体的姿态。根据Levenberg-Marquardt迭代最小化过程，该函数最小化了关于旋转和平移向量的投影误差。
代码示例
#include <iostream>
#include <opencv2/opencv.hpp>
#include <vector>
using namespace cv;
using namespace std;
int main()
{
// 假设我们有以下数据
vector< Point3f > objectPoints = { Point3f( 0.0f, 0.0f, 0.0f ), Point3f( 1.0f, 0.0f, 0.0f ), Point3f( 0.0f, 1.0f, 0.0f ), Point3f( 1.0f, 1.0f, 0.0f ) };
vector< Point2f > imagePoints = { Point2f( 300.0f, 300.0f ), Point2f( 400.0f, 300.0f ), Point2f( 300.0f, 400.0f ), Point2f( 400.0f, 400.0f ) };
// 相机内参矩阵
Mat cameraMatrix = ( Mat_< double >( 3, 3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
// 畸变系数
Mat distCoeffs = ( Mat_< double >( 5, 1 ) << 0.2624, -0.9531, -0.0054, 0.0026, 1.1633 );
// 初始的旋转向量和平移向量
Vec3d rvec_initial = Vec3d( 0.0, 0.0, 0.0 ); // 初始猜测值
Vec3d tvec_initial = Vec3d( 0.0, 0.0, 1000.0 ); // 初始猜测值
// 使用 solvePnP 获取初始估计
bool success = solvePnP( objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec_initial, tvec_initial );
if ( !success )
{
cerr << "solvePnP failed." << endl;
return -1;
}
// 使用 solvePnPRefineLM 进行精细化优化
TermCriteria criteria( TermCriteria::EPS + TermCriteria::COUNT, 100, 1e-5 );
solvePnPRefineLM( objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec_initial, tvec_initial, criteria );
cout << "精化后的旋转向量: " << rvec_initial << endl;
cout << "精化后的平移向量: " << tvec_initial << endl;
// 可选：将旋转向量转换为旋转矩阵
Mat R;
Rodrigues( rvec_initial, R );
cout << "精化后的旋转矩阵: " << endl << R << endl;
return 0;
}
运行结果
精化后的旋转向量: [0.0404742, -0.0162112, 0.000554714]
精化后的平移向量: [-0.251495, 0.504091, 5.228]
精化后的旋转矩阵:
[0.9998684653639389, -0.0008825542291305424, -0.01619484698273995;
0.0002265223551939896, 0.9991808950794823, -0.04046588187334291;
0.01621729503907217, 0.04045689071341727, 0.9990496681023513]