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] 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<double>("feature_weight", 0.7),
+        : FeatureAware(config.get<double>("overlap_rate", 0.9),
+              config.get<double>("feature_weight", 0.7),
               config.get<int>("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<size_t>(0.7 * n);
+        size_t new_n = static_cast<size_t>(overlap_rate * n);
         new_n = std::max<size_t>(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 = 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);
 }