introlab / rtabmap

RTAB-Map library and standalone application
https://introlab.github.io/rtabmap
Other
2.77k stars 785 forks source link

How to provide odometry? #5

Closed Tobias-Fischer closed 9 years ago

Tobias-Fischer commented 9 years ago

Hi,

Thanks for your great software package! I use the RGBD C++ version (https://github.com/introlab/rtabmap/blob/master/examples/RGBDMapping/main.cpp) slightly modified. This works great, however now I want to provide the odometry information provided by our robot. Could you please advise how to do that?

I suppose I have to delete the pipe from the camera thread to the odometry thread. Furthermore, I have to somehow update the pose (in my case, it will be 2D, so only x, y and the angle) of the robot provided by the wheels, and pass it to the odometry thread. However, I am stuck on how to do that.

It would be great if you hint me towards the right direction.

Thanks, Tobias

matlabbe commented 9 years ago

You should create your own odometry thread, which will publish OdometryEvent. These odometry events are already catched by RtabmapThread (so no need to use pipes). OdometryEvent requires the pose + RGB-D images. You set the pose with x,y,yaw from your robot and fill the RGB and depth images. For convenience, you could use a class similar to CameraThread to do that.

For example, you could change this line in CameraThread.cpp:

this->post(new CameraEvent(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), ++_seq, UTimer::now()));

to

float x = //... (from your robot's odometry)
float y = //... 
float theta = //...
Transform pose(x,y,0,0,0,theta);
SensorData data(rgb,
          depth,
          fx, fy, cx, cy,
          _cameraRGBD->getLocalTransform(),
          pose,
          1, // rotational variance
          1, // transitional variance
          ++_seq,
         UTimer::now());
this->post(new OdometryEvent(data));
Tobias-Fischer commented 9 years ago

Hi @matlabbe, Thanks for your fast reply! It seems to be straightforward to change it to use the robot's odometry, I'll give it a try in the next couple of days.

Thanks, Tobias

Tobias-Fischer commented 9 years ago

Hi, just a quick update: I am using the ROS version of rtabmap by now, so I don't have this issue anymore. Thanks again for your help :+1:

Best, Tobias