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.