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