From e996db19bce66c7c8a81817badc8a7b7a562af4e Mon Sep 17 00:00:00 2001 From: Nick Stathas Date: Sat, 2 Nov 2019 10:35:33 -0400 Subject: [PATCH] Add cmake project. Add PCL visualizer. --- .gitignore | 4 +++ .vscode/c_cpp_properties.json | 18 ++++++++++ .vscode/settings.json | 3 +- fast_raycast/CMakeLists.txt | 33 +++++++++++++++++ fast_raycast/src/main.cpp | 68 +++++++++++++++++++++-------------- 5 files changed, 99 insertions(+), 27 deletions(-) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 fast_raycast/CMakeLists.txt diff --git a/.gitignore b/.gitignore index 566d5b0..d10228e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,7 @@ +# Build +fast_raycast/build +fast_raycast/bin + # AWSRUN log.awsrun job_*.tar diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..3749173 --- /dev/null +++ b/.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 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index e89429c..36fb099 100644 --- a/.vscode/settings.json +++ b/.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" } -} \ No newline at end of file +} diff --git a/fast_raycast/CMakeLists.txt b/fast_raycast/CMakeLists.txt new file mode 100644 index 0000000..8bde253 --- /dev/null +++ b/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) diff --git a/fast_raycast/src/main.cpp b/fast_raycast/src/main.cpp index c0b7073..06a3109 100644 --- a/fast_raycast/src/main.cpp +++ b/fast_raycast/src/main.cpp @@ -4,6 +4,14 @@ #include #include +#include +#include + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#include +#pragma GCC diagnostic pop + namespace lcaster { using namespace Eigen; @@ -93,14 +101,30 @@ constexpr Solutions make_solutions(Rays const& rays) { *! Provides the point each ray intersects with as computed by some method. */ template -using Points = Matrix; +using Points = Array; + +using PointCloud = pcl::PointCloud; + +template +auto computePoints(Rays const& rays, Solutions const& solutions) { + return rays.origins() + + (rays.directions().array().colwise() * solutions).matrix(); +} template void computePoints(Rays const& rays, Solutions const& solutions, Points& points) { - points.noalias() = rays.origins() + - (rays.directions().array().colwise() * solutions).matrix(); + points = computePoints(rays, solutions); +} + +template +void computePoints(Rays const& rays, + Solutions 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 rays = Rays::Zero(); rays.origins().col(2) = decltype(rays.origins().col(2))::Ones(NRays, 1); @@ -279,32 +303,24 @@ int main() { Solutions solutions(rays.rays()); Solutions hit_height(rays.rays()); - auto start1 = std::chrono::high_resolution_clock::now(); + World::DV world(ground, {cone}); + World::ObjectIdxs 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 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(end1 - - start1) - .count() - << ","; - std::cerr << std::chrono::duration_cast(end2 - - start2) - .count() - << std::endl; - World::DV world(ground, {cone}); - World::ObjectIdxs 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; }