本文整理汇总了C++中eigen::Matrix3f::trace方法的典型用法代码示例。如果您正苦于以下问题：C++ Matrix3f::trace方法的具体用法？C++ Matrix3f::trace怎么用？C++ Matrix3f::trace使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类eigen::Matrix3f的用法示例。
在下文中一共展示了Matrix3f::trace方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: svd
Eigen::Vector3f rodrigues2(const Eigen::Matrix3f& matrix)
{
Eigen::JacobiSVD<Eigen::Matrix3f> svd(matrix, Eigen::ComputeFullV | Eigen::ComputeFullU);
Eigen::Matrix3f R = svd.matrixU() * svd.matrixV().transpose();
double rx = R(2, 1) - R(1, 2);
double ry = R(0, 2) - R(2, 0);
double rz = R(1, 0) - R(0, 1);
double s = sqrt((rx*rx + ry*ry + rz*rz)*0.25);
double c = (R.trace() - 1) * 0.5;
c = c > 1. ? 1. : c < -1. ? -1. : c;
double theta = acos(c);
if( s < 1e-5 )
{
double t;
if( c > 0 )
rx = ry = rz = 0;
else
{
t = (R(0, 0) + 1)*0.5;
rx = sqrt( std::max(t, 0.0) );
t = (R(1, 1) + 1)*0.5;
ry = sqrt( std::max(t, 0.0) ) * (R(0, 1) < 0 ? -1.0 : 1.0);
t = (R(2, 2) + 1)*0.5;
rz = sqrt( std::max(t, 0.0) ) * (R(0, 2) < 0 ? -1.0 : 1.0);
if( fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R(1, 2) > 0) != (ry*rz > 0) )
rz = -rz;
theta /= sqrt(rx*rx + ry*ry + rz*rz);
rx *= theta;
ry *= theta;
rz *= theta;
}
}
else
{
double vth = 1/(2*s);
vth *= theta;
rx *= vth;
ry *= vth;
rz *= vth;
}
return Eigen::Vector3d(rx, ry, rz).cast<float>();
}