画像点からx,y座標（3次元）を算出する。

質問

3D座標系でオブジェクトの位置を特定するタスクがあります。ほぼ正確なXとY座標を得る必要があるので、この写真のオレンジ色のボールのように、移動するオブジェクトの上部に置かれる、Z座標がわかっている1つのカラーマーカーを追跡することにしました。

まず、カメラのキャリブレーションを行って固有パラメータを取得し、その後、次のコードのように cv::solvePnP を使用して回転と並進のベクトルを取得しました。

std::vector<cv::Point2f> imagePoints;
std::vector<cv::Point3f> objectPoints;
//img points are green dots in the picture
imagePoints.push_back(cv::Point2f(271.,109.));
imagePoints.push_back(cv::Point2f(65.,208.));
imagePoints.push_back(cv::Point2f(334.,459.));
imagePoints.push_back(cv::Point2f(600.,225.));

//object points are measured in millimeters because calibration is done in mm also
objectPoints.push_back(cv::Point3f(0., 0., 0.));
objectPoints.push_back(cv::Point3f(-511.,2181.,0.));
objectPoints.push_back(cv::Point3f(-3574.,2354.,0.));
objectPoints.push_back(cv::Point3f(-3400.,0.,0.));

cv::Mat rvec(1,3,cv::DataType<double>::type);
cv::Mat tvec(1,3,cv::DataType<double>::type);
cv::Mat rotationMatrix(3,3,cv::DataType<double>::type);

cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
cv::Rodrigues(rvec,rotationMatrix);

すべての行列が揃った後、この式は画像点をWolrd座標に変換するのに役立ちます。

ここで、MはcameraMatrix、RはrotationMatrix、tはtvec、sは未知数である。Zconstはオレンジ色のボールがある高さを表し、この例では285mmです。 というわけで、まず、前の式を解いて、quot;s" を求め、その後、画像点を選択することによって、XとYの座標を求めることができるのです。

これを解くと、Zconstが分かっているので、行列の最後の行を使って、変数 "s"を求めることができますので、そのためのコードを以下に示します。

cv::Mat uvPoint = (cv::Mat_<double>(3,1) << 363, 222, 1); // u = 363, v = 222, got this point using mouse callback

cv::Mat leftSideMat  = rotationMatrix.inv() * cameraMatrix.inv() * uvPoint;
cv::Mat rightSideMat = rotationMatrix.inv() * tvec;

double s = (285 + rightSideMat.at<double>(2,0))/leftSideMat.at<double>(2,0)); 
//285 represents the height Zconst

std::cout << "P = " << rotationMatrix.inv() * (s * cameraMatrix.inv() * uvPoint - tvec) << std::endl;

この後、結果が出ました。P = [-2629.5, 1272.6, 285.]

で、これを計測と比較すると、こうなります。Preal = [-2629.6, 1269.5, 285.]です。

は誤差が非常に小さく、非常に良いのですが、このボックスをこの部屋の端に移動させると、誤差が20-40mmになるので、これを改善したいと思います。どなたか、お勧めの方法があれば教えてください。

どのように解決するのですか？

あなたの構成では、エッジの20～40mmの誤差が平均的です。うまくいっているようですね。

カメラやシステム構成を変更しない限り、より良い結果を得ることは難しいでしょう。カメラキャリブレーションをやり直して、より良い結果を期待することはできますが、これはあまり改善されません (最終的にはより悪い結果を得る可能性があるので、実際の固有パラメータを消去しないようにしましょう)。

count0 が言ったように、より高い精度が必要な場合は、複数の測定を行う必要があります。