3D map is not created when processing rgbd data using rtabmap with icp_odometry
Hi,
I want to process a bag of rgbd data using rtabmap with icp_odometry.
I can extract a 3d map from a bag that contains rgbd data, using rgbd_odometry.
But when using icp_odometry, the map is not generated...
I can see that the icp_odometry process runs
ps aux ros | grep odom
/opt/ros/melodic/lib/rtabmap_ros/icp_odometry --Reg/Strategy 1 scan:=/scan scan_cloud:=/scan_cloud odom:=odom __name:=icp_odometry __log:=/home/avnerm/.ros/log/0c1e8df6-235c-11e9-b42b-74d02b912ce1/rtabmap-icp_odometry-10.log
I'm seeing some runtime warnings when running icp_odometry:
[ WARN] [1548880246.541540256]: IcpOdometry: Transferring value 5 of "Icp/PointToPlaneK" to ros parameter "scan_normal_k" for convenience.
[ WARN] [1548880246.541801876]: IcpOdometry: Transferring value 1.0 of "Icp/PointToPlaneRadius" to ros parameter "scan_normal_radius" for convenience.
[ WARN] (2019-01-30 12:30:46.542) OdometryF2M.cpp:133::OdometryF2M() OdomF2M/BundleAdjustment=1 cannot be used with registration not done only with images (Reg/Strategy=1), disabling bundle adjustment.
The parameters, and output from the icp_odometry are:
[ INFO] [1548881374.690687725]: Initializing nodelet with 8 worker threads.
[ INFO] [1548881374.977402374, 1548455446.224796923]: Odometry: frame_id = base_link
[ INFO] [1548881374.977433059, 1548455446.224796923]: Odometry: odom_frame_id = odom
[ INFO] [1548881374.977449374, 1548455446.224796923]: Odometry: publish_tf = true
[ INFO] [1548881374.977461946, 1548455446.224796923]: Odometry: wait_for_transform = true
[ INFO] [1548881374.977483511, 1548455446.224796923]: Odometry: wait_for_transform_duration = 0.100000
[ INFO] [1548881374.977521110, 1548455446.224796923]: Odometry: initial_pose = xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000
[ INFO] [1548881374.977538825, 1548455446.224796923]: Odometry: ground_truth_frame_id =
[ INFO] [1548881374.977554142, 1548455446.224796923]: Odometry: ground_truth_base_frame_id = base_link
[ INFO] [1548881374.977570620, 1548455446.224796923]: Odometry: config_path =
[ INFO] [1548881374.977585390, 1548455446.224796923]: Odometry: publish_null_when_lost = true
[ INFO] [1548881374.977599455, 1548455446.224796923]: Odometry: guess_frame_id =
[ INFO] [1548881374.977614073, 1548455446.224796923]: Odometry: guess_min_translation = 0.000000
[ INFO] [1548881374.977629616, 1548455446.224796923]: Odometry: guess_min_rotation = 0.000000
[ INFO] (2019-01-30 12:49:35.080) Parameters.cpp:907::parseArguments() Parsed parameter "Reg/Strategy"="1"
[ INFO] [1548881375.080094503, 1548455446.224796923]: Update odometry parameter "Reg/Strategy"="1" from arguments
[ WARN] [1548881375.102105584, 1548455446.224796923]: IcpOdometry: Transferring value 5 of "Icp/PointToPlaneK" to ros parameter "scan_normal_k" for convenience.
[ WARN] [1548881375.102373097, 1548455446.224796923]: IcpOdometry: Transferring value 1.0 of "Icp/PointToPlaneRadius" to ros parameter "scan_normal_radius" for convenience.
[DEBUG] (2019-01-30 12:49:35.102) Odometry.cpp:63::create() type=0
[DEBUG] (2019-01-30 12:49:35.102) OdometryF2M.cpp:77::OdometryF2M()
[DEBUG] (2019-01-30 12:49:35.102) Registration.cpp:48::create() type=1
[ WARN] (2019-01-30 12:49:35.102) OdometryF2M.cpp:133::OdometryF2M() OdomF2M/BundleAdjustment=1 cannot be used with registration not done only with images (Reg/Strategy=1), disabling bundle adjustment.
[ INFO] [1548881375.106376949, 1548455446.224796923]: IcpOdometry: scan_cloud_max_points = 0
[ INFO] [1548881375.106407441, 1548455446.224796923]: IcpOdometry: scan_downsampling_step = 1
[ INFO] [1548881375.106449822, 1548455446.224796923]: IcpOdometry: scan_voxel_size = 0.000000
[ INFO] [1548881375.106461144, 1548455446.224796923]: IcpOdometry: scan_normal_k = 5
[ INFO] [1548881375.106471202, 1548455446.224796923]: IcpOdometry: scan_normal_radius = 1.000000
Here is the launch file:
<launch>
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" />
<!-- Corresponding config files -->
<arg name="rviz_cfg" default="/home/avnerm/avner/PGR/vision/trunk/ros/catkin_ws/src/bpc_slam_rtabmap/launch/rgbdslam_datasets.dataset4.kinect.rviz" />
<param name="/use_sim_time" value="true"/>
<param name="depth_registration" value="true" />
<rosparam command="delete" param="/rtabmap/rgbd_odometry" />
<rosparam command="delete" param="/rtabmap/icp_odometry" />
<rosparam command="delete" param="/rtabmap/Odom" />
<arg name="visual_odometry" default="false" />
<arg name="icp_odometry" default="true" />
<node pkg="rosbag" type="play" name="player" output="screen" args="-\-clock -l -\-pause -r 0.2 /mnt/hdd2/home/avnerm/avner/Slam/rtabmap/data/dataset4/dataset4b.bag"/>
<include file="/home/avnerm/avner/PGR/vision/trunk/ros/catkin_ws/src/bpc_slam_rtabmap/launch/kinect_frames.launch">
</include>
<arg name="pi" value="3.14159265358" />
<arg name="pi/2" value="1.5707963267948966" />
<node pkg="tf" type="static_transform_publisher" name="base_link1" args="0 0 0.2 0 0 0 base_link camera_link 100" />
<node pkg="tf" type="static_transform_publisher" name="base_link2" args="0 0 0.15 0 0 0 base_link base_scan 100" />
<node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 100" />
<node pkg="tf" type="static_transform_publisher" name="world_to_base_link1" args="0.0 0.0 0.0 0.0 0.0 0.0 /world /base_link 100" />
<include file="/opt/ros/melodic/share/rtabmap_ros/launch/rtabmap.launch">
<arg name="rviz" value="$(arg rviz)" />
<arg name="rtabmapviz" value="$(arg rtabmapviz)" />
<arg name="rviz_cfg" value="$(arg rviz_cfg)" />
<arg name="stereo" value="false" />
<arg name="rgb_topic" value="/camera/rgb/image_color" />
<!-- <arg name="depth_topic" value="/camera/depth/image" /> -->
<arg name="camera_info_topic" value="/camera/rgb/camera_info" />
<arg name="icp_odometry" value="$(arg icp_odometry)" />
<arg name="visual_odometry" value="$(arg visual_odometry)" />
<arg name="rtabmap_args" default="--Reg/Strategy 1"/>
</include>
</launch>
What should I do to:
- Get some results from "rostopic echo /rtabmap/odom_info", "rostopic echo /rtabmap/odom"
- View a the map cloud in /rtabmap/cloud_map, or /rtabmap/mapData ?
Thanks, Avner
Asked by Avner on 2019-01-30 16:01:22 UTC
Answers
Hi Avner,
When enabling icp_odometry
with rtabmap.launch, you should set scan_topic
(LaserScan) or scan_cloud_topic
(PointCloud2) argument, otherwise no /rtabmap/odom topic will be published. If you have only a depth image in the bag, convert it to a PointCloud2 (see point_cloud_xyz). You may have some warnings telling that icp_odometry or rtabmap nodes didn't receive any topics since 5 sec. You would have to set subscribe_scan
ou subscribe_scan_cloud
argument to true to add the scans to map and to visualize them.
If the point cloud is coming from a RGB-D camera, I don't really recommend the use of icp_odometry. It is meant to be used with a lidar (2d or 3d). Remember that with ICP, some geometry should always be seen by the camera or it will drift very fast and lose tracking. You may want to add some Icp/****
parameters to rtabmap_args
argument to avoid processing very dense clouds.
<arg name="rtabmap_args" value="--delete_db_on_start --Icp/VoxelSize 0.05" />
To see all Icp/
parameters, do:
$ rtabmap --params | grep Icp/
Asked by matlabbe on 2019-02-01 17:27:01 UTC
Comments