Modify/Shrink Eigen Permutation Matrix - c++

Question

I'm having trouble solving what I think should be a fairly simple problem. The basic problem is I want to modify an Eigen PermutationMatrix but I don't know how.
I'm doing a QR decomposition of some matrix X using the C++ Eigen library. I'm doing this on rank-deficient matrices and I need a particular output. Specifically, I need
R^{-1} * t(R^{-1})
The problem is that using Eigen::ColPivHouseholderQR returns a permuted version of R. This is easy enough to fix when X is full rank, but I'd like the fastest solution for when it is rank-deficient. Let me demonstrate:
using namespace Eigen;
// Do QR
ColPivHouseholderQR<MatrixXd> PQR(X);
// Get permutation matrix
ColPivHouseholderQR<MatrixXd>::PermutationType Pmat(PQR.colsPermutation());
int r(PQR.rank());
int p(X.cols());
// Notice I'm only getting r elements, so R_inv is r by r
MatrixXd R_inv = PQR.matrixQR().topLeftCorner(r, r).triangularView<Upper>().solve(MatrixXd::Identity(r, r));
// This only works if r = p and X is full-rank
R_inv = Pmat * R_inv * Pmat;
XtX_inv = R_inv * R_inv.transpose();
So the basic problem is that I would like to modify Pmat so that it only permutes the r columns of R_inv that I've extracted from PQR.matrixQR(). My basic problem is that I have no idea how to modify work with an Eigen PermutationMatrix, as it doesn't seem to have any of the methods or properties of a normal matrix.
One possible solution is the following: when I multiply Pmat * MatrixXd::Identity(p, p), I get a useful matrix.
For example, I get something like:
[0, 1, 0, 0,
1, 0, 0, 0,
0, 0, 0, 1,
0, 0, 1, 0]
If p = 4 and r = 3, then I would just like this sub-view, where I drop all columns right of the first r columns, and then remove all rows that are all 0:
[0, 1, 0,
1, 0, 0,
0, 0, 1]
So I could do the following:
P = Pmat * MatrixXd::Identity(p, p)
P.leftCols(p);
MatrixXd P = Pmat * Eigen::MatrixXd::Identity(p, p);
// https://stackoverflow.com/questions/41305178/removing-zero-columns-or-rows-using-eigen
// find non-zero columns:
Matrix<bool, 1, Dynamic> non_zeros = P.leftCols(r).cast<bool>().rowwise().any();
// allocate result matrix:
MatrixXd res(non_zeros.count(), r);
// fill result matrix:
Index j=0;
for(Index i=0; i<P.rows(); ++i)
{
if(non_zeros(i))
res.row(j++) = P.row(i).leftCols(r);
}
R_inv = res * R_inv * res;
XtX_inv = R_inv * R_inv.transpose();
but this seems expensive and doesn't take advantage of the fact that Pmat already knows which rows of Pmat should be dropped. I'm guessing there is an easier way to work with Pmat.
Is there any way to easily modify an Eigen PermutationMatrix to only consider columns that weren't placed beyond the first r positions?
Any help or tips would be greatly appreciated.
I've come up with another solution, which probably requires less computation.
// Get all column indices
ArrayXi Pmat_indices = Pmat.indices();
// Get the order for the columns you are keeping
ArrayXi Pmat_keep = Pmat_indices.head(r);
// Get the indices for columns you are discarding
ArrayXi Pmat_toss = Pmat_indices.tail(p - r);
// this code takes the indices you are keeping, and, while preserving order, keeps them in the range [0, r-1]
// For each one, see how many dropped indices are smaller, and subtract that difference
// Ex: p = 4, r = 2
// Pmat_indices = {3, 1, 0, 2}
// Pmat_keep = {3, 1}
// Pmat_toss = {0, 2}
// Now we go through each keeper, count how many in toss are smaller, and then modify accordingly
// 3 - 2 and 1 - 1
// Pmat_keep = {1, 0}
for(Index i=0; i<r; ++i)
{
Pmat_keep(i) = Pmat_keep(i) - (Pmat_toss < Pmat_keep(i)).count();
}
// Now this will order just the first few columns in the right order
PermutationMatrix<Dynamic, Dynamic> P = PermutationWrapper<ArrayXi>(Pmat_keep);
R_inv = P * R_inv * P;