80 Eigen::JacobiSVD<Eigen::Matrix<float, 3, 3> >
svd (
covariance_, Eigen::ComputeFullU | Eigen::ComputeFullV);
81 const Eigen::Matrix<float, 3, 3>& u =
svd.matrixU(),
83 Eigen::Matrix<float, 3, 3> s;
85 if (u.determinant()*v.determinant() < 0.0f)
88 Eigen::Matrix<float, 3, 3> r = u * s * v.transpose();
92 ret(0,0)=r(0,0);
ret(0,1)=r(0,1);
ret(0,2)=r(0,2);
ret(0,3)=
t(0);
93 ret(1,0)=r(1,0);
ret(1,1)=r(1,1);
ret(1,2)=r(1,2);
ret(1,3)=
t(1);
94 ret(2,0)=r(2,0);
ret(2,1)=r(2,1);
ret(2,2)=r(2,2);
ret(2,3)=
t(2);
95 ret(3,0)=0.0f;
ret(3,1)=0.0f;
ret(3,2)=0.0f;
ret(3,3)=1.0f;