[feat] Updated visualization to parse CSV-like files

Makefile

@@ -47,7 +47,7 @@ test: $(TEST_TARGET)
 
 .PHONY: view
 view: $(MAIN_TARGET)
-	./$(BUILD_DIR)/$(MAIN_TARGET) -S ex_data/scan$(N)/first.conf -D ex_data/scan$(N)/second.conf --method $(METHOD)
+	./$(BUILD_DIR)/$(MAIN_TARGET) -S ex_data/scan$(N)/first.csv -D ex_data/scan$(N)/second.csv --method $(METHOD)
 
 .PHONY: bench
 bench: $(BENCH_TARGET)

bench/bench.cpp

@@ -2,6 +2,7 @@
 #include <stdio.h>
 #include <numeric>
 #include <iostream>
+#include <vector>
 
 #include "icp/icp.h"
 #include "icp/driver.h"
@@ -32,19 +33,17 @@ struct BenchmarkParams {
 BenchmarkResult run_benchmark(BenchmarkParams params) {
     auto icp = icp::ICP::from_method(params.method).value();
     icp::ICPDriver driver(std::move(icp));
-    driver.set_transform_tolerance(0.1 * M_PI / 180, 0.1);
+    driver.set_transform_tolerance(params.angle_tol, params.trans_tol);
 
-    LidarScan source = parse_lidar_scan("ex_data/scan" + std::to_string(params.scan_id)
-                                        + "/first.conf");
-    LidarScan destination = parse_lidar_scan("ex_data/scan" + std::to_string(params.scan_id)
-                                             + "/second.conf");
+    auto source = parse_lidar_scan("ex_data/scan" + std::to_string(params.scan_id) + "/first.csv");
+    auto dest = parse_lidar_scan("ex_data/scan" + std::to_string(params.scan_id) + "/second.csv");
 
     std::vector<double> final_costs;
     std::vector<size_t> iteration_counts;
 
     const auto start = std::chrono::high_resolution_clock::now();
     for (size_t i = 0; i < params.num_iter; i++) {
-        auto result = driver.converge(source.points, destination.points, icp::RBTransform());
+        auto result = driver.converge(source, dest, icp::RBTransform());
         final_costs.push_back(result.cost);
         iteration_counts.push_back(result.iteration_count);
     }

common/parse_scan.cpp

@@ -1,42 +1,38 @@
 #include "parse_scan.h"
+#include <cmath>
+#include <cstddef>
+#include <cstdlib>
+#include <fstream>
+#include <stdexcept>
+#include <string_view>
+#include <vector>
+#include "icp/geo.h"
 
-void parse_scan_var(const char* var, const char* data, void* user_data) {
-    LidarScan* scan = static_cast<LidarScan*>(user_data);
-    if (strcmp(var, "range_min") == 0) {
-        scan->range_min = strtod(data, NULL);
-    } else if (strcmp(var, "range_max") == 0) {
-        scan->range_max = strtod(data, NULL);
-    } else if (strcmp(var, "angle_max") == 0) {
-        scan->angle_max = strtod(data, NULL);
-    } else if (strcmp(var, "angle_min") == 0) {
-        scan->angle_min = strtod(data, NULL);
-    } else if (strcmp(var, "angle_increment") == 0) {
-        scan->angle_increment = strtod(data, NULL);
-    } else if (isdigit(var[0])) {
-        long index = strtol(var, NULL, 10);
-        double angle = scan->angle_min + index * scan->angle_increment;
-        double range = strtod(data, NULL);
-        if (range >= scan->range_min && range <= scan->range_max) {
-            scan->points.push_back(icp::Vector(100 * range * std::cos(angle),
-                100 * range * std::sin(angle)));
-        }
+std::vector<icp::Vector> parse_lidar_scan(std::string path) {
+    std::ifstream in(path);
+    if (!in.is_open()) {
+        throw std::runtime_error("failed to read lidar scan: failed to open file");
     }
-}
 
-LidarScan parse_lidar_scan(std::string path) {
-    FILE* file = fopen(path.c_str(), "r");
-    if (!file) {
-        perror("parse_config: fopen");
-        std::exit(1);
-    }
+    std::vector<icp::Vector> result;
 
-    LidarScan scan;
-    if (conf_parse_file(file, parse_scan_var, &scan) != 0) {
-        perror("parse_config: conf_parse_file");
-        std::exit(1);
-    }
+    std::string line;
+    while (std::getline(in, line)) {
+        size_t index = line.find(',');
+        std::string_view view(line);
 
-    fclose(file);
+        std::string_view x_view = view.substr(0, index);
+        std::string_view y_view = view.substr(index + 1);
+
+        char* end = nullptr;
+        double x = std::strtod(x_view.cbegin(), &end);
+        double y = std::strtod(y_view.cbegin(), &end);
+        if (std::isinf(x) || std::isinf(y)) {
+            continue;
+        }
+
+        result.emplace_back(x, y);
+    }
 
-    return scan;
+    return result;
 }

common/parse_scan.h

@@ -1,17 +1,4 @@
 #include <vector>
 #include "icp/geo.h"
-extern "C" {
-#include <config/config.h>
-}
 
-struct LidarScan {
-    double range_max;
-    double range_min;
-    double angle_min;
-    double angle_max;
-    double angle_increment;
-
-    std::vector<icp::Vector> points;
-};
-
-LidarScan parse_lidar_scan(std::string path);
+std::vector<icp::Vector> parse_lidar_scan(std::string path);