Opencv

[opencvの学習] OpenCVに基づくクォータニオン、回転行列、オイラー角間の変換



Conversion Between Quaternion



Googleで検索したブログからコードのこの部分を抽出しました。もちろん、フォーマットやスタイルは統一されていません。 OpenCVのより純粋なバージョンを見たい場合は、突いてください OpenCVに基づくクォータニオン、回転行列、オイラー角間の変換（2） 。

クォータニオンから回転行列



void getRotation(double *Quaternion, double *rt_mat) { rt_mat[0] = 1 - 2 * (Quaternion[2] * Quaternion[2]) - 2 * (Quaternion[3] * Quaternion[3]) rt_mat[1] = 2 * Quaternion[1] * Quaternion[2] - 2 * Quaternion[0] * Quaternion[3] rt_mat[2] = 2 * Quaternion[1] * Quaternion[3] + 2 * Quaternion[0] * Quaternion[2] rt_mat[3] = 2 * Quaternion[1] * Quaternion[2] + 2 * Quaternion[0] * Quaternion[3] rt_mat[4] = 1 - 2 * (Quaternion[1] * Quaternion[1]) - 2 * (Quaternion[3] * Quaternion[3]) rt_mat[5] = 2 * Quaternion[2] * Quaternion[3] - 2 * Quaternion[0] * Quaternion[1] rt_mat[6] = 2 * Quaternion[1] * Quaternion[3] - 2 * Quaternion[0] * Quaternion[2] rt_mat[7] = 2 * Quaternion[2] * Quaternion[3] + 2 * Quaternion[0] * Quaternion[1] rt_mat[4] = 1 - 2 * (Quaternion[1] * Quaternion[1]) - 2 * (Quaternion[2] * Quaternion[2]) }

四元数への回転行列

void getQuaternion(Mat R, double Q[]) { double trace = R.at(0,0) + R.at(1,1) + R.at(2,2)

}



回転行列に対するオイラー角

if (trace > 0.0) { double s = sqrt(trace + 1.0) Q[3] = (s * 0.5) s = 0.5 / s Q[0] = ((R.at<double>(2,1) - R.at<double>(1,2)) * s) Q[1] = ((R.at<double>(0,2) - R.at<double>(2,0)) * s) Q[2] = ((R.at<double>(1,0) - R.at<double>(0,1)) * s) } else { int i = R.at<double>(0,0) < R.at<double>(1,1) ? (R.at<double>(1,1) < R.at<double>(2,2) ? 2 : 1) : (R.at<double>(0,0) < R.at<double>(2,2) ? 2 : 0) int j = (i + 1) % 3 int k = (i + 2) % 3 double s = sqrt(R.at<double>(i, i) - R.at<double>(j,j) - R.at<double>(k,k) + 1.0) Q[i] = s * 0.5 s = 0.5 / s Q[3] = (R.at<double>(k,j) - R.at<double>(j,k)) * s Q[j] = (R.at<double>(j,i) + R.at<double>(i,j)) * s Q[k] = (R.at<double>(k,i) + R.at<double>(i,k)) * s }

}

行列をオイラー角に回転します



// Calculates rotation matrix given euler angles. Mat eulerAnglesToRotationMatrix(Vec3f &theta) { // Calculate rotation about x axis Mat R_x = (Mat_(3,3) << 1, 0, 0, 0, cos(theta[0]), -sin(theta[0]), 0, sin(theta[0]), cos(theta[0]) )

}

//回転行列をオイラー角に計算します
//結果は、順序を除いてMATLABと同じです
//オイラー角の（xとzが交換されます）。
Vec3frotationMatrixToEulerAngles（マット＆R）
{{

// Calculate rotation about y axis Mat R_y = (Mat_<double>(3,3) << cos(theta[1]), 0, sin(theta[1]), 0, 1, 0, -sin(theta[1]), 0, cos(theta[1]) ) // Calculate rotation about z axis Mat R_z = (Mat_<double>(3,3) << cos(theta[2]), -sin(theta[2]), 0, sin(theta[2]), cos(theta[2]), 0, 0, 0, 1) // Combined rotation matrix Mat R = R_z * R_y * R_x return R

}

// Checks if a matrix is a valid rotation matrix. bool isRotationMatrix(Mat &R) { Mat Rt transpose(R, Rt) Mat shouldBeIdentity = Rt * R Mat I = Mat::eye(3,3, shouldBeIdentity.type()) return norm(I, shouldBeIdentity) <1e-6