Modified README, added hloc_client for auto service call in localization, removed usage

README.md

@@ -8,91 +8,95 @@ Docker for running hloc mapping and localization pipelines
 This process takes 30-50 minutes. 
 
 OR 
-* Pull from registry with: `docker pull registry.gitlab.inria.fr/spring/dockers/wp2_hloc:latest`
 
-2. Get associated docker containers:
 * Run `sh get_dockers.sh` 
-OR 
-* `docker pull colmap/colmap:latest`
-* `docker pull uodcvip/colmap:latest`
-* `docker pull registry.gitlab.inria.fr/spring/dockers/hloc_container:latest && docker image tag registry.gitlab.inria.fr/spring/dockers/hloc_container:latest hloc:latest && docker rmi registry.gitlab.inria.fr/spring/dockers/hloc_container:latest` 
 
 ## Instructions:
 
-## Mapping Mode:
-1. 
+## Mapping:
+1. First create a map with local SLAM (such as rtabmap)
+2. Run local SLAM in localization mode
+3. 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> `
+`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> `
+`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 hloc_localization_mode:=false`
+
 
 **Note: IMPORTANT: Pass the SAME `<cache in basestation>` which you mount in the docker to the argument `cache_folder`** 
 
 2. Move ARI around and images from the front, and rear fisheye are captured and saved in the docker. 
 
 3.   
-    a. Once the capture is done, open another terminal in the docker with `docker exec -it <container_name> bash`.  
+    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 "{}"`  
 
-4. 3.d will stop the capture, and start the mapping process which could take from minutes to hours depending on the number of images. You can monitor the progress in the terminal where the `docker run` was executed or in the `<cache in basestation>` folder. 
+4. 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/` . 
 
-5. When the mapping is done, if done successfully, you will see the message  `HLOC map composer done.` in the first terminal, and a response from the server call in the second terminal (could respond with an ERROR).
+5. 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.
 
 6. `ctrl + c` to close docker. Mapping is complete. 
 
 
 ## Localization Mode
-1. 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`
+* 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 <cache folder> is the one in the base station which was used for mapping.  
 
-Does not spawn a service anymore, but publishes `geometry_msgs/PoseStamped` messages to the topic `/hloc_localization/pose`
+* 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`
 
-2. Run the service call in a new terminal. The first call takes longer than subsequent calls. 
+* 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.
 
-NOTE: The map must be sufficiently large for the localizer to work. Otherwise it throws an error: `RuntimeError: selected index k out of range`. In this case, delete the cache, re-run mapping and then run localization.
+* 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`
 
-## Additional parameters:
-1. `pose_topic`: 
-Hloc/COLMAP use the front, and rear fisheye cameras for reconstructing the map, but the extrinsics calibration between them is not passed (currently), and hence 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. The alignment, and subseqeuent localization are way more accurate when using the local SLAM pose topic rather than the `/robot_pose` topic. 
 
-2. `save_folder` corresponding to the path where the images captured during mapping mode will be saved. This has been exposed as a parameter, but do not change it.
+## 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`   
 
-3. `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`   
-
-4.  
-a. `mapping_front_fisheye_topic`: Topic to subscribe to for front fisheye during mapping     
-b. `mapping_rear_fisheye_topic`: Topic to subscribe to for rear fisheye during mapping     
-c. `localization_front_fisheye_topic`: Topic to subscribe to for front fisheye during loc  
-c. `localization_rear_fisheye_topic`: Topic to subscribe to for rear fisheye during loc  
+### 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:
-1. Delete the cache folder between mapping sessions or re-running mapping after some error.
-
-2. IGNORE OUTPUT SUCH AS:  [WARNING] == The following "submitters" plugins could not be loaded ==
+## Mapping:
+1. 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' 
  
-
-## For Visualizing Alignment (On Basestation)
-1. Install the packages matplotlib, scipy, sqlite3 (maybe more required. #TODO UPDATE THE LIST ) 
-2. `python scripts/visualize_alignment.py <cache folder>/IOConverter/<inner_cache_folder>/images.txt <cache folder>/ModelsAligner/<inner_cache_folder>/images.txt`
-NOTE: Inner Cache folder is the folder which is created by the mapping process. It usually is a long alphanumeric .string.
-
-The red points are the positions according to the local mapping (from the `pose_topic`), and the blue points are the  positions from the Hloc mapping aligned to the local frame. 
-
-
-
-
-## TODO:
-1. Change it back to a service, which is called when rtabmap loses localization
-2. publish pose to /slam/initialpose when called.
\ No newline at end of file
+## 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:
+1. Add generalized pose regression solver for rig localization
\ No newline at end of file

get_dockers.sh

 #!/usr/bin/env bash
-    
 echo "Init docker containers"
+docker pull registry.gitlab.inria.fr/spring/dockers/wp2_hloc:latest
+echo "Hloc service client docker done"
 docker pull colmap/colmap:latest
-docker pull uodcvip/colmap:latest
 echo "Colmap done"
 docker pull registry.gitlab.inria.fr/spring/dockers/hloc_container:latest
 docker image tag registry.gitlab.inria.fr/spring/dockers/hloc_container:latest hloc:latest

launch/hloc_map_loc.launch

@@ -11,8 +11,10 @@
     <arg name="localization_front_fisheye_topic" default="/front_camera/fisheye/image_raw/compressed" />
     <arg name="localization_rear_fisheye_topic" default="/rear_camera/fisheye/image_raw/compressed" />
     <arg name="localization_camera" default="front" />
-
     <arg name="num_pairs_matching" default = "25" />
+    <arg name="time_ok_min"              default="0" />
+    <arg name="time_ok_max"              default="5" />
+    <arg name="time_warn_max"            default="10" />
 
     <node unless="$(arg hloc_localization_mode)" pkg="ari_hloc" type="capture_and_map.py" name="capture_and_map" >
     <param name="save_folder" type="string" value="$(arg save_folder)" />
@@ -48,4 +50,6 @@
     
     </node>
 
+  <node if="$(arg hloc_localization_mode)" pkg="ari_hloc" type="hloc_client.py" args="$(arg time_ok_min) $(arg time_ok_max) $(arg time_warn_max)" name="hloc_client" />
+
 </launch>

scripts/capture_and_map.py

@@ -77,10 +77,10 @@ def pose_callback(msg):
     (C,q_inv) = tf_listener.lookupTransform('map', 'front_fisheye_camera_optical_frame', rospy.Time()) # Gets q_inv,C (in the map frame)
 
     time = rospy.Time.now()
-    robot_position = C#[msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z]
-    robot_orientation = q_inv#[msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]
-    pose_dict['position'] = C#[msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z]
-    pose_dict['orientation'] = q_inv#[msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]
+    robot_position = C
+    robot_orientation = q_inv
+    pose_dict['position'] = C
+    pose_dict['orientation'] = q_inv
     pose_dict['time'] = str(time)
     pose_list.append(pose_dict)
 

scripts/hloc_client.py

+#!/usr/bin/env python
+import rospy, rostopic
+from geometry_msgs.msg import PoseWithCovarianceStamped
+import sys
+from ari_hloc.srv import InitPose,InitPoseResponse
+
+time_ok_min   = int(sys.argv[1])
+time_ok_max   = int(sys.argv[2])
+time_warn_max = int(sys.argv[3])
+
+def initialize_pose_hloc():
+    succ = False
+    rospy.wait_for_service('InitPose_Hloc')
+    try:
+        get_pose = rospy.ServiceProxy('InitPose_Hloc', InitPose)
+        resp = get_pose()
+        if resp.success == True:
+            succ = True
+            rospy.loginfo("Sent Hloc pose to /slam/initialpose")
+        else:
+            rospy.logwarn("Failed to send pose")
+
+    except rospy.ServiceException as e:
+        print("Service call failed: %s"%e)
+    
+    return succ 
+
+if __name__ == '__main__':
+    rospy.init_node('hloc_client')
+    last_update = rospy.Time.now()
+    r = rostopic.ROSTopicHz(-1)
+    s = rospy.Subscriber('/slam/localization_pose', PoseWithCovarianceStamped, r.callback_hz, callback_args='/slam/localization_pose' )
+    rospy.sleep(1)
+
+    rate = rospy.Rate(2)
+    while not rospy.is_shutdown():
+        avg_rate = r.get_hz('/slam/localization_pose')
+        
+        if avg_rate is not None:
+            last_update = rospy.Time.now()
+
+        diff = (rospy.Time.now()-last_update).secs  
+        if diff >= time_ok_min and diff <= time_ok_max:
+            rospy.loginfo("Pose update from SLAM  "+str(diff)+" seconds ago.")
+        elif diff > time_ok_max and diff <= time_warn_max:
+            rospy.logwarn("Warning: No pose update from SLAM for "+str(diff)+" seconds. Will call Hloc soon")
+        else:
+            rospy.loginfo("Calling Hloc initialization service")
+            success = initialize_pose_hloc()
+            rospy.loginfo(success)
+            last_update = rospy.Time.now()
+
+        rate.sleep()
+

third_party/Hierarchical-Localization/Dockerfile

@@ -11,8 +11,6 @@ RUN wget https://bootstrap.pypa.io/get-pip.py && python${PYTHON_VERSION} get-pip
 RUN update-alternatives --install /usr/bin/python3 python3 /usr/bin/python${PYTHON_VERSION} 1
 COPY . /app
 WORKDIR /app/
-RUN pip3 install --upgrade pip && pip3 install -r requirements.txt && pip3 install pycolmap
+RUN pip3 install --upgrade pip && pip3 install -r requirements.txt 
 RUN mkdir -p /app/third_party/netvlad/ && wget --no-check-certificate https://cvg-data.inf.ethz.ch/hloc/netvlad/Pitts30K_struct.mat -O /app/third_party/netvlad/VGG16-NetVLAD-Pitts30K.mat
-# RUN pip3 install git+https://github.com/mihaidusmanu/pycolmap
-RUN pip3 uninstall torch -y && pip3 install torch==1.4.0 
-RUN pip3 uninstall torchvision -y && pip3 install torchvision==0.5.0 -f https://download.pytorch.org/whl/torch_stable.html
\ No newline at end of file
+# RUN pip3 install git+https://github.com/mihaidusmanu/pycolmap
\ No newline at end of file

third_party/Hierarchical-Localization/requirements.txt

-torch>=1.1
-torchvision>=0.3
+torch==1.4.0
+torchvision==0.5.0 -f https://download.pytorch.org/whl/torch_stable.html
 numpy
 opencv-python
 tqdm
 matplotlib
 scipy
-h5py
\ No newline at end of file
+h5py
+pycolmap
\ No newline at end of file

third_party/meshroom/meshroom/nodes/Localization/HlocMapCreator.py

@@ -127,7 +127,7 @@ This node creates HLOC map out of the images and COLMAP SfM.
                     host_cache_folder=  host_cache_folder[:-1]
                 out_dir_docker = out_dir.replace(cache_folder, host_cache_folder)
             hloc_container = UtilsContainers("docker", "hloc:latest", out_dir_docker)
-            colmap_container = UtilsContainers("docker", "uodcvip/colmap", out_dir_docker)
+            colmap_container = UtilsContainers("docker", "colmap/colmap", out_dir_docker)
             # hloc_container = UtilsContainers("singularity", dir_path + "/hloc.sif", out_dir)
             # colmap_container = UtilsContainers("singularity", dir_path + "/colmap.sif", out_dir)
                 

third_party/meshroom/meshroom/nodes/SparseReconstruction/HoloLensMatcher.py

@@ -126,7 +126,7 @@ This node compute matches between all pairs of HoloLens rgb images.
             chunk.logger.info('Init COLMAP container')
             if sys.platform == 'win32':
                 out_dir = out_dir[0].lower() + out_dir[1::]
-                colmap_container = UtilsContainers("docker", "uodcvip/colmap", "/host_mnt/" + out_dir.replace(":",""))
+                colmap_container = UtilsContainers("docker", "colmap/colmap", "/host_mnt/" + out_dir.replace(":",""))
             else:
                 colmap_container = UtilsContainers("singularity", dir_path + "/colmap.sif", out_dir)
             colmap = Colmap(colmap_container)

third_party/meshroom/meshroom/nodes/SparseReconstruction/KeypointsDetector.py

@@ -118,7 +118,7 @@ The database is in the COLMAP format.
             chunk.logger.info('Init COLMAP container')
             if sys.platform == 'win32':
                 out_dir = out_dir[0].lower() + out_dir[1::]
-                colmap_container = UtilsContainers('docker', 'uodcvip/colmap', '/host_mnt/' + out_dir.replace(':',''))
+                colmap_container = UtilsContainers('docker', 'colmap/colmap', '/host_mnt/' + out_dir.replace(':',''))
             else:
                 output_folder_path = Path(out_dir)
                 cache_folder = str(output_folder_path.parent.parent)
@@ -127,7 +127,7 @@ The database is in the COLMAP format.
                         host_cache_folder=  host_cache_folder[:-1]
                     out_dir_docker = out_dir.replace(cache_folder, host_cache_folder)
 
-                colmap_container = UtilsContainers('docker', 'uodcvip/colmap',  out_dir_docker)
+                colmap_container = UtilsContainers('docker', 'colmap/colmap',  out_dir_docker)
                 # colmap_container = UtilsContainers('singularity', dir_path + '/colmap.sif', out_dir)
             colmap = Colmap(colmap_container)       
 

third_party/meshroom/meshroom/nodes/SparseReconstruction/Mapper.py

@@ -111,7 +111,7 @@ This node COLMAP mapper on database which contains matches.
             chunk.logger.info('Init COLMAP container')
             if sys.platform == 'win32':
                 out_dir = out_dir[0].lower() + out_dir[1::]
-                colmap_container = UtilsContainers("docker", "uodcvip/colmap", "/host_mnt/" + out_dir.replace(":",""))
+                colmap_container = UtilsContainers("docker", "colmap/colmap", "/host_mnt/" + out_dir.replace(":",""))
             else:
                 output_folder_path = Path(out_dir)
                 cache_folder = str(output_folder_path.parent.parent)
@@ -120,7 +120,7 @@ This node COLMAP mapper on database which contains matches.
                         host_cache_folder=  host_cache_folder[:-1]
                     out_dir_docker = out_dir.replace(cache_folder, host_cache_folder)
 
-                colmap_container = UtilsContainers("docker", "uodcvip/colmap", out_dir_docker)
+                colmap_container = UtilsContainers("docker", "colmap/colmap", out_dir_docker)
 
             colmap = Colmap(colmap_container)
             if chunk.node.ignoreTwoView.value:

third_party/meshroom/meshroom/nodes/SparseReconstruction/Matcher.py

@@ -151,7 +151,7 @@ This node compute matches between selected / all pairs of images.
             chunk.logger.info('Init containers')
             if sys.platform == 'win32':
                 out_dir = out_dir[0].lower() + out_dir[1::]
-                colmap_container = UtilsContainers('docker', 'uodcvip/colmap', '/host_mnt/' + out_dir.replace(':',''))
+                colmap_container = UtilsContainers('docker', 'colmap/colmap', '/host_mnt/' + out_dir.replace(':',''))
             else:
                 output_folder_path = Path(out_dir)
                 cache_folder = str(output_folder_path.parent.parent)
@@ -160,7 +160,7 @@ This node compute matches between selected / all pairs of images.
                         host_cache_folder=  host_cache_folder[:-1]
                     out_dir = out_dir.replace(cache_folder, host_cache_folder)
 
-                colmap_container = UtilsContainers('docker', 'uodcvip/colmap',  out_dir)
+                colmap_container = UtilsContainers('docker', 'colmap/colmap',  out_dir)
                 # colmap_container = UtilsContainers('singularity', dir_path + '/colmap.sif', out_dir)
             colmap = Colmap(colmap_container)
 

third_party/meshroom/meshroom/nodes/SparseReconstruction/TentativeMatcher.py

@@ -152,7 +152,7 @@ This node compute tentative matches between selected / all pairs of images.
             chunk.logger.info('Init COLMAP container')
             if sys.platform == 'win32':
                 out_dir = out_dir[0].lower() + out_dir[1::]
-                colmap_container = UtilsContainers('docker', 'uodcvip/colmap', '/host_mnt/' + out_dir.replace(':',''))
+                colmap_container = UtilsContainers('docker', 'colmap/colmap', '/host_mnt/' + out_dir.replace(':',''))
             else:
                 colmap_container = UtilsContainers('singularity', dir_path + '/colmap.sif', out_dir)
             colmap = Colmap(colmap_container)