search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
999 stars 558 forks source link

Can the pose estimation at t+1 be calculated from the new result at t time? #591

Open Mingchi-Yeh opened 3 years ago

Mingchi-Yeh commented 3 years ago

Hi , the red line in the figure is my slam result , as shown in the figure , it has a serious drift. 1592234128328

So I want to use the blue line to limit its error range. If the new slam result at t time is (x1,y1,z1), can I use the (x1,y1,z1) to estimate the slam pose (x2,y2,z2) at t+1 time ? I searched the source code of rtabmap_ros but I don't know where to alter it.

matlabbe commented 3 years ago

What the blue line represents? If you use rtabmap's odometry, look inside OdometryROS.cpp.

Mingchi-Yeh commented 3 years ago

The blue line is the other pose with 'geometry_msgs/PoseWithCovarianceStamped'.

Mingchi-Yeh commented 3 years ago

I spent some time to see the OdometryROS.cpp . So when I want to use the blue line to estimate the new odometry, I need to reset odometry_ by my blue line not groundTruth?

matlabbe commented 3 years ago

Note sure to understand what represents the blue line: "The blue line is the other pose". Which other pose? how is it computed?

Mingchi-Yeh commented 3 years ago

The blue line is the pose estimation by ultra wide band, it has [x y z qx qy qz qw]. Can I use [x y z qx qy qz qw] to let slam result don't have too much drift error?

matlabbe commented 3 years ago

Is the blue line in a global coordinate frame (not relative like odometry)? There a some options: 1) use the blue line as input odometry to rtabmap node. 2) use the blue line as guess odometry for odometry node (using guess_frame_id parameter). 3) do odometry fusion between blue and red lines with robot_localization package 4) use global_pose input topic of rtabmap node to feed your blue lines. Set Optimizer/PriorsIgnored to false, then rtabmap will use the blue line to optimize red line based on the blue line (used as priors).