From 427dbe9e350ae42bed9693f74d02ac392589ac2d Mon Sep 17 00:00:00 2001
From: Nick Stathas <nistath@mit.edu>
Date: Tue, 26 Nov 2019 23:59:01 -0500
Subject: [PATCH] Add more convenience params.

---
 fast_raycast/src/main.cpp  | 32 +++++++-------------------------
 process_results/heatmap.py |  6 +++---
 2 files changed, 10 insertions(+), 28 deletions(-)

diff --git a/fast_raycast/src/main.cpp b/fast_raycast/src/main.cpp
index 49b02aa..b56cdaa 100644
--- a/fast_raycast/src/main.cpp
+++ b/fast_raycast/src/main.cpp
@@ -1,4 +1,5 @@
 #include <assert.h>
+#include <chrono>
 #include <eigen3/Eigen/Eigen>
 #include <experimental/filesystem>
 #include <iostream>
@@ -6,7 +7,6 @@
 #include <memory>
 #include <numeric>
 #include <optional>
-#include <chrono>
 #include <thread>
 
 using namespace std::chrono_literals;
@@ -454,33 +454,10 @@ int main() {
   using namespace lcaster;
   using namespace lcaster::Intersection;
 
-  World::Lidar vlp32 =
-      World::Lidar(Vector3e(0.0, 0.0, 0.3), 32, 0.2 * M_PI / 180.0);
+  World::Lidar vlp32({0, 0, 0}, 32, 0.2 * M_PI / 180.0);
   vlp32.setRays("fast_raycast/sensor_info/VLP32_LaserInfo.csv");
   Rays<Dynamic>& rays = vlp32.rays();
 
-  // constexpr el_t HFOV = M_PI / 8;
-  // constexpr el_t HBIAS = -HFOV / 2;
-  // constexpr el_t VFOV = M_PI / 6;
-  // constexpr el_t VBIAS = -M_PI / 2;
-  //
-  // constexpr int NRings = 200;
-  // constexpr int NPoints = 200;
-  // constexpr int NRays = NPoints * NRings;
-  // Rays<Dynamic> rays = Rays<NRays>::Zero();
-  // rays.origins().col(2) = decltype(rays.origins().col(2))::Ones(NRays, 1);
-  //
-  // for (int ring = 0; ring < NRings; ++ring) {
-  //   const el_t z = -2 * cos(VFOV * ring / NRings + VBIAS) - 0.5;
-  //   for (int i = 0; i < NPoints; ++i) {
-  //     const el_t phase = HFOV * i / NPoints + HBIAS;
-  //     rays.directions()(ring * NPoints + i, 0) = cos(phase);
-  //     rays.directions()(ring * NPoints + i, 1) = sin(phase);
-  //     rays.directions()(ring * NPoints + i, 2) = z;
-  //   }
-  // }
-  // rays.directions().rowwise().normalize();
-
   Obstacle::Plane ground({0, 0, 1}, {0, 0, 0});
 
   constexpr el_t sc = 0.75;
@@ -596,6 +573,11 @@ int main() {
   grid.serialize(fname);
 
   std::cout << "Computing sample cloud...\n";
+
+  el_t height = 0;
+  std::cin >> height;
+  rays.origin_offset = {0, 0, height};
+
   Solutions<Dynamic> solutions(rays.rays());
   Solutions<Dynamic> hit_height(rays.rays());
   World::ObjectIdxs<Dynamic> object;
diff --git a/process_results/heatmap.py b/process_results/heatmap.py
index 085011b..093cd3c 100644
--- a/process_results/heatmap.py
+++ b/process_results/heatmap.py
@@ -42,8 +42,8 @@ def plot(self):
 pix2meter = 752 / 1.530
 
 if True:
-    grid1 = Grid.from_csv('/home/nistath/Git/rt/depp.csv')
-    grid2 = Grid.from_csv('/home/nistath/Git/rt/earr.csv')
+    grid1 = Grid.from_csv('/home/nistath/Git/rt/build/depp.csv')
+    grid2 = Grid.from_csv('/home/nistath/Git/rt/build/earr.csv')
 
     grid1.plot()
     plt.show()
@@ -52,7 +52,7 @@ def plot(self):
 
     grid = Grid(grid1.ranges, (grid1.grid + grid2.grid)/2)
 else:
-    grid = Grid.from_csv('/home/nistath/Git/rt/earr.csv')
+    grid = Grid.from_csv('/home/nistath/Git/rt/build/earr.csv')
 
 png_write(grid.grid, '/home/nistath/Desktop/grid.png', 'topleft', True)
 grid.plot()