From 646ef41b14e2b5f72e721963fafa490afa3d64fc Mon Sep 17 00:00:00 2001 From: Dimas Abreu Archanjo Dutra Date: Mon, 21 Mar 2022 15:55:56 -0400 Subject: [PATCH] Fixing error in residual scaling. (#474) --- solvers/ceres_solver.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/solvers/ceres_solver.cpp b/solvers/ceres_solver.cpp index 0e49352ee..c253068b3 100644 --- a/solvers/ceres_solver.cpp +++ b/solvers/ceres_solver.cpp @@ -310,13 +310,14 @@ void CeresSolver::AddConstraint(karto::Edge * pEdge) Eigen::Vector3d pose2d(diff.GetX(), diff.GetY(), diff.GetHeading()); karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse(); - Eigen::Matrix3d sqrt_information; - sqrt_information(0, 0) = precisionMatrix(0, 0); - sqrt_information(0, 1) = sqrt_information(1, 0) = precisionMatrix(0, 1); - sqrt_information(0, 2) = sqrt_information(2, 0) = precisionMatrix(0, 2); - sqrt_information(1, 1) = precisionMatrix(1, 1); - sqrt_information(1, 2) = sqrt_information(2, 1) = precisionMatrix(1, 2); - sqrt_information(2, 2) = precisionMatrix(2, 2); + Eigen::Matrix3d information; + information(0, 0) = precisionMatrix(0, 0); + information(0, 1) = information(1, 0) = precisionMatrix(0, 1); + information(0, 2) = information(2, 0) = precisionMatrix(0, 2); + information(1, 1) = precisionMatrix(1, 1); + information(1, 2) = information(2, 1) = precisionMatrix(1, 2); + information(2, 2) = precisionMatrix(2, 2); + Eigen::Matrix3d sqrt_information = information.llt().matrixU(); // populate residual and parameterization for heading normalization ceres::CostFunction * cost_function = PoseGraph2dErrorTerm::Create(pose2d(0),