SteveMacenski / slam_toolbox

Slam Toolbox for lifelong mapping and localization in potentially massive maps with ROS
GNU Lesser General Public License v2.1
1.67k stars 525 forks source link

Localization Bug #701

Open Crazylov3 opened 6 months ago

Crazylov3 commented 6 months ago

Required Info:

Steps to reproduce issue

Run localization mode with the provided pose graph and data.

Expected behavior

The built map remains stable over time.

Actual behavior

The built map experiences drift and changes over time. Localization degrades quickly and cannot recover because a new map is being created.

Additional information

I'm not sure if this is intentional or a bug, but here's the issue: After completing the mapping process, I use the map to navigate the robot. I expect the map to remain unchanged over time. However, I notice that in localization mode, the map continues to change, causing the localization performance to degrade quickly.

After reviewing your code, I found the following in the CeresSolver::Compute() function:

// populate contraint for static initial pose
if (!was_constant_set_ && first_node_ != nodes_->end())
{
  ROS_DEBUG("CeresSolver: Setting first node as a constant pose:"
    "%0.2f, %0.2f, %0.2f.", first_node_->second(0),
    first_node_->second(1), first_node_->second(2));
  problem_->SetParameterBlockConstant(&first_node_->second(0));
  problem_->SetParameterBlockConstant(&first_node_->second(1));
  problem_->SetParameterBlockConstant(&first_node_->second(2));
  was_constant_set_ = !was_constant_set_;
}

You are only setting the first node (at the map pose) as constant for the solver. This allows other nodes from the built map to change when this function is called. While this may be beneficial for lifelong mapping, I don't see any advantage for general use cases. Am I missing something?"

Crazylov3 commented 6 months ago

I can create a pull request for this if it's indeed a bug, or at least to add a parameter that allows toggling this behavior on or off.

SteveMacenski commented 6 months ago

This is intentional, its not a pure localization technique. It is intentional that the map adjusts in the local time horizon with new sensor data connected to the previously mapped graph.

pbeeson commented 1 month ago

I think having a parameter to toggle this off would be good. You'd need to ensure you are only setting loaded map nodes as constant, and not any newly added nodes as a result of localization. There are cases where we know the global map doesn't change and one bad edge on a bot that's uptime is weeks or months could change the map is ways we don't want.