ROS - 2D occupancy grid

[senia]

Hi,
Im using rtabmap with R200 in ROS, using the "rgbd_mapping" launch file.
I want to see the 2D occupancy grid, as I enabled in the standalone version by checking "Occupancy from 3D cloud projection on the ground.", Yet I only see the path walked(image attached). I found and run the demo robot mapping example Yet I couldn't understand what makes that launch file show the 2D grid. I didnt manage to make it work with the R200, and copying the "RTAB-Map's parameters" from the example to the rgbd_mapping launch file didn't worked either.
Hope to get a hint and explanation.
Thank you.

[matlabbe]

Hi,

In rtabmapviz, you should be also able to activate the 3D projection map with the demo robot mapping. The camera doesn't see much ground, so there will be obstacles everywhere (as the ceiling is detected as obstacle).  

In rviz, you just have to subscribe to /rtabmap/proj_map, the occupancy map will be automatically generated by rtabmap node.

Note that you may want to update rtabmap and rtabmap_ros. I recently pushed some changes about how the occupancy map is generated from projection (now handling the orientation of the camera, assuming that the camera is at the horizontal at the start).

Also, if you opened the GraphView after mapping, the occupancy maps are not automatically regenerated (only the new data will appear). To generate old maps, you must clear cache and redownload the map.

cheers

[senia]

Hi,
Thanks for the fast replay, I managed to see the projection map yet only by playing the rosbag provided in the demo with the demo launch file. But I want to see the 2D occupancy map while mapping live(with "rgbd_mapping").

Also, maybe there is just a simple way to save the map as a JPG?
I will update rtabmap and rtabmap_ros.
Again, thank you very much for the help and support.

[matlabbe]

In rtabmapviz, you can right-click on the GraphView to save the map:


The resulting map is saved by default on ROS in "~/.ros/ScreensCaptured" folder. Each cell corresponds to 1 pixel, by default it is 5 cm / cell:


I updated the context menu to hide the graph for example, if we just want the map. The example above was done live (with a Kinect):

$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ roslaunch rtabmap_ros rgbd_mapping.launch rtabmap_args:="--delete_db_on_start"

// In rtabmapviz, open Preferences->3D Rendering, then check 
// "Occupancy from 3D cloud projection on the ground." Click ok, 
// then show up the Graph View in Window->Show view... ->Graph view.

cheers

[senia]

Hi,
Mapping with Kinect works GREAT - thank you very much.
Yet I encountered a new problem, mapping using Intel R200.
I'm using the official ROS RealSense drivers, with a resize image package for the depth stream, as the r200 only give 628x468 depth resolution( that need to match the color normal resolution of 640x480 ).
And I noticed that the pointcloud is flipped:

I tried to flip it(90 degrees) inside the resize-image package, yet Rtabmap displaye and problem stayes the same, which destroys my 2D map, as its like I start mapping sideways and not straight ahead as should.
Could you point out where I could change the axis settings for a quick fix?
Other small issue I saw, compiling rtabmap_ros with g2o ROS binaries(ros-indigo-libg2o) causes a crash at launch without any signs or messages(found it only with gdb), so I think built from source is not just faster, but essential.
Thanks.

[matlabbe]

Hi,

You have to add a TF to rotate the camera in ROS coordinate world (x forward, y left, z up) from camera frame (x right, y down, z forward). Example, assuming camera frame is "camera_link":

<arg name="pi/2" value="1.5707963267948966" />
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
        args="$(arg optical_rotate) base_link camera_link 100" /> 

Then if you set frame_id to "base_link" for rgbd_mapping.launch, you will have the point cloud in the right orientation.

Thx for the comment on g2o, I updated the installation instructions.

cheers

[senia]

Hi,
Setting your code(R200 with the above driver, camera frame is by default "r200_frame") at the beginning of the  "rgbd_mapping" launch file, and launching with "frame_id:=base_link" argument fixed it. Don't now how you hit the right transform from the first try. I liked the use of frustum (just maybe make the base a little longer - to sign as the bottom)
Thank you very much, the 2D map is now getting perfect(allmost-perfect, as the R200 has a lot of noise compared to the kinect).
One last question, do you know if this(tea cup) offset (rgb vs depth):

is created by an un-calibrated camera, or there maybe other reason for that? As calibration fails to commit/upload the calibration info to the R200 camera(service "set_camera_info" not found), and I haven't found an option to add external calibration info(.yaml) to rtabmap in ros.
Thanks.

[matlabbe]

Hi,

The transform is the standard rotation from camera optical frame to ROS world frame. If the RGB and depth doesn't match together, that means they are not registered. Looking at realsense_r200_rgbd.launch, did you try launching with "depth_registration:=true"? In the Limitations section, it seems that hardware registered depth image is not provided. You may try this topic "camera/depth_registered/sw_registered/image_rect_raw" instead for the default depth image.

cheers

[senia]

**The drivers I initially used are old and depreceted. Use the one matlabbe recommended.
I thought I would update a solution here when my issue in the Realsense drivers github got resolved. but I don't see a solution in the horizon.

[senia]

Hi again,
only after seeing your post, I understood what the /proj_map represent, and the deference between it and the /grid_map, and why I started to get much noisier (/proj_map) grids.
So I used the depthimage_to_laserscan as you suggested, and changed this line in the launch file:
  <arg name="subscribe_scan"          default="true"/>         
(from false to true) so it will use the added /scan_topic (/scan which is created using depthimage_to_laserscan).
How could I know(using rtabmapviz) which grid map I see in the graph view, and how could I switch between them? Are they both saved?
Thank you very much.

[matlabbe]

Hi,

By default in rtabmapviz, if the nodes contain laser scans, the grid map shown in Graph View is the one built by the laser scans (it would be similar than topic /rtabmap/grid_map).

To show the grid map computed by projection of the point clouds on the ground (like topic /rtabmap/proj_map), open Preferences -> 3D Rendering, then check "Occupancy from 3D cloud projection on the ground. Laser scans are ignored when activated." under Occupancy Grid Map section.

You would have to refresh the cache when switching from one version to the other in order to regenerate the grid map. "Edit-> Clear the cache", then "Edit -> Download all clouds".

For information, the occupancy grid maps are not saved as is in the database, they are always regenerated when the database is opened again.

cheers

[jixujoy]

1.How do you subscribe to the /rtabmap/proj_map and save the grid map as a picture?

2. For rviz, I can see the grid_map displayed there, but how can I save it?

I tried all the ways listed in trying to make the GUI version of rtabmaprviz "create 2D grid map" unlocked, but I still cannot use its functionality. I am trying to write a subscriber to get the grid map. Here are the partial code I wrote. I do not know what package I can use to save the grid_map. What should I write in getGrid() function?

//Here are all the ROS required packages
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>

//standar stream files read and write
#include <iostream>
#include <fstream>
#include <chrono>

void getGrid(const nav_msgs::OccupancyGrid & msg){

}


int main(int argc, char **argv){

    ros::init(argc, argv, "grid_grabber");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/rtabmap/proj_map", 1, getGrid);
   
    ros::spin();
    return 0;
}

[matlabbe]

You can try:

$ rosrun map_server map_saver map:=/rtabmap/proj_map