How to improve mapping accuracy based on ArUco identification code?

classic Classic list List threaded Threaded
6 messages Options
Reply | Threaded
Open this post in threaded view
|

How to improve mapping accuracy based on ArUco identification code?

ChenTengyun
Hi Mathieu,

    From the GitHub Wiki and rtabmap forum, I have seen a lot of your professional replies to rtabmap + marker questions. I judge you are an expert in this field and am glad to communicate with you. At present, I also have problems with rtabmap + marker. I hope to get your full support.

【Objectives I need to achieve and their basic ideas】:
1. Each mapping starts from the same place identified by an ArUco to ensure that the reference coordinate system is the same for each mapping;
2. ArUco identification codes with known 3D coordinates are deployed around the survey area in advance. Its reference coordinate system is the starting point of each mapping, and the IDs and coordinates of all ArUco identification codes in the area are configured to the rtabmap system in advance;
3. During each mapping, when the camera moves and detects the pre deployed ArUco identification code, the known coordinate value of the ArUco identification code is used to correct the error of the visual odometer in time;
4.Finally, the mapping accuracy of the whole area is improved.

test scenario plan sketch map

【help context】:
1. Based on the existing rtabmap code on GitHub, the ArUco identification code is added to help improve the mapping accuracy. For this goal, a specific code modification scheme is proposed. I hope to hear your suggestions. Or provide some reference documents or reference codes.
2. Based on Ubuntu 18.04 + ROS melodic, modify the rtabmap code, compile the source code, replace the corresponding module of rtabmap in the system, run it and debug it. I hope you can provide some reference opinions and materials for this work.

cheers,
ChenTengyun
Reply | Threaded
Open this post in threaded view
|

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

matlabbe
Administrator
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
Reply | Threaded
Open this post in threaded view
|

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

javad0111
Hi Mathieu,

I am working on the same problem as ChenTengyun. But I want to try the case when one Tag with special id would be the origin of Map frame (or maybe map frame), that's why I added an if-condition to have prior constraint for only one tag. Changes are added here as following:

if(!uContains(poses, jter->first))
{
        poses.insert(std::make_pair(jter->first, poses.at(*iter) * jter->second.transform()));
}
links.insert(std::make_pair(jter->first, jter->second.inverse()));
Link::Type kPosePrior;
if (jter->first== 104){
        Transform MARKER_ABSOLUTE_POSE=Transform(0, 0, 0, 0, 0, 0, 1);
        std::cout<<"check inside the code of uContains ******************"<<std::endl; //for checking if this line is read.
        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)));
}


But the problem is I see no changes with this modification! I would appreciate it if you could help me out, thanks in advance.

Kind regards,
Javad


Reply | Threaded
Open this post in threaded view
|

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

matlabbe
Administrator
Hi Javad,

In your code, the landmark should have negative ids.

Note that a marker prior parameter has been added recently to do so.
rtabmap --params | grep Marker/Priors
Param: Marker/Priors = ""                                  [World prior locations of the markers. The map will be transformed in marker's
                                                            world frame when a tag is detected. Format is the marker's ID followed by its position 
                                                            (angles in rad), markers are separated by vertical line ("id1 x y z roll pitch yaw|id2 x y z
                                                             roll pitch yaw"). Example:  "1 0 0 1 0 0 0|2 1 0 1 0 0 1.57" (marker 2 is 1 meter forward
                                                             than marker 1 with 90 deg yaw rotation).]
Param: Marker/PriorsVarianceAngular = "0.001"              [Angular variance to set on marker priors.]
Param: Marker/PriorsVarianceLinear = "0.001"               [Linear variance to set on marker priors.]

In your case, setting this:
Marker/Priors="104 10 0 0 0 0 0"
will make the map translate so that landmark 104 is on absolute pose 10 meters in x.
Reply | Threaded
Open this post in threaded view
|

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

javad0111
Thanks for the reply.

1) I see that Marker/Prior parameter was recently added. Does it mean that I do not need to change the source code? So removing the part that I mentioned in my previous message.

2) I did the steps below, but I still see no changes or  translation from map frame: here is the picture of built map and frames.



     i. launching this launch file: aruco_multiple.launch
     ii. launching this launch file: rtabmao_l515_bag.launch
     iii. playing the bag file that publishes depth image , rbd image, camera info, and tf_static topics. There are three tags in the images with id of 104, 100, and 101. Link to the bag file: l515_with_aruco_42_seconds.bag
     iv. rosparam set /rtabmap/rtabmap/Marker/Priors "104 0 0 0 0 0 0"

Thanks again and kind regards,
Javad
Reply | Threaded
Open this post in threaded view
|

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

matlabbe
Administrator
1) Yes

2) Forgot to mention that you need to set Optimizer/PriorsIgnored to false too, while also calling update_parameters service so that rtabmap actually read again the modified rosparam:

roslaunch rtabmap_l515_bag.launch use_sim_time:=true
roslaunch aruco_multiple.launch
rosparam set /rtabmap/rtabmap/Marker/Priors "104 0 0 0 0 0 0" 
rosparam set /rtabmap/rtabmap/Optimizer/PriorsIgnored false
rosservice call /rtabmap/update_parameters 
rosbag play --clock --pause l515_with_aruco_42_seconds.bag

cheers,
Mathieu