Fixed #1434 (gtsam very huge error estimated on aarch64), also decrea…

…sed log level of voxel filtering to debug.
introlab · Jan 10, 2025 · f8b1d50 · f8b1d50
1 parent 2f431ce
commit f8b1d50
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 13 deletions.
diff --git a/corelib/src/optimizer/OptimizerGTSAM.cpp b/corelib/src/optimizer/OptimizerGTSAM.cpp
@@ -209,15 +209,15 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			UDEBUG("hasGPSPrior=%s", hasGPSPrior?"true":"false");
 			if(isSlam2d())
 			{
-				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:std::numeric_limits<double>::min()));
+				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:1e-9));
 				graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
 				addedPrior.push_back(ConstraintToFactor(rootId, rootId, -1));
 			}
 			else
 			{
 				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
 						(gtsam::Vector(6) <<
-								(hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), (hasGPSPrior?1e-2:std::numeric_limits<double>::min()), // roll, pitch, fixed yaw if there are no priors
+								(hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), (hasGPSPrior?1e-2:1e-9), // roll, pitch, fixed yaw if there are no priors
 								(hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz
 								).finished());
 				graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
@@ -924,23 +924,39 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			// early stop condition
 			UDEBUG("iteration %d error =%f", i+1, error);
 			double errorDelta = lastError - error;
-			if((isam2_ || i>0) && errorDelta < this->epsilon())
+			if(fabs(error) > 1000000000000.0)
 			{
-				if(errorDelta < 0)
+				UERROR("Error computed (%e) is very huge! There could be an issue with too small "
+				       "variance set in some factors (set log level to debug to print the values). Aborting!", error);
+				if(ULogger::level() >= ULogger::kDebug)
 				{
-					UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
+					if(optimizer)
+						graph.printErrors(optimizer->values());
+					else if(isam2_)
+						graph.printErrors(isam2_->calculateEstimate());
 				}
-				else
+				return optimizedPoses;
+			}
+			else
+			{
+				if((isam2_ || i>0) && errorDelta < this->epsilon())
+				{
+					if(errorDelta < 0)
+					{
+						UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
+					}
+					else
+					{
+						UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
+						break;
+					}
+				}
+				else if(i==0 && error < this->epsilon())
 				{
-					UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
+					UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
 					break;
 				}
 			}
-			else if(i==0 && error < this->epsilon())
-			{
-				UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
-				break;
-			}
 			lastError = error;
 		}
 		if(finalError)

diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp
@@ -694,7 +694,7 @@ typename pcl::PointCloud<PointT>::Ptr voxelizeImpl(
 
 		if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
 		{
-			UWARN("Leaf size is too small for the input dataset. Integer indices would overflow. "
+			UDEBUG("Leaf size is too small for the input dataset. Integer indices would overflow. "
 				  "We will split space to be able to voxelize (lvl=%d cloud=%d min=[%f %f %f] max=[%f %f %f] voxel=%f).",
 				  level,
 				  (int)(indices->empty()?cloud->size():indices->size()),