i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed #1434 (gtsam very huge error estimated on aarch64), also decreased log level of voxel filtering to debug.

Browse Source
This commit is contained in:
matlabbe
2025-01-09 16:57:51 -08:00
parent 2f431ce385
commit f8b1d50656
2 changed files with 29 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -209,7 +209,7 @@ 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));
}
@@ -217,7 +217,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
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())
{
UDEBUG("Stop optimizing, not enough improvement (%f < %f)", 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())
{
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)

2
corelib/src/util3d_filtering.cpp
View File

@@ -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()),