6

我正在使用函数 solvePnP 通过视觉标记估计我的机器人的姿势。有时我会在两个连续的帧中得到错误的结果。在文件 question.cpp 中，您可以看到这些结果之一。

点集对应于两个连续帧中的相同标记。它们之间的差异很小，solvePnP的结果也很不同，但只是在旋转向量上。翻译向量没问题。

这大约每 30 帧发生一次。我用相同的数据测试了 CV_ITERATIVE 和 CV_P3P 方法，它们返回相同的结果。

这是该问题的一个示例：

#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <iostream>
using namespace std;
using namespace cv;
int main(){
vector<Point2f> points1, points2;

//First point's set
points1.push_back(Point2f(384.3331f,  162.23618f));
points1.push_back(Point2f(385.27521f, 135.21503f));
points1.push_back(Point2f(409.36746f, 139.30315f));
points1.push_back(Point2f(407.43854f, 165.64435f));

//Second point's set
points2.push_back(Point2f(427.64938f, 158.77661f));
points2.push_back(Point2f(428.79471f, 131.60953f));
points2.push_back(Point2f(454.04532f, 134.97353f));
points2.push_back(Point2f(452.23096f, 162.13156f));

//Real object point's set
vector<Point3f> object;
object.push_back(Point3f(-88.0f,88.0f,0));
object.push_back(Point3f(-88.0f,-88.0f,0));
object.push_back(Point3f(88.0f,-88.0f,0));
object.push_back(Point3f(88.0f,88.0f,0));

//Camera matrix
Mat cam_matrix = Mat(3,3,CV_32FC1,Scalar::all(0));
cam_matrix.at<float>(0,0) = 519.0f;
cam_matrix.at<float>(0,2) = 320.0f;
cam_matrix.at<float>(1,1) = 522.0f;
cam_matrix.at<float>(1,2) = 240.0f;
cam_matrix.at<float>(2,2) = 1.0f;

//PnP
Mat rvec1i,rvec2i,tvec1i,tvec2i;
Mat rvec1p,rvec2p,tvec1p,tvec2p;
solvePnP(Mat(object),Mat(points1),cam_matrix,Mat(),rvec1i,tvec1i,false,CV_ITERATIVE);
solvePnP(Mat(object),Mat(points2),cam_matrix,Mat(),rvec2i,tvec2i,false,CV_ITERATIVE);
solvePnP(Mat(object),Mat(points1),cam_matrix,Mat(),rvec1p,tvec1p,false,CV_P3P);
solvePnP(Mat(object),Mat(points2),cam_matrix,Mat(),rvec2p,tvec2p,false,CV_P3P);

//Print result
cout<<"Iterative: "<<endl;
cout<<" rvec1 "<<endl<<" "<<rvec1i<<endl<<endl;
cout<<" rvec2 "<<endl<<" "<<rvec2i<<endl<<endl;
cout<<" tvec1 "<<endl<<" "<<tvec1i<<endl<<endl;
cout<<" tvec1 "<<endl<<" "<<tvec2i<<endl<<endl;

cout<<"P3P: "<<endl;
cout<<" rvec1 "<<endl<<" "<<rvec1p<<endl<<endl;
cout<<" rvec2 "<<endl<<" "<<rvec2p<<endl<<endl;
cout<<" tvec1 "<<endl<<" "<<tvec1p<<endl<<endl;
cout<<" tvec1 "<<endl<<" "<<tvec2p<<endl<<endl;

return 0;

}

这是结果：

Iterative: 
rvec1 
[-0.04097605099283788; -0.3679435501353919; 0.07086072250132323]
rvec2 
[0.4135950235376482; 0.6834759799439329; 0.1049879790744613]
tvec1 
[502.4723979671957; -582.21069174737; 3399.430492848247]
tvec2 
[774.9623278021523; -594.8332356366083; 3338.42153723169]
P3P: 
rvec1 
[-0.08738607323881876; -0.363959462471951; 0.06617591006606272]
 rvec2 
[0.4239629869157338; 0.7210136877984544; 0.1133539043199323]
tvec1 
[497.3941378807597; -574.3015171812298; 3354.522829883918]
tvec1 
[760.2641421675842; -582.2718972605966; 3275.390313948845]

谢谢。

4

1 回答 1

12

我假设您的输入图像是 640x480 并将两个观察到的标记绘制到白色画布上。第一帧的标记为红色，第二帧的标记为蓝色。

观察到的标记

正方形在两张图像中大致面向相机，并且在屏幕上非常小。这意味着很难估计位置和旋转。尤其是到物体的距离以及围绕 x 和 y 轴的旋转。围绕这些轴的旋转的适度变化几乎不会引起注意，因为这些点大多会朝向或远离相机移动。标记检测的错误将对结果产生很大影响。

估计的标记位置和方向的不确定性可以使用从projectPoints方法获得的雅可比来估计。

// Compute covariance matrix of rotation and translation
Mat J;
vector<Point2f> p;
projectPoints(object, rvec1i, tvec1i, cam_matrix, Mat(), p, J);
Mat Sigma = Mat(J.t() * J, Rect(0,0,6,6)).inv();

// Compute standard deviation
Mat std_dev;
sqrt(Sigma.diag(), std_dev);
cout << "rvec1, tvec1 standard deviation:" << endl << std_dev << endl;

rvec1, tvec1 标准差：
[0.0952506404906549；0.09211686006979068; 0.02923763901152477; 18.61834775099151；21.61443561870643；124.9111908058696]

此处获得的标准偏差应根据观察点的不确定性（以像素为单位）进行缩放。可以看到围绕 x 和 y 的旋转不确定性大于围绕 z 的旋转不确定性，并且到相机的距离具有很大的不确定性。

如果将结果复制到 matlab，则可以像这样绘制协方差矩阵：

imagesc(sqrt(abs(Sigma)))

协方差可视化

该图像告诉我们，平移 z 方向的不确定性最大，并且该方向的估计与估计的 x 和 y 位置（在 3D 空间中）密切相关。因为旋转和平移使用不同的单位，所以很难将旋转误差与位置误差联系起来。

如果您想要更稳定的标记估计，我建议使用扩展卡尔曼滤波器或类似的东西过滤数据。这将使您受益于知道图像是序列的一部分并跟踪不确定性，这样您就不会被信息很少的观察所愚弄。OpenCV 有一些卡尔曼滤波功能可能会派上用场。

也许你很久以前就解决了你的问题，但我希望这篇文章能给别人带来一些见解！

于 2014-01-03T20:11:20.080 回答