// icp_odometry with guess from odom_combined frame $ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Reg/Force3DoF true --Reg/Strategy 1 --RGBD/ProximityPathMaxNeighbors 10 --RGBD/ProximityPathFilteringRadius 1 --RGBD/ProximityByTime false --Mem/STMSize 30 --Mem/LaserScanVoxelSize 0.05 --Icp/VoxelSize 0.0 --Mem/LaserScanNormalK 5 --Mem/LaserScanNormalRadius 1 --Icp/Epsilon 0.001 --Icp/MaxTranslation 0.5 --RGBD/OptimizeMaxError 1 --Icp/PointToPlane true --Icp/CorrespondenceRatio 0.10 --Icp/PMOutlierRatio 0.95 --Mem/BinDataKept true --Grid/RangeMax 0 --Kp/DetectorStrategy 0 --Kp/MaxFeatures 200 --SURF/HessianThreshold 100 --Vis/MaxFeatures 500 --Mem/UseOdomFeatures false" odom_args:="--Icp/VoxelSize 0.05 --Icp/PointToPlaneRadius 1 --Odom/GuessMotion true --Odom/Strategy 0" rgbd_sync:=true subscribe_scan:=true frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true visual_odometry:=false odom_guess_frame_id:=odom_combined odom_guess_min_translation:=0.1 odom_guess_min_rotation:=0.1 odom_topic:=odom icp_odometry:=true database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db // proximity space short range scan_topic:=/base_scan_t_filtered // proximity space longer range scan_topic:=/base_scan_t // RGB-D back-end rgb_topic:=/camera/rgb/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/rgb/camera_info // Stereo back-end stereo:=true stereo_namespace:=/wide_stereo left_camera_info_topic:=/wide_stereo/left/camera_info right_camera_info_topic:=/wide_stereo/right/camera_info_scaled approx_rgbd_sync:=false approx_sync:=true // odometry odom_frame_id:="odom_combined" odom_tf_angular_variance:=0.0001 odom_tf_linear_variance:=0.0001 icp_odometry:=false // disable loop closure detection --Kp/MaxFeatures -1 //neighbor link refine --RGBD/NeighborLinkRefining true --RGBD/OptimizeMaxError 1.5 // odom frame to frame --Odom/Strategy 1 $ rosparam load scan_filter.yaml scan_to_scan_filter_chain $ rosrun laser_filters scan_to_scan_filter_chain scan:=/base_scan_t scan_filtered:=/base_scan_t_filtered $ ./republish_scan.py _offset:=82.2 //stereo $ ./republish_camera_info.py camera_info_in:=/wide_stereo/right/camera_info camera_info_out:=/wide_stereo/right/camera_info_scaled $ export ROS_NAMESPACE=wide_stereo $ rosrun stereo_image_proc stereo_image_proc left/image_raw:=left/image_raw right/image_raw:=right/image_raw left/camera_info:=left/camera_info right/camera_info:=right/camera_info_scaled $ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-14-25_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275 // or // To make ground truth matching dataset 2012-01-25-12-14-25.bag (transform is found by exporting scans according to ground truth for each sequence, then use pcl_icp) // xyz=0.006236,-0.351500,0.000000 rpy=0.000000,0.000000,-0.017832 // old -0.00273873 -0.38818 0 0 0 0 $ rosrun tf static_transform_publisher 0.006236 -0.351500 0 -0.017832 0 0 world world_offset 100 $ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world_offset _offset_time:=82.2 _offset_x:=-0.275 // otherwise $ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275 // rtabmap $ ./sync_path_gt.py _frame_id:=scan_gt mapPath:=/rtabmap/mapPath $ rostopic hz /rtabmap/grid_map $ rosbag play --clock --pause ./stata-mit/2012-01-25-12-14-25.bag // or $ rosbag play --clock --pause ./stata-mit/2012-01-25-12-33-29.bag //copy results (assuming we are in bags directory) $ cp rtabmap.db gt_poses.txt rmse.txt slam_poses.txt ../results/stata_mit/2012-01-25-12-14-25/rtabmap //or $ cp rtabmap.db gt_poses.txt rmse.txt slam_poses.txt ../results/stata_mit/2012-01-25-12-33-29/rtabmap /// OTHER SCAN LIDAR SLAM $ roscore $ rosparam set use_sim_time true $ ./republish_scan.py _offset:=82.2 //2012-01-25-12-14-25 $ rosbag play --clock --pause ./stata-mit/2012-01-25-12-14-25.bag ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-14-25_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275 //2012-01-25-12-33-29 $ rosbag play --clock --pause ./stata-mit/2012-01-25-12-33-29.bag $ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275 // for short-lidar $ rosparam load scan_filter.yaml scan_to_scan_filter_chain $ rosrun laser_filters scan_to_scan_filter_chain scan:=/base_scan_t scan_filtered:=/base_scan_t_filtered // for fake lidar kinect $ rosrun depthimage_to_laserscan depthimage_to_laserscan image:=/camera/depth/image_raw camera_info:=/camera/rgb/camera_info scan:=/camera_scan _output_frame_id:=openni_rgb_frame _range_max:=6 // gmapping $ rosrun gmapping slam_gmapping scan:=base_scan_t _odom_frame:=odom_combined _particles:=100 _base_frame:=base_footprint $ ./sync_path_gt.py _frame_id:=scan_gt // hector_slam $ rosrun hector_mapping hector_mapping _pub_map_odom_transform:=true _map_frame:=map _base_frame:=base_footprint _odom_frame:=odom_combined scan:=/base_scan_t _map_size:=4096 $ ./sync_path_gt.py _frame_id:=scan_gt mapPath:=trajectory $ roslaunch hector_geotiff geotiff_mapper.launch trajectory_source_frame_name:=base_footprint // etchzasl_icp_mapper (in Mapper::gotScan(), change odomFrame to scanMsgIn.header.frame_id... OR in libpointmatcher_ros/pointcloud.cpp, change in rosMsgToPointMatcherCloud() the target_frame of transformPoint() to point's frame_id) // Remove _offset_x:=-0.275 from gt_tf_broadcaster.py above $ rosrun ethzasl_icp_mapper mapper scan:=/base_scan_t _odom_frame:=/odom_combined _map_frame:=map _subscribe_scan:=true _subscribe_cloud:=false _minMapPointCount:=1000 _minReadingPointCount:=150 _icpConfig:="/home/mathieu/catkin_ws/src/ethzasl_icp_mapping/ethzasl_icp_mapper/launch/2D_scans/icp.yaml" _inputFiltersConfig:="/home/mathieu/catkin_ws/src/ethzasl_icp_mapping/ethzasl_icp_mapper/launch/2D_scans/input_filters.yaml" _mapPostFiltersConfig:="/home/mathieu/catkin_ws/src/ethzasl_icp_mapping/ethzasl_icp_mapper/launch/2D_scans/map_post_filters.yaml" _minOverlap:=0.5 $ rosrun ethzasl_icp_mapper occupancy_grid_builder scan:=/base_scan_t $ ./ethzasl_icp_mapper_sync_gt.py _frame_id:=scan_gt // cartographer (markers time may be slightly off as they are not updated at the sime time than scan) $ ./pose_to_odom.py $ roslaunch cartographer.launch bag_filename:=/home/mathieu/bags/2012-01-25-12-33-29.bag $ ./cartographer_sync_markers_gt.py _frame_id:=scan_gt // karto (don't use graph as markers, but GetAllProcessedScans with GetCorrectedPose for each scan pose, then set time of marker to last scan) $ rosrun slam_karto slam_karto _base_frame:=base_footprint _odom_frame:=odom_combined scan:=/base_scan_t _map_update_interval:=1 $ ./slam_karto_sync_markers_gt.py _frame_id:=scan_gt mapPath:=/rtabmap/mapPath // mrpt_graphslam_2d (add scan_topic and odom_topic arguments to launch, don't play with odom_ombined tf) $ roslaunch mrpt_graphslam_2d graphslam.launch scan_topic:=base_scan_t odom_topic:=odom_combined base_link_frame_ID:=base_footprint odometry_frame_ID:=/odom_combined start_rviz:=true $ ./pose_to_odom.py $ ./sync_path_gt.py _frame_id:=scan_gt mapPath:=/feedback/robot_trajectory