From f43af26c5df0463b0c778c0abb93f75c64a3e29b Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Wed, 4 Dec 2024 13:11:58 -0500 Subject: [PATCH 1/5] Add ICP driver interface --- include/icp/icp.h | 22 -------- include/icp/icp_driver.h | 106 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 106 insertions(+), 22 deletions(-) create mode 100644 include/icp/icp_driver.h diff --git a/include/icp/icp.h b/include/icp/icp.h index ad5a639..196345d 100644 --- a/include/icp/icp.h +++ b/include/icp/icp.h @@ -78,16 +78,6 @@ namespace icp { virtual void setup(); 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; @@ -135,18 +125,6 @@ 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; diff --git a/include/icp/icp_driver.h b/include/icp/icp_driver.h new file mode 100644 index 0000000..ef374cf --- /dev/null +++ b/include/icp/icp_driver.h @@ -0,0 +1,106 @@ +#pragma once + +#include +#include "icp/icp.h" + +namespace icp { + class ICPDriver { + public: + /** The result of running `ICPDriver::converge`. */ + struct ConvergenceReport { + /** The least cost achieved. */ + double final_cost; + + /** The number of iterations performed. */ + size_t iteration_count; + + /** The final 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); + + /** + * @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 ConvergenceReport + */ + ConvergenceReport converge(const std::vector& a, const std::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 |delta cost| / |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 |delta cost| < absolute_cost_tolerance. + * + * @param absolute_cost_tolerance The absolute cost tolerance. + */ + void set_absolute_cost_tolerance(double absolute_cost_tolerance); + + /** + * @brief Sets the angle tolerance in radians. `converge` will return when the change in + * rotation is less than angle_tolerance. + * + * @param angle_tolerance The angle tolerance in radians. + */ + void set_angle_tolerance(double angle_tolerance); + + /** + * @brief Set the translation tolerance in scan units. `converge` will return when the + * change in translation is less than translation_tolerance. + * + * @param translation_tolerance The translation tolerance in scan units. + */ + void set_translation_tolerance(double translation_tolerance); + + private: + std::unique_ptr icp_; + + std::optional min_iterations_; + std::optional max_iterations_; + std::optional stop_cost_; + std::optional relative_cost_tolerance_; + std::optional absolute_cost_tolerance_; + std::optional angle_tolerance_rad_; + std::optional translation_tolerance_; + }; +} From 40426ea584714583ba18a304bdccad22202abcba Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Wed, 4 Dec 2024 16:53:26 -0500 Subject: [PATCH 2/5] Driver works, but we're gonna need a better testing strategy --- Makefile | 2 +- include/icp/icp.h | 14 ++--- include/icp/icp_driver.h | 50 ++++++++++-------- lib/icp/icp.cpp | 33 ------------ lib/icp/icp_driver.cpp | 107 +++++++++++++++++++++++++++++++++++++++ src/main.cpp | 24 +++++---- tests/test.cpp | 63 ++++++++++++----------- 7 files changed, 184 insertions(+), 109 deletions(-) create mode 100644 lib/icp/icp_driver.cpp diff --git a/Makefile b/Makefile index a78969a..08fb081 100644 --- a/Makefile +++ b/Makefile @@ -58,7 +58,7 @@ $(TESTNAME): CFLAGS += -DTEST -include $(TESTDEPS) N := 3 -METHOD := feature_aware +METHOD := vanilla ifeq ($(shell uname), Darwin) AR := /usr/bin/libtool diff --git a/include/icp/icp.h b/include/icp/icp.h index 196345d..03fc9bc 100644 --- a/include/icp/icp.h +++ b/include/icp/icp.h @@ -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,13 +62,6 @@ namespace icp { /** The destination point cloud. */ std::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 matches; @@ -128,8 +120,8 @@ namespace icp { /** 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`, diff --git a/include/icp/icp_driver.h b/include/icp/icp_driver.h index ef374cf..55880e2 100644 --- a/include/icp/icp_driver.h +++ b/include/icp/icp_driver.h @@ -1,20 +1,20 @@ #pragma once #include -#include "icp/icp.h" +#include "icp.h" namespace icp { class ICPDriver { public: /** The result of running `ICPDriver::converge`. */ - struct ConvergenceReport { - /** The least cost achieved. */ - double final_cost; + struct ConvergenceState { + /** The cost achieved. */ + double cost; /** The number of iterations performed. */ size_t iteration_count; - /** The final transform. */ + /** The transform. */ RBTransform transform; }; @@ -31,14 +31,16 @@ namespace icp { * @param a The source point cloud. * @param b The destination point cloud. * @param t The initial guess for the transformation. - * @return ConvergenceReport + * @return ConvergenceState */ - ConvergenceReport converge(const std::vector& a, const std::vector& b, + ConvergenceState converge(const std::vector& a, const std::vector& b, RBTransform t); /** * @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); @@ -61,8 +63,10 @@ namespace icp { // 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 |delta cost| / |cost| < - * relative_cost_tolerance. + * 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. */ @@ -70,29 +74,31 @@ namespace icp { /** * @brief Set the absolute cost tolerance. `converge` will return when the cost changes by - * less than this amount, i.e. when |delta cost| < absolute_cost_tolerance. + * 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); /** - * @brief Sets the angle tolerance in radians. `converge` will return when the change in - * rotation is less than angle_tolerance. - * - * @param angle_tolerance The angle tolerance in radians. - */ - void set_angle_tolerance(double angle_tolerance); - - /** - * @brief Set the translation tolerance in scan units. `converge` will return when the - * change in translation is less than translation_tolerance. + * @brief Set the transform tolerance. `converge` will return when both the angle and + * translation tolerances are met. * - * @param translation_tolerance The translation tolerance in scan units. + * @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_translation_tolerance(double translation_tolerance); + void set_transform_tolerance(double angle_tolerance, double translation_tolerance); private: + bool should_terminate(ConvergenceState current_state, + std::optional last_state); + std::unique_ptr icp_; std::optional min_iterations_; diff --git a/lib/icp/icp.cpp b/lib/icp/icp.cpp index a8dee4e..4341d07 100644 --- a/lib/icp/icp.cpp +++ b/lib/icp/icp.cpp @@ -33,10 +33,6 @@ namespace icp { this->a = a; this->b = b; - // Cost is infinite initially - previous_cost = std::numeric_limits::infinity(); - current_cost = std::numeric_limits::infinity(); - // Ensure arrays are the right size matches.resize(this->a.size()); @@ -52,35 +48,6 @@ namespace icp { return std::sqrt(sum_squares / a.size()); } - ICP::ConvergenceReport ICP::converge(size_t burn_in, double convergence_threshold) { - ConvergenceReport result{}; - - // Repeat until convergence - while (current_cost > convergence_threshold || current_cost == INFINITY - || result.iteration_count < burn_in) { - // Store previous iteration results - previous_cost = current_cost; - RBTransform previous_transform = transform; - - iterate(); - - // If cost rose, revert to previous transformation/cost and - // exit - current_cost = calculate_cost(); - if (current_cost >= previous_cost && result.iteration_count > burn_in) { - transform = previous_transform; - current_cost = previous_cost; - break; - } - - result.iteration_count++; - } - - result.final_cost = current_cost; - - return result; - } - const RBTransform& ICP::current_transform() const { return transform; } diff --git a/lib/icp/icp_driver.cpp b/lib/icp/icp_driver.cpp new file mode 100644 index 0000000..28b822f --- /dev/null +++ b/lib/icp/icp_driver.cpp @@ -0,0 +1,107 @@ +#include "icp/icp_driver.h" +#include + +namespace icp { + ICPDriver::ICPDriver(std::unique_ptr icp) { + icp_ = std::move(icp); + } + + ICPDriver::ConvergenceState ICPDriver::converge(const std::vector& a, + const std::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 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 last_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; + } + + if (!last_state) { + return false; + } + + 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; + } + + 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; + } +} diff --git a/src/main.cpp b/src/main.cpp index f333c88..3373f21 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -117,10 +117,10 @@ void run_benchmark(const char* method, std::unique_ptr icp, const Lida const auto start = std::chrono::high_resolution_clock::now(); for (size_t i = 0; i < N; i++) { - icp->begin(source.points, destination.points, icp::RBTransform()); - icp::ICP::ConvergenceReport result = icp->converge(burn_in, convergence_threshold); - final_costs.push_back(result.final_cost); - iteration_counts.push_back(result.iteration_count); + // icp->begin(source.points, destination.points, icp::RBTransform()); + // icp::ICP::ConvergenceState result = icp->converge(burn_in, convergence_threshold); + // final_costs.push_back(result.final_cost); + // iteration_counts.push_back(result.iteration_count); } const auto end = std::chrono::high_resolution_clock::now(); @@ -220,12 +220,16 @@ int main(int argc, const char** argv) { std::unique_ptr icp = std::move(icp_opt.value()); - // std::vector a = {icp::Vector(100, 200), icp::Vector(130, 420), - // icp::Vector(-100, -200), icp::Vector(-50, -100)}; - // std::vector b = {icp::Vector(100, -200), icp::Vector(130, -420), - // icp::Vector(-100, 200), icp::Vector(-50, 100)}; - // LidarView* view = new LidarView(a, b, method); - + // std::vector a = {icp::Vector(0, 0), icp::Vector(100, 100)}; + // std::vector b = {}; + // double angle = (double)8 * M_PI / 180.0; + // icp::Vector center = icp::get_centroid(a); + // icp::Matrix rotation_matrix{ + // {std::cos(angle), -std::sin(angle)}, {std::sin(angle), std::cos(angle)}}; + // for (const auto& point: a) { + // b.push_back(rotation_matrix * (point - center) + center); + // } + // LidarView* view = new LidarView(a, b, std::move(icp)); // launch_gui(view, "test"); // return 0; diff --git a/tests/test.cpp b/tests/test.cpp index c57c974..81c8b52 100644 --- a/tests/test.cpp +++ b/tests/test.cpp @@ -5,6 +5,7 @@ extern "C" { } #include "icp/icp.h" +#include "icp/icp_driver.h" #define BURN_IN 0 #define TRANS_EPS 2 @@ -12,64 +13,64 @@ extern "C" { void test_kdtree(void) {} -void test_icp(const std::string& method) { - std::unique_ptr icp = icp::ICP::from_method(method).value(); +void test_icp(const std::string& method, const icp::ICP::Config& config) { + std::unique_ptr icp = icp::ICP::from_method(method, config).value(); + icp::ICPDriver driver(std::move(icp)); + driver.set_min_iterations(BURN_IN); + driver.set_max_iterations(100); + driver.set_transform_tolerance(0.1 * M_PI / 180, 0.1); { std::vector a = {icp::Vector(0, 0)}; std::vector b = {icp::Vector(100, 0)}; - icp->begin(a, b, icp::RBTransform()); - icp::ICP::ConvergenceReport result = icp->converge(BURN_IN, 0); + auto result = driver.converge(a, b, icp::RBTransform()); // should not need more than 10 for such a trivial situation or else // there is a serious issue with the algorithm assert_true(result.iteration_count <= BURN_IN + 10); - // again, for such a trivial situation, we should have easily achieved - // the convergence requested - assert_equal(0, result.final_cost); - assert_true(fabs(icp->current_transform().translation.x() - 100) <= TRANS_EPS); - assert_true(fabs(icp->current_transform().translation.y() - 0) <= TRANS_EPS); + assert_true(std::abs(result.transform.translation.x() - 100) <= TRANS_EPS); + assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS); } { std::vector a = {icp::Vector(0, 0), icp::Vector(100, 100)}; std::vector b = {icp::Vector(0, 0), icp::Vector(100, 100)}; - icp->begin(a, b, icp::RBTransform()); - icp::ICP::ConvergenceReport result = icp->converge(BURN_IN, 0); + auto result = driver.converge(a, b, icp::RBTransform()); - assert_true(result.final_cost < 1); - - assert_true(fabs(icp->current_transform().translation.x() - 0) <= TRANS_EPS); - assert_true(fabs(icp->current_transform().translation.y() - 0) <= TRANS_EPS); + assert_true(std::abs(result.transform.translation.x() - 0) <= TRANS_EPS); + assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS); } for (int deg = 0; deg < 20; deg++) { - std::vector a = {icp::Vector(0, 0), icp::Vector(100, 100)}; + std::vector a = {icp::Vector(-100, -100), icp::Vector(100, 100)}; std::vector b = {}; double angle = (double)deg * M_PI / 180.0; - icp::Vector center(50, 50); - icp::Matrix rotation_matrix{{cos(angle), -sin(angle)}, {sin(angle), cos(angle)}}; + icp::Vector center = icp::get_centroid(a); + icp::Matrix rotation_matrix{ + {std::cos(angle), -std::sin(angle)}, {std::sin(angle), std::cos(angle)}}; + for (const auto& point: a) { b.push_back(rotation_matrix * (point - center) + center); } - icp->begin(a, b, icp::RBTransform()); - icp->converge(BURN_IN, 0); - // assert_true(fabs(icp->current_transform().translation.x() - 0) - // <= TRANS_EPS); - // assert_true(fabs(icp->current_transform().translation.y() - 0) - // <= TRANS_EPS); + std::cout << "testing angle: " << deg << '\n'; + + auto result = driver.converge(a, b, icp::RBTransform()); + + assert_true(std::abs(result.transform.translation.x() - 0) <= TRANS_EPS); + assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS); } { std::vector a = {icp::Vector(0, 0), icp::Vector(0, 100)}; std::vector b = {icp::Vector(100, 0), icp::Vector(100, 100)}; - icp->begin(a, b, icp::RBTransform()); - icp->converge(BURN_IN, 0); - assert_true(fabs(icp->current_transform().translation.x() - 100) <= TRANS_EPS); - assert_true(fabs(icp->current_transform().translation.y() - 0) <= TRANS_EPS); + + auto result = driver.converge(a, b, icp::RBTransform()); + + assert_true(std::abs(result.transform.translation.x() - 100) <= TRANS_EPS); + assert_true(std::abs(result.transform.translation.y() - 0) <= TRANS_EPS); } } @@ -77,8 +78,6 @@ void test_main() { test_kdtree(); icp::ICP::register_builtin_methods(); - for (const auto& method: icp::ICP::registered_methods()) { - std::cout << "testing icp method: " << method << '\n'; - test_icp(method); - } + + test_icp("vanilla", icp::ICP::Config()); } From 0ed0636e0e0b8468056f143af06af025bab75e8e Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Fri, 6 Dec 2024 10:39:27 -0500 Subject: [PATCH 3/5] Fix tests --- include/icp/impl/feature_aware.h | 3 ++- lib/icp/impl/feature_aware.cpp | 12 ++++++------ tests/test.cpp | 19 +++++++++++++++---- 3 files changed, 23 insertions(+), 11 deletions(-) diff --git a/include/icp/impl/feature_aware.h b/include/icp/impl/feature_aware.h index 0dd38a2..2b448e7 100644 --- a/include/icp/impl/feature_aware.h +++ b/include/icp/impl/feature_aware.h @@ -10,7 +10,7 @@ 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(); @@ -49,6 +49,7 @@ namespace icp { Eigen::MatrixXd norm_feature_dists; + double overlap_rate; int symmetric_neighbors; double feature_weight; double neighbor_weight; diff --git a/lib/icp/impl/feature_aware.cpp b/lib/icp/impl/feature_aware.cpp index 05dd7b9..6f03ba6 100644 --- a/lib/icp/impl/feature_aware.cpp +++ b/lib/icp/impl/feature_aware.cpp @@ -5,16 +5,16 @@ #include "icp/impl/feature_aware.h" namespace icp { - FeatureAware::FeatureAware(double feature_weight, int symmetric_neighbors) + FeatureAware::FeatureAware(double overlap_rate, double feature_weight, int symmetric_neighbors) : ICP(), + overlap_rate(overlap_rate), symmetric_neighbors(symmetric_neighbors), feature_weight(feature_weight), - neighbor_weight(1 - feature_weight) { - std::cout << "Feature weight: " << feature_weight << std::endl; - } + neighbor_weight(1 - feature_weight) {} FeatureAware::FeatureAware(const Config& config) - : FeatureAware(config.get("feature_weight", 0.7), + : FeatureAware(config.get("overlap_rate", 0.9), + config.get("feature_weight", 0.7), config.get("symmetric_neighbors", 10)) {} FeatureAware::~FeatureAware() {} @@ -64,7 +64,7 @@ namespace icp { */ std::sort(matches.begin(), matches.end(), [](const auto& a, const auto& b) { return a.cost < b.cost; }); - size_t new_n = static_cast(0.7 * n); + size_t new_n = static_cast(overlap_rate * n); new_n = std::max(new_n, 1); // TODO: bad for scans with 0 points // yeah, i know this is inefficient. we'll get back to it later. diff --git a/tests/test.cpp b/tests/test.cpp index 81c8b52..5ec3ede 100644 --- a/tests/test.cpp +++ b/tests/test.cpp @@ -8,12 +8,12 @@ extern "C" { #include "icp/icp_driver.h" #define BURN_IN 0 -#define TRANS_EPS 2 -#define RAD_EPS ((double)(1e-1)) +#define TRANS_EPS 0.5 +#define RAD_EPS ((double)(0.01)) void test_kdtree(void) {} -void test_icp(const std::string& method, const icp::ICP::Config& config) { +void test_icp_generic(const std::string& method, const icp::ICP::Config& config) { std::unique_ptr icp = icp::ICP::from_method(method, config).value(); icp::ICPDriver driver(std::move(icp)); driver.set_min_iterations(BURN_IN); @@ -79,5 +79,16 @@ void test_main() { icp::ICP::register_builtin_methods(); - test_icp("vanilla", icp::ICP::Config()); + test_icp_generic("vanilla", icp::ICP::Config()); + + icp::ICP::Config trimmed_config; + // fails with lower overlap rates on these super small examples + trimmed_config.set("overlap_rate", 1.0); + test_icp_generic("trimmed", trimmed_config); + + icp::ICP::Config feature_config; + feature_config.set("overlap_rate", 1.0); + feature_config.set("feature_weight", 0.7); + feature_config.set("symmetric_neighbors", 1); + test_icp_generic("feature_aware", feature_config); } From 01ec871a1af3736ef5c1bdb7ee18577fd3762df9 Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Fri, 6 Dec 2024 10:48:36 -0500 Subject: [PATCH 4/5] Add cost increase terminator back --- lib/icp/icp_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/icp/icp_driver.cpp b/lib/icp/icp_driver.cpp index 28b822f..d9286b0 100644 --- a/lib/icp/icp_driver.cpp +++ b/lib/icp/icp_driver.cpp @@ -48,7 +48,7 @@ namespace icp { double delta_cost = current_state.cost - last_state.value().cost; if (delta_cost > 0) { - // return true; + return true; } if (absolute_cost_tolerance_ && std::abs(delta_cost) < absolute_cost_tolerance_.value()) { From 4a081daacd0b755c9afa40d3b54fb57b403fded9 Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Fri, 6 Dec 2024 12:43:21 -0500 Subject: [PATCH 5/5] Fix driver --- .vscode/launch.json | 3 +- .vscode/settings.json | 3 +- Makefile | 4 +-- include/icp/{icp_driver.h => driver.h} | 6 ---- include/icp/icp.h | 17 +++++++--- include/icp/impl/feature_aware.h | 2 ++ include/icp/impl/trimmed.h | 2 ++ include/icp/impl/vanilla.h | 2 ++ lib/icp/{icp_driver.cpp => driver.cpp} | 9 +++--- lib/icp/impl/feature_aware.cpp | 44 +++++++++++++++----------- lib/icp/impl/trimmed.cpp | 36 +++++++++++++-------- lib/icp/impl/vanilla.cpp | 34 ++++++++++++-------- src/main.cpp | 19 +++++++---- tests/test.cpp | 2 +- 14 files changed, 111 insertions(+), 72 deletions(-) rename include/icp/{icp_driver.h => driver.h} (94%) rename lib/icp/{icp_driver.cpp => driver.cpp} (95%) diff --git a/.vscode/launch.json b/.vscode/launch.json index b479799..d1655db 100644 --- 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 index 069e832..b51d3c0 100644 --- 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" } } \ No newline at end of file diff --git a/Makefile b/Makefile index 08fb081..ae4e444 100644 --- 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 b/include/icp/driver.h similarity index 94% rename from include/icp/icp_driver.h rename to include/icp/driver.h index 55880e2..3acebd9 100644 --- a/include/icp/icp_driver.h +++ b/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 index 03fc9bc..73255ea 100644 --- 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& a, const std::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 index 2b448e7..90fa01b 100644 --- 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& points, Vector cm, std::vector& features); diff --git a/include/icp/impl/trimmed.h b/include/icp/impl/trimmed.h index 8291a54..f4cd12c 100644 --- 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 a_current; icp::Vector b_cm; diff --git a/include/icp/impl/vanilla.h b/include/icp/impl/vanilla.h index 8bb8063..1a42d19 100644 --- 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 a_current; icp::Vector b_cm; }; diff --git a/lib/icp/icp_driver.cpp b/lib/icp/driver.cpp similarity index 95% rename from lib/icp/icp_driver.cpp rename to lib/icp/driver.cpp index d9286b0..0bd567a 100644 --- a/lib/icp/icp_driver.cpp +++ b/lib/icp/driver.cpp @@ -1,4 +1,4 @@ -#include "icp/icp_driver.h" +#include "icp/driver.h" #include namespace icp { @@ -30,6 +30,7 @@ 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; } @@ -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 index 6f03ba6..025b7f2 100644 --- 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::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::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& points, Vector cm, std::vector& 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 index 97c3970..0f04e32 100644 --- 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::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::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 index 39d2ba7..19781b8 100644 --- 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::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::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; + } + } + } +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 3373f21..401efa5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -15,6 +15,7 @@ extern "C" { #include "sim/lidar_view.h" #include "icp/impl/vanilla.h" #include "icp/impl/trimmed.h" +#include "icp/driver.h" struct LidarScan { double range_max; @@ -104,12 +105,17 @@ void run_benchmark(const char* method, std::unique_ptr icp, const Lida constexpr size_t N = 50; constexpr size_t burn_in = 0; - constexpr double convergence_threshold = 20.0; + constexpr double angle_tol = 0.1 * M_PI / 180; + constexpr double trans_tol = 0.1; std::cout << "* Method name: " << method << '\n'; std::cout << "* Number of trials: " << N << '\n'; std::cout << "* Burn-in period: " << burn_in << '\n'; - std::cout << "* Ideal convergence threshold: " << convergence_threshold << '\n'; + std::cout << "* Angle tolerance: " << angle_tol << " rad\n"; + std::cout << "* Translation tolerance: " << trans_tol << '\n'; + + icp::ICPDriver driver(std::move(icp)); + driver.set_transform_tolerance(0.1 * M_PI / 180, 0.1); std::vector final_costs; std::vector iteration_counts; @@ -117,10 +123,9 @@ void run_benchmark(const char* method, std::unique_ptr icp, const Lida const auto start = std::chrono::high_resolution_clock::now(); for (size_t i = 0; i < N; i++) { - // icp->begin(source.points, destination.points, icp::RBTransform()); - // icp::ICP::ConvergenceState result = icp->converge(burn_in, convergence_threshold); - // final_costs.push_back(result.final_cost); - // iteration_counts.push_back(result.iteration_count); + auto result = driver.converge(source.points, destination.points, icp::RBTransform()); + final_costs.push_back(result.cost); + iteration_counts.push_back(result.iteration_count); } const auto end = std::chrono::high_resolution_clock::now(); @@ -153,6 +158,8 @@ void run_benchmark(const char* method, std::unique_ptr icp, const Lida << "* Mean iterations: " << mean_iterations << " (real: " << (mean_iterations - burn_in) << ")\n"; std::cout << "* Average time per invocation: " << (diff.count() / N) << "s\n"; + std::cout << "* Average time per iteration: " << ((diff.count() / N) / mean_iterations) + << "s\n"; } int main(int argc, const char** argv) { diff --git a/tests/test.cpp b/tests/test.cpp index 5ec3ede..4da0632 100644 --- a/tests/test.cpp +++ b/tests/test.cpp @@ -5,7 +5,7 @@ extern "C" { } #include "icp/icp.h" -#include "icp/icp_driver.h" +#include "icp/driver.h" #define BURN_IN 0 #define TRANS_EPS 0.5