Fix tests

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;

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.

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);
 }