Extracting submatrices in Eigen (C++).
23 Jun 2021Whenever I needed to copy a submatrix in Eigen (the matrix library in C++), I would just walk through it with two for loops like it was 1998. BUT recent experience with the docs and other languages led me to believe there was a submatrix function in Eigen somewhere, I just wasn’t using the right search terms or looking in the right place.
And, this time I found it! The function is a Matrix** class member block() from
Faisal Khan’s post on Eigen, scroll down to ‘Block Operations’ has the information about the block operator, but a lot of good information about Eigen too.
Here’s the Eigen Docs: Block operations where I noticed this:
As usual with Eigen expressions, this abstraction has zero runtime cost provided that you let your compiler optimize.
Here’s a snippet that shows the block operator with a sequence of steps I do frequently: have a 4 x 4 projection matrix and want to extract the rotation matrix and translation vector.
Matrix4d ProjectionMat;
Matrix3d RotationMat;
MatrixXd translationVec;
...
RotationMat = ProjectionMat.block<3, 3>(0, 0);
translationVec = ProjectionMat.block<3, 1>(0, 3);
What about vectors? YES WHAT ABOUT THEM? The Eigen Docs: Block operations includes these, but down the page.
Interestingly, there are some special head, tail, segment functions that I will be using … a lot.
Example: create a vector (planeNormal) that is the first three elements of the 4-element planeEq vector. What I frequently do is normalize the planeEq vector such that its normal vector is a unit vector, so that I can compute the distance of points to the plane using a fast dot product. (More info, equation 9 from Mathworld, Point-Plane distance.)
Vector4d planeEq = svd.matrixV().col(3);
Vector3d planeNormal = planeEq.head<3>();
planeEq = planeEq/planeNormal.norm();
Update: there is also a dynamic version of the block function, described here.