Fix driver

cornellev · Dec 6, 2024 · 4a081da · 4a081da
1 parent 01ec871
commit 4a081da
Show file tree

Hide file tree

Showing 14 changed files with 111 additions and 72 deletions.
diff --git a/.vscode/launch.json b/.vscode/launch.json
@@ -13,7 +13,8 @@
                 "ex_data/scan1/second.conf",
                 "--method",
                 "vanilla",
-                "--gui"
+                // "--gui",
+                "--bench"
             ],
             "stopAtEntry": false,
             "cwd": "${workspaceFolder}",

diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -109,6 +109,7 @@
         "text_encoding": "cpp",
         "strstream": "cpp",
         "source_location": "cpp",
-        "stdfloat": "cpp"
+        "stdfloat": "cpp",
+        "dense": "cpp"
     }
 }
diff --git a/Makefile b/Makefile
@@ -58,7 +58,7 @@ $(TESTNAME): CFLAGS += -DTEST
 -include $(TESTDEPS)
 
 N		:= 3
-METHOD	:= vanilla
+METHOD	:= feature_aware
 
 ifeq ($(shell uname), Darwin)
 AR 		:= /usr/bin/libtool
@@ -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)

diff --git a/include/icp/icp_driver.h → include/icp/driver.h b/include/icp/icp_driver.h → include/icp/driver.h
@@ -39,8 +39,6 @@ namespace icp {
         /**
          * @brief Sets the minimum number of iterations to run.
          *
-         * @note Increases in cost cause immediate termination.
-         *
          * @param min_iterations The minimum number of iterations to run.
          */
         void set_min_iterations(uint64_t min_iterations);
@@ -66,8 +64,6 @@ namespace icp {
          * changes by less than this fraction of the current cost, i.e. when |`current_cost` -
          * `prev_cost`| / < `relative_cost_tolerance`.
          *
-         * @note Increases in cost cause immediate termination.
-         *
          * @param relative_cost_tolerance The relative cost tolerance.
          */
         void set_relative_cost_tolerance(double relative_cost_tolerance);
@@ -77,8 +73,6 @@ namespace icp {
          * less than this amount, i.e. when |`current_cost` - `prev_cost`|  <
          * `absolute_cost_tolerance`.
          *
-         * @note Increases in cost cause immediate termination.
-         *
          * @param absolute_cost_tolerance The absolute cost tolerance.
          */
         void set_absolute_cost_tolerance(double absolute_cost_tolerance);

diff --git a/include/icp/icp.h b/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.
@@ -67,7 +67,12 @@ namespace icp {
 
         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:
         /** Configuration for ICP instances. */
@@ -100,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;
 
         /**

diff --git a/include/icp/impl/feature_aware.h b/include/icp/impl/feature_aware.h
@@ -18,6 +18,8 @@ namespace icp {
         void iterate() override;
 
     private:
+        void compute_matches();
+
         void compute_features(const std::vector<icp::Vector>& points, Vector cm,
             std::vector<FeatureVector>& features);
 

diff --git a/include/icp/impl/trimmed.h b/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;

diff --git a/include/icp/impl/vanilla.h b/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;
     };

diff --git a/lib/icp/icp_driver.cpp → lib/icp/driver.cpp b/lib/icp/icp_driver.cpp → lib/icp/driver.cpp
@@ -1,4 +1,4 @@
-#include "icp/icp_driver.h"
+#include "icp/driver.h"
 #include <limits>
 
 namespace icp {
@@ -30,6 +30,7 @@ namespace icp {
 
     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;
         }
@@ -42,15 +43,13 @@ namespace icp {
             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 (delta_cost > 0) {
-            return true;
-        }
-
         if (absolute_cost_tolerance_ && std::abs(delta_cost) < absolute_cost_tolerance_.value()) {
             return true;
         }

diff --git a/lib/icp/impl/feature_aware.cpp b/lib/icp/impl/feature_aware.cpp
@@ -30,33 +30,19 @@ namespace icp {
         compute_features(b, b_cm, b_features);
 
         norm_feature_dists = compute_norm_dists(a_features, b_features);
+
+        compute_matches();
     }
 
     void FeatureAware::iterate() {
         const size_t n = a.size();
-        const size_t m = b.size();
 
         for (size_t i = 0; i < n; i++) {
             a_current[i] = transform.apply_to(a[i]);
         }
 
         /* TODO: write smth #step Matching Step: */
-        Eigen::MatrixXd norm_dists = compute_norm_dists(a_current, b);
-
-        for (size_t i = 0; i < n; i++) {
-            matches[i].point = i;
-            matches[i].cost = std::numeric_limits<double>::infinity();
-            for (size_t j = 0; j < m; j++) {
-                double dist = norm_dists(i, j);
-                double feature_dist = norm_feature_dists(i, j);
-                double cost = neighbor_weight * dist + feature_weight * feature_dist;
-
-                if (cost < matches[i].cost) {
-                    matches[i].cost = cost;
-                    matches[i].pair = j;
-                }
-            }
-        }
+        compute_matches();
 
         /*
             #step
@@ -101,6 +87,28 @@ namespace icp {
         transform.translation += trimmed_b_cm - R * trimmed_cm;
     }
 
+    void FeatureAware::compute_matches() {
+        const size_t n = a.size();
+        const size_t m = b.size();
+
+        Eigen::MatrixXd norm_dists = compute_norm_dists(a_current, b);
+
+        for (size_t i = 0; i < n; i++) {
+            matches[i].point = i;
+            matches[i].cost = std::numeric_limits<double>::infinity();
+            for (size_t j = 0; j < m; j++) {
+                double dist = norm_dists(i, j);
+                double feature_dist = norm_feature_dists(i, j);
+                double cost = neighbor_weight * dist + feature_weight * feature_dist;
+
+                if (cost < matches[i].cost) {
+                    matches[i].cost = cost;
+                    matches[i].pair = j;
+                }
+            }
+        }
+    }
+
     void FeatureAware::compute_features(const std::vector<Vector>& points, Vector cm,
         std::vector<FeatureVector>& features) {
         for (size_t i = 0; i < points.size(); i++) {
@@ -127,4 +135,4 @@ namespace icp {
             features[i] = feature;
         }
     }
-}
+}  // namespace icp
diff --git a/lib/icp/impl/trimmed.cpp b/lib/icp/impl/trimmed.cpp
@@ -30,30 +30,19 @@ namespace icp {
     void Trimmed::setup() {
         a_current.resize(a.size());
         b_cm = get_centroid(b);
+
+        compute_matches();
     }
 
     void Trimmed::iterate() {
         const size_t n = a.size();
-        const size_t m = b.size();
 
         for (size_t i = 0; i < n; i++) {
             a_current[i] = transform.apply_to(a[i]);
         }
 
         /* #step Matching Step: see \ref vanilla_icp for details. */
-        for (size_t i = 0; i < n; i++) {
-            matches[i].point = i;
-            matches[i].cost = std::numeric_limits<double>::infinity();
-            for (size_t j = 0; j < m; j++) {
-                // Point-to-point matching
-                double dist_ij = (b[j] - a_current[i]).squaredNorm();
-
-                if (dist_ij < matches[i].cost) {
-                    matches[i].cost = dist_ij;
-                    matches[i].pair = j;
-                }
-            }
-        }
+        compute_matches();
 
         /*
             #step
@@ -102,4 +91,23 @@ namespace icp {
         /* #step Transformation Step: see \ref vanilla_icp for details. */
         transform.translation += trimmed_b_cm - R * trimmed_cm;
     }
+
+    void Trimmed::compute_matches() {
+        const size_t n = a.size();
+        const size_t m = b.size();
+
+        for (size_t i = 0; i < n; i++) {
+            matches[i].point = i;
+            matches[i].cost = std::numeric_limits<double>::infinity();
+            for (size_t j = 0; j < m; j++) {
+                // Point-to-point matching
+                double dist_ij = (b[j] - a_current[i]).squaredNorm();
+
+                if (dist_ij < matches[i].cost) {
+                    matches[i].cost = dist_ij;
+                    matches[i].pair = j;
+                }
+            }
+        }
+    }
 }
diff --git a/lib/icp/impl/vanilla.cpp b/lib/icp/impl/vanilla.cpp
@@ -24,11 +24,12 @@ namespace icp {
     void Vanilla::setup() {
         a_current.resize(a.size());
         b_cm = get_centroid(b);
+
+        compute_matches();
     }
 
     void Vanilla::iterate() {
         const size_t n = a.size();
-        const size_t m = b.size();
 
         for (size_t i = 0; i < n; i++) {
             a_current[i] = transform.apply_to(a[i]);
@@ -49,18 +50,7 @@ namespace icp {
             https://courses.cs.duke.edu/spring07/cps296.2/scribe_notes/lecture24.pdf
             -> use k-d tree
          */
-        for (size_t i = 0; i < n; i++) {
-            matches[i].cost = std::numeric_limits<double>::infinity();
-            for (size_t j = 0; j < m; j++) {
-                // Point-to-point matching
-                double dist_ij = (b[j] - a_current[i]).squaredNorm();
-
-                if (dist_ij < matches[i].cost) {
-                    matches[i].cost = dist_ij;
-                    matches[i].pair = j;
-                }
-            }
-        }
+        compute_matches();
 
         /*
             #step
@@ -118,3 +108,21 @@ namespace icp {
         transform.translation += b_cm - R * a_current_cm;
     }
 }
+
+void icp::Vanilla::compute_matches() {
+    const size_t n = a.size();
+    const size_t m = b.size();
+
+    for (size_t i = 0; i < n; i++) {
+        matches[i].cost = std::numeric_limits<double>::infinity();
+        for (size_t j = 0; j < m; j++) {
+            // Point-to-point matching
+            double dist_ij = (b[j] - a_current[i]).squaredNorm();
+
+            if (dist_ij < matches[i].cost) {
+                matches[i].cost = dist_ij;
+                matches[i].pair = j;
+            }
+        }
+    }
+}