Mig dev (#15)

ori-drs · Nov 8, 2024 · 013585e · 013585e
2 parents fe360f9 + bb75488
commit 013585e
Show file tree

Hide file tree

Showing 16 changed files with 1,182 additions and 413 deletions.
diff --git a/.docker_loc/Dockerfile b/.docker_loc/Dockerfile
@@ -41,25 +41,6 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
     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 \
@@ -76,15 +57,49 @@ RUN apt-get update && apt-get install --no-install-recommends -y \
     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
-
+######################
+# Install SC-LIO-SAM #
+######################
+# 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
 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
+# Install ros packages
+RUN git clone https://github.com/ori-drs/SC-LIO-SAM.git ${CATKIN_WS_DIR}/src/SC-LIO-SAM
+RUN cd ${CATKIN_WS_DIR} && catkin build lio_sam
 
-RUN cd ${CATKIN_WS_DIR} && catkin build lio_sam_6axis
+#########################
+# Install Fast-LIO-SLAM #
+#########################
+# Install fast_lio dependences
+RUN git clone https://github.com/Livox-SDK/livox_ros_driver ${CATKIN_WS_DIR}/src/livox_ros_driver
+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
+RUN git clone https://ceres-solver.googlesource.com/ceres-solver -b 2.0.0 ${CATKIN_WS_DIR}/src/ceres-solver
+RUN apt-get install libgoogle-glog-dev libgflags-dev
+RUN apt-get install libatlas-base-dev -y
+RUN apt-get install libeigen3-dev
+RUN cd ${CATKIN_WS_DIR}/src/ceres-solver/ && mkdir build && cd build && cmake .. && make -j3 && make install
+# Install ros packages
+RUN git clone https://github.com/ori-drs/FAST_LIO_SLAM.git ${CATKIN_WS_DIR}/src/FAST_LIO_SLAM
+RUN cd ${CATKIN_WS_DIR}/src/FAST_LIO_SLAM && git submodule update --init
+RUN cd ${CATKIN_WS_DIR} && catkin build livox_ros_driver
+RUN cd ${CATKIN_WS_DIR} && catkin build fast_lio
+RUN cd ${CATKIN_WS_DIR} && catkin build aloam_velodyne
+
+###############
+# Install HBA #
+###############
+# Install ros packages
+RUN git clone https://github.com/ori-drs/HBA.git ${CATKIN_WS_DIR}/src/HBA
+RUN cd ${CATKIN_WS_DIR} && catkin build hba
 
 # Trajectory evaluation dependences
 RUN apt-get update
@@ -103,11 +118,6 @@ 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 .
 

diff --git a/oxford_spires_utils/bash_command.py b/oxford_spires_utils/bash_command.py
@@ -18,13 +18,16 @@ def print_with_colour(text, colour=TerminalColors.CYAN):
     print(f"{colour}{text}{TerminalColors.ENDC}")
 
 
-def run_command(cmd, log_path=None, print_command=True):
+def run_command(cmd, log_path=None, print_command=True, print_output=True):
     if print_command:
         print_with_colour(f"Running command: {cmd}")
     process = subprocess.Popen(cmd, stdout=subprocess.PIPE, shell=True, universal_newlines=True)
-    for line in process.stdout:
-        print(line, end="")
-        if log_path is not None:
-            assert isinstance(log_path, (Path, str))
-            with open(log_path, "a") as f:
-                f.write(line)
+    if print_output:
+        for line in process.stdout:
+            print(line, end="")
+            if log_path is not None:
+                assert isinstance(log_path, (Path, str))
+                with open(log_path, "a") as f:
+                    f.write(line)
+    else:
+        return process
diff --git a/oxford_spires_utils/trajectory/file_interfaces/vilens_slam.py b/oxford_spires_utils/trajectory/file_interfaces/vilens_slam.py
@@ -1,8 +1,10 @@
+import os
+
 import evo
 import numpy as np
 from evo.tools.file_interface import csv_read_matrix
 
-from .base import BasicTrajReader
+from .base import BasicTrajReader, BasicTrajWriter
 from .timestamp import TimeStamp
 
 
@@ -28,3 +30,43 @@ def read_file(self):
         quat_xyzw = mat[:, 6:10]
         quat_wxyz = np.roll(quat_xyzw, 1, axis=1)  # xyzw -> wxyz
         return evo.core.trajectory.PoseTrajectory3D(xyz, quat_wxyz, timestamps=timestamps)
+
+class VilensSlamTrajWriter(BasicTrajWriter):
+    """
+    write VILENS SLAM trajectory format file (poses.csv)
+    """
+
+    def __init__(self, file_path, **kwargs):
+        super().__init__(file_path)
+
+    def write_file(self, pose):
+        """
+        this is used for CSV style trajectory file 
+        @param self.file_path: path to save the file
+        @param pose: PoseTrajectory3D from evo
+        """
+
+        if not isinstance(pose, evo.core.trajectory.PoseTrajectory3D):
+            raise ValueError("pose should be PoseTrajectory3D from evo")
+        if not os.path.exists(os.path.dirname(self.file_path)):
+            os.makedirs(os.path.dirname(self.file_path))
+
+        assert pose.check()[0] is True, pose.check()[1]
+
+        print("Writing pose message in CSV format to: ", self.file_path)
+        with open(self.file_path, "w") as f:
+            f.writelines("# counter, sec, nsec, x, y, z, qx, qy, qz, qw\n")
+            for i in range(pose.num_poses):
+                f.write(str(i) + ", ")
+                timestamp_str = TimeStamp(t_float128=pose.timestamps[i]).t_string_sec
+                f.write(timestamp_str + ", ")
+                timestamp_str = TimeStamp(t_float128=pose.timestamps[i]).t_string_nsec
+                f.write(timestamp_str + ", ")
+                f.write(str(pose.positions_xyz[i][0]) + ", ")
+                f.write(str(pose.positions_xyz[i][1]) + ", ")
+                f.write(str(pose.positions_xyz[i][2]) + ", ")
+                f.write(str(pose.orientations_quat_wxyz[i][1]) + ", ")
+                f.write(str(pose.orientations_quat_wxyz[i][2]) + ", ")
+                f.write(str(pose.orientations_quat_wxyz[i][3]) + ", ")
+                f.write(str(pose.orientations_quat_wxyz[i][0]))
+                f.writelines("\n")