2D mapping with lidar doesn't generate occupied cell
Locked
2D mapping with lidar doesn't generate occupied cell
 
|
This post was updated on .
Hi ! First of all thank you Mathieu for all the impressive work you have done. I would like to know if you can help us on this particular problem. We are currently working on a project which aims to implement autonomous and semi-autonomous navigation on electronic wheelchairs. So far we were only using a Zed2 camera mounted above the wheelchair in order to run the rtabmap SLAM algorithm. The map was generated correctly and loop closures were fine. However, we recently added a rplidar A2 in order to improve the mapping and localization performances. When we are performing mapping with the lidar, the map is generated. The problem comes from the fact that only the empty cells are generated and not the occupied ones as you can see on the image below. 
In order to enable lidar mapping we set the "FromDepth" parameter to false. We tried to tune many parameters to find out where the problem could come from. We noticed that when changing the value of "OccupancyThr" to 0,40 the map becomes fully occupied and when we input 0,41 we come back to a full empty map. Here is the launch file we are using and the yaml file that holds most of our rtabmap parameters : <?xml version="1.0"?> <launch> <arg name="namespace" default="educat"/> <!-- Localization-only mode --> <arg name="localization" default="false"/> <arg if="$(arg localization)" name="args" default=""/> <arg unless="$(arg localization)" name="args" default="--delete_db_on_start"/> <arg name="output_method" default="screen"/> <!-- "log" or "screen" --> <arg name="real_scan" default="true"/> <!-- True if a lidar is publishing to /scan topic --> <arg name="svo_file" default="" /> <!-- <arg name="svo_file" default="path/to/svo/file.svo"> --> <arg name="stream" default="" /> <!-- <arg name="stream" default="<ip_address>:<port>"> --> <arg name="zed_node_name" default="zed_nodelet" /> <arg name="camera_model" default="zed2" /> <arg name="camera_name" default="zed" /> <arg name="publish_urdf" default="true" /> <arg name="base_frame" default="base_footprint" /> <!-- Name of the Nodelet Manager --> <arg name="nodelet_manager_name" default="educat_nodelet_manager" /> <group ns="$(arg namespace)"> <!-- Nodelet Manager --> <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager" output="$(arg output_method)" /> <!-- ################################################ Description ################################################ --> <!-- ROS URDF description --> <group if="$(arg publish_urdf)"> <param name="educat_description" command="$(find xacro)/xacro '$(find educat_pwc_bringup)/urdf/educat_pwc_model.urdf'"/> <node name="educat_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="$(arg output_method)"> <remap from="robot_description" to="educat_description" /> </node> </group> <!-- ################################################ LIDAR/scan ################################################ --> <!-- Fake scan if we don't use a lidar --> <node unless="$(arg real_scan)" pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan"> <remap from="image" to="$(arg zed_node_name)/depth_registered/image_raw"/> <remap from="camera_info" to="$(arg zed_node_name)/depth_registered/camera_info"/> <remap from="scan" to="/scan"/> <param name="range_max" type="double" value="4"/> </node> <!-- RPLIDAR node --> <group if="$(arg real_scan)"> <node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="$(arg output_method)"> <param name="serial_port" type="string" value="/dev/ttyUSB0"/> <param name="serial_baudrate" type="int" value="115200"/><!--A1/A2 --> <param name="frame_id" type="string" value="rplidar_center_link"/> <param name="inverted" type="bool" value="false"/> <param name="angle_compensate" type="bool" value="true"/> </node> <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="laser_filter"> <rosparam command="load" file="$(find educat_pwc_bringup)/config/laser_config.yaml" /> </node> </group> <!-- ################################################ CAMERA ################################################ --> <!-- http://wiki.ros.org/zed-ros-wrapper#Node_Parameters --> <node pkg="nodelet" type="nodelet" name="$(arg zed_node_name)" args="load zed_nodelets/ZEDWrapperNodelet $(arg nodelet_manager_name)" required="true" output="$(arg output_method)"> <rosparam file="$(find educat_pwc_bringup)/params/zed_param.yaml" command="load" /> <param name="svo_file" value="$(arg svo_file)" /> <param name="stream" value="$(arg stream)" /> <param name="publish_urdf" value="$(arg publish_urdf)" /> </node> <!-- ################################################ Graph-based SLAM ################################################ --> <!-- RGB-D related topics --> <arg name="rgb_topic" default="$(arg zed_node_name)/rgb/image_rect_color" /> <arg name="depth_topic" default="$(arg zed_node_name)/depth/depth_registered" /> <arg name="camera_info_topic" default="$(arg zed_node_name)/rgb/camera_info" /> <arg name="depth_camera_info_topic" default="$(arg zed_node_name)/depth/camera_info" /> <arg name="scan_topic" default="scan_filtered" /> <arg name="odom_topic" default="$(arg zed_node_name)/odom" /> <arg name="imu_topic" default="$(arg zed_node_name)/imu/data"/> <!-- Synchronize topics coming from different sensors before feeding them to rtabmap --> <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync $(arg nodelet_manager_name)" required="true" output="$(arg output_method)"> <remap from="rgb/image" to="$(arg rgb_topic)"/> <remap from="depth/image" to="$(arg depth_topic)"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/> <remap from="rgbd_image" to="rgbd_image"/> <!-- output --> <!-- Should be true for not synchronized camera topics (e.g., false for kinectv2, zed, realsense, true for xtion, kinect360)--> <param name="approx_sync" value="false"/> </node> <!-- RTABmap --> <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output_method)" required="true" args="$(arg args)" launch-prefix=""> <rosparam command="load" file="$(find educat_pwc_bringup)/params/rtabmap_param.yaml" /> <param name="subscribe_scan" value="$(arg real_scan)"/> <remap from="rgb/image" to="$(arg rgb_topic)"/> <remap from="depth/image" to="$(arg depth_topic)"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/> <remap from="scan" to="$(arg scan_topic)" /> <remap from="grid_map" to="map" /> <remap from="odom" to="$(arg odom_topic)"/> <remap from="imu" to="$(arg imu_topic)"/> <remap from="rgbd_image" to="rgbd_image"/> <!-- localization mode --> <param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/> <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/> <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/> </node> </group> </launch> subscribe_depth: false
subscribe_rgbd: true
subscribe_rgb: false
subscribe_stereo: false
subscribe_scan_cloud: false
subscribe_user_data: false
subscribe_odom_info: false
visual_odometry: false
frame_id: "base_footprint"
map_frame_id: "map"
odom_frame_id: "odom"
database_path: "~/.ros/rtabmap.db"
config_path: "~/.ros/rtabmap.cfg"
odom_tf_angular_variance: 0.001 # If TF is used to get odometry, this is the default angular variance
odom_tf_linear_variance: 0.001 # If TF is used to get odometry, this is the default linear variance
tf_delay: 0.02
publish_tf: true # Set to false if fusing different poses (map->odom) with UKF. True here because we set the map publishing from the zed cam to false
odom_sensor_sync: false
wait_for_transform_duration: 1
queue_size: 10
approx_sync: true
#If set to true, constraint rtabmap to only use 3 DoF loop closure and graph optimization (increase robustness of the map)
Reg:
Force3DoF: true
Strategy: 1 #1=ICP (Iterative Closest Point)
Optimizer:
Strategy: 1 #Graph optimization strategy 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.
Grid:
3D: false
MapFrameProjection: false
GroundIsObstacle: false
PreVoxelFiltering: true
RayTracing: true
FromDepth: false #False if you want mapping from 2D scan
CellSize: 0.05
DepthDecimation: 1
DepthRoiRatios: [0.0, 0.0, 0.0, 0.0]
FootprintHeight: 2.0
FootprintLength: 1.5
FootprintWidth: 0.8
MinGroundHeight: -0.5
MaxGroundHeight: 0.1
MaxObstacleHeight: 1.5
NoiseFilteringMinNeighbors: 5
NoiseFilteringRadius: 0
RangeMin: 0.1
RangeMax: 12.0 #12m for rplidar A2
Scan2dUnknownSpaceFilled: true
ScanDecimation: 1
NormalsSegmentation: true
ClusterRadius: 0.1
MaxGroundAngle: 45.0
FlatObstacleDetected: true
MinClusterSize: 10
NormalK: 20
GridGlobal:
Eroded: false # Erode obstacle cells
FootprintRadius: 0.5 # Footprint radius (m) used to clear all obstacles under the graph
FullUpdate: true # When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not
MaxNodes: 0 # Maximum nodes assembled in the map starting from the last node (0=unlimited)
MinSize: 1.0 # Minimum map size (m)
OccupancyThr: 0.70 # Occupancy threshold (value between 0 and 1). 0.70 if mapping with camera
ProbClampingMax: 0.971 # Probability clamping maximum (value between 0 and 1)
ProbClampingMin: 0.1192 # Probability clamping minimum (value between 0 and 1)
ProbHit: 0.7 # Probability of a hit (value between 0.5 and 1)
ProbMiss: 0.4 # Probability of a miss (value between 0 and 0.5)
UpdateError: 0.01 # Graph changed detection error (m). Update map only if poses in new optimized graph have moved more than this value
RGBD:
Enabled: true
CreateOccupancyGrid: true
ProximityBySpace: true #Find local loop closures based on the robot position in the map. It is useful when the robot, for example, is coming back in the opposite direction.
AngularUpdate: 0.01
LinearUpdate: 0.01
OptimizeFromGraphEnd: false
LoopClosureReextractFeatures: false
OptimizeMaxError: 3.0
SavedLocalizationIgnored: false
NeighborLinkRefining: true #Correct odometry using the input lidar topic with ICP.
ProximityPathMaxNeighbors: 10 #Do also proximity detection by space by merging close scans together
LocalRadius: 5
Icp:
VoxelSize: 0.05
MaxCorrespondenceDistance: 0.1
CorrespondenceRatio: 0.4
Kp:
DetectorStrategy: 0 #default=8 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch 12=SURF/FREAK
ByteToFloat: true #False to avoid conversion from bytes to float. We keep bits to float => slower but more accurate. True with HOG, false with binary
RoiRatios: [0.0, 0.0, 0.0, 0.3] #Region of interest ratios [left, right, top, bottom], filter 30% of the bottom to avoid processing ground
SURF:
GpuVersion: true #GPU-SURF Use GPU version of SURF. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.
Rtabmap:
LoopThr: 0.06 #Default = 0.11
DetectionRate: 1 #Detection rate (Hz). RTAB-Map will filter input images to satisfy this rate.
MemoryThr: 400 #Maximum nodes in the Working Memory (0 means infinity). When the number of nodes in Working Memory (WM) exceeds this threshold, some nodes are transferred to Long-Term Memory to keep WM size fixed
Vis:
FeatureType: 0 #0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY
Have you ever been faced with this kind of problem? Thanks in advance and I wish you a pleasant day ! |
Re: 2D mapping with lidar doesn't generate occupied cell
|
|
Re: 2D mapping with lidar doesn't generate occupied cell
Administrator
|
Re: 2D mapping with lidar doesn't generate occupied cell
|
|
| Free forum by Nabble | Edit this page |