Friday, June 12, 2009

Easy rotation with Eigen quaternions

Currently working on improving an alignment software for time-of-flight depth maps, I have to say I find the functions offered by Eigen quite useful.
Considering the two points clouds below (rendered with VTK), the first step to align them is to rotate them correctly.

For this task, I use a simple RANSAC to find the largest plane in the point cloud. Then, having the plane normal vector nVec3, finding the rotation matrix mat that will align this plane with the Z-axis is 2 Eigen calls away :

Eigen::Vector3d nVecZ; nVecZ.UnitZ();
Eigen::Quaterniond qz; qz.setFromTwoVectors(nVec3, nVecZ);
Eigen::Matrix3d mat = qz.toRotationMatrix();

And here is the result for the dataset above:

That's 2 rotation DoF taken care of. Imposing the matched planes to have the same distance to origin takes care of one translation DoF. One rotation and 2 translations to go ;-)

1 comment: