Some small fixes

Dockerfile

@@ -8,40 +8,42 @@ RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selectio
 
 RUN apt-get update && apt-get upgrade -y && apt-get install -y --no-install-recommends apt-utils python3-pip git ros-noetic-tf2-geometry-msgs libsm6
 
-RUN addgroup --gid 1000 jari || true
-RUN useradd -rm -d /home/jari -s /bin/bash -u 1000 -g 1000 jari || true
-USER jari
-ENV USER jari
+ARG USER=yag
+
+RUN addgroup --gid 1000 ${USER} || true
+RUN useradd -rm -d /home/${USER} -s /bin/bash -u 1000 -g 1000 ${USER} || true
+USER ${USER}
+ENV USER ${USER}
 
 RUN mkdir -p ~/catkin_ws/src
 RUN cd ~/catkin_ws/src && git clone https://github.com/safijari/sba_python.git && cd sba_python && git submodule update --init --recursive
 
 USER root
 
 RUN source /opt/ros/noetic/setup.bash \
-    && cd /home/jari/catkin_ws \
+    && cd /home/$USER/catkin_ws \
     && rosdep update \
     && rosdep install -y -r --from-paths src --ignore-src --rosdistro=noetic -y
 
 RUN apt install python3-catkin-tools ros-noetic-slam-toolbox-msgs -y
 
-USER jari
+USER ${USER}
 
 RUN pip install tiny-tf numba opencv-python-headless argh
 
 RUN pip install --force-reinstall numpy scipy scikit-image tqdm
 
 RUN pip install --force-reinstall msgpack ipython ipdb
 
-COPY ./ /home/jari/catkin_ws/src/yag-slam/
+COPY ./ /home/$USER/catkin_ws/src/yag-slam/
 
 RUN source /opt/ros/noetic/setup.bash
 
 RUN source /opt/ros/noetic/setup.bash \ 
-    && cd /home/jari/catkin_ws/src \
+    && cd /home/$USER/catkin_ws/src \
     && catkin_init_workspace \
     && cd .. \
     && catkin config --install \
     && catkin build -DCMAKE_BUILD_TYPE=Release
 
-CMD /home/jari/catkin_ws/src/yag-slam/run_docker.sh
+CMD /home/$USER/catkin_ws/src/yag-slam/run_docker.sh

ros1/slam_node_ros1

@@ -22,12 +22,8 @@ import cv2
 from threading import Thread, Lock
 import traceback
 from queue import Queue
-import json
 import argh
 import yaml
-import numpy as np
-import msgpack
-import zlib
 
 
 def tftopose2(msg):
@@ -86,8 +82,8 @@ class YAGSlamRos1(Thread):
         self.min_rotation = _p('~min_rotation', 0.5)
         self.loop_search_min_chain_size = _p('~loop_search_min_chain_size', 10)
         self.loop_search_distance = _p('~loop_search_distance', 4.0)
-        self.min_response_coarse = _p('~min_response_coarse', 0.35)
-        self.min_response_fine = _p('~min_response_fine', 0.45)
+        self.min_response_coarse = _p('~min_response_coarse', 0.6)
+        self.min_response_fine = _p('~min_response_fine', 0.7)
         self.range_threshold_for_map = _p('~range_threshold_for_map', 12)
         self.range_threshold = _p('~range_threshold', 30)
         self.scan_buffer_len = _p('~scan_buffer_len', 10)
@@ -122,11 +118,11 @@ class YAGSlamRos1(Thread):
         self.start()
 
     def save_graph(self, trigger_cmd):
-        out = zlib.compress(msgpack.packb(self.mapper.serialize()))
+        out = self.mapper.binarize()
         if trigger_cmd is not None:
             fixed_path = trigger_cmd.filename
         else:
-            fixed_path = os.path.expanduser("~") + "/.jari/maps/" + "map.graph"
+            fixed_path = "/tmp/map.graph"
         with open(fixed_path, "wb") as ff:
             print("Saving graph at {}".format(fixed_path))
             ff.write(out)

yag_slam/graph_slam.py

@@ -48,7 +48,8 @@ def __init__(
             loop_search_dist=3,
             loop_search_min_chain_size=10,
             min_response_coarse=0.35,
-            min_response_fine=0.45):
+            min_response_fine=0.45,
+            verbose=False):
 
         self.seq_matcher = seq_matcher
         self.loop_matcher = loop_matcher
@@ -67,6 +68,8 @@ def __init__(
         self.min_response_coarse = min_response_coarse
         self.min_response_fine = min_response_fine
 
+        self.verbose = verbose
+
     @classmethod
     def default(cls):
         return cls(default_config, default_config_loop)
@@ -216,11 +219,12 @@ def try_to_close_loop(self, scan):
             # resp 0.35 for coarse, 0.4 for fine?
             # covar 3.0 for coarse?????
             if res_coarse.response < self.min_response_coarse:
-                print("not good coarse response {}".format(res_coarse.response))
+                if self.verbose:
+                    print(f"Loop closure coarse response is not good {} < {self.min_response_coarse}".format(res_coarse.response))
                 continue
 
             if res_coarse.covariance[0][0] > 3.0 or res_coarse.covariance[1][1] > 3.0:
-                print("WARN: covariance too high for coarse")
+                print("WARN: Covariance is too high for coarse coarse response during loop closure")
 
             p = res_coarse.best_pose
 
@@ -230,7 +234,7 @@ def try_to_close_loop(self, scan):
             res = self.seq_matcher.match_scan(tmpscan, chain, False, True)
 
             if res.response < self.min_response_fine:
-                print("not good fine response {}".format(res.response))
+                print("Loop closure fine response is not good {}".format(res.response))
                 continue
 
             scan.corrected_pose = res.best_pose