Skip to content

Commit

Permalink
Merge branch 'master' into constrain-dofs
Browse files Browse the repository at this point in the history
  • Loading branch information
@themightyoarfish
themightyoarfish committed Mar 28, 2024
2 parents fd61871 + d61cee4 commit 78d6e4d
Show file tree
Hide file tree
Showing 2 changed files with 52 additions and 13 deletions.
9 changes: 9 additions & 0 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,15 @@ option(BUILD_apps "Build application programs" ON)
option(BUILD_test "Build test programs" OFF)
option(BUILD_PYTHON_BINDINGS "Build python bindings" OFF)

if(${CMAKE_SYSTEM_PROCESSOR} STREQUAL "aarch64" OR ${CMAKE_SYSTEM_PROCESSOR} STREQUAL "arm64")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mtune=native")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -mtune=native")
else()
add_definitions(-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
set(CMAKE_C_FLAGS "-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2 -march=native")
set(CMAKE_CXX_FLAGS "-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2 -march=native")
endif()

set(CMAKE_BUILD_TYPE "Release")

find_package(PCL REQUIRED)
Expand Down
56 changes: 43 additions & 13 deletions src/align.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,9 @@
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/crop_box.h>

#include <pcl/registration/ndt.h>
#include <pcl/registration/gicp.h>
Expand All @@ -20,7 +22,7 @@

// benchmark for PCL's registration methods
template <typename Registration>
void test_pcl(Registration& reg, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& target, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& source) {
void test_pcl(Registration& reg, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& target, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& source, bool visualize = false) {
pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>);

double fitness_score = 0.0;
Expand All @@ -35,21 +37,31 @@ void test_pcl(Registration& reg, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&
double single = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1e6;
std::cout << "single:" << single << "[msec] " << std::flush;

// 100 times
if (visualize) {
pcl::visualization::PCLVisualizer viewer;
viewer.addPointCloud(target, "tgt");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "tgt");
viewer.addPointCloud(aligned, "aligned");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 1, "aligned");
while (!viewer.wasStopped()) viewer.spin();
viewer.close();
}

// 10 times
t1 = std::chrono::high_resolution_clock::now();
for (int i = 0; i < 100; i++) {
for (int i = 0; i < 10; i++) {
reg.setInputTarget(target);
reg.setInputSource(source);
reg.align(*aligned);
}
t2 = std::chrono::high_resolution_clock::now();
double multi = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1e6;
std::cout << "100times:" << multi << "[msec] fitness_score:" << fitness_score << std::endl;
std::cout << "10times:" << multi << "[msec] fitness_score:" << fitness_score << std::endl;
}

// benchmark for fast_gicp registration methods
template <typename Registration>
void test(Registration& reg, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& target, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& source) {
void test(Registration& reg, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& target, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& source, bool visualize = false) {
pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>);

double fitness_score = 0.0;
Expand All @@ -69,9 +81,9 @@ void test(Registration& reg, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& tar

std::cout << "single:" << single << "[msec] " << std::flush;

// 100 times
// 10 times
t1 = std::chrono::high_resolution_clock::now();
for (int i = 0; i < 100; i++) {
for (int i = 0; i < 10; i++) {
reg.clearTarget();
reg.clearSource();
reg.setInputTarget(target);
Expand All @@ -80,14 +92,24 @@ void test(Registration& reg, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& tar
}
t2 = std::chrono::high_resolution_clock::now();
double multi = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1e6;
std::cout << "100times:" << multi << "[msec] " << std::flush;
std::cout << "10times:" << multi << "[msec] " << std::flush;

if (visualize) {
pcl::visualization::PCLVisualizer viewer;
viewer.addPointCloud(target, "tgt");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "tgt");
viewer.addPointCloud(aligned, "aligned");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 1, "aligned");
while (!viewer.wasStopped()) viewer.spin();
viewer.close();
}

// for some tasks like odometry calculation,
// you can reuse the covariances of a source point cloud in the next registration
t1 = std::chrono::high_resolution_clock::now();
pcl::PointCloud<pcl::PointXYZ>::ConstPtr target_ = target;
pcl::PointCloud<pcl::PointXYZ>::ConstPtr source_ = source;
for (int i = 0; i < 100; i++) {
for (int i = 0; i < 10; i++) {
reg.swapSourceAndTarget();
reg.clearSource();

Expand All @@ -100,7 +122,7 @@ void test(Registration& reg, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& tar
t2 = std::chrono::high_resolution_clock::now();
double reuse = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1e6;

std::cout << "100times_reuse:" << reuse << "[msec] fitness_score:" << fitness_score << std::endl;
std::cout << "10times_reuse:" << reuse << "[msec] fitness_score:" << fitness_score << std::endl;
}

/**
Expand Down Expand Up @@ -134,15 +156,23 @@ int main(int argc, char** argv) {

// downsampling
pcl::ApproximateVoxelGrid<pcl::PointXYZ> voxelgrid;
voxelgrid.setLeafSize(0.1f, 0.1f, 0.1f);
constexpr float leaf_size = 0.1;
voxelgrid.setLeafSize(leaf_size, leaf_size, leaf_size);
pcl::CropBox<pcl::PointXYZ> crop_box;
crop_box.setMin(Eigen::Vector4f(-1, 6, -5, 1));
crop_box.setMax(Eigen::Vector4f(10, 10, 10, 1));

pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>());
voxelgrid.setInputCloud(target_cloud);
crop_box.setInputCloud(target_cloud);
crop_box.filter(*filtered);
voxelgrid.setInputCloud(filtered);
voxelgrid.filter(*filtered);
target_cloud = filtered;

filtered.reset(new pcl::PointCloud<pcl::PointXYZ>());
voxelgrid.setInputCloud(source_cloud);
crop_box.setInputCloud(source_cloud);
crop_box.filter(*filtered);
voxelgrid.setInputCloud(filtered);
voxelgrid.filter(*filtered);
source_cloud = filtered;

Expand Down

0 comments on commit 78d6e4d

Please sign in to comment.