Skip to content

Commit

Permalink
Applied formatter (#25)
Browse files Browse the repository at this point in the history
Signed-off-by: Shintaro SAKODA <[email protected]>
  • Loading branch information
SakodaShintaro authored Dec 26, 2023
1 parent 57487a2 commit cd8838d
Show file tree
Hide file tree
Showing 12 changed files with 3,390 additions and 4,152 deletions.
12 changes: 3 additions & 9 deletions apps/align.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
#include <pclomp/gicp_omp.h>

// align point clouds and measure processing time
pcl::PointCloud<pcl::PointXYZ>::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr registration, const pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud ) {
pcl::PointCloud<pcl::PointXYZ>::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr registration, const pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud) {
registration->setInputTarget(target_cloud);
registration->setInputSource(source_cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
Expand All @@ -22,7 +22,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::
auto t2 = ros::WallTime::now();
std::cout << "single : " << (t2 - t1).toSec() * 1000 << "[msec]" << std::endl;

for(int i=0; i<10; i++) {
for(int i = 0; i < 10; i++) {
registration->align(*aligned);
}
auto t3 = ros::WallTime::now();
Expand All @@ -32,7 +32,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::
return aligned;
}


int main(int argc, char** argv) {
if(argc != 3) {
std::cout << "usage: align target.pcd source.pcd" << std::endl;
Expand Down Expand Up @@ -79,18 +78,13 @@ int main(int argc, char** argv) {
pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr gicp_omp(new pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>());
aligned = align(gicp_omp, target_cloud, source_cloud);


std::cout << "--- pcl::NDT ---" << std::endl;
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
ndt->setResolution(1.0);
aligned = align(ndt, target_cloud, source_cloud);

std::vector<int> num_threads = {1, omp_get_max_threads()};
std::vector<std::pair<std::string, pclomp::NeighborSearchMethod>> search_methods = {
{"KDTREE", pclomp::KDTREE},
{"DIRECT7", pclomp::DIRECT7},
{"DIRECT1", pclomp::DIRECT1}
};
std::vector<std::pair<std::string, pclomp::NeighborSearchMethod>> search_methods = {{"KDTREE", pclomp::KDTREE}, {"DIRECT7", pclomp::DIRECT7}, {"DIRECT1", pclomp::DIRECT1}};

pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
ndt_omp->setResolution(1.0);
Expand Down
Loading

0 comments on commit cd8838d

Please sign in to comment.