Allow for different sized point clouds

Author: ethanuppal

Doxyfile

@@ -0,0 +1,39 @@
+\mainpage Introduction
+
+\section overview_sec Overview
+
+<table>
+    <tr>
+        <th style="text-align:left; vertical-align:top">Item</th>
+        <th style="text-align:left; vertical-align:top">Location</th>
+    </tr>
+    <tr>
+        <td style="text-align:left; vertical-align:top"><b>Documentation</b></td>
+        <td style="text-align:left; vertical-align:top">icp.h</td>
+    </tr>
+    <tr>
+        <td style="text-align:left; vertical-align:top"><b>Repository</b></td>
+        <td style="text-align:left; vertical-align:top">
+            <a href="https://github.com/cornellev/scan-matching">https://github.com/cornellev/scan-matching</a>
+        </td>
+    </tr>
+    <tr>
+        <td style="text-align:left; vertical-align:top"><b>Contents</b></td>
+        <td style="text-align:left; vertical-align:top">
+            \ref overview_sec <br> 
+            \ref feature_sec <br> 
+            \ref install_sec <br> 
+            \ref example_sec
+        </td>
+    </tr>
+</table>
+
+\section feature_sec Features
+
+\section install_sec Usage
+
+\subsection install_unix_subsec Unix-like (MacOS, Linux)
+
+\subsection install_windows_subsec Windows
+
+\section example_sec Example

book/asset/doxygen-style.css

@@ -1,19 +1,21 @@
+# editing code to make sin wave makes a lot of these useless/weird now
+
 window_width = 1200
 window_height = 900
 
-n = 700
+n = 300
 scale = 2
 
 slope = -1.0
-intercept = 200
+intercept = -450
 
-x_delta = -200
+x_delta = -100
 
 perturbation_range = 10
 
-x_displace = 300
-y_displace = 300
+x_displace = 50
+y_displace = 0
 
-angle_displace = 0.5
+angle_displace = 0.1
 
 # angle_displace = 0.785398 # M_PI_4 in radians

book/asset/img/closed-folder.png

@@ -62,8 +62,8 @@ namespace icp {
         std::vector<double> dist;
 
         /** A new ICP instance for `n`-point matching and an optimization rate
-         * of `rate` */
-        ICP(size_t n, double rate);
+         * of `rate`. @deprecated `n` is ignored. */
+        ICP(size_t n = 0, double rate = 0.01);
 
     public:
         virtual ~ICP() = default;
@@ -96,10 +96,14 @@ namespace icp {
          * methods. */
         static const std::vector<std::string>& registered_methods();
 
-        /** Factory constructor for the ICP method `name`. @pre `name` is a
-         * valid registered method. */
-        static std::unique_ptr<ICP> from_method(std::string name, size_t n,
-            double rate);
+        /**
+         * Factory constructor for the ICP method `name`. @pre `name` is a
+         * valid registered method.
+         *
+         * @deprecated `n` is ignored.
+         */
+        static std::unique_ptr<ICP> from_method(std::string name, size_t n = 0,
+            double rate = 0.01);
     };
 
     struct Methods {

book/asset/img/document.png

@@ -14,6 +14,8 @@ namespace icp {
             const std::vector<icp::Point>& b) override {
             size_t n = a.size();
             size_t m = b.size();
+            pair.resize(n);
+            dist.resize(n);
 
             // compute center of mass of a
             double a_cm_x = 0;

book/asset/img/off_sync.png

@@ -34,14 +34,16 @@ void LidarView::construct_instance() {
     // Create a line of points for the wall
     for (int i = 0; i < sim_config::n; ++i) {
         double x = i * sim_config::scale;
-        double y = sim_config::slope * x + sim_config::intercept + dis(gen);
+        double y = ((std::sin(i / 10.0) + 1) * sim_config::scale * 100)
+                   + sim_config::intercept + dis(gen);  // now this is ugly fix
         wall[i] = {x, y};
     }
 
     // Sample the first 3/4th and the last 3/4th as our LiDAR scans
     source.assign(wall.begin(), wall.begin() + sim_config::n * 3 / 4);
     destination.assign(wall.begin() + sim_config::n - (sim_config::n * 3 / 4),
         wall.end());
+    double shift_because_scans_same_start = destination[0].x - source[0].x;
 
     // Separate the scans to simulate robot movement
     for (icp::Point& point: source) {
@@ -51,6 +53,11 @@ void LidarView::construct_instance() {
                    - y_unshift;
     }
 
+    double dest_move_x = sim_config::window_width / 2
+                         + sim_config::x_displace / 2 - x_unshift;
+    double dest_move_y = sim_config::window_height / 2
+                         + sim_config::y_displace / 2 - y_unshift;
+
     // TODO: make less scuffed way to do this
     icp->iterate(source, source);
     double cx = icp->transform().cx;
@@ -62,15 +69,10 @@ void LidarView::construct_instance() {
     for (icp::Point& point: source) {
         double new_x = com_rot.transform_x(point.x, point.y);
         double new_y = com_rot.transform_y(point.x, point.y);
-        point.x = new_x + sim_config::x_delta;
+        point.x = new_x + sim_config::x_delta + shift_because_scans_same_start;
         point.y = new_y;
     }
 
-    double dest_move_x = sim_config::window_width / 2
-                         + sim_config::x_displace / 2 - x_unshift;
-    double dest_move_y = sim_config::window_height / 2
-                         + sim_config::y_displace / 2 - y_unshift;
-
     for (icp::Point& point: destination) {
         point.x += dest_move_x + sim_config::x_delta;
         point.y += dest_move_y;
@@ -83,6 +85,7 @@ void LidarView::construct_instance() {
         // where you should be. if it works it works but should clean
         point.x += dest_move_x + sim_config::x_delta;
         point.y += dest_move_y;
+        std::cout << point.x << ',' << point.y << '\n';
     }
 }
 
@@ -101,22 +104,22 @@ void LidarView::draw(SDL_Renderer* renderer, const SDL_Rect* frame,
     SDL_SetRenderDrawColor(renderer, 0, 0, 0, 0);
     SDL_RenderClear(renderer);
 
-    SDL_SetRenderDrawColor(renderer, 0, 255, 0, SDL_ALPHA_OPAQUE);
-    for (const icp::Point& point: source) {
-        SDL_DrawCircle(renderer, point.x, point.y, CIRCLE_RADIUS);
-    }
+    // SDL_SetRenderDrawColor(renderer, 0, 255, 0, SDL_ALPHA_OPAQUE);
+    // for (const icp::Point& point: source) {
+    //     SDL_DrawCircle(renderer, point.x, point.y, CIRCLE_RADIUS);
+    // }
 
-    SDL_SetRenderDrawColor(renderer, 128, 128, 128, SDL_ALPHA_OPAQUE);
-    for (const icp::Point& point: wall) {
-        SDL_DrawCircle(renderer, point.x, point.y, CIRCLE_RADIUS);
-    }
+    // SDL_SetRenderDrawColor(renderer, 128, 128, 128, SDL_ALPHA_OPAQUE);
+    // for (const icp::Point& point: wall) {
+    //     SDL_DrawCircle(renderer, point.x, point.y, CIRCLE_RADIUS);
+    // }
 
     SDL_SetRenderDrawColor(renderer, 0, 0, 255, SDL_ALPHA_OPAQUE);
     for (const icp::Point& point: destination) {
         SDL_DrawCircle(renderer, point.x, point.y, CIRCLE_RADIUS);
     }
 
-    SDL_SetRenderDrawColor(renderer, 0, 255, 255, 100);
+    SDL_SetRenderDrawColor(renderer, 255, 0, 0, 100);
     for (const icp::Point& point: source) {
         SDL_DrawCircle(renderer, icp->transform().transform_x(point.x, point.y),
             icp->transform().transform_y(point.x, point.y), CIRCLE_RADIUS);