Improve localization in extremely narrow corridors

[ghost-60]

I have built a map for a corridor and was trying to localize in it. (using localization = true)

The issue is that it constantly runs inside the walls and out of bounds especially around the corners. Like below:

I am trying to tune parameters but haven't been able to successfully localize yet. Parameters below(for both building the map and localization): "Rtabmap/DetectionRate" type="string" value="1.0" "RGBD/OptimizeMaxError" type="string" value="0" "MaxFeatures" type="int" value="0" "queue_size" type="int" value="30" "Grid/FromDepth" type="string" value="true" "Grid/RangeMax" type="string" value="3" "Grid/CellSize" type="string" value="0.01" "Grid/MaxObstacleHeight" type="string" value="1" "GridGlobal/OccupancyThr" type="string" value="0.65" "Grid/NoiseFilteringRadius" type="string" value="0.5" "Grid/NoiseFilteringMinNeighbors" type="string" value="10" "Vis/MaxFeatures" type="int" value="2500" "SURF/HessianThreshold" type="int" value="100" "RGBD/LoopClosureReextractFeatures" type="bool" value="true" "Vis/BundleAdjustment" type="int" value="1" "Odom/Strategy" type="int" value="5"

Any help would be appreciated!

[matlabbe]

Can you show the occupancy grid map? Are you planning with move_base?

[ghost-60]

This is the grid map. Yeah I am using move_base for planning. As you can see, the planned path is going through the walls which is also another issue.

Here is how the corridor actually looks like:

Here is my move_base launch file: https://github.com/ghost-60/Guidance-System/blob/master/visguide/launch/PathPlanning.launch

Here are my yaml parameters for move_base: https://github.com/ghost-60/Guidance-System/tree/master/visguide/launch

[matlabbe]

I meant /rtabmap/grid_map, not /rtabmap/cloud_map. Also, if you can share the database, it can be easier to debug here.

[ghost-60]

Here is the rtabmap.db file: https://drive.google.com/file/d/1NdsYXj2RBAzICF6M_R6xA4EF_xANhIGu/view?usp=sharing

this is what I get under rtabmap/grid_map:

[matlabbe]

Grid/CellSize is 0.01, which is very small! Here set at 0.1:

Note that using a stereo camera indoor with only white walls everywhere won't generate very good obstacle maps. The floor is also reflective, the ground cannot be detected either. The stereo algorithm even closes the corridor corner here:

There are also large odometry drifts when the camera is facing a white wall. The whole map is then bending and the obstacle segmentation won't work well.

What is the target application? Will there be a ground mobile robot? With a ground robot, we can force 2D odometry, we can also segment obstacles based on their height from the ground (so the wavy ground because of reflections won't appear as obstacles).

[ghost-60]

Thanks for the input! I will try changing that parameter and update here. I am currently trying to use rtabmap mainly as a path planning in indoor spaces where a person is wearing a zed-mini camera and when specified a destination the model will output text/voice commands to guide him to it(like saying "go x meters forward and turn left"). A human is always handling the camera in this case. I am facing 2 major issues:

1. When localization is not working properly(like going in the walls), path planning algorithm doesn't work at all.
2. Even when localizing correctly, planned path is going through the walls. The camera is at approximately 1.5-2 meters from the ground.

[matlabbe]

Try zed-m odometry, I think the output will be aligned with gravity. Otherwise, we can feed IMU to rtabmap's odometry to add gravity constraints.

<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
   ...
   <remap from="imu"  to="/zed_imu_topic"/>
   <param name="wait_imu_to_init"       type="bool"   value="true"/>
   <param name="Optimizer/GravitySigma" type="string" value="0.1"/>
</node>

Which output of rtabmap are you using for planning? Note that even with increasing the grid cell size, there could be holes in walls if the camera cannot generate depth on them. I think your planner should not plan a path through "unknown" cells, only "empty" cells. You could add Grid/RayTracing=true to fill the empty space. You may also try Grid/3D=false to speed up the ray tracing.

cheers, Mathieu

[ghost-60]

I am using /rtabmap/grid_map for planning in move base. The launch file is given below for reference https://github.com/ghost-60/Guidance-System/blob/master/visguide/launch/PathPlanning.launch

Even using the Grid/RayTracing=true the planner is still going through "unknown cells" and not empty cells.

Here is my rtabmap.launch file I am using: https://github.com/ghost-60/Guidance-System/blob/master/visguide/launch/rtabMap.launch

Is the planner behaving that way due to move_base yaml parameters or due to the database itself?

[ghost-60]

What I also don't understand is why it doesn't localize properly during map building itself because its able to do closed-loop detection but not correcting its odometry (going into unknown cells).

Is it a bug?or a closed-loop detection problem? Or is there a parameter that could prevent going to "unknown cells" altogether?

[matlabbe]

I don't how the planner is working, but if it uses the grid map as above, it clearly plan through obstacles. If the standard global planner of move_base is used, you may have to switch the static_map plugin, see the last issue shown at the bottom of this page:

If sometimes the robot is planning a straight path through an obstacle, it may be related to this issue. This can be fixed by using rtabmap_ros::StaticLayer instead of costmap_2d::StaticLayer here.

For the localization, it is more related to motion estimation accuracy or wrong localizations. A video of the issue would be useful.

[ghost-60]

Thanks a lot!

Path planning problem is really caused by the wrong name of staticLayer and I have changed it to rtabmap_ros.

Localization problem still exist, and to make things clear I record a video. https://youtu.be/RIW_zK5qIOc

The map will show at the beginning, so the localization does find itself at the starting point. But later it begins to mess up, going into "unknown area" and the global path planner will get confused.

[matlabbe]

In the video, if the camera is localized on start, it didn't localize for the whole trajectory afterwards, so the odometry drift make the pose go outside of the corridors. The problem is that there is no localization, look at the terminal to see if there are warnings about loop closures rejected, and see why. My feeling is that the environment doesn't have enough discriminative visual features for reliable localization.

[ghost-60]

Thanks a lot for the help! I saw some warnings that i think is related to loop closures.

But I am not able to exactly figure out the reason behind it. Is it only because of visual odometry (not enough features)?

Here is what the launch terminal looks like when localizing:

[ghost-60]

Hi,

I changed the data set to another corridor with more features, but I am still receiving "loop rejection" messages during mapping/localizing. The warning messages are as follows:

The pointcloud also is misaligned with the axis. Might be the reason for loop rejection.

For your reference, I attached the database. https://drive.google.com/open?id=1TrYKBIR_S91oAxKDl29yQkODpgljW88g

Thanks

[matlabbe]

The map is bending because the odometry drifts in z, roll and/or pitch. I would suggest to use a VIO approach, you may try VINS-Fusion as input odometry to rtabmap. You can also set Optimizer/GravitySigma to 0.3 for rtabmap node, while feeding IMU data from ZED-mini. rtabmap will then try to correct the zed odometry using gravity. Note that in your database this parameter is 0, are you using the same launch file than here: https://github.com/introlab/rtabmap_ros/issues/357#issuecomment-540745774 ?

[ghost-60]

Yeah I am using that same launch file but I have not updated it for a while. I am currently trying to fuse imu using rosbag .Previously I was using a recorded svo file which didn't published imu data. Will update after that. Thank you very much for all the help!