norlab-ulaval / libpointmatcher

An Iterative Closest Point (ICP) library for 2D and 3D mapping in Robotics
BSD 3-Clause "New" or "Revised" License
1.61k stars 544 forks source link

Change storage ordering of the eigen vectors descriptors #468

Closed aguenette closed 3 years ago

aguenette commented 3 years ago

The eigen vectors were serialized row major instead of column major, which shouldn't be the case since everything else is ordered column major in libpm. So, it should be assumed that it's also the case for the eigen vectors. This PR will have an impact the users using the following filters:

and the following descriptors:

So, retrieving the eigen vectors from a 3D point cloud before this PR could be done like this:

PM::Matrix eigenVectors = cloud.getDescriptorsViewByName("eigVectors"); // [ a₁, b₁, c₁, a₂, b₂, c₂, a₃, b₃, c₃ ]ᵀ    

Eigen::Map<PM::Vector, 0, Eigen::InnerStride<3>> a(eigenVectors.col(n).data(), 3);     // [ a₁, a₂, a₃ ]ᵀ
Eigen::Map<PM::Vector, 0, Eigen::InnerStride<3>> b(eigenVectors.col(n).data() + 1, 3); // [ b₁, b₂, b₃ ]ᵀ
Eigen::Map<PM::Vector, 0, Eigen::InnerStride<3>> c(eigenVectors.col(n).data() + 2, 3); // [ c₁, c₂, c₃ ]ᵀ

And after this PR it could be done like this:

PM::Matrix eigenVectors = cloud.getDescriptorsViewByName("eigVectors"); // [ a₁, a₂, a₃, b₁, b₂, b₃, c₁, c₂, c₃ ]ᵀ    

PM::Vector a = eigenVectors.block(0, n, 3, 1); // [ a₁, a₂, a₃ ]ᵀ
PM::Vector b = eigenVectors.block(3, n, 3, 1); // [ b₁, b₂, b₃ ]ᵀ
PM::Vector c = eigenVectors.block(6, n, 3, 1); // [ c₁, c₂, c₃ ]ᵀ

Then, reconstructing the matrix is the same for both method:

PM::Matrix R(3,3);
R << a, b, c;

As we can see, it is more straightforward to retrieve the eigen vectors when they ordered column major.

pomerlef commented 3 years ago

@aguenette update to current master.

aguenette commented 3 years ago

@pomerlef It's merge.