Merge pull request #22 from cornellev/interface_improvement

Added ICP Driver

.vscode/launch.json

@@ -13,7 +13,8 @@
                 "ex_data/scan1/second.conf",
                 "--method",
                 "vanilla",
-                "--gui"
+                // "--gui",
+                "--bench"
             ],
             "stopAtEntry": false,
             "cwd": "${workspaceFolder}",

.vscode/settings.json

@@ -109,6 +109,7 @@
         "text_encoding": "cpp",
         "strstream": "cpp",
         "source_location": "cpp",
-        "stdfloat": "cpp"
+        "stdfloat": "cpp",
+        "dense": "cpp"
     }
 }

Makefile

@@ -85,7 +85,7 @@ $(TESTNAME): $(TESTOBJ) $(LIBNAME)
 
 .PHONY: clean
 clean:
-	@rm -f $(LIBOBJ) $(LIBDEPS) $(LIBNAME) $(MAINOBJ) $(MAINDEPS) $(MAINNAME)
+	@rm -f $(LIBOBJ) $(LIBDEPS) $(LIBNAME) $(MAINOBJ) $(MAINDEPS) $(MAINNAME) $(TESTOBJ) $(TESTDEPS) $(TESTNAME)
 
 .PHONY: view
 view: $(MAINNAME)

include/icp/driver.h

@@ -0,0 +1,106 @@
+#pragma once
+
+#include <optional>
+#include "icp.h"
+
+namespace icp {
+    class ICPDriver {
+    public:
+        /** The result of running `ICPDriver::converge`. */
+        struct ConvergenceState {
+            /** The cost achieved. */
+            double cost;
+
+            /** The number of iterations performed. */
+            size_t iteration_count;
+
+            /** The transform. */
+            RBTransform transform;
+        };
+
+        /**
+         * @brief Constructs a new ICPDriver using the given ICP method.
+         *
+         * @param icp The ICP method to use.
+         */
+        ICPDriver(std::unique_ptr<ICP> icp);
+
+        /**
+         * @brief Runs ICP to convergence based on the termination conditions set.
+         *
+         * @param a The source point cloud.
+         * @param b The destination point cloud.
+         * @param t The initial guess for the transformation.
+         * @return ConvergenceState
+         */
+        ConvergenceState converge(const std::vector<Vector>& a, const std::vector<Vector>& b,
+            RBTransform t);
+
+        /**
+         * @brief Sets the minimum number of iterations to run.
+         *
+         * @param min_iterations The minimum number of iterations to run.
+         */
+        void set_min_iterations(uint64_t min_iterations);
+
+        /**
+         * @brief Sets the maximum number of iterations to run.
+         *
+         * @param max_iterations The maximum number of iterations to run.
+         */
+        void set_max_iterations(uint64_t max_iterations);
+
+        /**
+         * @brief Sets the cost at which to stop ICP. `converge` will return when a cost below
+         * `stop_cost` is achieved.
+         *
+         * @param stop_cost The cost at which to stop ICP.
+         */
+        void set_stop_cost(double stop_cost);
+
+        // TODO: fix docs to use math once doxygen is fixed
+        /**
+         * @brief Sets the relative cost tolerance. `converge` will return when the cost
+         * changes by less than this fraction of the current cost, i.e. when |`current_cost` -
+         * `prev_cost`| / < `relative_cost_tolerance`.
+         *
+         * @param relative_cost_tolerance The relative cost tolerance.
+         */
+        void set_relative_cost_tolerance(double relative_cost_tolerance);
+
+        /**
+         * @brief Set the absolute cost tolerance. `converge` will return when the cost changes by
+         * less than this amount, i.e. when |`current_cost` - `prev_cost`|  <
+         * `absolute_cost_tolerance`.
+         *
+         * @param absolute_cost_tolerance The absolute cost tolerance.
+         */
+        void set_absolute_cost_tolerance(double absolute_cost_tolerance);
+
+        /**
+         * @brief Set the transform tolerance. `converge` will return when both the angle and
+         * translation tolerances are met.
+         *
+         * @param angle_tolerance The angle tolerance in radians. The tolerance is met when the
+         * angle of rotation between the current and previous transformation around the axis of
+         * rotation is less than `angle_tolerance`.
+         * @param translation_tolerance The translation tolerance in scan units. The tolerance is
+         * met when the change in translation is less than `translation_tolerance`.
+         */
+        void set_transform_tolerance(double angle_tolerance, double translation_tolerance);
+
+    private:
+        bool should_terminate(ConvergenceState current_state,
+            std::optional<ConvergenceState> last_state);
+
+        std::unique_ptr<ICP> icp_;
+
+        std::optional<uint64_t> min_iterations_;
+        std::optional<uint64_t> max_iterations_;
+        std::optional<double> stop_cost_;
+        std::optional<double> relative_cost_tolerance_;
+        std::optional<double> absolute_cost_tolerance_;
+        std::optional<double> angle_tolerance_rad_;
+        std::optional<double> translation_tolerance_;
+    };
+}

include/icp/icp.h

@@ -34,8 +34,8 @@ namespace icp {
      * perform the following steps.
      *
      * 1. Call `icp->begin(a, b, initial_guess)`.
-     * 2. Call either `icp->converge(convergence_threshold)` or repeatedly
-     * `icp->iterate()`.
+     * 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.
@@ -53,8 +53,7 @@ namespace icp {
             double cost;
         };
 
-        /** The current point cloud transformation that is being optimized.
-         */
+        /** The current point cloud transformation that is being optimized. */
         RBTransform transform;
 
         /** The source point cloud. */
@@ -63,31 +62,19 @@ namespace icp {
         /** The destination point cloud. */
         std::vector<Vector> b;
 
-        /** Keeps track of the previous cost to ensure that progress is being
-         * made. @see ICP::current_cost. */
-        double previous_cost;
-
-        /** The RMS (root mean square) cost of the current transformation. */
-        double current_cost;
-
         /** The pairing of each point in `a` to its closest in `b`. */
         std::vector<Match> matches;
 
         ICP();
 
-        virtual void setup();
+        /**
+         * @brief Per-method setup code.
+         *
+         * @post For implementers: must fill `matches` with match data for the initial point clouds.
+         */
+        virtual void setup() = 0;
 
     public:
-        /** The result of running `ICP::converge`. */
-        struct ConvergenceReport {
-            /** The least cost achieved. */
-            double final_cost;
-
-            /** The number of iterations performed, including the burn-in
-             * period. */
-            size_t iteration_count;
-        };
-
         /** Configuration for ICP instances. */
         class Config {
             using Param = std::variant<int, double, std::string>;
@@ -118,13 +105,15 @@ namespace icp {
         virtual ~ICP() = default;
 
         /** Begins the ICP process for point clouds `a` and `b` with an initial
-         * guess for the transform `t`. */
+         * guess for the transform `t`.*/
         void begin(const std::vector<Vector>& a, const std::vector<Vector>& b, RBTransform t);
 
         /** Perform one iteration of ICP for the point clouds `a` and `b`
          * provided with ICP::begin.
          *
-         * @pre ICP::begin must have been invoked. */
+         * @pre ICP::begin must have been invoked.
+         * @post For implementers: must fill `matches` with newest match data.
+         */
         virtual void iterate() = 0;
 
         /**
@@ -135,23 +124,11 @@ namespace icp {
          */
         double calculate_cost() const;
 
-        /**
-         * Perform ICP for the point clouds `a` and `b` provided with ICP::begin
-         * until the cost is below `convergence_threshold` or until no progress
-         * is being made. At least `burn_in` iterations will be performed. Start
-         * with zero burn-in, and slowly increase if convergence requirements
-         * are not met.
-         *
-         * @returns Information about the convergence.
-         * @pre ICP::begin must have been invoked.
-         */
-        ConvergenceReport converge(size_t burn_in, double convergence_threshold);
-
         /** The current transform. */
         const RBTransform& current_transform() const;
 
-        /** Registers methods built into libcevicp. Must be called before constructing ICP instances
-         * for built-in methods. */
+        /** Registers methods built into `libcevicp`. Must be called before constructing ICP
+         * instances for built-in methods. */
         static void register_builtin_methods();
 
         /** Registers a new ICP method that can be created with `constructor`,

include/icp/impl/feature_aware.h

@@ -10,14 +10,16 @@ namespace icp {
         using FeatureVector = Eigen::VectorXd;
 
     public:
-        FeatureAware(double feature_weight, int symmetric_neighbors);
+        FeatureAware(double overlap_rate, double feature_weight, int symmetric_neighbors);
         FeatureAware(const Config& config);
         ~FeatureAware();
 
         void setup() override;
         void iterate() override;
 
     private:
+        void compute_matches();
+
         void compute_features(const std::vector<icp::Vector>& points, Vector cm,
             std::vector<FeatureVector>& features);
 
@@ -49,6 +51,7 @@ namespace icp {
 
         Eigen::MatrixXd norm_feature_dists;
 
+        double overlap_rate;
         int symmetric_neighbors;
         double feature_weight;
         double neighbor_weight;

include/icp/impl/trimmed.h

@@ -15,6 +15,8 @@ namespace icp {
         void iterate() override;
 
     private:
+        void compute_matches();
+
         double overlap_rate;
         std::vector<icp::Vector> a_current;
         icp::Vector b_cm;

include/icp/impl/vanilla.h

@@ -17,6 +17,8 @@ namespace icp {
         void iterate() override;
 
     private:
+        void compute_matches();
+
         std::vector<icp::Vector> a_current;
         icp::Vector b_cm;
     };

lib/icp/driver.cpp

@@ -0,0 +1,106 @@
+#include "icp/driver.h"
+#include <limits>
+
+namespace icp {
+    ICPDriver::ICPDriver(std::unique_ptr<ICP> icp) {
+        icp_ = std::move(icp);
+    }
+
+    ICPDriver::ConvergenceState ICPDriver::converge(const std::vector<Vector>& a,
+        const std::vector<Vector>& b, RBTransform t) {
+        icp_->begin(a, b, t);
+
+        ConvergenceState state{};
+        state.iteration_count = 0;
+        state.cost = icp_->calculate_cost();
+        state.transform = icp_->current_transform();
+
+        std::optional<ConvergenceState> last_state{};
+
+        while (!should_terminate(state, last_state)) {
+            icp_->iterate();
+            last_state = state;
+            state.iteration_count++;
+            state.cost = icp_->calculate_cost();
+            state.transform = icp_->current_transform();
+        }
+
+        return state;
+    }
+
+    bool ICPDriver::should_terminate(ConvergenceState current_state,
+        std::optional<ConvergenceState> last_state) {
+        // absolute conditions based only on current state
+        if (stop_cost_ && current_state.cost < stop_cost_.value()) {
+            return true;
+        }
+
+        if (min_iterations_ && current_state.iteration_count < min_iterations_.value()) {
+            return false;
+        }
+
+        if (max_iterations_ && current_state.iteration_count >= max_iterations_.value()) {
+            return true;
+        }
+
+        // end if we don't have a last state
+        if (!last_state) {
+            return false;
+        }
+
+        // relative conditions based on progress
+        double delta_cost = current_state.cost - last_state.value().cost;
+        if (absolute_cost_tolerance_ && std::abs(delta_cost) < absolute_cost_tolerance_.value()) {
+            return true;
+        }
+
+        double relative_cost_change = std::abs(delta_cost) / current_state.cost;
+        if (relative_cost_tolerance_ && relative_cost_change < relative_cost_tolerance_.value()) {
+            return true;
+        }
+
+        if (angle_tolerance_rad_ && translation_tolerance_) {
+            icp::Vector prev_rot_vector = last_state.value().transform.rotation * icp::Vector(1, 0);
+            icp::Vector current_rot_vector = current_state.transform.rotation * icp::Vector(1, 0);
+            double dot = prev_rot_vector.dot(current_rot_vector);
+            double angle = std::acos(std::clamp(dot, -1.0, 1.0));
+
+            auto translation = current_state.transform.translation
+                               - last_state.value().transform.translation;
+
+            if (angle < angle_tolerance_rad_.value()
+                && translation.norm() < translation_tolerance_.value()) {
+                return true;
+            }
+        }
+
+        return false;
+    }
+
+    void ICPDriver::set_min_iterations(uint64_t min_iterations) {
+        assert(!max_iterations_ || min_iterations <= max_iterations_.value());
+        min_iterations_ = min_iterations;
+    }
+
+    void ICPDriver::set_max_iterations(uint64_t max_iterations) {
+        assert(!min_iterations_ || max_iterations >= min_iterations_.value());
+        max_iterations_ = max_iterations;
+    }
+
+    void ICPDriver::set_stop_cost(double stop_cost) {
+        stop_cost_ = stop_cost;
+    }
+
+    void ICPDriver::set_relative_cost_tolerance(double relative_cost_tolerance) {
+        relative_cost_tolerance_ = relative_cost_tolerance;
+    }
+
+    void ICPDriver::set_absolute_cost_tolerance(double absolute_cost_tolerance) {
+        absolute_cost_tolerance_ = absolute_cost_tolerance;
+    }
+
+    void ICPDriver::set_transform_tolerance(double angle_tolerance, double translation_tolerance) {
+        angle_tolerance_rad_ = angle_tolerance;
+        translation_tolerance_ = translation_tolerance;
+    }
+}