From b86fabfc6e4c715975bf1258534300d256b0f3c7 Mon Sep 17 00:00:00 2001 From: YHY Date: Fri, 2 Nov 2018 15:52:54 +0800 Subject: [PATCH] Fixed optimization bug under degeneracy --- src/lib/BasicLaserMapping.cpp | 7 +++++-- src/lib/BasicLaserOdometry.cpp | 5 ++++- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/lib/BasicLaserMapping.cpp b/src/lib/BasicLaserMapping.cpp index 349f0ba8..c0e98d2e 100644 --- a/src/lib/BasicLaserMapping.cpp +++ b/src/lib/BasicLaserMapping.cpp @@ -848,6 +848,9 @@ void BasicLaserMapping::optimizeTransformTobeMapped() matAtB = matAt * matB; matX = matAtA.colPivHouseholderQr().solve(matAtB); + // NOTE: Handling with the degeneracy problem according to the paper + // J. Zhang, M. Kaess and S. Singh, "On degeneracy of optimization-based state estimation problems," + // 2016 IEEE International Conference on Robotics and Automation (ICRA), Stockholm, 2016 if (iterCount == 0) { Eigen::Matrix matE; @@ -877,7 +880,7 @@ void BasicLaserMapping::optimizeTransformTobeMapped() break; } } - matP = matV.inverse() * matV2; + matP = matV2 * matV.inverse(); } if (isDegenerate) @@ -908,4 +911,4 @@ void BasicLaserMapping::optimizeTransformTobeMapped() } -} // end namespace loam \ No newline at end of file +} // end namespace loam diff --git a/src/lib/BasicLaserOdometry.cpp b/src/lib/BasicLaserOdometry.cpp index 1506f1e5..d430b884 100644 --- a/src/lib/BasicLaserOdometry.cpp +++ b/src/lib/BasicLaserOdometry.cpp @@ -558,6 +558,9 @@ void BasicLaserOdometry::process() matX = matAtA.colPivHouseholderQr().solve(matAtB); + // NOTE: Handling with the degeneracy problem according to the paper + // J. Zhang, M. Kaess and S. Singh, "On degeneracy of optimization-based state estimation problems," + // 2016 IEEE International Conference on Robotics and Automation (ICRA), Stockholm, 2016 if (iterCount == 0) { Eigen::Matrix matE; @@ -587,7 +590,7 @@ void BasicLaserOdometry::process() break; } } - matP = matV.inverse() * matV2; + matP = matV2 * matV.inverse(); } if (isDegenerate)