Floor Plan Based Active Global Localization and Navigation Aid for Persons with Blindness and Low Vision

We presents a real-time system for floor plan based active global localization and navigation, specifically designed to assist individuals with blindness and low vision in unfamiliar environments. Architectural floor plans along with a stereo-inertial camera are employed to achieve active localization of the agent using a particle filter and semantically informed exploration. This work enhances accessibility and is adaptable for mobile robots, validated through extensive real-world indoor experiments.

📖 Paper: RA-L

📹 Video: Youtube

Contributors: Raktim Gautam Goswami¹, Harshit Sinha¹, Venkata Amith Palacherla¹, Jagennath Hari¹, Prashanth Krishnamurthy¹, JohnRoss Rizzo², Farshad Khorrami¹

We would like to thank Tanishq Bhansali¹ for his help in conducting the experiments.

¹Control/Robotics Research Laboratory (CRRL), Department of Electrical and Computer Engineering, NYU Tandon School of Engineering

²Rusk Institute of Rehabilitation, New York University Grossman School of Medicine

💡 Contributions

• Active Global Localization: Semantics-driven active global localization leveraging architectural floor plans and stereo-inertial sensors.
• Odometry Drift Correction: A dynamic approach for correcting the agent’s time-varying odometry drift utilizing the floor plan, independent of prior knowledge of the agent’s initial pose.
• Loop Closure on Reversal: Implementation of loop closure for reversal through the application of ICP and Bundle Adjustment techniques.
• ⁠Development of an efficient, real-time semantic end-to-end system designed to facilitate navigation assistance for persons with blindness and low vision.
• Extensive experiments on real-world environments showcasing the method’s efficacy.

Table of Contents

• Installations
  • RTAB-MAP Installation
  • ROS Installation
  • Dependencies
  • Changing the Semantic Segmentation Model in RTAB-Map
• Usage
  • Building the Package
  • Modifying the Files for Custom FloorPlans
  • Running the Algorithm

1. Installations

This package has been deployed on an NVIDIA Jetson Orin device, operating with JetPack version 5.2 on Ubuntu 20.04. It has been installed within the ROS1 workspace.

1.1 RTAB-MAP

RTAB-MAP version 0.20.23 should be installed outside the ROS workspace.
For detailed instructions on the installation and configuration of RTAB-MAP, please refer to the official installation guidelines available on RTAB-MAP GitHub.

1.2 ROS Installation

This package is designed to operate within ROS Noetic. For instructions on setting up and configuring a ROS workspace, please refer to the ROS installation guide.

1.3 Dependencies

• depthimage_to_laserscan: This package depends on depthimage_to_laserscan, which should be installed in the ROS workspace.
• OpenCV 4.6: OpenCV version 4.6 is required for both this package and RTAB-MAP standalone. To manage OpenCV dependencies within ROS, vision_opencv can be utilized, ensuring compatibility and maintaining the required OpenCV version.

1.4 Changing the Semantic Segmentation Model in RTAB-Map

This package uses the HRNet model for semantic segmentation, as described in:

J. Wang, K. Sun, T. Cheng, B. Jiang, C. Deng, Y. Zhao, D. Liu, Y. Mu, M. Tan, X. Wang, et al., “Deep high-resolution representation learning for visual recognition,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 43, pp. 3349–3364, 2020.

To change the semantic segmentation model in RTAB-Map, follow these steps:

1. Navigate to the RTAB-Map core library source directory:

cd ~/libraries/rtabmap/corelib/src/

2. Open memory.cpp and modify the model path at line 137.

3. Build and install RTAB-MAP

cd ~/libraries/rtabmap/build
cmake ..
make
sudo make install

4. Rebuild RTAB-MAP in the ROS workspace:

cd ~/your_ros_workspace
catkin build --cmake-args -DWITH_TORCH=ON -DCMAKE_BUILD_TYPE=Release

2. Usage

2.1 Building the Package

1. Navigate to the src folder of your ROS WORKSPACE.
2. Clone this repository to the source folder. (remeber to but a period at the end)

git clone https://github.com/raktimgg/FloorPlanActiveLocalization.git . 

3. Once cloned, it should look like this

ROS_WORKSPACE_PATH/
    build/
    devel/
    src/
      localization/
      localization_navigation/
      sem_cloud/
      README.md

4. Building the workspace

cd ..
catkin build --cmake-args -DWITH_TORCH=ON -DCMAKE_BUILD_TYPE=Release \
-Drtabmap_ros_DIR="/home/username/your_workspace/devel/.private/rtabmap_ros/share/rtabmap_ros/cmake"

2.2 Modifying the Files for Custom FloorPlans

1. To change the permissible sapce, doors, walls text file follow the steps below:

cd ~/your_ros_workspace/src/localization/src/CPP

2. Open slam2_decoupled.cpp and edit lines 93-95 to set your desired paths for each model file:

   Line 93: /home/crrl/crrl_ws/src/localization/src/CPP/doors_walls/doors4.txt

   Line 94: /home/crrl/crrl_ws/src/localization/src/CPP/doors_walls/walls4.txt

   Line 95: /home/crrl/crrl_ws/src/localization/src/CPP/permissible_space4.txt

3. Go to the sem_cloud source folder:

cd ~/your_ros_workspace/src/sem_cloud/src

4. Open exploration.cpp and update lines 604-609 with your custom floor plan paths:

   Line 604: /home/crrl/crrl_ws/src/localization/src/CPP/permissible_space_library.txt

   Line 605: /home/crrl/crrl_ws/src/localization/src/CPP/doors_walls/walls_library.txt

   Line 606: /home/crrl/crrl_ws/src/localization/src/CPP/doors_walls/doors_library.txt

   Line 607: /home/crrl/crrl_ws/src/localization/src/CPP/corner_pos.txt

   Line 608: /home/crrl/crrl_ws/src/localization/src/CPP/permissible_space_library.txt

   Line 609: /home/crrl/crrl_ws/src/sem_cloud/include/goal_rewards_library.txt

5. Go to the configuration directory for the floor plan:

cd ~/your_ros_workspace/src/localization_navigation/config

Modify the costMapFromFloorPlan.yaml file to specify your custom floor plan image model.

2.3 Running the algorithm

To launch the setup, use separate terminals to run the following commands:

Terminal 1: Localization Launch

roslaunch localization_navigation localization.launch

Terminal 2: Path Planning

roslaunch localization_navigation path_planner.launch

Terminal 3: Exploration with ZED

rosrun sem_cloud explore_ZED

Terminal 4: Notification Service

cd ~/your_ros_workspace/src/localization/src/python && python3 notification_python.py