Search code examples
opencvprojection

Opencv : project points manually

I'm trying to reproduce the behavior of the method projectPoints() from OpenCV.

In the two images below, red/green/blue axis are obtained with OpenCV's method, whereas magenta/yellow/cyan axis are obtained with my own method :

image1

enter image description here

image2

enter image description here

With my method, axis seem to have a good orientation but translations are incorrect.

Here is my code : 

void drawVector(float x, float y, float z, float r, float g, float b, cv::Mat &pose, cv::Mat &cameraMatrix, cv::Mat &dst) {
    //Origin = (0, 0, 0, 1)
    cv::Mat origin(4, 1, CV_64FC1, double(0));
    origin.at<double>(3, 0) = 1;

    //End = (x, y, z, 1)
    cv::Mat end(4, 1, CV_64FC1, double(1));
    end.at<double>(0, 0) = x; end.at<double>(1, 0) = y; end.at<double>(2, 0) = z;

    //multiplies transformation matrix by camera matrix
    cv::Mat mat = cameraMatrix * pose.colRange(0, 4).rowRange(0, 3);

    //projects points
    origin = mat * origin;
    end = mat * end;

    //draws corresponding line
    cv::line(dst, cv::Point(origin.at<double>(0, 0), origin.at<double>(1, 0)),
        cv::Point(end.at<double>(0, 0), end.at<double>(1, 0)),
        CV_RGB(255 * r, 255 * g, 255 * b)); 
}

void drawVector_withProjectPointsMethod(float x, float y, float z, float r, float g, float b, cv::Mat &pose, cv::Mat &cameraMatrix, cv::Mat &dst) {
    std::vector<cv::Point3f> points;
    std::vector<cv::Point2f> projectedPoints;

    //fills input array with 2 points
    points.push_back(cv::Point3f(0, 0, 0));
    points.push_back(cv::Point3f(x, y, z));

    //Gets rotation vector thanks to cv::Rodrigues() method.
    cv::Mat rvec;
    cv::Rodrigues(pose.colRange(0, 3).rowRange(0, 3), rvec);

    //projects points using cv::projectPoints method
    cv::projectPoints(points, rvec, pose.colRange(3, 4).rowRange(0, 3), cameraMatrix, std::vector<double>(), projectedPoints);

    //draws corresponding line
    cv::line(dst, projectedPoints[0], projectedPoints[1],
        CV_RGB(255 * r, 255 * g, 255 * b)); 
}

void drawAxis(cv::Mat &pose, cv::Mat &cameraMatrix, cv::Mat &dst) {
    drawVector(0.1, 0, 0, 1, 1, 0, pose, cameraMatrix, dst);
    drawVector(0, 0.1, 0, 0, 1, 1, pose, cameraMatrix, dst);
    drawVector(0, 0, 0.1, 1, 0, 1, pose, cameraMatrix, dst);

    drawVector_withProjectPointsMethod(0.1, 0, 0, 1, 0, 0, pose, cameraMatrix, dst);
    drawVector_withProjectPointsMethod(0, 0.1, 0, 0, 1, 0, pose, cameraMatrix, dst);
    drawVector_withProjectPointsMethod(0, 0, 0.1, 0, 0, 1, pose, cameraMatrix, dst);
}

What am I doing wrong ?

Solution

  • I just forgot to divide the resulting points by their last component after projection :

    Given the matrix of the camera wich serve to take an image, and for any point (x, y, z, 1) in 3d space, its projection on that image is computed like following :

    //point3D has 4 component (x, y, z, w), point2D has 3 (x, y, z).
point2D = cameraMatrix * point3D;

//then we have to divide the 2 first component of point2D by the third.
point2D /= point2D.z;