世界座標でのカメラの位置をcv :: solvePnPから

Question

私は較正されたカメラ（固有の行列と歪み係数）を持っていて、画像内のいくつかの3d点と対応する点（2d点）を知っています。

私はcv::solvePnPが私を助けることができることを知っている、とthisとthisを読んだ後、私はsolvePnP rvecとtvecのI出力は、カメラ座標系でのオブジェクトの回転と平行移動していることを理解しています。

ワールド座標系でカメラの回転/平行移動を調べる必要があります。それ上記のリンクから

は、コードはPythonで、簡単なようです：

found,rvec,tvec = cv2.solvePnP(object_3d_points, object_2d_points, camera_matrix, dist_coefs) 
rotM = cv2.Rodrigues(rvec)[0] 
cameraPosition = -np.matrix(rotM).T * np.matrix(tvec) 

私は（私はC++を使用しています）のpython/numpyの低いものを知らないが、これは、多くのことはありません。私にセンス：

• はrvec、solvePnPからtvec出力は3×1行列であり、3つの要素ベクトル
• cv2.Rodrigues（rvec）は3×3行列である
• cv2.Rodrigues（rvec）[0]であります3×1行列、 3要素ベクトル
• cameraPositionは、3x3行列である3x1 * 1x3行列乗算です。 openglで簡単にglTranslatefとglRotateを呼び出してどうすればいいですか？

Answer 1

「ワールド座標」を「オブジェクト座標」とすると、pnpアルゴリズムの結果の逆変換を取得する必要があります。

変換行列を逆転させるトリックがあります。この変換行列は、通常は高価な逆演算を保存し、Pythonのコードを説明します。変換[R|t]が与えられると、inv([R|t]) = [R'|-R'*t]が得られます。ここで、R'は、転置がRです。それで、あなたは（テストしていない）コード化することができます。

cv::Mat rvec, tvec; 
solvePnP(..., rvec, tvec, ...); 
// rvec is 3x1, tvec is 3x1 

cv::Mat R; 
cv::Rodrigues(rvec, R); // R is 3x3 

R = R.t(); // rotation of inverse 
tvec = -R * tvec; // translation of inverse 

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 
// fill the last row of T (NOTE: depending on your types, use float or double) 
double *p = T.ptr<double>(3); 
p[0] = p[1] = p[2] = 0; p[3] = 1; 

// T is a 4x4 matrix with the pose of the camera in the object frame 

更新：その後、OpenGLとTを使用するには、カメラフレームの軸はOpenCVのおよびOpenGL間で異なることを心に留めておく必要があります。

OpenCVはコンピュータビジョンで通常使用される参照を使用します.Xは右に、Yは下にZは前面に（this imageのように）指します。 OpenGLでのカメラのフレームは、Xが右に、Yが上に、Zが後に（this imageのように）です。したがって、180度のX軸回りに回転を適用する必要があります。この回転行列の式はwikipediaです。

// T is your 4x4 matrix in the OpenCV frame 
cv::Mat RotX = ...; // 4x4 matrix with a 180 deg rotation around X 
cv::Mat Tgl = T * RotX; // OpenGL camera in the object frame 

これらの変換は常に混乱していると私はいくつかの段階で間違っているので、塩の粒でこれがかかる場合があります。

最後に、OpenCVの行列は、行優先順序でメモリに格納され、OpenGL配列は列優先順序で格納されることを考慮してください。

Answer 2

カメラの位置を指定する標準の4x4ポーズマトリックスにしたい場合は、左上の3x3の正方形、右側の3つの要素としてtvec、下段として0,0,0,1

pose = [rotation tvec(0) 
     matrix  tvec(1) 
     here  tvec(2) 
     0 , 0, 0, 1] 

として使用rotM、カメラの姿勢の代わりの姿勢を取得する（それを反転world）