Add Fast-LIO and LIO-SAM to a localisation benchmark in a new docker (#…

…10)

.docker_loc/Dockerfile

@@ -0,0 +1,121 @@
+##############
+# Base image #
+##############
+ARG UBUNTU_VERSION=20.04
+ARG NVIDIA_CUDA_VERSION=11.8.0
+
+FROM nvidia/cuda:${NVIDIA_CUDA_VERSION}-devel-ubuntu${UBUNTU_VERSION}
+
+# Install ROS
+ARG ROS_DISTRO=noetic
+# Prevent stop building ubuntu at time zone selection.
+ENV DEBIAN_FRONTEND=noninteractive 
+RUN apt-get update && \
+    apt-get install -y --no-install-recommends \
+    lsb-release \
+    curl && \
+    sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && \
+    curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - && \
+    apt-get update && \
+    apt install -y \
+    ros-noetic-desktop-full \
+    python3-rosdep \
+    python3-catkin-tools \
+    ros-noetic-catkin &&\
+    rm -rf /var/lib/apt/lists/* && \
+    echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc && \
+    rosdep init && \
+    rosdep update
+
+# create catkin workspace
+ARG CATKIN_WS_DIR=/home/catkin_ws
+RUN . /opt/ros/noetic/setup.sh && mkdir -p ${CATKIN_WS_DIR}/src && cd ${CATKIN_WS_DIR} && catkin build && \
+    echo "source ${CATKIN_WS_DIR}/devel/setup.bash" >> ~/.bashrc
+
+# Install essential tools
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    git \
+    vim \
+    python-is-python3 \
+    python3-pip \
+    libyaml-cpp-dev && \
+    rm -rf /var/lib/apt/lists/*
+
+# Install fast_lio dependences
+RUN git clone https://github.com/ori-drs/livox_ros_driver2.git ${CATKIN_WS_DIR}/src/livox_ros_driver2
+RUN git clone https://github.com/Livox-SDK/Livox-SDK2.git ${CATKIN_WS_DIR}/src/Livox-SDK2
+RUN cd ${CATKIN_WS_DIR}/src/Livox-SDK2/ && mkdir build && cd build && cmake .. && make -j && make install
+
+# Install lio_sam dependences
+RUN apt-get update
+RUN apt-get install libboost-all-dev
+RUN apt-get install software-properties-common -y
+RUN add-apt-repository ppa:borglab/gtsam-release-4.0
+RUN apt-get update
+RUN apt-get install libgtsam-dev libgtsam-unstable-dev -y
+
+# Install ros packages
+RUN git clone https://github.com/catkin/catkin_simple.git ${CATKIN_WS_DIR}/src/catkin_simple 
+RUN git clone https://github.com/ori-drs/FAST_LIO.git -b ros1 ${CATKIN_WS_DIR}/src/FAST_LIO
+RUN cd ${CATKIN_WS_DIR}/src/FAST_LIO && git submodule update --init
+RUN git clone https://github.com/ori-drs/LIO_SAM_6AXIS.git -b dev/frontier_hesai_ho ${CATKIN_WS_DIR}/src/LIO_SAM_6AXIS
+
+# Install C++ Dependencies
+RUN apt-get update && apt-get install --no-install-recommends -y \
+    libblosc-dev \
+    libboost-iostreams-dev \
+    libboost-numpy-dev \
+    libboost-python-dev \
+    libboost-system-dev \
+    libeigen3-dev \
+    libtbb-dev \
+    libgflags-dev \
+    libgl1-mesa-glx \
+    libgoogle-glog-dev \
+    protobuf-compiler \
+    python3-catkin-tools \
+    && rm -rf /var/lib/apt/lists/*
+
+RUN cd ${CATKIN_WS_DIR} && catkin build livox_ros_driver2
+RUN cd ${CATKIN_WS_DIR} && catkin build fast_lio
+
+RUN apt-get update
+RUN apt-get install wget
+RUN cd ${CATKIN_WS_DIR}/src && wget https://sourceforge.net/projects/geographiclib/files/distrib-C++/GeographicLib-2.4.tar.gz && tar xfpz GeographicLib-2.4.tar.gz
+RUN cd ${CATKIN_WS_DIR}/src/GeographicLib-2.4 && mkdir BUILD && cd BUILD && cmake .. && make && make install
+
+RUN cd ${CATKIN_WS_DIR} && catkin build lio_sam_6axis
+
+# Trajectory evaluation dependences
+RUN apt-get update
+RUN apt-get install tmux -y
+RUN pip install libtmux
+RUN pip install evo
+
+# Final setup
+ARG GID
+ARG UID
+ENV UNAME=docker_dev
+RUN addgroup --gid $GID $UNAME
+RUN adduser --disabled-password --gecos '' --uid $UID --gid $GID $UNAME
+
+ARG SPIRES_DIR=/home/oxford_spires_dataset
+WORKDIR ${SPIRES_DIR}
+
+COPY ./oxford_spires_utils/ ${SPIRES_DIR}/oxford_spires_utils/
+COPY ./spires_ros/fast_lio2/launch_osd ${CATKIN_WS_DIR}/src/FAST_LIO/launch
+RUN cd ${CATKIN_WS_DIR}/src/FAST_LIO && mkdir config
+COPY ./spires_ros/fast_lio2/hesai.yaml ${CATKIN_WS_DIR}/src/FAST_LIO/config
+COPY ./spires_ros/lio_sam/launch_lsam ${CATKIN_WS_DIR}/src/LIO_SAM_6AXIS/LIO-SAM-6AXIS/launch
+COPY ./spires_ros/lio_sam/config_lsam ${CATKIN_WS_DIR}/src/LIO_SAM_6AXIS/LIO-SAM-6AXIS/config
+COPY ./pyproject.toml ${SPIRES_DIR}/pyproject.toml
+# RUN pip install -e .
+
+# Make the outputs of the container match the host
+RUN chown -R ${UID}:${GID} ${SPIRES_DIR}
+USER ${UNAME}
+RUN echo "PS1='${debian_chroot:+($debian_chroot)}\[\033[01;33m\]\u@docker-\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\$ '" >> ~/.bashrc
+RUN echo "alias recon_benchmark=\"python ${SPIRES_DIR}/scripts/reconstruction_benchmark/main.py\"" >> ~/.bashrc
+RUN echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
+
+CMD ["/bin/bash"]

.docker_loc/docker-compose.yml

@@ -0,0 +1,22 @@
+services:
+  spires:
+    container_name: spires_container
+    build:
+      context: ..
+      dockerfile: .docker_loc/Dockerfile
+      args:
+        - USERNAME=${USER:-user}
+        - UID=${UID}
+        - GID=${GID}
+    environment:
+      - NVIDIA_VISIBLE_DEVICES=all
+      - NVIDIA_DRIVER_CAPABILITIES=all
+      - DISPLAY=$DISPLAY
+    runtime: nvidia
+    network_mode: "host"
+    tty: true
+    volumes:
+      - ../scripts:${SPIRES_DIR}/scripts
+      - ../tests:${SPIRES_DIR}/tests
+      - ${HOST_SPIRES_DATA_DIR}:${SPIRES_DATA_DIR}
+      - /tmp/.X11-unix:/tmp/.X11-uni # To display emerging windows, it is necessary to run 'xhost +' before composing the docker.

scripts/localisation_benchmark/fast_lio2.py

@@ -0,0 +1,53 @@
+import sys
+import time
+
+import libtmux
+
+if __name__ == "__main__":
+
+    # TODO: put it in def an get path from arg.
+
+    path = '/home/oxford_spires_dataset/data/'
+    sequence = '2024-03-14_blenhein_1'
+    seq_path = path + sequence
+
+    server = libtmux.Server()
+    server.cmd('new-session', '-d', '-P', '-F#{session_id}')
+    session = server.sessions[0]
+    window_1 = session.new_window(attach=False, window_name="launch fast_lio2")
+    window_2 = session.new_window(attach=False, window_name="rosbag") 
+    pane_1 = window_1.panes.get()
+    pane_2 = window_2.panes.get()
+
+    pane_1.send_keys('cd /home/catkin_ws/')
+    pane_1.send_keys('source devel/setup.bash')
+    pane_1.send_keys('roslaunch fast_lio {}.launch'.format(sequence))
+
+    pane_2.send_keys('cd {}/rosbag/'.format(seq_path))
+    pane_2.send_keys('rosbag play *.bag --clock')
+
+    # Check if the rosbag is "Done."
+    print("Fast-LIO2 launched!! - {}.launch".format(sequence))
+    print("Starting rosbag files in {}/rosbag/ ...".format(seq_path))
+    while (1):
+        pane_2.clear()
+        time.sleep(1)
+        cap_curr = pane_2.capture_pane()
+        for line in cap_curr:
+            if "[RUNNING]" in line:
+                print(line)
+                sys.stdout.write("\033[F")
+        if 'Done.' in cap_curr:
+            sys.stdout.write("\033[K")
+            print('Done.') 
+            break
+
+    print("PROCESS FINISHED!!")
+    print("Output in: {}/output_loc/Fast-LIO2.bag".format(sequence))
+
+    # TODO: Copy to output folder
+
+    # TODO: Transform to TUM format
+
+
+    server.kill()

scripts/localisation_benchmark/lio_sam.py

@@ -0,0 +1,58 @@
+import sys
+import time
+
+import libtmux
+
+if __name__ == "__main__":
+
+    # TODO: put it in def an get path from arg.
+
+    path = '/home/oxford_spires_dataset/data/'
+    sequence = '2024-03-14_blenhein_1'
+    seq_path = path + sequence
+
+    server = libtmux.Server()
+    server.cmd('new-session', '-d', '-P', '-F#{session_id}')
+    session = server.sessions[0]
+    window_1 = session.new_window(attach=False, window_name="launch lio_sam")
+    window_2 = session.new_window(attach=False, window_name="rosbag") 
+    window_3 = session.new_window(attach=False, window_name="rosservice")
+    pane_1 = window_1.panes.get()
+    pane_2 = window_2.panes.get()
+    pane_3 = window_3.panes.get()
+
+    pane_1.send_keys('cd /home/catkin_ws/')
+    pane_1.send_keys('source devel/setup.bash')
+    pane_1.send_keys('roslaunch lio_sam_6axis {}.launch'.format(sequence))
+
+    pane_2.send_keys('cd {}/rosbag/'.format(seq_path))
+    pane_2.send_keys('rosbag play *.bag --clock')
+
+    # Check if the rosbag is "Done."
+    print("LIO-SAM launched!! - {}.launch".format(sequence))
+    print("Starting rosbag files in {}/rosbag/ ...".format(seq_path))
+    while (1):
+        pane_2.clear()
+        time.sleep(1)
+        cap_curr = pane_2.capture_pane()
+        for line in cap_curr:
+            if "[RUNNING]" in line:
+                print(line)
+                sys.stdout.write("\033[F")
+        if 'Done.' in cap_curr:
+            sys.stdout.write("\033[K")
+            print('Done.') 
+            break
+    print("Saving output ...")
+    pane_3.send_keys('rosservice call /lio_sam_6axis/save_map')
+    time.sleep(10)
+
+    print("PROCESS FINISHED!!")
+    print("Output in: {}/output_loc/map/optimized_odom_tum.txt".format(sequence))
+
+    # TODO: Copy to output folder
+
+    # TODO: Transform to TUM format
+
+
+    server.kill()

spires_ros/fast_lio2/hesai.yaml

@@ -0,0 +1,38 @@
+common:
+    #lid_topic:  "/os_cloud_node/points"
+    #imu_topic:  "/os_cloud_node/imu"
+    lid_topic:  "/hesai/pandar/"
+    imu_topic:  "/alphasense_driver_ros/imu"
+    time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
+    time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
+                                  # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
+
+preprocess:
+    lidar_type: 5                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
+    scan_line: 64
+    timestamp_unit: 3                 # 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+    blind: 4
+
+mapping:
+    acc_cov: 0.1
+    gyr_cov: 0.1
+    b_acc_cov: 0.0001
+    b_gyr_cov: 0.0001
+    fov_degree:    180
+    det_range:     150.0
+    extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic
+    extrinsic_T: [ -0.00459163, -0.00200303, -0.0547992 ]
+    extrinsic_R: [ -0.0106465,  -0.999911, 0.00806769,
+                   0.999941,  -0.0106296, 0.00171369,
+                   -0.00166849, 0.00808546,  0.999966]
+
+publish:
+    path_en:  false
+    scan_publish_en:  true       # false: close all the point cloud output
+    dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
+    scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
+
+pcd_save:
+    pcd_save_en: false
+    interval: 10                 # how many LiDAR frames saved in each pcd file; 
+                                 # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

spires_ros/fast_lio2/launch_osd/2024-03-14_blenhein_1.launch

@@ -0,0 +1,27 @@
+<launch>
+<!-- Launch file for ouster OS2-64 LiDAR -->
+
+    <arg name="rviz" default="true" />
+
+    <rosparam command="load" file="$(find fast_lio)/config/hesai.yaml" />
+
+    <param name="feature_extract_enable" type="bool" value="0"/>
+    <param name="point_filter_num" type="int" value="4"/>
+    <param name="max_iteration" type="int" value="3" />
+    <param name="filter_size_surf" type="double" value="0.5" />
+    <param name="filter_size_map" type="double" value="0.5" />
+    <param name="cube_side_length" type="double" value="1000" />
+    <param name="runtime_pos_log_enable" type="bool" value="0" />
+    <node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" /> 
+
+    <arg name="path" default="/home/oxford_spires_dataset/data/2024-03-14_blenhein_1/rosbag/"/>
+    <arg name="output" default="/home/oxford_spires_dataset/data/2024-03-14_blenhein_1/output_loc/Fast-LIO2.bag"/>
+
+    <node pkg="rosbag" type="record" name="rosbagrecord" args="record -O $(arg output) /Odometry" />
+    <!-- <node pkg="rosbag" type="play" name="rosbagplay" args="$(arg path)$(arg bag_file_1)" required="true" /> -->
+
+    <!-- <group if="$(arg rviz)">
+    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
+    </group> -->
+
+</launch>