opencv vector<point3f> to 3 column mat - c++

Question

I have 2 vectors (p1 and p2) of point3f variables which represent 2 3D pointclouds. In order to match these two point clouds I want to use SVD to find a transformation for this. The problem is that SVD requires a matrix (p1*p2 transpose). My question is how do I convert a vector of size Y to a Yx3 matrix?
I tried cv::Mat p1Matrix(p1) but this gives me a row vector with two dimensions.I also found fitLine but I think this only works for 2D.
Thank you in advance.

Answer 1

How about something like:
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;
}
If this gives you the desired result, you can make the code faster by using a pointer instead of the rather slow at<>() function.

Answer 2

I use reshape function for convert vector of points to 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);