Localization Bug

[Crazylov3]

Required Info:

• Operating System:
  • 20.04
• Installation type:
  • from source
• ROS Version
  • ROS1
• Version or commit hash:
  • noetic-devel
• Laser unit:
  • Lt-i2

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]

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]

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.