Parsing an Eigen::Isometry3d matrix

[Tawsif84]

I am having pretty much difficulty with parsing an Eigen::Isometry3d matrix which I fill from an Eigen::Matrix4f pose converted to double:

Eigen::Isometry3d ps_lfr2rob_fin; ps_lfr2rob_fin.matrix() = finallidar2rob.cast\();

I have tried many permutations to fill an SE3d type matrix and printing it thereafter to little avail! Why is it so difficult to parse a matrix. Please help out!

manif::SE3d se3h(ps_lfr2rob_fin); cout<<"XXXX:" << se3h<<endl;

I am getting the following error at runtime: terminate called after throwing an instance of 'manif::invalid_argument' what(): SE3 assigned data not normalized !

[joansola]

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.

[Tawsif84]

Hi Prof,

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

[joansola]

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?

[Tawsif84]

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.

[joansola]

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