Merge occupancy grid and obstacle tracking impl to main

.vscode/c_cpp_properties.json

@@ -0,0 +1,24 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/opt/ros/humble/include/**",
+        "/root/ws/src/ackermann_kf/go2_driver/include/**",
+        "/usr/include/**",
+        "${workspaceFolder}/**",
+        "/usr/include/opencv4",
+        "/root/ws/src/cev_msgs/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

CMakeLists.txt

@@ -10,46 +10,112 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(cev_msgs REQUIRED)
+find_package(PCL REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(pcl_ros REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 
-
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Obstacle.msg"
   "msg/ObstacleArray.msg"
-  DEPENDENCIES std_msgs geometry_msgs
+  DEPENDENCIES std_msgs geometry_msgs cev_msgs nav_msgs
 )
 
 add_executable(obstacle_node src/obstacle_node.cpp)
+add_executable(occupancy_grid src/occupancy_grid.cpp)
+add_executable(obstacle_tracker src/obstacle_tracker.cpp)
 
 ament_target_dependencies(
   obstacle_node 
   rclcpp 
+  cev_msgs
   sensor_msgs 
   std_msgs 
+  pcl_conversions
+  pcl_ros
+  geometry_msgs
+  tf2
+  tf2_geometry_msgs
+  visualization_msgs
+)
+
+ament_target_dependencies(
+  occupancy_grid
+  rclcpp 
+  cev_msgs
+  sensor_msgs 
+  std_msgs
+  nav_msgs
+  pcl_conversions
+  pcl_ros
+  geometry_msgs
+  tf2
+  tf2_geometry_msgs
+  visualization_msgs
+)
+
+ament_target_dependencies(
+  obstacle_tracker
+  rclcpp 
+  cev_msgs
+  sensor_msgs 
+  std_msgs
+  nav_msgs
+  pcl_conversions
+  pcl_ros
   geometry_msgs
   tf2
   tf2_geometry_msgs
   visualization_msgs
 )
 
+set(CEV_ICP_INCLUDE lib/cev_icp/include)
+set(CEV_ICP_LIB lib/cev_icp/lib)
+
+include_directories(${CEV_ICP_INCLUDE})
+link_directories(${CEV_ICP_LIB})
+include_directories(
+  ${PCL_INCLUDE_DIRS}
+)
 
 target_link_libraries(obstacle_node
   ${OpenCV_LIBS}
+  ${PCL_LIBRARIES}
 )
 
-ament_export_dependencies(rosidl_default_runtime)
+target_link_libraries(occupancy_grid
+  ${OpenCV_LIBS}
+  ${PCL_LIBRARIES}
+)
 
+target_link_libraries(obstacle_tracker
+  ${OpenCV_LIBS}
+  ${PCL_LIBRARIES}
+  cevicp
+)
+
+ament_export_dependencies(rosidl_default_runtime)
 
 rosidl_target_interfaces(obstacle_node
   ${PROJECT_NAME} "rosidl_typesupport_cpp")
 
+rosidl_target_interfaces(occupancy_grid
+  ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+rosidl_target_interfaces(obstacle_tracker
+  ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
 install(TARGETS
   obstacle_node
+  occupancy_grid
+  obstacle_tracker
   DESTINATION lib/${PROJECT_NAME})
 
 

README.md

@@ -1 +1,21 @@
 # Obstacle_node
+Hi all, I'm going to start yapping again!
+
+## Angle Bins and Ray Tracing Via Bresenham's Line Algo
+
+![Occupancy Grid 1](demos/occ.png)
+![Occupancy Grid 2](demos/occupancygrid.png)
+
+## Multi-Hypothesis Tracking
+```
+cd Obstacle_node
+git submodule add https://github.com/cornellev/icp.git lib/cev_icp
+git submodule update --init --recursive
+cd lib/cev_icp
+sudo make install LIB_INSTALL=/usr/local/lib HEADER_INSTALL=/usr/local/include
+```
+
+# Important Links
+[Rasterize a point cloud](https://r-lidar.github.io/lasR/reference/rasterize.html)
+
+[Dynamic Obstacle Detection and Tracking Based on 3D Lidar](https://www.jstage.jst.go.jp/article/jaciii/22/5/22_602/_pdf)

overlay/icp.h

@@ -0,0 +1,180 @@
+/**
+ * @author Ethan Uppal
+ * @copyright Copyright (C) 2024 Ethan Uppal.
+ * Copyright (C) 2025 Cornell Electric Vehicles.
+ * SPDX-License-Identifier: MIT
+ */
+
+#pragma once
+
+#include <cmath>
+#include <vector>
+#include <memory>
+#include <optional>
+#include <Eigen/Core>
+
+#include "geo.h"
+#include "config.h"
+
+namespace icp {
+
+    /**
+     * Interface for iterative closest points.
+     * Generally, you should interact with ICP instances through this interface or `ICPDriver`,
+     * though interacting with implementations directly isn't explicitly disallowed.
+     * Read \ref write_icp_instance for additional information.
+     *
+     * \par Example
+     * @code
+     * // Construct a new vanilla ICP instance.
+     * std::unique_ptr<icp::ICP> icp = icp::ICP::from_method("vanilla");
+     * @endcode
+     *
+     * \par Usage
+     * Let `a` and `b` be two point clouds of type `std::vector<Vector>`.
+     * Then, given an ICP instance `icp` of type `std::unique_ptr<icp::ICP>`,
+     * perform the following steps.
+     *
+     * 1. Call `icp->begin(a, b, initial_guess)`.
+     * 2. Repeatedly call `icp->iterate()` until convergence. `ICPDriver` can also be used to
+     * specify convergence conditions.
+     *
+     * If these steps are not followed as described here, the behavior is
+     * undefined.
+     *
+     * At any point in the process, you may query `icp->calculate_cost()` and
+     * `icp->transform()`. Do note, however, that `icp->calculate_cost()` is not
+     * a constant-time operation.
+     */
+    template<const Dimension Dim>
+    class ICP {
+    private:
+        using MethodConstructor = std::function<std::unique_ptr<ICP<Dim>>(const Config&)>;
+        static std::unordered_map<std::string, MethodConstructor> methods;
+
+    protected:
+        using Vector = icp::Vector<Dim>;
+        using RBTransform = icp::RBTransform<Dim>;
+        using PointCloud = icp::PointCloud<Dim>;
+
+        /** A matching between `point` and `pair` at (arbitrary) cost `cost`.  */
+        struct Match {
+            /** An index into the source point cloud. */
+            Eigen::Index point;
+
+            /** An index into the destination point cloud. */
+            Eigen::Index pair;
+
+            /** The (arbitrary) cost of the pair. */
+            double cost;
+        };
+
+        /** The current point cloud transformation that is being optimized. */
+        RBTransform transform = RBTransform::Identity();
+
+        /** The source point cloud. */
+        PointCloud a;
+
+        /** The destination point cloud. */
+        PointCloud b;
+
+        /** The pairing of each point in `a` to its closest in `b`. */
+        std::vector<Match> matches;
+
+        ICP(): a(Dim, 0), b(Dim, 0) {}
+
+        /**
+         * @brief Per-method setup code.
+         *
+         * @post For implementers: must fill `matches` with match data for the initial point
+         * clouds.
+         */
+        virtual void setup() = 0;
+
+    public:
+        static std::optional<std::unique_ptr<ICP<Dim>>> from_method(const std::string& name,
+            const Config& config) {
+            if (methods.find(name) == methods.end()) {
+                return {};
+            }
+
+            return methods[name](config);
+        }
+
+        static bool is_method_registered(const std::string& name) {
+            return methods.find(name) != methods.end();
+        }
+
+        static std::vector<std::string> registered_methods() {
+            std::vector<std::string> keys;
+            for (auto it = methods.begin(); it != methods.end(); ++it) {
+                keys.push_back(it->first);
+            }
+            return keys;
+        }
+
+        virtual ~ICP() = default;
+
+        /** Begins the ICP process for point clouds `source` and `target` with an initial
+         * guess for the transform `guess`.*/
+        void begin(const PointCloud& source, const PointCloud& target, const RBTransform& guess) {
+            // Initial transform guess
+            this->transform = guess;
+
+            // Copy in point clouds
+            this->a = source;
+            this->b = target;
+
+            // Ensure arrays are the right size
+            matches.resize(this->a.cols());
+
+            // Per-instance customization routine
+            setup();
+        }
+
+        /** Perform one iteration of ICP for the point clouds `a` and `b`
+         * provided with ICP::begin.
+         *
+         * @pre ICP::begin must have been invoked.
+         * @post For implementers: must fill `matches` with newest match data.
+         */
+        virtual void iterate() = 0;
+
+        /**
+         * Computes the cost of the current transformation.
+         *
+         * \par Efficiency:
+         * `O(a.size())` where `a` is the source point cloud.
+         */
+        [[nodiscard]] virtual double calculate_cost() const {
+            double sum_squares = 0.0;
+            for (auto& match: matches) {
+                sum_squares += match.cost;
+            }
+            return std::sqrt(sum_squares / a.cols());
+        }
+
+        /** The current transform. */
+        [[nodiscard]] RBTransform current_transform() const {
+            return transform;
+        }
+
+        /**
+         * @brief Gets the current point matching computed by ICP.
+         *
+         * @return A reference to the matching. Invalidates if `begin` or `iterate` are called.
+         */
+        [[nodiscard]] const std::vector<Match>& get_matches() const {
+            return matches;
+        }
+    };
+
+    using ICP2 = ICP<Dimension::TwoD>;
+    using ICP3 = ICP<Dimension::ThreeD>;
+
+    template<>
+    std::unordered_map<std::string, ICP2::MethodConstructor> ICP2::methods;
+
+    template<>
+    std::unordered_map<std::string, ICP3::MethodConstructor> ICP3::methods;
+}