Fix bugs in test1; now tests pass

.gitignore

@@ -11,3 +11,4 @@ docs/
 *.out
 *.log
 *.fls
+_temp

AUTHOR.md

@@ -1 +1 @@
-This library was made by Ethan Uppal with love.
+This library was made by Ethan Uppal.

README.md

@@ -3,7 +3,7 @@
 <!-- EDIT README.md.build INSTEAD. -->
 # icp
 
-> Last updated 2024-05-05 01:09:17.393145.  
+> Last updated 2024-05-05 02:05:04.868544.  
 > Made by [Ethan Uppal](https://www.ethanuppal.com).
 
 ## Introduction
@@ -23,7 +23,7 @@ Please see there for information on how to download and how to use the library.
 You can build the documentation yourself locally with `make docs`.
 The main page will be located at `docs/index.html` relative to the project root.
 
-## v1.1.1 
+## v1.2.0 
 
 The algorithm runs extremely fast now.
 We only need it to run at 6hz with our current LiDAR.

book/desmos.txt

@@ -1,2 +1,10 @@
 scan1 -> https://www.desmos.com/calculator/bgdtvvv5if
 scan2 -> https://www.desmos.com/calculator/5sqw86nifl
+
+! 5/5/25 1:44 AM
+did the first test in test.cpp as https://www.desmos.com/calculator/96yvwk9v7g
+also https://www.desmos.com/calculator/i9qmjsyzwh
+it seems to work here (gets t^*_x = 100) but not in implementation, so there is a bug
+
+! 5/5/25 2:05 AM
+should be fixed now

main.cpp

@@ -92,6 +92,7 @@ void launch_gui(LidarView* view, std::string visualized = "LiDAR scans") {
     std::cout << "* Press the red <X> on the window to exit\n";
     std::cout << "* Press SPACE to toggle the simulation\n";
     std::cout << "* Press D to display the current transform\n";
+    std::cout << "* Press I to step forward a single iteration\n";
 
     window.present();
 }
@@ -106,6 +107,7 @@ void run_benchmark(const char* method, const LidarScan& source,
     constexpr size_t burn_in = 0;
     constexpr double convergence_threshold = 20.0;
 
+    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
@@ -224,6 +226,13 @@ int main(int argc, const char** argv) {
         std::exit(1);
     }
 
+    // std::vector<icp::Vector> a = {icp::Vector(0, 0), icp::Vector(0, 100)};
+    // std::vector<icp::Vector> b = {icp::Vector(100, 0), icp::Vector(100,
+    // 100)}; LidarView* view = new LidarView(a, b, method);
+
+    // launch_gui(view, "test");
+    // return 0;
+
     if (*read_scan_files) {
         LidarScan source, destination;
         std::cerr << "source\n";
@@ -236,7 +245,7 @@ int main(int argc, const char** argv) {
         // return 1;
         if (*use_gui) {
             icp::ICP::Config config;
-            config.set("overlap_rate", 0.5);
+            config.set("overlap_rate", 0.7);
             LidarView* view = new LidarView(source.points, destination.points,
                 method, config);
 

src/icp/impl/test1.cpp

@@ -70,8 +70,9 @@ namespace icp {
 
             transform.translation = Vector::Zero();
             for (size_t i = 0; i < n; i++) {
-                transform.translation += b[matches[i].pair]
-                                         - a_rot[matches[i].point];
+                transform.translation += (b[matches[i].pair] + b_cm)
+                                         - transform.rotation
+                                               * (a[matches[i].point] + a_cm);
             }
             transform.translation /= n;
 
@@ -93,8 +94,8 @@ namespace icp {
     static bool static_initialization = []() {
         assert(ICP::register_method("test1",
             [](const ICP::Config& config) -> std::unique_ptr<ICP> {
-                /* #conf "overlap_rate" A `double` between 0 and 1 for the
-                 * overlap rate. The default is 1. */
+                /* #conf "overlap_rate" A `double` between `0.0` and `1.0` for
+                 * the overlap rate. The default is `1.0`. */
                 double overlap_rate = config.get<double>("overlap_rate", 1.0);
                 assert(overlap_rate >= 0 && overlap_rate <= 1);
                 return std::make_unique<Test1>(overlap_rate);

src/icp/impl/trimmed.cpp

@@ -11,9 +11,6 @@
 
 /* #name Trimmed */
 
-/* #conf "overlap_rate" A `double` between 0 and 1 for the overlap rate. The
- default is 1. */
-
 /* #desc Trimmed ICP is identical to \ref vanilla_icp with the addition of an
 overlap rate parameter, which specifies the percentage of points between the two
 point sets that have correspondences. When the overlap rate is 1, the algorithm
@@ -92,6 +89,8 @@ namespace icp {
     static bool static_initialization = []() {
         assert(ICP::register_method("trimmed",
             [](const ICP::Config& config) -> std::unique_ptr<ICP> {
+                /* #conf "overlap_rate" A `double` between `0.0` and `1.0` for
+                 * the overlap rate. The default is `1.0`. */
                 double overlap_rate = config.get<double>("overlap_rate", 1.0);
                 assert(overlap_rate >= 0 && overlap_rate <= 1);
                 return std::make_unique<Trimmed>(overlap_rate);

src/sim/lidar_view.cpp

@@ -26,16 +26,26 @@ LidarView::~LidarView() noexcept {
     icp.release();
 }
 
+void LidarView::step() {
+    icp->iterate();
+    iterations++;
+}
+
 void LidarView::on_event(const SDL_Event& event) {
     bool space_before = keyboard.query(SDLK_SPACE);
     bool d_before = keyboard.query(SDLK_d);
+    bool i_before = keyboard.query(SDLK_i);
     keyboard.update(event);
     bool space_after = keyboard.query(SDLK_SPACE);
     bool d_after = keyboard.query(SDLK_d);
+    bool i_after = keyboard.query(SDLK_i);
 
     if (!space_before && space_after) {
         is_iterating = !is_iterating;
     }
+    if (!i_before && i_after) {
+        step();
+    }
     if (!d_before && d_after) {
         std::cerr << "DEBUG PRINT:\n";
         std::cerr << "icp->current_transform() = "
@@ -80,7 +90,6 @@ void LidarView::draw(SDL_Renderer* renderer, const SDL_Rect* frame __unused,
         b_cm.y() + view_config::y_displace, 20);
 
     if (is_iterating) {
-        icp->iterate();
-        iterations++;
+        step();
     }
 }

src/sim/lidar_view.h

@@ -19,7 +19,7 @@ class LidarView final : public View {
     bool is_iterating;
     size_t iterations;
 
-    // void construct_instance();
+    void step();
 
 public:
     /** Constructs a new lidar view visualizing ICP (by method `method`) on
@@ -29,7 +29,6 @@ class LidarView final : public View {
         const icp::ICP::Config& config = icp::ICP::Config());
 
     ~LidarView() noexcept override;
-
     void on_event(const SDL_Event& event) override;
     void draw(SDL_Renderer* renderer, const SDL_Rect* frame,
         double dtime) override;

test.cpp

@@ -28,7 +28,6 @@ void test_icp(const std::string& method) {
         // 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)
@@ -85,6 +84,7 @@ void test_icp(const std::string& method) {
 void test_main() {
     test_kdtree();
     for (const auto& method: icp::ICP::registered_methods()) {
+        std::cout << "testing icp method: " << method << '\n';
         test_icp(method);
     }
 }