Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
25 changes: 20 additions & 5 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
cmake_minimum_required(VERSION 3.10.0)
project(fast_gicp)

option(BUILD_VGICP_CUDA "Build GPU-powered VGICP" OFF)
option(BUILD_VGICP_CUDA "Build GPU-powered VGICP" ON)
option(BUILD_apps "Build application programs" ON)
option(BUILD_test "Build test programs" OFF)
option(BUILD_PYTHON_BINDINGS "Build python bindings" OFF)
Expand Down Expand Up @@ -33,9 +33,10 @@ if (OPENMP_FOUND)
endif()

if(BUILD_VGICP_CUDA)
find_package(CUDA REQUIRED)
include_directories(${CUDA_INCLUDE_DIRS})
link_directories(${CUDA_LIBRARY_DIRS})
enable_language(CUDA)
find_package(CUDAToolkit REQUIRED)
include_directories(${CUDAToolkit_INCLUDE_DIRS})
link_directories(${CUDAToolkit_LIBRARY_DIR})
endif()

###################################
Expand All @@ -52,6 +53,8 @@ endif()
## Build ##
###########

link_directories(/home/neil/devel/glyd/.toolchain/c97040d85c983530acbe2e02c2327d0db40f6e7acb4dd8cbc280076c92c33ccf/sysroot/lib)

add_library(fast_gicp SHARED
src/fast_gicp/gicp/lsq_registration.cpp
src/fast_gicp/gicp/fast_gicp.cpp
Expand All @@ -61,6 +64,9 @@ add_library(fast_gicp SHARED
target_link_libraries(fast_gicp
${PCL_LIBRARIES}
)
target_link_directories(fast_gicp
PUBLIC /home/neil/devel/glyd/.toolchain/c97040d85c983530acbe2e02c2327d0db40f6e7acb4dd8cbc280076c92c33ccf/sysroot/lib
)
if (OPENMP_FOUND)
if (TARGET OpenMP::OpenMP_CXX)
target_link_libraries(fast_gicp OpenMP::OpenMP_CXX)
Expand Down Expand Up @@ -110,7 +116,7 @@ if(BUILD_VGICP_CUDA)
set(CUDA_NVCC_FLAGS "--expt-relaxed-constexpr")
add_definitions(-DUSE_VGICP_CUDA)

cuda_add_library(fast_vgicp_cuda SHARED
add_library(fast_vgicp_cuda SHARED
src/fast_gicp/cuda/fast_vgicp_cuda.cu
src/fast_gicp/cuda/brute_force_knn.cu
src/fast_gicp/cuda/covariance_estimation.cu
Expand All @@ -127,9 +133,16 @@ if(BUILD_VGICP_CUDA)
include
thirdparty/Eigen
thirdparty/nvbio
${OpenMP_CXX_FLAGS}
ld
${catkin_INCLUDE_DIRS}
)
target_link_directories(fast_vgicp_cuda
/home/neil/devel/glyd/.toolchain/c97040d85c983530acbe2e02c2327d0db40f6e7acb4dd8cbc280076c92c33ccf/sysroot/lib
)
target_link_libraries(fast_vgicp_cuda
${OpenMP_CXX_FLAGS}

${catkin_LIBRARIES}
)
cuda_add_cublas_to_target(fast_vgicp_cuda)
Expand All @@ -141,6 +154,8 @@ if(BUILD_VGICP_CUDA)
)
target_link_libraries(fast_gicp
fast_vgicp_cuda
${OpenMP_CXX_FLAGS}
ld
)
add_dependencies(fast_gicp fast_vgicp_cuda)
if(catkin_FOUND)
Expand Down
84 changes: 42 additions & 42 deletions src/kitti.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -8,8 +8,8 @@
#include <pcl/point_cloud.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_color_handlers.h>
// #include <pcl/visualization/pcl_visualizer.h>
// #include <pcl/visualization/point_cloud_color_handlers.h>

#include <fast_gicp/gicp/fast_gicp.hpp>
#include <fast_gicp/gicp/fast_vgicp.hpp>
Expand Down Expand Up @@ -105,52 +105,52 @@ int main(int argc, char** argv) {
pcl::PointCloud<pcl::PointXYZ>::Ptr trajectory(new pcl::PointCloud<pcl::PointXYZ>);
trajectory->push_back(pcl::PointXYZ(0.0f, 0.0f, 0.0f));

pcl::visualization::PCLVisualizer vis;
vis.addPointCloud<pcl::PointXYZ>(trajectory, "trajectory");
// pcl::visualization::PCLVisualizer vis;
// vis.addPointCloud<pcl::PointXYZ>(trajectory, "trajectory");

// for calculating FPS
boost::circular_buffer<std::chrono::high_resolution_clock::time_point> stamps(30);
stamps.push_back(std::chrono::high_resolution_clock::now());

for (int i = 1; i < kitti.size(); i++) {
// set the current frame as source
voxelgrid.setInputCloud(kitti.cloud(i));
pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
voxelgrid.filter(*source);
gicp.setInputSource(source);

// align and swap source and target cloud for next registration
pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>);
gicp.align(*aligned);
gicp.swapSourceAndTarget();

// accumulate pose
poses[i] = poses[i - 1] * gicp.getFinalTransformation().cast<double>();

// FPS display
stamps.push_back(std::chrono::high_resolution_clock::now());
std::cout << stamps.size() / (std::chrono::duration_cast<std::chrono::nanoseconds>(stamps.back() - stamps.front()).count() / 1e9) << "fps" << std::endl;

// visualization
trajectory->push_back(pcl::PointXYZ(poses[i](0, 3), poses[i](1, 3), poses[i](2, 3)));
vis.updatePointCloud<pcl::PointXYZ>(trajectory, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(trajectory, 255.0, 0.0, 0.0), "trajectory");
vis.spinOnce();
}
// for (int i = 1; i < kitti.size(); i++) {
// // set the current frame as source
// voxelgrid.setInputCloud(kitti.cloud(i));
// pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
// voxelgrid.filter(*source);
// gicp.setInputSource(source);

// // align and swap source and target cloud for next registration
// pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>);
// gicp.align(*aligned);
// gicp.swapSourceAndTarget();

// // accumulate pose
// poses[i] = poses[i - 1] * gicp.getFinalTransformation().cast<double>();

// // FPS display
// stamps.push_back(std::chrono::high_resolution_clock::now());
// std::cout << stamps.size() / (std::chrono::duration_cast<std::chrono::nanoseconds>(stamps.back() - stamps.front()).count() / 1e9) << "fps" << std::endl;

// // visualization
// trajectory->push_back(pcl::PointXYZ(poses[i](0, 3), poses[i](1, 3), poses[i](2, 3)));
// // vis.updatePointCloud<pcl::PointXYZ>(trajectory, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(trajectory, 255.0, 0.0, 0.0), "trajectory");
// // vis.spinOnce();
// }

// save the estimated poses
std::ofstream ofs("/tmp/traj.txt");
for (const auto& pose : poses) {
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 4; j++) {
if (i || j) {
ofs << " ";
}

ofs << pose(i, j);
}
}
ofs << std::endl;
}
// std::ofstream ofs("/tmp/traj.txt");
// for (const auto& pose : poses) {
// for (int i = 0; i < 3; i++) {
// for (int j = 0; j < 4; j++) {
// if (i || j) {
// ofs << " ";
// }

// ofs << pose(i, j);
// }
// }
// ofs << std::endl;
// }

return 0;
}
}