Opencv：プロジェクトポイントを手動で

Question

OpenCVからprojectPoints()メソッドの動作を再現しようとしています。

画像1

：マゼンタ/イエロー/シアン軸が自分の方法を用いて得られるのに対し、下の2枚の画像における

、赤/緑/青軸は、OpenCVのの方法で得られます画像2

私の方法では

は、軸が良いORIENTAを持っているように見えます翻訳は間違っています。ここで

は私のコードです：

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); 
} 

私が間違って何をしているのですか？

Answer 1

私は投影後に最後の成分によって得られた点を分割するのを忘れ：カメラWICHのマトリックスを考える

は、画像を撮影するのに役立つ、任意のポイントの（X、Y、Z、1） 3次元空間では、その画像上のその投影は次のように計算されます：

//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;