Fix invert PSD matrix.
This CL fixes and simplifies the implementation of InvertPSDMatrix.
InvertPSDMatrix uses a JacobiSVD with thin U and V. This requires a
variable number of columns, which is not always the case, which
triggers an assert in debug mode. The type of the matrix is changed
to always use a dynamic number of columns.
In addition, instead of calculating the inverse of the matrix by hand,
the SVD solve method is used.
Change-Id: I353c741cf537b58eb5b18663902318babe1a66de
diff --git a/internal/ceres/invert_psd_matrix.h b/internal/ceres/invert_psd_matrix.h
index 2a61c60..21d301a 100644
--- a/internal/ceres/invert_psd_matrix.h
+++ b/internal/ceres/invert_psd_matrix.h
@@ -67,16 +67,10 @@
MType::Identity(size, size));
}
- Eigen::JacobiSVD<MType> svd(m, Eigen::ComputeThinU | Eigen::ComputeThinV);
- const double tolerance =
- std::numeric_limits<double>::epsilon() * size * svd.singularValues()(0);
-
- return svd.matrixV() *
- (svd.singularValues().array() > tolerance)
- .select(svd.singularValues().array().inverse(), 0)
- .matrix()
- .asDiagonal() *
- svd.matrixU().adjoint();
+ // For a thin SVD the number of columns of the matrix need to be dynamic.
+ using SVDMType = typename EigenTypes<kSize, Eigen::Dynamic>::Matrix;
+ Eigen::JacobiSVD<SVDMType> svd(m, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ return svd.solve(MType::Identity(size, size));
}
} // namespace internal
