rtabmap yaw value keeps incrementing

asked 2022-02-11 11:09:53 -0600

goldgunner gravatar image

updated 2022-02-15 18:09:17 -0600

matlabbe gravatar image


I'm trying to implement SLAM on a drone so i can fly it autonomously, the camera i'm using is RS D415, Slam algorithm is Rtabmap, i'm feeding to it IMU values from the drone's flight controller, and then i send the x,y,z and yaw to the flight controller

when i launch rtabmap launch files, the mapping is ok, but when ever i move the camera, the new frames are not connected with each others, and the map is duplicated multiple times, when i checked the values sent to the Flight controller, the linear accel and the angular velocity seem to be correct, however, the orientation is incorrect, when i visualize the received values on the Flight controller (pixhawk 4) x y and z are correct but the yaw value keeps incrementing from 0 to PI all the time, which. does anyone have a clue why im i having this issue?

image description

Thank you in advance

edit retag flag offensive close merge delete


Are you able to show the problem in rviz? That could be an ENU vs NED issue, or TF not correctly set. If you don't feed imu to rtabmap, is yaw still drifting?

matlabbe gravatar image matlabbe  ( 2022-02-14 12:32:27 -0600 )edit

Thank you for your reply, I do have a screenshot of the problem on rviz, here's the link: https://drive.google.com/file/d/1ugG2...

if i do not feed imu values, the same problem occures. sometimes, even if i do not move the camera, the moment i launch rviz, i get the circle drift representation of the envrironment as shown on the screenshot.

at a given moment i tried feeding compass values to rtab, and results were slightly better, the camera could localize it self when tilted to the left or right, but when tilted up or down, it loses itself, "i think this is related to compass up and down headings were reversed, because when i tilted it down, floor was displayed above the created scene and not under", however, compass is not a solution for me unfortunatly.

I don't know much about TF not correctly set.

goldgunner gravatar image goldgunner  ( 2022-02-14 13:09:01 -0600 )edit

Can you run rosrun tf2_tools view_frames.py and share the TF tree? It looks like you have multiple TF for odom and one is drifting.

matlabbe gravatar image matlabbe  ( 2022-02-15 18:11:34 -0600 )edit

Here's a link to my drive containing TF tree, TF on rviz, launch files used, python script for sending data to FC, and a video of the problem on rviz. https://drive.google.com/drive/folder...

goldgunner gravatar image goldgunner  ( 2022-02-16 03:45:26 -0600 )edit

The drifting looks coming from ekf_localization. Something in there (maybe imu) makes it drifting. It is why I asked if you remove IMU (so now ekf_localization) and use only rgbd_odometry from rtabmap to get odometry if it will still drift. Try debugging every parts separately.

matlabbe gravatar image matlabbe  ( 2022-02-16 07:45:57 -0600 )edit

You were right, the drift was caused by ekf_localization, in fact, i was fusing yaw from two different sources. The yaw from IMU. and yaw from rgbd_odometry, and this does not work in my case because rtablmap yaw value will likely start at 0 while imu value starts at a the current angle from north. i tried to turn off the imu by cutting power from it, but it did not help, "maybe there's always values fed in even if imu is not detected" so i did what you said, i changed the param of odom0_config to true for all the values, and imu0_config to false, so it only reads values from odom, and it worked, and then i played with params by enabling orientation coming from odom0, and enabling x,y,z, vx, vy, vz coming from imu0, it worked fine also on rviz. I'm really thankful ...(more)

goldgunner gravatar image goldgunner  ( 2022-02-17 10:54:12 -0600 )edit

Now i'm facing another issue, i don't know whether i open another ticket for it or not, the thing is, on rviz, everything works fine, heading is correct, and when i move the camera back and fourth it followes the exact path, however, when i set ekf as current home position on mission planner and the drone icon apears on the map, the heading is great, but when i move the camera "and the imu" the drone's path is not consistent with real movements, and after few seconds, it drifts away and disapears completely from the map. I think that values received on mavlink are not the exact as the ones from ekf, or maybe there's something wrong with NED conversion on mavros?

goldgunner gravatar image goldgunner  ( 2022-02-17 11:06:23 -0600 )edit