Add cmake project. Add PCL visualizer.

.gitignore

@@ -1,3 +1,7 @@
+# Build
+fast_raycast/build
+fast_raycast/bin
+
 # AWSRUN
 log.awsrun
 job_*.tar

.vscode/c_cpp_properties.json

@@ -0,0 +1,18 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/lib/ccache/gcc",
+            "cStandard": "c11",
+            "cppStandard": "c++17",
+            "intelliSenseMode": "clang-x64",
+            "compileCommands": "${workspaceFolder}/fast_raycast/build/compile_commands.json",
+            "configurationProvider": "vector-of-bool.cmake-tools"
+        }
+    ],
+    "version": 4
+}

.vscode/settings.json

@@ -1,4 +1,5 @@
 {
+  "cmake.sourceDirectory": "${workspaceRoot}/fast_raycast",
   "files.associations": {
     "*.p": "python",
     "eigen": "cpp",
@@ -61,4 +62,4 @@
     "string_view": "cpp",
     "cassert": "cpp"
   }
-}
+}

fast_raycast/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required( VERSION 3.0.0 )
+
+# Create Project
+project( fast_raycast CXX )
+set (CMAKE_CXX_STANDARD 17)
+
+include_directories(inc)
+file(GLOB SOURCES "src/*.cpp")
+
+add_executable(${PROJECT_NAME} ${SOURCES})
+
+# Find Packages
+find_package( PCL 1.8 REQUIRED )
+
+if( PCL_FOUND )
+  # [C/C++]>[General]>[Additional Include Directories]
+  include_directories( ${PCL_INCLUDE_DIRS} )
+
+  # [C/C++]>[Preprocessor]>[Preprocessor Definitions]
+  add_definitions( ${PCL_DEFINITIONS} )
+
+  # For Use Not PreCompiled Features
+  # add_definitions( -DPCL_NO_PRECOMPILE )
+
+  # [Linker]>[General]>[Additional Library Directories]
+  link_directories( ${PCL_LIBRARY_DIRS} )
+
+  # [Linker]>[Input]>[Additional Dependencies]
+  target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES})
+endif()
+
+find_package(Eigen3 3.3 REQUIRED NO_MODULE)
+target_link_libraries(${PROJECT_NAME} Eigen3::Eigen)

fast_raycast/src/main.cpp

@@ -4,6 +4,14 @@
 #include <limits>
 #include <optional>
 
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#include <pcl/visualization/cloud_viewer.h>
+#pragma GCC diagnostic pop
+
 namespace lcaster {
 
 using namespace Eigen;
@@ -93,14 +101,30 @@ constexpr Solutions<NRays, T> make_solutions(Rays<NRays> const& rays) {
  *! Provides the point each ray intersects with as computed by some method.
  */
 template <int NRays = Dynamic>
-using Points = Matrix<el_t, NRays, 3>;
+using Points = Array<el_t, NRays, 3>;
+
+using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
+
+template <int NRays = Dynamic>
+auto computePoints(Rays<NRays> const& rays, Solutions<NRays> const& solutions) {
+  return rays.origins() +
+         (rays.directions().array().colwise() * solutions).matrix();
+}
 
 template <int NRays = Dynamic>
 void computePoints(Rays<NRays> const& rays,
                    Solutions<NRays> const& solutions,
                    Points<NRays>& points) {
-  points.noalias() = rays.origins() +
-                     (rays.directions().array().colwise() * solutions).matrix();
+  points = computePoints(rays, solutions);
+}
+
+template <int NRays = Dynamic>
+void computePoints(Rays<NRays> const& rays,
+                   Solutions<NRays> const& solutions,
+                   PointCloud& cloud) {
+  cloud.resize(rays.rays());
+  cloud.getMatrixXfMap().block(0, 0, 3, rays.rays()) =
+      computePoints(rays, solutions).transpose();
 }
 
 namespace Obstacle {
@@ -255,8 +279,8 @@ int main() {
   constexpr el_t VFOV = M_PI / 6;
   constexpr el_t VBIAS = -M_PI / 2;
 
-  constexpr int NRings = 20;
-  constexpr int NPoints = 20;
+  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);
@@ -279,32 +303,24 @@ int main() {
   Solutions<Dynamic> solutions(rays.rays());
   Solutions<Dynamic> hit_height(rays.rays());
 
-  auto start1 = std::chrono::high_resolution_clock::now();
+  World::DV world(ground, {cone});
+  World::ObjectIdxs<Dynamic> object;
+  // world.computeSolution(rays, solutions, hit_height, object);
   // ground.computeSolution(rays, solutions);
-  cone.computeSolution(rays, solutions, true, &hit_height);
-  auto end1 = std::chrono::high_resolution_clock::now();
-  // std::cout << solutions << "\n";
-
+  cone.computeSolution(rays, solutions);
+  (void)world;
   Points<Dynamic> points(rays.rays(), 3);
-  auto start2 = std::chrono::high_resolution_clock::now();
   computePoints(rays, solutions, points);
-  auto end2 = std::chrono::high_resolution_clock::now();
-  // std::cout << points << "\n";
-
-  std::cerr << std::chrono::duration_cast<std::chrono::nanoseconds>(end1 -
-                                                                    start1)
-                   .count()
-            << ",";
-  std::cerr << std::chrono::duration_cast<std::chrono::nanoseconds>(end2 -
-                                                                    start2)
-                   .count()
-            << std::endl;
 
-  World::DV world(ground, {cone});
-  World::ObjectIdxs<Dynamic> object;
-  world.computeSolution(rays, solutions, hit_height, object);
+  std::cout << points << "\n";
 
-  // std::cout << solutions;
+  PointCloud::Ptr cloud(new PointCloud);
+  computePoints(rays, solutions, *cloud);
+
+  pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
+  viewer.showCloud(cloud);
+  while (!viewer.wasStopped()) {
+  }
 
   return 0;
 }