orthogonize.cpp

gistfile1.txt

Eigen::Affine3d m3d_utils::orthogonize(const Eigen::Affine3d& p )
{
    Eigen::Matrix4d ret = p.matrix();
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(ret.block<3,3>(0,0), Eigen::ComputeFullU | Eigen::ComputeFullV);
    double d = (svd.matrixU() * svd.matrixV().transpose()).determinant();
    Eigen::Matrix3d diag = Eigen::Matrix3d::Identity() * d;
    ret.block<3,3>(0,0) = svd.matrixU() * diag * svd.matrixV().transpose();
    return Eigen::Affine3d (ret);
//    Eigen::Affine3d pose_orto(Eigen::Affine3d::Identity());
//    Eigen::Quaterniond q1(p.matrix().block<3,3>(0,0)); q1.normalize();
//    pose_orto.translate(p.matrix().block<3,1>(0,3));
//    pose_orto.rotate(q1);
//    return pose_orto;
}