r2q function returning a non unitary qutarnion when passed a Eigen::Isometry3d::linear() object
Function located in include/core/math/rotations.h
The following code
Eigen::Vector4d cqt = R2q(T.linear()).coeffs();
Eigen::Matrix3d R = T.linear();
Eigen::Vector4d cqt2 = R2q(R).coeffs();
std::cout << "cqt: " << cqt.transpose() << std::endl;
std::cout << "cqt2: " << cqt2.transpose() << std::endl;
std::cout << "cqt.norm(): " << cqt.norm() << std::endl;
std::cout << "cqt2.norm(): " << cqt2.norm() << std::endl;
where T is an Eigen::Isometry3d
.
It returns the following results
cqt: 0.0058 0.35 -0.35 0.71
cqt2: -0.013 -0.0082 -0.00011 1
cqt.norm(): 0.86
cqt2.norm(): 1
Have you experienced this strange behavior? The 2 versions were working before.