The problem is that if optimizer doesn't optimize correctly, the actual correct loop closure transforms would be updated wrongly. The optimizer should always uses the original constraints for odom cache (like local optimized graph).
Remove:
|
Transform newT = newOptPoseInv * optPoses.at(iter->first); |
|
UDEBUG("Adjusted localization link %d->%d after optimization", iter->second.from(), iter->second.to()); |
|
UDEBUG("from %s", iter->second.transform().prettyPrint().c_str()); |
|
UDEBUG(" to %s", newT.prettyPrint().c_str()); |
|
iter->second.setTransform(newT); |
|
|
|
// Update link in the referred signatures |
|
if(iter->first > 0) |
|
_memory->updateLink(iter->second, false); |
The problem is that if optimizer doesn't optimize correctly, the actual correct loop closure transforms would be updated wrongly. The optimizer should always uses the original constraints for odom cache (like local optimized graph).
Remove:
rtabmap/corelib/src/Rtabmap.cpp
Lines 3398 to 3406 in 376c823