Parsing an Eigen::Isometry3d matrix about manif HOT 5 CLOSED

The error message tells you what-s going on: data is not normalized. In manif::SE(3), the rotation part is a Eigen::Quaternion, and the norm of the quaternion MUST BE 1.

To fix it, your original data must be correct:

• You should make sure your original matrix has a valid rotation matrix (det(R) == 1 and R.tr*R == Id).

If it is correct, then it means that passing from float to double you miss some precision and that the data is not normalized.

• Then what you can do is convert the rotation part to quaternion, then normalize the quaternnion, then construct the SE3 object from translation and quaternion.

from manif.

Hi Prof,

Thank you very much for this answer, i'll try straight away!

from manif.

You could alternatively raise the tolerance level of the tests by modifying some constant in manif, but first you should make sure that your original data is correct. If I remember correctly, our tests already considered the possibility of working with float with the selection of the appropriate tolerance level in the unit tests, and therefore I bet your first guess is look at your data.

If your data is OK but you still do not pass, please report back and we'll see if we raise the tolerance for the whole project. @artivis what is your opinion?

from manif.

Among other stuffs I've tried is to convert my 4x4 matrix which is basically the result of a 3d ICP to the tuple (tx, ty, ty, thetax, thetay, thetaz) as follows, then insert it in the SE3 constructor:

void Registration::convertTf2Vector(tf::Transform &tf_ ,
Eigen::Vector3d &tt,
Eigen::Vector3d &euler
)
{

Eigen::Isometry3d pose;
tf::transformTFToEigen(tf_, pose );

tt =  pose.translation();

tf::Matrix3x3 m(tf_.getRotation());

double roll, pitch, yaw;
m.getEulerYPR(yaw, pitch, roll);

euler << yaw, pitch, roll;

}

I will try to convert the tf to quaternion and translation and apply your recommendations, i'll let you know. Thank you very much for your valuable consideration, i feel privileged.

from manif.

Indeed, if your data starts from Euler angles, then you are better off converting it to quaternion directly. thanks for the feedback.

from manif.