Skip to content

Commit

Permalink
revert to inverse + llt.
Browse files Browse the repository at this point in the history
  • Loading branch information
B1ueber2y committed Jun 19, 2024
1 parent 0c9837d commit d120066
Showing 1 changed file with 1 addition and 11 deletions.
12 changes: 1 addition & 11 deletions _pyceres/factors/bindings.h
Original file line number Diff line number Diff line change
Expand Up @@ -10,17 +10,7 @@
namespace py = pybind11;

inline Eigen::MatrixXd SqrtInformation(const Eigen::MatrixXd& covariance) {
// LLT decomposition of Fisher information matrix with SVD
Eigen::JacobiSVD<Eigen::MatrixXd> svd(
covariance, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::MatrixXd U = svd.matrixU();
Eigen::VectorXd singularValues = svd.singularValues();
const double epsilon = 1e-12;
Eigen::VectorXd invSqrtSingularValues =
singularValues.array().max(0.).sqrt().max(epsilon).inverse();
Eigen::MatrixXd invSqrtSingularValuesDiag =
invSqrtSingularValues.asDiagonal();
return U * invSqrtSingularValuesDiag;
return covariance.inverse().llt().matrixL();
}

// Mahalanobis squared distance between two parameters.
Expand Down

0 comments on commit d120066

Please sign in to comment.