齊次矩陣與cayley形式轉換
cv::Matx<double, 4, 4> R11(-0.446271, -0.00127545, 0.894897, 0.0661307,
0.0251105 ,0.999587, 0.0139469, 0.00074488,
-0.894545 ,0.0286955, -0.446055, -0.147211,
0,0,0,1 );
cv::Matx<double, 4, 4> R12(-0.488612 ,0.0263332, -0.872104, -0.0923294,
-0.0144887, 0.999162, 0.0382872, 0.00384811,
0.872381 ,0.0313432, -0.487821, -0.142081,
0,0,0,1 );
cout<<"--------------------------caylay-------------------------------------"<<endl;
cout<<hom2cayley<double>(R11);
cout<<"---------------------------------------------------------------------"<<endl;
cout<<hom2cayley<double>(R12);
cout<<endl<<"--------------------------caylay-------------------------------------"<<endl;
/**
* 4x4 homogeneous transformation matrix to Cayley + translation representation
*
* @param T 4x4 homogeneous transformation matrix
*
* @return c 6x1 Cayley parameters and translation
*/
template<typename T>
cv::Matx<T, 6, 1> hom2cayley(const cv::Matx<T, 4, 4>& M)
{
cv::Matx<T, 3, 3> R(M(0, 0), M(0, 1), M(0, 2),
M(1, 0), M(1, 1), M(1, 2),
M(2, 0), M(2, 1), M(2, 2));
cv::Matx<T, 3, 1> C = rot2cayley(R);return cv::Matx<T, 6, 1>(C(0, 0), C(1, 0), C(2, 0),
M(0, 3), M(1, 3), M(2, 3));
}/**
* 6x1 minimal homogeneous transformation vector to homogeneous 4x4 transformation matrix
*
* @param c 6x1 Cayley parameters and translation
*
* @return T 4x4 homogeneous transformation matrix
*/
template<typename T>
cv::Matx<T, 4, 4> cayley2hom(const cv::Matx<T, 6, 1>& cayleyRep)
{
cv::Matx<T, 3, 1> cayleyR(cayleyRep(0, 0), cayleyRep(1, 0), cayleyRep(2, 0));
cv::Matx<T, 3, 3> R = cayley2rot(cayleyR);cv::Matx<T, 4, 4> homM(
R(0, 0), R(0, 1), R(0, 2), cayleyRep(3, 0),
R(1, 0), R(1, 1), R(1, 2), cayleyRep(4, 0),
R(2, 0), R(2, 1), R(2, 2), cayleyRep(5, 0),
T(0), T(0), T(0), T(1));return homM;
}