Added time limit to ICPDriver

.github/workflows/ci.yaml

@@ -12,6 +12,8 @@ jobs:
     steps:
       - name: Checkout
         uses: actions/checkout@v4
+      - name: apt update
+        run: sudo apt-get update
       - name: Install Eigen
         run: sudo apt-get install libeigen3-dev
       - name: Install SDL2

include/icp/driver.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <optional>
+#include <chrono>
 #include "icp.h"
 
 namespace icp {
@@ -30,7 +31,8 @@ namespace icp {
         ICPDriver(std::unique_ptr<ICP> icp);
 
         /**
-         * @brief Runs ICP to convergence based on the termination conditions set.
+         * @brief Runs ICP to convergence based on the termination conditions set. If no conditions
+         * are set, ICP will run indefinitely. This is rarely desirable.
          *
          * @param a The source point cloud.
          * @param b The destination point cloud.
@@ -41,7 +43,8 @@ namespace icp {
             RBTransform t);
 
         /**
-         * @brief Sets the minimum number of iterations to run.
+         * @brief Sets the minimum number of iterations to run. This number of iterations will
+         * always be performed.
          *
          * @param min_iterations The minimum number of iterations to run.
          */
@@ -93,6 +96,14 @@ namespace icp {
          */
         void set_transform_tolerance(double angle_tolerance, double translation_tolerance);
 
+        /**
+         * @brief Set the time limit for `converge`. Note that `coverge` may take slightly longer
+         * than this time (up to 1 iteration time) to return.
+         *
+         * @param time_limit The time limit.
+         */
+        void set_time_limit(std::chrono::duration<double> time_limit);
+
     private:
         bool should_terminate(ConvergenceState current_state,
             std::optional<ConvergenceState> last_state);
@@ -106,5 +117,8 @@ namespace icp {
         std::optional<double> absolute_cost_tolerance_;
         std::optional<double> angle_tolerance_rad_;
         std::optional<double> translation_tolerance_;
+        std::optional<std::chrono::duration<double>> time_limit_;
+
+        std::chrono::time_point<std::chrono::steady_clock> start_time_;
     };
 }

lib/icp/driver.cpp

@@ -8,6 +8,7 @@ namespace icp {
 
     ICPDriver::ConvergenceState ICPDriver::converge(const std::vector<Vector>& a,
         const std::vector<Vector>& b, RBTransform t) {
+        start_time_ = std::chrono::steady_clock::now();
         icp_->begin(a, b, t);
 
         ConvergenceState state{};
@@ -31,10 +32,6 @@ 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;
-        }
-
         if (min_iterations_ && current_state.iteration_count < min_iterations_.value()) {
             return false;
         }
@@ -43,6 +40,17 @@ namespace icp {
             return true;
         }
 
+        if (stop_cost_ && current_state.cost < stop_cost_.value()) {
+            return true;
+        }
+
+        if (time_limit_) {
+            auto current_time = std::chrono::steady_clock::now();
+            if (current_time - start_time_ > time_limit_.value()) {
+                return true;
+            }
+        }
+
         // end if we don't have a last state
         if (!last_state) {
             return false;
@@ -103,4 +111,8 @@ namespace icp {
         angle_tolerance_rad_ = angle_tolerance;
         translation_tolerance_ = translation_tolerance;
     }
+
+    void ICPDriver::set_time_limit(std::chrono::duration<double> time_limit) {
+        time_limit_ = time_limit;
+    }
 }

tests/test.cpp

@@ -6,6 +6,7 @@ extern "C" {
 
 #include "icp/icp.h"
 #include "icp/driver.h"
+#include <chrono>
 
 #define BURN_IN 0
 #define TRANS_EPS 0.5