Skip to content

Commit

Permalink
fix(ndt_omp): update clang-format and format code (#58)
Browse files Browse the repository at this point in the history 
* sync to autoware.universe

Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com>

* apply clang-format

Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com>

---------

Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com>
  • Loading branch information
@a-maumau
a-maumau authored Jul 19, 2024
1 parent ef654bd commit c482edd
Show file tree
Hide file tree
Showing 25 changed files with 2,121 additions and 1,449 deletions.
159 changes: 44 additions & 115 deletions .clang-format
View file
Original file line number Diff line number Diff line change
@@ -1,118 +1,47 @@
---
Language: Cpp
# BasedOnStyle: Google
# Modified from https://github.com/ament/ament_lint/blob/master/ament_clang_format/ament_clang_format/configuration/.clang-format
Language: Cpp
BasedOnStyle: Google

AccessModifierOffset: -2
AlignAfterOpenBracket: Align
AlignConsecutiveAssignments: false
AlignConsecutiveDeclarations: false
AlignEscapedNewlines: Left
AlignOperands: true
AlignTrailingComments: true
AllowAllParametersOfDeclarationOnNextLine: true
AllowShortBlocksOnASingleLine: false
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: Empty
AllowShortIfStatementsOnASingleLine: true
AllowShortLoopsOnASingleLine: true
AlwaysBreakAfterDefinitionReturnType: None
AlwaysBreakAfterReturnType: None
AlwaysBreakBeforeMultilineStrings: true
AlwaysBreakTemplateDeclarations: true
BinPackArguments: true
BinPackParameters: false
AlignAfterOpenBracket: AlwaysBreak
AllowShortFunctionsOnASingleLine: InlineOnly
BraceWrapping:
AfterClass: false
AfterControlStatement: false
AfterEnum: false
AfterFunction: false
AfterNamespace: false
AfterObjCDeclaration: false
AfterStruct: false
AfterUnion: false
AfterExternBlock: false
BeforeCatch: false
BeforeElse: false
IndentBraces: false
SplitEmptyFunction: true
SplitEmptyRecord: true
SplitEmptyNamespace: true
BreakBeforeBinaryOperators: None
BreakBeforeBraces: Attach
BreakBeforeInheritanceComma: false
BreakBeforeTernaryOperators: true
BreakConstructorInitializersBeforeComma: false
BreakConstructorInitializers: BeforeColon
BreakAfterJavaFieldAnnotations: false
BreakStringLiterals: false
ColumnLimit: 256
CommentPragmas: '^ IWYU pragma:'
CompactNamespaces: false
ConstructorInitializerAllOnOneLineOrOnePerLine: true
ConstructorInitializerIndentWidth: 4
ContinuationIndentWidth: 4
Cpp11BracedListStyle: true
DerivePointerAlignment: true
DisableFormat: false
ExperimentalAutoDetectBinPacking: false
FixNamespaceComments: true
ForEachMacros:
- foreach
- Q_FOREACH
- BOOST_FOREACH
IncludeBlocks: Preserve
AfterClass: true
AfterFunction: true
AfterNamespace: true
AfterStruct: true
BreakBeforeBraces: Custom
ColumnLimit: 100
ConstructorInitializerIndentWidth: 0
ContinuationIndentWidth: 2
DerivePointerAlignment: false
PointerAlignment: Middle
ReflowComments: true
IncludeCategories:
- Regex: '^<ext/.*\.h>'
Priority: 2
- Regex: '^<.*\.h>'
Priority: 1
- Regex: '^<.*'
Priority: 2
- Regex: '.*'
Priority: 3
IncludeIsMainRegex: '([-_](test|unittest))?$'
IndentCaseLabels: true
IndentPPDirectives: None
IndentWidth: 2
IndentWrappedFunctionNames: false
JavaScriptQuotes: Leave
JavaScriptWrapImports: true
KeepEmptyLinesAtTheStartOfBlocks: false
MacroBlockBegin: ''
MacroBlockEnd: ''
MaxEmptyLinesToKeep: 1
NamespaceIndentation: None
ObjCBlockIndentWidth: 2
ObjCSpaceAfterProperty: false
ObjCSpaceBeforeProtocolList: false
PenaltyBreakAssignment: 2
PenaltyBreakBeforeFirstCallParameter: 1
PenaltyBreakComment: 300
PenaltyBreakFirstLessLess: 120
PenaltyBreakString: 1000
PenaltyExcessCharacter: 0
PenaltyReturnTypeOnItsOwnLine: 200
PointerAlignment: Left
RawStringFormats:
- Language: TextProto
Delimiters:
- pb
BasedOnStyle: google
ReflowComments: true
SortIncludes: false
SortUsingDeclarations: false
SpaceAfterCStyleCast: false
SpaceAfterTemplateKeyword: false
SpaceBeforeAssignmentOperators: true
SpaceBeforeParens: Never
SpaceInEmptyParentheses: false
SpacesBeforeTrailingComments: 2
SpacesInAngles: false
SpacesInContainerLiterals: true
SpacesInCStyleCastParentheses: false
SpacesInParentheses: false
SpacesInSquareBrackets: false
Standard: Auto
TabWidth: 8
UseTab: Never
...

# C++ system headers
- Regex: <[a-z_]*>
Priority: 6
CaseSensitive: true
# C system headers
- Regex: <.*\.h>
Priority: 5
CaseSensitive: true
# Boost headers
- Regex: boost/.*
Priority: 4
CaseSensitive: true
# Message headers
- Regex: .*_msgs/.*
Priority: 3
CaseSensitive: true
- Regex: .*_srvs/.*
Priority: 3
CaseSensitive: true
# Other Package headers
- Regex: <.*>
Priority: 2
CaseSensitive: true
# Local package headers
- Regex: '".*"'
Priority: 1
CaseSensitive: true
67 changes: 41 additions & 26 deletions apps/align.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,23 @@
#include <iostream>
#include <chrono>
#include <multigrid_pclomp/multigrid_ndt_omp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/ndt.h>
#include <pcl/point_types.h>
#include <pcl/registration/gicp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/ndt.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <pclomp/ndt_omp.h>
#include <pclomp/gicp_omp.h>
#include <multigrid_pclomp/multigrid_ndt_omp.h>
#include <pclomp/ndt_omp.h>

#include <chrono>
#include <iostream>

// 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 @@ -23,7 +27,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::
auto t2 = std::chrono::system_clock::now();
std::cout << "single : " << (t2 - t1).count() / 1e6 << "[msec]" << std::endl;

for(int i = 0; i < 10; i++) {
for (int i = 0; i < 10; i++) {
registration->align(*aligned);
}
auto t3 = std::chrono::system_clock::now();
Expand All @@ -33,8 +37,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::
return aligned;
}

int main(int argc, char** argv) {
if(argc != 3) {
int main(int argc, char ** argv)
{
if (argc != 3) {
std::cout << "usage: align target.pcd source.pcd" << std::endl;
return 0;
}
Expand All @@ -45,11 +50,11 @@ int main(int argc, char** argv) {
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>());

if(pcl::io::loadPCDFile(target_pcd, *target_cloud)) {
if (pcl::io::loadPCDFile(target_pcd, *target_cloud)) {
std::cerr << "failed to load " << target_pcd << std::endl;
return 0;
}
if(pcl::io::loadPCDFile(source_pcd, *source_cloud)) {
if (pcl::io::loadPCDFile(source_pcd, *source_cloud)) {
std::cerr << "failed to load " << source_pcd << std::endl;
return 0;
}
Expand All @@ -70,30 +75,37 @@ int main(int argc, char** argv) {

// benchmark
std::cout << "--- pcl::GICP ---" << std::endl;
pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr gicp(new pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>());
pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr gicp(
new pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr aligned = align(gicp, target_cloud, source_cloud);

std::cout << "--- pclomp::GICP ---" << std::endl;
pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr gicp_omp(new pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>());
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>());
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>());
pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(
new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
ndt_omp->setResolution(1.0);

pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr mg_ndt_omp(new pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr mg_ndt_omp(
new pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
mg_ndt_omp->setResolution(1.0);

for(int n : num_threads) {
for(const auto& search_method : search_methods) {
std::cout << "--- pclomp::NDT (" << search_method.first << ", " << n << " threads) ---" << std::endl;
for (int n : num_threads) {
for (const auto & search_method : search_methods) {
std::cout << "--- pclomp::NDT (" << search_method.first << ", " << n << " threads) ---"
<< std::endl;
ndt_omp->setNumThreads(n);
ndt_omp->setNeighborhoodSearchMethod(search_method.second);
aligned = align(ndt_omp, target_cloud, source_cloud);
Expand All @@ -106,9 +118,12 @@ int main(int argc, char** argv) {

// visualization
pcl::visualization::PCLVisualizer vis("vis");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_handler(target_cloud, 255.0, 0.0, 0.0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_handler(source_cloud, 0.0, 255.0, 0.0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_handler(aligned, 0.0, 0.0, 255.0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_handler(
target_cloud, 255.0, 0.0, 0.0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_handler(
source_cloud, 0.0, 255.0, 0.0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_handler(
aligned, 0.0, 0.0, 255.0);
vis.addPointCloud(target_cloud, target_handler, "target");
vis.addPointCloud(source_cloud, source_handler, "source");
vis.addPointCloud(aligned, aligned_handler, "aligned");
Expand Down
Loading

0 comments on commit c482edd

Please sign in to comment.