SLAM with D435i

SLAM with RealSense™ D435i camera on ROS:

The RealSense™ D435i is equipped with build in IMU. Admittedly, the IMU is not the state of the art but still, combined with some powerful open source tools, it’s possible under certain restrictions, to a achieve tasks of mapping and localization.

There are 4 main nodes to the process:

• realsense2_camera
• imu_filter_madgwick
• rtabmap_ros
• robot_localization

Installation:

The first thing to do is to install the components: ##realsense2_camera:## Follow the installation guide in: https://github.com/intel-ros/realsense [Need to fix build – to work with ddynamic_reconfigure means we need to “git clone https://github.com/awesomebytes/ddynamic_reconfigure.git” before catkin_make] ##imu_filter_madgwick:## sudo apt-get install ros-kinetic-imu-filter-madgwick rtabmap_ros: sudo apt-get install ros-kinetic-rtabmap-ros robot_localization: sudo apt-get install ros-kinetic-robot-localization

Running:

Hold the camera steady with a clear view and run the following command:

roslaunch realsense2_camera opensource_tracking.launch

Wait a little for the system to fix itself.

Personalize RViz:

The pointcloud and a bunch of arrows, or axes marks, will appear on screen. These axes represent all the different coordinate systems involved. For clarity you could remove most of them.

From the Displays Panel: #TF -> Frames#, and then leave as marked only #map# and #camera_link#. The first represent the world coordinate system and the second, the camera. You may want to watch the on-line video as well: From the Displays panel: #Image->Image Topic#: rewrite to “/camera/color/image_raw”

[picture: os_tracking_startup_screen.jpg] [picture: os_tracking_display_panel.jpg]

Start moving around and watch the “camera_link” axes mark moving accordingly, in regards to the “map” axes. #Notice:# The built-in IMU can only keep track for a very short time. Moving or turning too quick will break the sequence of successful point cloud matches and will result in the system losing track. It could happen that the system will recover immediately but if not, the longer the time passed since the break, the farther away it will drift from the correct position. The odds for recovery get very slim, very quickly. The parameters set in the launch file are most likely not ideal but this is a good starting point for calibrating.

For saving a rosbag file you may use the following command:

rosbag record -O my_bagfile_1.bag /camera/aligned_depth_to_color/camera_info  camera/aligned_depth_to_color/image_raw /camera/color/camera_info /camera/color/image_raw /camera/imu /camera/imu_info /tf_static

To replay a saved rosbag file:

roscore >/dev/null 2>&1 &
rosparam set use_sim_time true
rosbag play my_bagfile_1.bag --clock
roslaunch realsense2_camera opensource_tracking.launch offline:=true

The process looks like that: [movie: tracking_process.gif]

and the resulting point cloud: [movie: tracking_results.gif]

While the system is up, you can create a 2D map using:

rosrun map_server map_saver map:=/rtabmap/proj_map –f my_map_1

and a resulting map would look something like that: [picture: map.jpg]

You can save the point cloud using:

rosrun pcl_ros pointcloud_to_pcd input:=/rtabmap/cloud_map

#Notice:# The app will keep on saving pointsclouds. Use ctrl-C to stop it after it reports saving the 1st file. The app prints to screen the file names it saves. For example: 1543906154413083.pcd

And watch it later using:

pcl_viewer 1543906154413083.pcd

[Install using: sudo apt-get install pcl-tools]