Re: MapOptimizerNode: mapCorrection

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/MapOptimizerNode-mapCorrection-tp1707p1754.html

Hi,

With TF /map -> /odom -> /base_link, we should get:
/map->/base_link = /map->/odom * /odom->/base_link
or
PoseInMapFrame = mapCorrection * PoseInOdomFrame

so

mapCorrection = PoseInMapFrame * inverse(PoseInOdomFrame)

In the code, we compute the error of the last pose in the graph (assuming that graph is optimized from the first pose):

// mapCorrection = PoseInMapFrame * inverse(PoseInOdomFrame)
mapCorrection = optimizedPoses.at(posesOut.rbegin()->first) * posesOut.rbegin()->second.inverse();

cheers