opencv vector <point3f>到3列mat </point3f>

时间:2014-07-28 08:37:07

标签: c++ opencv slam-algorithm

我有2个矢量(p1和p2)的point3f变量代表2个3D点云。为了匹配这两个点云,我想使用SVD为此找到转换。问题是SVD需要一个矩阵(p1 * p2转置)。我的问题是如何将大小为Y的向量转换为Yx3矩阵?

我试过cv :: Mat p1Matrix(p1),但这给了我一个带有两个维度的行向量。我也找到了fitLine,但我认为这只适用于2D。

提前谢谢。

2 个答案:

答案 0 :(得分:0)

如下:

cv::Mat p1copy(3, p1.size(), CV_32FC1);

for (size_t i = 0, end = p1.size(); i < end; ++i) {
    p1copy.at<float>(0, i) = p1[i].x;
    p1copy.at<float>(1, i) = p1[i].y;
    p1copy.at<float>(2, i) = p1[i].z;
}

如果这样可以获得所需的结果you can make the code faster by using a pointer instead of the rather slow at<>() function

答案 1 :(得分:0)

我使用reshape函数将点矢量转换为Mat。

vector<Point3f> P1,P2;
Point3f c1,c2;//center of two set
... //data association for two set
Mat A=Mat(P1).reshape(1).t();
Mat B=Mat(P2).reshape(1).t();

Mat AA,BB,CA,CB;
repeat(Mat(c1),1,P1.size(),CA);
repeat(Mat(c2),1,P2.size(),CB);
AA=A-CA;
BB=B-CB;
Mat H=AA*BB.t();
SVD svd(H);
Mat R_;
transpose(svd.u*svd.vt,R_);
if(determinant(R_)<0)
    R_.at<float>(0,2)*=-1,R_.at<float>(1,2)*=-1,R_.at<float>(2,2)*=-1;
Mat t=Mat(c2)-R_*Mat(c1);