- 操作系统：ubuntu22.04
- OpenCV版本：OpenCV4.9
- IDE:Visual Studio Code
- 编程语言：C++11
算法描述
从3D-2D点对应关系出发，并基于一个初始解，精细化姿态（将物体坐标系中的3D点变换到相机坐标系的旋转和平移）。
cv::solvePnPRefineVVS 是 OpenCV 中用于精细化优化由 cv::solvePnP 或 cv::solvePnPRansac 等函数得到的初始估计值的另一种方法。它使用了一种称为“Virtual Visual Servoing”（虚拟视觉伺服）的技术，通过最小化重投影误差来提高姿态估计（旋转和平移向量）的精度。与 solvePnPRefineLM 使用 Levenberg-Marquardt 算法不同，solvePnPRefineVVS 使用了不同的优化策略。
函数原型
void cv::solvePnPRefineVVS
(
InputArray objectPoints,
InputArray imagePoints,
InputArray cameraMatrix,
InputArray distCoeffs,
InputOutputArray rvec,
InputOutputArray tvec,
TermCriteria criteria = TermCriteria(TermCriteria::EPS+TermCriteria::COUNT, 20, FLT_EPSILON),
double VVSlambda = 1
)
参数
- 参数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迭代算法的标准。
- 参数VVSlambda 虚拟视觉伺服控制律的增益，等效于阻尼高斯-牛顿公式中的α增益。
代码示例
#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;
}
// 使用 solvePnPRefineVVS 进行精细化优化
TermCriteria criteria( TermCriteria::EPS + TermCriteria::COUNT, 100, 1e-5 );
double VVSlambda = 1.0;
solvePnPRefineVVS( objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec_initial, tvec_initial, criteria, VVSlambda );
cout << "精化后的旋转向量: " << rvec_initial << endl;
cout << "精化后的平移向量: " << tvec_initial << endl;
// 可选：将旋转向量转换为旋转矩阵
Mat R;
Rodrigues( rvec_initial, R );
cout << "精化后的旋转矩阵: " << endl << R << endl;
return 0;
}
运行结果
精化后的旋转向量: [0.0404834, -0.0162116, 0.000550362]
精化后的平移向量: [-0.251497, 0.504088, 5.22796]
精化后的旋转矩阵:
[0.9998684609178976, -0.0008782866442889067, -0.01619535347700669;
0.0002220893934479208, 0.9991805272568761, -0.04047498767105974;
0.01621763046736627, 0.040466066812103, 0.9990492910256142]