Re: How to improve mapping accuracy based on ArUco identification code?

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/How-to-improve-mapping-accuracy-based-on-ArUco-identification-code-tp8393p8497.html

Hi ChenTengyun,

Currently rtabmap detects the tags and add their position to map based on current odometry. If a tag is seen again, like a loop closure detection, it will add constraints to close the loop and correct the map, while optimizing the position of that tag. What I can understand of what you are trying to do, is that knowing "a priori" exactly the 3D position of those tags, we would want to optimize the map while forcing them to keep their "a priori" position. This could be possible to do by adding a Prior constraint on the pose of the tag (xyz and/or orientation) to a fixed value before optimizing the graph. To do so, rtabmap would have to load a config file telling that tag 1 is at some position, tag 2 at this other position and so on. The prior of the tag could be added just after this line, where jter->first is the id of the tag, then do:

// Get MARKER_ABSOLUTE_POSE from the config file
links.insert(std::make_pair(jter->first, Link(jter->first,jter->first, kPosePrior, MARKER_ABSOLUTE_POSE, cv::Mat::eye(6,6,CV_64FC1)*0.001)));

That prior will be eventually added to g2o optimization here (when g2o is used).

cheers,
Mathieu