2D rotations in Eigen (C++).
03 May 2021Eigen, the matrix library in C++, is so cool. Having written some of these functions during the 00s in my own code, agonizing about whether I got the subscripts right, and now to have this – it feels like magic. Here’s an example.
So I have a homography, which is a 3x3 matrix that happens to be in OpenCV’s Mat format. So that this gets written, I’ll use what I am working on now. I want to extract the rotation matrix from this homography, and grab the rotation angle. For this I can use Eigen’s Rotation2D class.
You’ll want to include these includes, possibly using namespace Eigen if you like doing so. I am putting the namespace in front of everything in the code example so you know where everything comes from, though it reads a little clunky.
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
So I have
cv::Mat H = findHomography(... , ...);
and know that we you can decompose a homography into a chain of transformations (see Richard Hartley and Andrew Zisserman’s book Multiple View Geometry in Computer Vision, chapter two and specifically 2.4.6). I have some prior information about this case, and a general idea that the rotations will fall into 4 general cases. I just want that angle!
Ok, to get from a homography to a rotation matrix we use our friend SVD. First, copy the OpenCV Mat over to an Eigen MatrixXd object and then run Eigen’s SVD function on it. SVD needs a matrix with a dynamic size, so MatrixXd, versus Matrix2d. How do I know this? Runtime error.
Eigen::MatrixXd mat2d(2,2);
// copy
for (int r = 0; r < 2; r++){
for (int c = 0; c < 2; c++){
mat2d(r, c) = H.at<double>(r, c);
}
}
// svd
Eigen::JacobiSVD<MatrixXd> svd(mat2d, ComputeThinU | ComputeThinV);
// ortho = U*V^T
Eigen::Matrix2d ortho2d = svd.matrixU()*svd.matrixV().transpose();
ortho2d is an orthonormal 2x2 matrix, in other words, the rotation matrix we’ve been looking for. Here’s the Eigen::Rotation2D class part.
Eigen::Rotation2D<double> rot2d(ortho2d);
Eigen::Rotation2D is a templated class, so when you declare a variable it has to be with a type. I was initially confused because in Eigen d is double and say for matrices, when you declare a variable, you select types Matrix3f (3x3, float), Matrix2d (2x2, double), etc. In this case, 2D does not change, which represents ‘two dimensional’ and you change the type to float, double, whatever.
This isn’t super well-documented in the front matter of the class, but this is the standard that the angle of the rotation will be in radians and counter clock-wise. I used the matrix constructor by sticking in the Matrix2d ortho2d.
Then, I can quickly use the angle:
out << "angle: " << rot2d.smallestPositiveAngle() << endl;
smallestPositiveAngle() is very handy because it returns the angle within [0, 2pi]. For anyone who has ever converted angles to try to reason about them … this is great. There are other functions too.
I haven’t tried to break it yet and see what happens if I try to construct an Eigen:Rotation2d object with a non orthonormal matrix. The constructor documentation says the argument matrix needs to be a rotation matrix.