Added docker functionality. Added removal of NaNs for Ouster Lidar.

README.md

@@ -27,7 +27,9 @@ All experiments, including both simulation and real-world experiments in paper a
 - PCL 
 
 ## Build
-```
+
+### Local
+``` bash
 mkdir -p /workspace/src
 cd /workspace/src
 git clone https://github.com/M-Evanovic/LL-Localizer.git
@@ -36,6 +38,32 @@ catkin build
 source devel/setup.bash
 ```
 
+### Docker
+<details>
+  <summary>Docker details (click to expand)</summary>
+
+  > Docker setup was tested on Ubuntu 24.04.
+
+  Need to give docker permission to use gui. Run the following on your host machine.
+
+  ``` bash
+  xhost +local:docker
+  ```
+
+  Clone repo, build docker image, and start container.
+  ``` bash
+  git clone https://github.com/M-Evanovic/LL-Localizer.git
+  cd LL-Localizer
+  ./docker/run_docker.sh
+  ```
+
+  In a new terminal, enter the container so you can launch ll_localizer in one and play a rosbag in another.
+  > Make sure to change the volume path for your bags in the `docker/compose.yaml` file to match the path to your data.
+  ```bash
+  docker exec -it ll_localizer bash
+  ```
+</details>
+
 ## Run SLAM Mode
 - Set the path ```/localization_param/map_file_name``` to ```" "``` in ```/config/config.yaml```. When no prior map is available, the system automatically switches to SLAM mode.
 - Derectly launch ll_localizer:
@@ -59,6 +87,23 @@ roslaunch ll_localizer localizer_velodyne.launch
 rosbag play yourbag.bag
 ```
 
+## Debug
+<details>
+  <summary><strong>Debug Ouster</strong></summary>
+
+  - If you get an error when running an ouster saying it can't find the 'ring' field, it's likely because you have a newer lidar. The old lidar's ring fild was a `uint8_t` but the newer ones are `uint16_t`. This change will need to be made in `pointcloud_preprocess.h` on lines 34 and 50.
+
+</details>
+
+<details>
+  <summary><strong>Immediate Crash</strong></summary>
+
+  - If ll_localizer crashes right after you start playing your rosbag, your lidar points might contain NaN values. Remove NaN values from data before processing. This was implemented for the ouster lidar on lines 144-146 of `pointcloud_preprocess.cc`. Similar should be possible with the other lidar types.
+
+</details>
+
+
+
 ## Datasets
 - [NCLT](https://robots.engin.umich.edu/nclt/): The University of Michigan North Campus Long-Term Vision and LIDAR Dataset
 - [M2DGR](https://github.com/SJTU-ViSYS/M2DGR): A Multi-modal and Multi-scenario SLAM Dataset for Ground Robots

docker/compose.yaml

@@ -0,0 +1,23 @@
+services:
+  ll_localizer:
+    build:
+      context: ..
+      dockerfile: docker/dockerfile
+    image: ll_localizer
+    container_name: ll_localizer
+    volumes:
+      - ../:/root/catkin_ws/src/LL-Localizer # Volumes in entire LL-Localizer folder to allow customization
+      - ${HOME}/data_byu/bags/longterm_mapping/ros1_bags:/bags # Change to match location of bag data
+      - /tmp/.X11-unix:/tmp/.X11-unix # Needed to display RViz within the container
+    environment:
+      DISPLAY: "${DISPLAY}"
+      DISABLE_ROS1_EOL_WARNINGS: 1
+      NVIDIA_VISIBLE_DEVICES: all
+      NVIDIA_DRIVER_CAPABILITIES: all
+    runtime: nvidia
+    working_dir: /root/catkin_ws
+    network_mode: host
+    stdin_open: true
+    tty: true
+    # command: ["/ros_entrypoint.sh", "roscore"]
+

docker/dockerfile

@@ -0,0 +1,40 @@
+# Start from a ROS Noetic base image.
+FROM ros:noetic
+
+# Install common build tools and necessary dependencies
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    git \
+    libgoogle-glog-dev \
+    libeigen3-dev \
+    libpcl-dev \
+    libtbb-dev \
+    python3-catkin-tools \
+    && rm -rf /var/lib/apt/lists/*
+
+# --- Install ROS Dependencies ---
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    ros-noetic-catkin \
+    ros-noetic-rviz \
+    ros-noetic-pcl-ros \
+    ros-noetic-eigen-conversions \
+    pcl-tools \
+    && rm -rf /var/lib/apt/lists/*
+
+# --- Copy in LL-Localizer Source Code ---
+# Set the working directory
+WORKDIR /root/catkin_ws/src/
+
+# Copy your project source code into the Docker image
+COPY . ./LL-Localizer
+
+WORKDIR /root/catkin_ws
+# Build the ROS workspace
+RUN /bin/bash -c "source /opt/ros/noetic/setup.bash && \
+    catkin_make -j6 --pkg ll_localizer"
+
+
+RUN echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc && \
+    echo "if [ -f ~/catkin_ws/devel/setup.bash ]; then" >> ~/.bashrc && \
+    echo "  source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc && \
+    echo "fi" >> ~/.bashrc
+

docker/run_docker.sh

@@ -0,0 +1,17 @@
+#!/bin/bash
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+
+# Allow local docker access to X server
+xhost +local:docker
+
+# Ensure access is revoked on script exit
+trap 'xhost -local:docker' EXIT
+
+# Run script in the directory of the script
+cd "$SCRIPT_DIR"
+
+docker compose -f compose.yaml -p ll_localizer up -d
+
+docker exec -it ll_localizer /bin/bash
+

include/pointcloud_preprocess.h

@@ -31,7 +31,7 @@ struct EIGEN_ALIGN16 Point {
     float intensity;
     uint32_t t;
     uint16_t reflectivity;
-    uint8_t ring;
+    uint16_t ring;
     uint16_t ambient;
     uint32_t range;
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -47,7 +47,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
                                       // use std::uint32_t to avoid conflicting with pcl::uint32_t
                                   (std::uint32_t, t, t)
                                   (std::uint16_t, reflectivity, reflectivity)
-                                  (std::uint8_t, ring, ring)
+                                  (std::uint16_t, ring, ring)
                                   (std::uint16_t, ambient, ambient)
                                   (std::uint32_t, range, range)
                                   )

src/pointcloud_preprocess.cc

@@ -82,11 +82,69 @@ void PointCloudPreprocess::AviaHandler(const livox_ros_driver::CustomMsg::ConstP
     }
 }
 
+// Copied directly from newer version of PCL
+template <typename PointT> void
+removeNaNFromPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+                            pcl::PointCloud<PointT> &cloud_out,
+                            pcl::Indices &index)
+{
+   // If the clouds are not the same, prepare the output
+   if (&cloud_in != &cloud_out)
+   {
+     cloud_out.header = cloud_in.header;
+     cloud_out.resize (cloud_in.size ());
+     cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
+     cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
+   }
+   // Reserve enough space for the indices
+   index.resize (cloud_in.size ());
+
+   // If the data is dense, we don't need to check for NaN
+   if (cloud_in.is_dense)
+   {
+     // Simply copy the data
+     cloud_out = cloud_in;
+     for (std::size_t j = 0; j < cloud_out.size (); ++j)
+       index[j] = j;
+   }
+   else
+   {
+     std::size_t j = 0;
+     for (std::size_t i = 0; i < cloud_in.size (); ++i)
+     {
+       if (!std::isfinite (cloud_in[i].x) ||
+           !std::isfinite (cloud_in[i].y) ||
+           !std::isfinite (cloud_in[i].z))
+         continue;
+       cloud_out[j] = cloud_in[i];
+       index[j] = i;
+       j++;
+     }
+     if (j != cloud_in.size ())
+     {
+       // Resize to the correct size
+       cloud_out.resize (j);
+       index.resize (j);
+     }
+
+     cloud_out.height = 1;
+     cloud_out.width  = static_cast<std::uint32_t>(j);
+
+     // Removing bad points => dense (note: 'dense' doesn't mean 'organized')
+     cloud_out.is_dense = true;
+   }
+}
+
 void PointCloudPreprocess::Oust64Handler(const sensor_msgs::PointCloud2::ConstPtr &msg) {
     cloud_out.clear();
     cloud_full.clear();
     pcl::PointCloud<ouster_ros::Point> pl_orig;
     pcl::fromROSMsg(*msg, pl_orig);
+
+    // Remove NaNs
+    std::vector<int> indices;
+    removeNaNFromPointCloud(pl_orig, pl_orig, indices);
+
     int plsize = pl_orig.size();
     cloud_out.reserve(plsize);