Multiply std::vector<cv::Point3f> by a cv::Mat?

Question

as above, I have a std::vector of cv::Point3f. I have a transformation matrix. I need to multiply the vector by the inverse of the Mat.

My Mat: ( T is the resulting transformation)

cv::Mat R(3,3,rvec.type());
cv::Rodrigues(rvec, R); // R is 3x3
cv::Mat T(4, 4, R.type()); // T is 4x4
T(cv::Range(0, 3), cv::Range(0, 3)) = R * 1; // copies R into T
T(cv::Range(0, 3), cv::Range(3, 4)) = tvec * 1; // copies tvec into T
float *p = T.ptr<float>(3);
p[0] = p[1] = p[2] = 0; p[3] = 1;

my vector:

std::vector<cv::Point3f> objectPoints;

I have tried:

cv::Mat V = T.inv() * cv::Mat(objectPoints, false) 
V.copyTo(cv::Mat(objectPoints, false));

(Assertion failed, type error)

for (int i = 0; i < objectPoints.size(); i++)
{
    cv::Mat dst = cv::Mat(objectPoints[i], false);  
    dst = -T*dst; //USE MATRIX ALGEBRA 
//  cv::Point3f tmp3 = cv::Point3f(dst(0, 0), dst(1, 0), dst(2, 0));

}

(Assertion failed)

    std::vector<cv::Point3f> p3d;
perspectiveTransform(objectPoints, p3d, -T);

(runs, but the values are very incorrect)

cv::transform(objectPoints, p3d, -T);

(Assertion error)

What is the correct way (if there is a way!) to do this?

Thank you.

Answer 1

As Rick M. pointed out, you're trying to multiply a 4x4 matrix with a length-3 point. To perform a transformation with just one matrix multiplication (ie with the 4x4 combined RT matrix), you first have to represent the point in homogeneous coordinates, which essentially just involves tacking on a 1 as the 4th element of your point; after the transformation, you divide the new point by the 4th element to maintain its value as a 1. Here's a nice source on 3D-3D transformations, with homogeneous coordinates discussed on slide 14.

Since OpenCV doesn't have a Point4f class, you'll have to add this 1 when you're creating the Mat form of the point. This is untested but might work:

std::vector<cv::Point3f> dstPoint;
for (int i = 0; i < objectPoints.size(); i++) {
    // Convert Point3f to 4x1 Mat (in homogeneous coordinates, with 1 as 4th element)
    cv::Point3f pt = objectPoints[i];
    cv::Mat ptMat = (cv::Mat_<float>(4,1) << pt.x, pt.y, pt.z, 1);

    // Perform matrix multiplication and store as Mat_ for easy element access
    cv::Mat_<float> dstMat(T.inv() * ptMat); 

    // Divide first three resulting elements by the 4th (homogenizing 
    // the point) and store as Point3f
    float scale = dstMat(0,3);
    cv::Point3f dst(dstMat(0,0)/scale, dstMat(0,1)/scale, dstMat(0,2)/scale);
    dstPoints.push_back(dst)
}

Would test, but I'm at work and don't have OpenCV on this computer.

UPDATE:

When copying to T, try this instead:

cv::Mat T(4, 4, cv::DataType<float>::type);
cv::Mat rot   = T(cv::Range(0, 3), cv::Range(0, 3));
cv::Mat trans = T(cv::Range(0, 3), cv::Range(3, 4));
R.copyTo(rot);
tvec.copyTo(trans);

Answer 2

Based on the answer by DCSmith, I have it working. I had to make this small change:

cv::Mat T(4, 4, cv::DataType<float>::type);
R.copyTo(T(cv::Rect(0, 0, 3, 3)));
tvec.copyTo(T(cv::Rect(3, 0, 1, 3)));

To make the entire function look like:

std::vector<cv::Point3f> p3d;

cv::Mat R(3,3, cv::DataType<float>::type);
cv::Rodrigues(rvec, R); // R is 3x3
cv::Mat T(4, 4, cv::DataType<float>::type);

R.copyTo(T(cv::Rect(0, 0, 3, 3)));
tvec.copyTo(T(cv::Rect(3, 0, 1, 3)));

float *p = T.ptr<float>(3);
p[0] = p[1] = p[2] = 0; p[3] = 1;


std::vector<cv::Point3f> dstPoint;
for (int i = 0; i < objectPoints.size(); i++) {
    cv::Point3f pt = objectPoints[i];
    cv::Mat ptMat = (cv::Mat_<float>(4, 1) << pt.x, pt.y, pt.z, 1);

    // Perform matrix multiplication and store as Mat_ for easy element access
    cv::Mat_<float> dstMat = T.inv() * ptMat;

    // Divide first three resulting elements by the 4th (homogenizing 
    // the point) and store as Point3f
    float scale = dstMat(0, 3);
    cv::Point3f dst(dstMat(0, 0) / scale, dstMat(0, 1) / scale, dstMat(0, 2) / scale);
    p3d.push_back(dst);
}

Thank you for your help!