Spring
WP2_Mapping_Localization
hloc mapping localization

Repository



hloc docker
Docker for running hloc mapping and localization pipelines

Instructions to build docker

Clone this repository with git clone git@gitlab.inria.fr:spring/wp2_mapping_localization/hloc-mapping-localization.git


docker build -t <Image name> .
This process takes 30-50 minutes.

OR

Run sh get_dockers.sh


Instructions:

Mapping:

First create a map with local SLAM (such as rtabmap)
Run local SLAM in localization mode
Make sure the front, and rear fisheye cameras are publishing and accessible.

For the mapping mode.
docker run -it --rm --env ROS_MASTER_URI=http://10.68.0.1:11311 --network=host --gpus all -v /var/run/docker.sock:/var/run/docker.sock -v <cache in basestation>:/root/catkin_ws/src/ari_hloc/pipelines/MeshroomCache registry.gitlab.inria.fr/spring/dockers/wp2_hloc:latest pose_topic:=<relevant pose topic> cache_folder:=<cache in basestation> hloc_localization_mode:=false
For the last -v  replace <cache in basestation>  with the path in basestation where you want the cache to be stored. This cache will be used by the localizer.
Default cache_folder is /home/hloc_mapping/cache. If nothing is passed, it creates the cache folder in this location in your basestation.
In this case, the commmand would look like:
docker run -it --rm --env ROS_MASTER_URI=http://10.68.0.1:11311 --network=host --gpus all -v /var/run/docker.sock:/var/run/docker.sock -v /home/hloc_mapping/cache:/root/catkin_ws/src/ari_hloc/pipelines/MeshroomCache registry.gitlab.inria.fr/spring/dockers/wp2_hloc:latest pose_topic:=<relevant pose topic> hloc_localization_mode:=false
Example command:
docker run -it --rm --env ROS_MASTER_URI=http://10.68.0.1:11311 --network=host --gpus all -v /var/run/docker.sock:/var/run/docker.sock -v /home/hloc_mapping/cache:/root/catkin_ws/src/ari_hloc/pipelines/MeshroomCache registry.gitlab.inria.fr/spring/dockers/wp2_hloc:latest pose_topic:=/robot_pose  cache_folder:=/home/hloc_mapping/cache hloc_localization_mode:=false
Note: IMPORTANT: Pass the SAME <cache in basestation> which you mount in the docker to the argument cache_folder


Move ARI around and images from the front, and rear fisheye are captured and saved in the docker.


a. Once you're finished with the capture, enter the container with docker exec -it hloc_mapper bash.

b. source activate meshroom

c. source devel/setup.bash

d. rosservice call /stop_capture "{}"


3.d will stop the capture, and start the mapping process which could take from minutes to hours depending on the number of images. To monitor the progress, you can find the logs inside the docker container in /root/.ros/logs/latest/ .


When the mapping is done, if done successfully, you will see a response from the server call in the second terminal. Response is a boolean which returns success:True if the process was completed and false if there was an error mid way.


ctrl + c to close docker. Mapping is complete.


Localization Mode

Run  docker run -it --rm --env ROS_MASTER_URI=http://10.68.0.1:11311 --network=host --gpus all -v <cache folder>:/root/catkin_ws/src/ari_hloc/pipelines/MeshroomCache <docker image> hloc_localization_mode:=true


Where the  is the one in the base station which was used for mapping.


It should start in a few seconds, and you might see output such as:
[WARN] [1685701817.113652]: Warning: No pose update form SLAM for a while. Will call Hloc soon if there is no pose update from rtabmap


This works as a ROS service-client architecture. A service is spawned which runs localization based on the fisheye image, and sends the pose to rtabmap's /slam/initialpose which in turn updates the relevant TF.


The service is called through a client which continually monitors to check if rtabmap publishes pose updates through /slam/localization_pose. If it does not publish an update for some n seconds, it calls the service.


Example command:
docker run -it --rm --env ROS_MASTER_URI=http://10.68.0.1:11311 --network=host --gpus all -v /home/hloc_mapping/cache/:/root/catkin_ws/src/ari_hloc/pipelines/MeshroomCache registry.gitlab.inria.fr/spring/dockers/wp2_hloc:latest hloc_localization_mode:=true cache_folder:=/home/hloc_mapping/cache/ localization_camera:=front

Parameters:

Common:


hloc_localization_mode: true for localization, false for mapping. Default: true.

save_folder corresponding to the path where the images captured during mapping mode will be saved. This has been exposed as a parameter, but there is no reason to change it.

cache_folder corresponding to the path where the cache is stored in the host system. Important parameter to pass if your cache is not the default /home/hloc_mapping/cache


Mapping:


pose_topic: Default: /robot_pose
Hloc/COLMAP use the front, and rear fisheye cameras for reconstructing the map, and the map is reconstructed only up to scale. To obtain scale and align the HLoc map with the local map, as well as some additional pre-processing of the input data, the pose data of the robot during the capture is used.

By default, we use the /robot_pose topic, but depending on the local odometry used, and the required frame to align the map, any PoseWithCovarianceStamped topic can be passed to this.


mapping_front_fisheye_topic: Topic to subscribe to for front fisheye during mapping


mapping_rear_fisheye_topic: Topic to subscribe to for rear fisheye during mapping

Defaults are: /front_camera/fisheye/image_raw/compressed and /rear_camera/fisheye/image_raw/compressed


Localization:


localization_front_fisheye_topic: Topic to subscribe to for front fisheye during loc

localization_rear_fisheye_topic: Topic to subscribe to for rear fisheye during loc

Defaults same as for corresponding mapping topics

localization_camera : Camera with which you want to localize; front or rear. Default: front


num_pairs_matching : Number of possible matches for Hloc. With higher number, you get higher accuracy but longer processing time. Defualt: 25.

time_ok_min, time_ok_max: Parameters for diagnostics primarily. Defaults: 0 and 5


time_warn_max: Number of seconds after which, if there is no update from SLAM, Hloc will be called.


NOTE:

Mapping:

IGNORE [WARNING] == The following "submitters" plugins could not be loaded ==


simpleFarmSubmitter: [Errno 2] No such file or directory: '/root/catkin_ws/src/ari_hloc/third_party/meshroom/meshroom/submitters/simpleFarmConfig.json'


Localization:

The pose from Hloc can be visualized on rviz, with the /slam/initialpose topic.
If the pose isn't shown on Rviz when Hloc seems to be called, then make sure you can send information from the basestation which is received in ARI (try manually publishing with some topic).

Usually, you should sync the time between the base station and ARI to fix this.


TODO:

Add generalized pose regression solver for rig localization