diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index a566089..13398c3 100644 --- a/.github/workflows/ci.yaml +++ b/.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 diff --git a/include/icp/driver.h b/include/icp/driver.h index f78af9a..364bacf 100644 --- a/include/icp/driver.h +++ b/include/icp/driver.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include "icp.h" namespace icp { @@ -30,7 +31,8 @@ namespace icp { ICPDriver(std::unique_ptr 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 time_limit); + private: bool should_terminate(ConvergenceState current_state, std::optional last_state); @@ -106,5 +117,8 @@ namespace icp { std::optional absolute_cost_tolerance_; std::optional angle_tolerance_rad_; std::optional translation_tolerance_; + std::optional> time_limit_; + + std::chrono::time_point start_time_; }; } diff --git a/lib/icp/driver.cpp b/lib/icp/driver.cpp index 0bd567a..a97429a 100644 --- a/lib/icp/driver.cpp +++ b/lib/icp/driver.cpp @@ -8,6 +8,7 @@ namespace icp { ICPDriver::ConvergenceState ICPDriver::converge(const std::vector& a, const std::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 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 time_limit) { + time_limit_ = time_limit; + } } diff --git a/tests/test.cpp b/tests/test.cpp index 4da0632..6704ce4 100644 --- a/tests/test.cpp +++ b/tests/test.cpp @@ -6,6 +6,7 @@ extern "C" { #include "icp/icp.h" #include "icp/driver.h" +#include #define BURN_IN 0 #define TRANS_EPS 0.5