jobs:
- job: osx
- displayName: macOS High Sierra
+ strategy:
+ matrix:
+ macOS Catalina:
+ VMIMAGE: 'macOS-10.15'
+ macOS Mojave:
+ VMIMAGE: 'macOS-10.14'
timeoutInMinutes: 0
pool:
- vmImage: 'macOS-10.13'
+ vmImage: '$(VMIMAGE)'
variables:
BUILD_DIR: '$(Agent.WorkFolder)/build'
GOOGLE_TEST_DIR: '$(Agent.WorkFolder)/googletest'
- script: |
mkdir $BUILD_DIR && cd $BUILD_DIR
cmake $(Build.SourcesDirectory) \
+ -DCMAKE_OSX_SYSROOT="/Applications/Xcode.app/Contents/Developer/Platforms/MacOSX.platform/Developer/SDKs/MacOSX.sdk" \
-DCMAKE_CXX_FLAGS="$CMAKE_CXX_FLAGS" \
-DGTEST_SRC_DIR="$GOOGLE_TEST_DIR/googletest" \
-DGTEST_INCLUDE_DIR="$GOOGLE_TEST_DIR/googletest/include" \
cmake --build . -- test_filters test_registration test_registration_api
cmake --build . -- -j2
displayName: 'Build Library'
- - script: cd $BUILD_DIR/test && ctest -V -T Test
+ - script: cd $BUILD_DIR && cmake --build . -- tests
displayName: 'Run Unit Tests'
- task: PublishTestResults@2
inputs:
cmake --build . -- -j2
displayName: 'Build Library'
- script: |
- cd $BUILD_DIR/test
- ctest -V -T Test
+ cd $BUILD_DIR && cmake --build . -- tests
displayName: 'Run Unit Tests'
- task: PublishTestResults@2
inputs:
cmake --build . -- -j2
displayName: 'Build Library'
- script: |
- cd $BUILD_DIR/test
- ctest -V -T Test
+ cd $BUILD_DIR && cmake --build . -- tests
displayName: 'Run Unit Tests'
- task: PublishTestResults@2
inputs:
displayName: 'CMake Configuration'
- script: cd %BUILD_DIR% && cmake --build . --config %CONFIGURATION%
displayName: 'Build Library'
- - script: cd %BUILD_DIR%/test && ctest -C %CONFIGURATION% -V -T Test
+ - script: cd %BUILD_DIR% && cmake --build . --target tests --config %CONFIGURATION%
displayName: 'Run Unit Tests'
- task: PublishTestResults@2
inputs:
cmake --build . -- install
displayName: 'Install PCL'
- script: |
- $(Build.SourcesDirectory)/.ci/scripts/build_tutorials.sh -k -s -e $EXCLUDE_TUTORIALS $INSTALL_DIR/share/pcl-1.9 $(Build.SourcesDirectory) $BUILD_DIR/tutorials
+ $(Build.SourcesDirectory)/.ci/scripts/build_tutorials.sh -k -s -e $EXCLUDE_TUTORIALS $INSTALL_DIR/share/pcl-1.10 $(Build.SourcesDirectory) $BUILD_DIR/tutorials
displayName: 'Build Tutorials'
-ARG UBUNTU_DISTRO=16.04
-FROM ubuntu:${UBUNTU_DISTRO}
+# For valid combinations, check the following repo:
+# https://gitlab.com/nvidia/container-images/cuda/tree/master/dist
+# To enable cuda, use "--build-arg USE_CUDA=true" during image build process
+ARG USE_CUDA
+ARG CUDA_VERSION="9.0"
+ARG UBUNTU_DISTRO="16.04"
+# Known conflicts:
+# * 9.1 is an issue with 16.04 for Eigen
+ARG BASE_CUDA_IMAGE=${USE_CUDA:+"nvidia/cuda:${CUDA_VERSION}-devel-ubuntu${UBUNTU_DISTRO}"}
+ARG BASE_IMAGE=${BASE_CUDA_IMAGE:-"ubuntu:${UBUNTU_DISTRO}"}
+
+FROM ${BASE_IMAGE}
ARG VTK_VERSION=6
ENV DEBIAN_FRONTEND=noninteractive
format() {
# don't use a directory with whitespace
- local whitelist="2d ml simulation stereo"
+ local whitelist="2d ml octree simulation stereo"
local PCL_DIR="${2}"
local formatter="${1}"
--- /dev/null
+---
+name: Bug report
+about: Create a report to help us improve PCL
+title: "[module_name] Provide a general summary of the issue"
+labels: 'status: triage, kind: bug'
+assignees: ''
+
+---
+
+<!--- WARNING: This is an issue tracker. Before opening a new issue make sure you read https://github.com/PointCloudLibrary/pcl/blob/master/CONTRIBUTING.md#using-the-issue-tracker. -->
+**Describe the bug**
+
+A clear and concise description of what the bug is.
+
+**Context**
+
+What are you trying to accomplish? Providing context helps us come up with a solution that is most useful in the real world
+
+**Expected behavior**
+
+A clear and concise description of what you expected to happen.
+
+**Current Behavior**
+
+What happens instead of the expected behavior?
+
+**To Reproduce**
+
+Provide a link to a live example, or an unambiguous set of steps to reproduce this bug. A reproducible example helps to provide faster answers.
+
+**Screenshots/Code snippets**
+
+In order to help explain your problem, please consider adding
+* screenshots of the GUI issues
+* code snippets: [syntax for code](https://github.com/adam-p/markdown-here/wiki/Markdown-Cheatsheet#code) with correct language highlights
+
+**Your Environment (please complete the following information):**
+
+ - OS: [e.g. Ubuntu 16.04]
+ - Compiler: [:eg GCC 8.1]
+ - PCL Version [e.g. 1.10, HEAD]
+
+**Possible Solution**
+
+Not obligatory, but suggest a fix/reason for the bug. Feel free to create a PR if you feel comfortable.
+
+**Additional context**
+
+Add any other context about the problem here.
--- /dev/null
+---
+name: Compilation failure
+about: Help! My code doesn't compile (but it should)
+title: '[compile error] "Please add a short description here"'
+labels: 'status: triage, kind: compile error'
+assignees: ''
+
+---
+
+<!--- WARNING: This is an issue tracker. Before opening a new issue make sure you read https://github.com/PointCloudLibrary/pcl/blob/master/CONTRIBUTING.md#using-the-issue-tracker. -->
+**Describe the error**
+
+Please paste the compilation results/errors.
+
+**To Reproduce**
+
+Provide a link to a live example, or an unambiguous set of steps to reproduce this bug. A reproducible example helps to provide faster answers. Custom OS, custom PCL configs or custom build systems make the issue difficult to reproduce.
+* Best reproducability: A docker image + code snippets provided here
+* Good reproducability: Common Linux OS + default PCL config + code snippets provided here
+* Poor reproducability: code snippets
+
+Remember to reproduce the error in a clean rebuild (removing all build objects and starting build from scratch)
+
+**Screenshots/Code snippets/Build information**
+
+In order to help explain your problem, please consider adding
+* screenshots of the GUI issues
+* code snippets: [syntax for code](https://github.com/adam-p/markdown-here/wiki/Markdown-Cheatsheet#code) with correct language highlights
+* debugging information:
+ * the output of cmake using `cmake --debug`
+ * the failing build command(s) using `VERBOSE=1 make`, `ninja -v` or similar
+ * steps taken to narrow down the problem
+
+**Your Environment (please complete the following information):**
+
+ - OS: [e.g. Ubuntu 16.04]
+ - Compiler: [:eg GCC 8.1]
+ - PCL Version [e.g. 1.10, HEAD] (NB: please ensure you don't have multiple versions available)
+ - PCL Type: [Installed/Compiled from source]
+
+If PCL was compiled from source or failure in compiling PCL itself:
+ - GPU, Kinfu, CUDA enabled? Yes/No
+ - List and Version of dependencies used
+ - Compilation flags are used
+
+If compiling against PCL:
+ - Checked `CMakeLists.txt` for simple errors like missing `find_package` or `target_link_libraries`?
+ * [CMakeLists.txt for PCL >= 1.9](https://github.com/kunaltyagi/pcl-cmake-minimum/blob/master/CMakeLists.txt)
+ * [CMakeLists.txt for older versions](https://github.com/PointCloudLibrary/pcl/blob/update-issue-templates/doc/tutorials/content/sources/concatenate_clouds/CMakeLists.txt)
+
+**Possible Solution**
+
+Not obligatory, but suggest a fix/reason for the bug. Feel free to create a PR if you feel comfortable.
+
+**Additional context**
+
+Add any other context about the problem here.
--- /dev/null
+---
+name: Feature request
+about: Suggest an idea for PCL
+title: '[module_name] "Please insert a short description of the feature request"'
+labels: 'status: triage, kind: request'
+assignees: ''
+
+---
+
+<!--- WARNING: This is an issue tracker. Before opening a new issue make sure you read https://github.com/PointCloudLibrary/pcl/blob/master/CONTRIBUTING.md#using-the-issue-tracker. -->
+**Is your feature request related to a problem? Please describe.**
+
+A clear and concise description of what the problem is. Ex. I'm always frustrated when [...].
+
+**Context**
+
+How has the lack of this feature this issue affected you? What are you trying to accomplish?
+Providing context helps us come up with a solution that is most useful in the real world
+
+**Expected behavior**
+
+Please tell us how the situation should be instead
+
+**Current Behavior**
+
+Please explain the difference from current behavior
+
+**Describe the solution you'd like**
+
+A clear and concise description of what you want to happen. Not obligatory, but feel free to suggest ideas on how to implement the addition or change
+
+**Describe alternatives you've considered**
+
+A clear and concise description of any alternative solutions or features you've considered.
+
+**Additional context**
+
+Add any other context or screenshots about the feature request here.
--- /dev/null
+---
+name: Something else
+about: It's none of the above
+title: "[custom] Provide a general summary of the issue"
+labels: 'status: triage'
+assignees: ''
+
+---
+
+<!--- WARNING: This is an issue tracker. Before opening a new issue make sure you read https://github.com/PointCloudLibrary/pcl/blob/master/CONTRIBUTING.md#using-the-issue-tracker. -->
+++ /dev/null
-<!--- WARNING: This is an issue tracker. Before opening a new issue make sure you read https://github.com/PointCloudLibrary/pcl/blob/master/CONTRIBUTING.md#using-the-issue-tracker. -->
-
-<!--- Provide a general summary of the issue in the Title above -->
-
-## Your Environment
-<!--- Include as many relevant details about the environment you experienced the bug in -->
-* Operating System and version:
-* Compiler:
-* PCL Version:
-
-## Context
-<!--- How has this issue affected you? What are you trying to accomplish? -->
-<!--- Providing context helps us come up with a solution that is most useful in the real world -->
-
-## Expected Behavior
-<!--- If you're describing a bug, tell us what should happen -->
-<!--- If you're suggesting a change/improvement, tell us how it should work -->
-
-## Current Behavior
-<!--- If describing a bug, tell us what happens instead of the expected behavior -->
-<!--- If suggesting a change/improvement, explain the difference from current behavior -->
-
-## Code to Reproduce
-<!--- Provide a link to a live example, or an unambiguous set of steps to -->
-<!--- reproduce this bug. Include code to reproduce, if relevant -->
-
-## Possible Solution
-<!--- Not obligatory, but suggest a fix/reason for the bug, -->
-<!--- or ideas how to implement the addition or change -->
# Configuration for probot-stale - https://github.com/probot/stale
# Number of days of inactivity before an Issue or Pull Request becomes stale
-daysUntilStale: 60
-# Number of days of inactivity before a stale Issue or Pull Request is closed
-# False means never close
-daysUntilClose: false
+daysUntilStale: 30
+
+# Number of days of inactivity before an Issue or Pull Request with the stale label is closed.
+# Set to false to disable. If disabled, issues still need to be closed manually, but will remain marked as stale.
+daysUntilClose: 7
+
+# Only issues or pull requests with all of these labels are check if stale. Defaults to `[]` (disabled)
+onlyLabels:
+ - "needs: author reply"
+ - "needs: more work"
+
# Issues or Pull Requests with these labels will never be considered stale. Set to `[]` to disable
-exemptLabels:
- - "status: needs review"
- - "status: needs testing"
- - "status: needs decision"
- - "status: ready to merge"
+exemptLabels: []
+
+# Set to true to ignore issues in a project (defaults to false)
+exemptProjects: false
+
+# Set to true to ignore issues in a milestone (defaults to false)
+exemptMilestones: false
+
+# Set to true to ignore issues with an assignee (defaults to false)
+exemptAssignees: false
+
# Label to use when marking as stale
staleLabel: "status: stale"
+
# Comment to post when marking as stale. Set to `false` to disable
-markComment: |
- This pull request has been automatically marked as stale because it hasn't had
- any activity in the past 60 days. Commenting or adding a new commit to the
- pull request will revert this.
-
- Come back whenever you have time. We look forward to your contribution.
-# Comment to post when removing the stale label. Set to `false` to disable
-unmarkComment: false
-# Comment to post when closing a stale Issue or Pull Request. Set to `false` to disable
-closeComment: false
+markComment: >
+ This issue has been automatically marked as stale because it has not had any activity in the past month. It will be closed if no further activity occurs.
+
+# Comment to post when removing the stale label.
+# unmarkComment: >
+# Your comment here.
+
+# Comment to post when closing a stale Issue or Pull Request.
+# closeComment: >
+# Your comment here.
+
+# Limit the number of actions per hour, from 1-30. Default is 30
+limitPerRun: 30
+
# Limit to only `issues` or `pulls`
-only: pulls
+# only: issues
+
+# Configuration settings that are specific to just 'pulls':
+pulls:
+ daysUntilClose: false
+ markComment: >
+ This pull request has been automatically marked as stale because it has not had any activity in the past month. Commenting or adding a new commit to the pull request will revert this.
# ChangeList
+## *= 1.10.1 (18.03.2020) =*
+
+### Notable changes
+
+**Deprecation** *of public APIs, scheduled to be removed after two minor releases*
+
+* **[common]** Deprecate several `PointWithViewpoint` ctors; make ctors more uniform in PCL point types [[#3597](https://github.com/PointCloudLibrary/pcl/pull/3597)]
+
+**Removal** *of the public APIs deprecated in previous releases*
+
+* **[common]** Remove deprecated checks for `USE_ROS` macro [[#3690](https://github.com/PointCloudLibrary/pcl/pull/3690)]
+
+**Behavior changes** *in classes, apps, or tools*
+
+* **[tools]** Continue on PCD load failure in `pcl_train_linemod_template` [[#3652](https://github.com/PointCloudLibrary/pcl/pull/3652)]
+
+### Changes grouped by module
+
+#### CMake:
+
+* Fix CMake grouping of tools targets [[#3709](https://github.com/PointCloudLibrary/pcl/pull/3709)]
+* Enable `PCL_ONLY_CORE_POINT_TYPES` in mingw builds [[#3694](https://github.com/PointCloudLibrary/pcl/pull/3694)]
+* Downgrade RSSDK2 warning message to status [[#3683](https://github.com/PointCloudLibrary/pcl/pull/3683)]
+* Fix test targets arguments for MSVC [[#3636](https://github.com/PointCloudLibrary/pcl/pull/3636)]
+* Remove duplicate `/bigobj` for MSVC [[#3604](https://github.com/PointCloudLibrary/pcl/pull/3604)]
+
+#### libpcl_common:
+
+* Better PointType ctor and reduced warnings in `register_point_struct.h` [[#3732](https://github.com/PointCloudLibrary/pcl/pull/3732)]
+* **[removal]** Remove deprecated checks for `USE_ROS` macro [[#3690](https://github.com/PointCloudLibrary/pcl/pull/3690)]
+* Replace `dirent` with `boost::filesystem` [[#3676](https://github.com/PointCloudLibrary/pcl/pull/3676)]
+* Fix code accidentally casting away const-ness [[#3648](https://github.com/PointCloudLibrary/pcl/pull/3648)]
+* Fix compilation of CUDA code on Windows [[#3634](https://github.com/PointCloudLibrary/pcl/pull/3634)]
+* Remove undefined behavior and add stricter checks in console arg parsing [[#3613](https://github.com/PointCloudLibrary/pcl/pull/3613)]
+* **[deprecation]** Deprecate several `PointWithViewpoint` ctors; make ctors more uniform in PCL point types [[#3597](https://github.com/PointCloudLibrary/pcl/pull/3597)]
+
+#### libpcl_cuda:
+
+* Fix memory leaks in CUDA apps [[#3587](https://github.com/PointCloudLibrary/pcl/pull/3587)]
+* Fix compilation of CUDA/GPU modules [[#3576](https://github.com/PointCloudLibrary/pcl/pull/3576)]
+
+#### libpcl_features:
+
+* Add precompiled `computeApproximateCovariances`; fix compilation error for the same [[#3711](https://github.com/PointCloudLibrary/pcl/pull/3711)]
+* Fix vector initialization in `NormalEstimationOMP` [[#3614](https://github.com/PointCloudLibrary/pcl/pull/3614)]
+* Fix indexing bug in `IntegralImageNormalEstimation` [[#3574](https://github.com/PointCloudLibrary/pcl/pull/3574)]
+
+#### libpcl_filters:
+
+* Set `is_dense` based on actual cloud contents in `removeNaNNormalsFromPointCloud()` [[#3685](https://github.com/PointCloudLibrary/pcl/pull/3685)]
+
+#### libpcl_gpu:
+
+* Fix compile error in KinFuLS `initRegistration` [[#3737](https://github.com/PointCloudLibrary/pcl/pull/3737)]
+* Fix illegal memory acces in CUDA Octree builder [[#3627](https://github.com/PointCloudLibrary/pcl/pull/3627)]
+* Fix compile issue in `people_app` [[#3618](https://github.com/PointCloudLibrary/pcl/pull/3618)]
+* Fix compilation of CUDA/GPU modules [[#3576](https://github.com/PointCloudLibrary/pcl/pull/3576)]
+
+#### libpcl_io:
+
+* Fix `if/ifdef` WIN32 issues [[#3668](https://github.com/PointCloudLibrary/pcl/pull/3668)]
+* Add `Grabber::toggle()` method [[#3615](https://github.com/PointCloudLibrary/pcl/pull/3615)]
+* Close the correct file in `pcl::io::savePLYFileBinary` [[#3601](https://github.com/PointCloudLibrary/pcl/pull/3601)]
+* Fix entropy range encoding in octree-based pointcloud compression [[#3579](https://github.com/PointCloudLibrary/pcl/pull/3579)]
+
+#### libpcl_surface:
+
+* Add default initialization of grid resolution in `MarchingCubes` [[#3718](https://github.com/PointCloudLibrary/pcl/pull/3718)]
+
+#### PCL Apps:
+
+* Fix `if/ifdef` WIN32 issues [[#3668](https://github.com/PointCloudLibrary/pcl/pull/3668)]
+
+#### PCL Docs:
+
+* Fix missing standard includes, reduce warnings in doxygen-enabled builds [[#3755](https://github.com/PointCloudLibrary/pcl/pull/3755)]
+
+#### PCL Tutorials:
+
+* Fix documentation for point cloud stream compression executable name [[#3693](https://github.com/PointCloudLibrary/pcl/pull/3693)]
+* Fix segfault in NARF keypoint extraction tutorial [[#3623](https://github.com/PointCloudLibrary/pcl/pull/3623)]
+
+#### PCL Tools:
+
+* Use linemod member method to save templates [[#3691](https://github.com/PointCloudLibrary/pcl/pull/3691)]
+* **[behavior change]** Continue on PCD load failure in `pcl_train_linemod_template` [[#3652](https://github.com/PointCloudLibrary/pcl/pull/3652)]
+* Warn user about unorganized PCDs in `pcl_train_linemod_template` [[#3644](https://github.com/PointCloudLibrary/pcl/pull/3644)]
+
+
## *= 1.10.0 (19.01.2020) =*
Starting with PCL 1.10, to ensure compatibility with future PCL releases, please
set(CMAKE_BUILD_TYPE RelWithDebInfo CACHE STRING "build type default to RelWithDebInfo, set to Release to improve performance" FORCE)
endif()
-project(PCL VERSION 1.10.0)
+project(PCL VERSION 1.10.1)
string(TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
### ---[ Find universal dependencies
# ---[ Include pkgconfig
include(FindPkgConfig)
-# ---[ Release/Debug specific flags
-if(CMAKE_BUILD_TYPE STREQUAL "Release" OR CMAKE_BUILD_TYPE STREQUAL "RelWithDebInfo")
- add_definitions("-DBOOST_DISABLE_ASSERTS -DEIGEN_NO_DEBUG")
-endif()
if(WIN32 AND NOT MINGW)
set(CMAKE_DEBUG_POSTFIX "d" CACHE STRING "Add postfix to target for Debug build.")
set(CMAKE_RELEASE_POSTFIX "" CACHE STRING "Add postfix to target for Release build.")
set(CMAKE_COMPILER_IS_PATHSCALE 1)
elseif(MSVC)
set(CMAKE_COMPILER_IS_MSVC 1)
+elseif(MINGW)
+ set(CMAKE_COMPILER_IS_MINGW 1)
endif()
# Create a variable with expected default CXX flags
endif()
if(CMAKE_COMPILER_IS_MSVC)
- add_definitions("-DBOOST_ALL_NO_LIB -D_SCL_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_WARNINGS -DNOMINMAX -DPCL_ONLY_CORE_POINT_TYPES /bigobj ${SSE_DEFINITIONS}")
-
+ add_definitions("-DBOOST_ALL_NO_LIB -D_SCL_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_WARNINGS -DNOMINMAX -DPCL_ONLY_CORE_POINT_TYPES ${SSE_DEFINITIONS}")
+
+ add_compile_options(/bigobj)
+
if("${CMAKE_CXX_FLAGS}" STREQUAL "${CMAKE_CXX_FLAGS_DEFAULT}")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /bigobj /fp:precise /wd4800 /wd4521 /wd4251 /wd4275 /wd4305 /wd4355 ${SSE_FLAGS_STR}")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /fp:precise /wd4800 /wd4521 /wd4251 /wd4275 /wd4305 /wd4355 ${SSE_FLAGS_STR}")
# Add extra code generation/link optimizations
if(CMAKE_MSVC_CODE_LINK_OPTIMIZATION)
set(CLANG_LIBRARIES "stdc++")
endif()
+if(CMAKE_COMPILER_IS_MINGW)
+ add_definitions(-DPCL_ONLY_CORE_POINT_TYPES)
+endif()
+
include("${PCL_SOURCE_DIR}/cmake/pcl_utils.cmake")
DISSECT_VERSION()
GET_OS_INFO()
elseif(${VTK_RENDERING_BACKEND} STREQUAL "OpenGL2")
set(VTK_RENDERING_BACKEND_OPENGL_VERSION "2")
endif()
- set(HAVE_VTK ON)
else()
set(VTK_FOUND OFF)
- set(HAVE_VTK OFF)
message("Warning: You are to build PCL in STATIC but VTK is SHARED!")
message("Warning: VTK disabled!")
endif()
endif()
else()
set(VTK_FOUND OFF)
- set(HAVE_VTK OFF)
endif()
[![Release][release-image]][releases]
[![License][license-image]][license]
-[release-image]: https://img.shields.io/badge/release-1.10.0-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-1.10.1-green.svg?style=flat
[releases]: https://github.com/PointCloudLibrary/pcl/releases
[license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat
Documentation
-------------
- [Tutorials](http://www.pointclouds.org/documentation/tutorials/)
-- [PCL trunk documentation](http://docs.pointclouds.org/trunk/) (updated daily)
+- [PCL trunk documentation](https://pointcloudlibrary.github.io/documentation/) (updated nightly)
Contributing
------------
nearestKSearch (flann_index_, histogram, NN_, indices, distances);
//gather NN-search results
- double score = 0;
for (int i = 0; i < NN_; ++i)
{
- score = distances[0][i];
index_score is;
is.idx_models_ = indices[0][i];
is.idx_input_ = static_cast<int> (idx);
- is.score_ = score;
+ is.score_ = distances[0][i];
indices_scores.push_back (is);
}
}
nearestKSearch (flann_index_, histogram, NN_, indices, distances);
//gather NN-search results
- double score = 0;
for (int i = 0; i < NN_; ++i)
{
- score = distances[0][i];
index_score is;
is.idx_models_ = indices[0][i];
is.idx_input_ = static_cast<int> (idx);
- is.score_ = score;
+ is.score_ = distances[0][i];
indices_scores.push_back (is);
}
}
if (text_color_variant.canConvert<QColor> ())
{
QColor text_color = text_color_variant.value<QColor> ();
- QPalette palette = option.palette;
QStyleOptionViewItem option_copy = option;
option_copy.palette.setColor (QPalette::HighlightedText, text_color);
QStyledItemDelegate::paint (painter, option_copy, index);
{
QDir plugin_dir = QCoreApplication::applicationDirPath ();
qDebug() << plugin_dir.path ()<< " "<<QDir::cleanPath ("../lib/cloud_composer_plugins");
-#if _WIN32
+#ifdef _WIN32
if (!plugin_dir.cd (QDir::cleanPath ("cloud_composer_plugins")))
#else
if (!plugin_dir.cd (QDir::cleanPath ("../lib/cloud_composer_plugins")))
#endif
{
- #if _WIN32
+ #ifdef _WIN32
if (!plugin_dir.cd (QDir::cleanPath ("cloud_composer_plugins")))
#else
if (!plugin_dir.cd (QDir::cleanPath ("../lib")))
}
}
QStringList plugin_filter;
-#if _WIN32
+#ifdef _WIN32
plugin_filter << "pcl_cc_tool_*.dll";
#else
plugin_filter << "libpcl_cc_tool_*.so";
parent_item = model_->invisibleRootItem();
else
parent_item = model_->itemFromIndex (parent);
- QString project_name = model_->getName ();
for (int row = start; row <= end; ++row)
{
QStandardItem* new_item = parent_item->child(row);
parent_item = model_->invisibleRootItem();
else
parent_item = model_->itemFromIndex (parent);
- QString project_name = model_->getName ();
//qDebug () << "Rows about to be removed, parent = "<<parent_item->text ()<<" start="<<start<<" end="<<end;
for (int row = start; row <= end; ++row)
{
const PointIHS& pt_d_t_4 = cloud_data_transformed->operator [] (ind_4);
VertexIndex& vi_0 = vertex_indices [ind_0];
- VertexIndex& vi_1 = vertex_indices [ind_1];
- VertexIndex& vi_2 = vertex_indices [ind_2];
- VertexIndex& vi_3 = vertex_indices [ind_3];
- VertexIndex& vi_4 = vertex_indices [ind_4];
const float weight = -pt_d_0.normal_z; // weight = -dot (normal, [0; 0; 1])
// 4 - 2 1 //
// \ \ //
// * 3 - 0 //
+
+ VertexIndex& vi_1 = vertex_indices [ind_1];
+ VertexIndex& vi_2 = vertex_indices [ind_2];
+ VertexIndex& vi_3 = vertex_indices [ind_3];
+ VertexIndex& vi_4 = vertex_indices [ind_4];
+
this->addToMesh (pt_d_t_0,pt_d_t_1,pt_d_t_2,pt_d_t_3, vi_0,vi_1,vi_2,vi_3, mesh_model);
if (Mesh::IsManifold::value) // Only needed for the manifold mesh
{
# include <OpenGL/gl.h>
# include <OpenGL/glu.h>
#else
-#if _WIN32
+#ifdef _WIN32
// Need this to pull in APIENTRY, etc.
#include "windows.h"
#endif
# RSSDK2_INCLUDE_DIRS The location(s) of RealSense SDK 2.0 headers
# RSSDK2_LIBRARIES Libraries needed to use RealSense SDK 2.0
-find_package(realsense2)
+find_package(realsense2 QUIET)
set(RSSDK2_FOUND ${realsense2_FOUND})
set(RSSDK2_INCLUDE_DIRS ${realsense2_INCLUDE_DIR})
if(RSSDK2_FOUND)
message(STATUS "RealSense SDK 2 found (include: ${RSSDK2_INCLUDE_DIRS}, lib: ${RSSDK2_LIBRARIES}, version: ${realsense2_VERSION})")
+else ()
+ message(STATUS "Could NOT find RSSDK2")
endif()
# must link explicitly against boost only on Windows
target_link_libraries(${_exename} ${Boost_LIBRARIES})
+ #Only applies to MSVC
+ if(MSVC)
+ #Requires CMAKE version 3.13.0
+ if(CMAKE_VERSION VERSION_LESS "3.13.0" AND (NOT ArgumentWarningShown))
+ message(WARNING "Arguments for unit test projects are not added - this requires at least CMake 3.13. Can be added manually in \"Project settings -> Debugging -> Command arguments\"")
+ SET (ArgumentWarningShown TRUE PARENT_SCOPE)
+ else()
+ #Only add if there are arguments to test
+ if(PCL_ADD_TEST_ARGUMENTS)
+ string (REPLACE ";" " " PCL_ADD_TEST_ARGUMENTS_STR "${PCL_ADD_TEST_ARGUMENTS}")
+ set_target_properties(${_exename} PROPERTIES VS_DEBUGGER_COMMAND_ARGUMENTS ${PCL_ADD_TEST_ARGUMENTS_STR})
+ endif()
+ endif()
+ endif()
+
set_target_properties(${_exename} PROPERTIES FOLDER "Tests")
add_test(NAME ${_name} COMMAND ${_exename} ${PCL_ADD_TEST_ARGUMENTS})
#pragma once
-#ifdef USE_ROS
- #error USE_ROS setup requires PCL to compile against ROS message headers, which is now deprecated
-#endif
-
-#include <string>
-#include <vector>
-#include <boost/shared_ptr.hpp>
-#include <pcl/pcl_macros.h>
-#include <ostream>
+#include <string> // for string
+#include <ostream> // for ostream
+
+#include <pcl/make_shared.h> // for shared_ptr
namespace pcl
{
#pragma once
-#include <string>
-#include <vector>
-#include <ostream>
+#include <string> // for string
+#include <vector> // for vector
+#include <ostream> // for ostream
-#ifdef USE_ROS
- #error USE_ROS setup requires PCL to compile against ROS message headers, which is now deprecated
-#endif
-
-// Include the correct Header path here
-#include <pcl/PCLHeader.h>
+#include <pcl/PCLHeader.h> // for PCLHeader
namespace pcl
{
#pragma once
-#ifdef USE_ROS
- #error USE_ROS setup requires PCL to compile against ROS message headers, which is now deprecated
-#endif
-
#include <ostream>
#include <vector>
#include <boost/predef/other/endian.h>
-// Include the correct Header path here
#include <pcl/PCLHeader.h>
#include <pcl/PCLPointField.h>
}
s << "is_dense: ";
s << " " << v.is_dense << std::endl;
-
+
return (s);
}
#pragma once
-#ifdef USE_ROS
- #error USE_ROS setup requires PCL to compile against ROS message headers, which is now deprecated
-#endif
-
-#include <string>
-#include <vector>
-#include <ostream>
-#include <pcl/pcl_macros.h>
+#include <string> // for string
+#include <ostream> // for ostream
+
+#include <pcl/pcl_macros.h> // for shared_ptr
namespace pcl
{
#include <fstream>
#include <iostream>
+#include <vector>
-namespace pcl
+namespace pcl
{
/** \brief This represents a bivariate polynomial and provides some functionality for it
- * \author Bastian Steder
+ * \author Bastian Steder
* \ingroup common
*/
template<typename real>
- class BivariatePolynomialT
+ class BivariatePolynomialT
{
public:
//-----CONSTRUCTOR&DESTRUCTOR-----
/** Calculate the value of the polynomial at the given point */
real
- getValue (real x, real y) const;
+ getValue (real x, real y) const;
/** Calculate the gradient of this polynomial
* If forceRecalc is false, it will do nothing when the gradient already exists */
* !!Currently only implemented for degree 2!! */
void
findCriticalPoints (std::vector<real>& x_values, std::vector<real>& y_values, std::vector<int>& types) const;
-
+
/** write as binary to a stream */
void
writeBinary (std::ostream& os) const;
/** read binary from a file */
void
readBinary (const char* filename);
-
+
/** How many parameters has a bivariate polynomial of the given degree */
static unsigned int
getNoOfParametersFromDegree (int n) { return ((n+2)* (n+1))/2;}
int degree;
real* parameters;
BivariatePolynomialT<real>* gradient_x, * gradient_y;
-
+
protected:
//-----METHODS-----
/** Delete all members */
#pragma once
#include <iostream>
-#ifndef _WIN32
- #include <dirent.h>
-#endif
#include <vector>
#include <algorithm>
#define BIVARIATE_POLYNOMIAL_HPP
#include <algorithm>
+#include <cmath>
+#include <fstream>
+#include <iostream>
+#include <vector>
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename real>
pcl::BivariatePolynomialT<real>::deepCopy (const pcl::BivariatePolynomialT<real>& other)
{
if (this == &other) return;
- if (degree != other.degree)
+ if (degree != other.degree)
{
memoryCleanUp ();
degree = other.degree;
parameters = new real[getNoOfParameters ()];
}
- if (other.gradient_x == NULL)
+ if (other.gradient_x == NULL)
{
delete gradient_x; gradient_x=NULL;
delete gradient_y; gradient_y=NULL;
}
- else if (gradient_x==NULL)
+ else if (gradient_x==NULL)
{
gradient_x = new pcl::BivariatePolynomialT<real> ();
gradient_y = new pcl::BivariatePolynomialT<real> ();
std::copy_n(other.parameters, getNoOfParameters (), parameters);
- if (other.gradient_x != NULL)
+ if (other.gradient_x != NULL)
{
gradient_x->deepCopy (*other.gradient_x);
gradient_y->deepCopy (*other.gradient_y);
pcl::BivariatePolynomialT<real>::calculateGradient (bool forceRecalc)
{
if (gradient_x!=NULL && !forceRecalc) return;
-
+
if (gradient_x == NULL)
gradient_x = new pcl::BivariatePolynomialT<real> (degree-1);
if (gradient_y == NULL)
gradient_y = new pcl::BivariatePolynomialT<real> (degree-1);
-
+
unsigned int parameterPosDx=0, parameterPosDy=0;
- for (int xDegree=degree; xDegree>=0; xDegree--)
+ for (int xDegree=degree; xDegree>=0; xDegree--)
{
- for (int yDegree=degree-xDegree; yDegree>=0; yDegree--)
+ for (int yDegree=degree-xDegree; yDegree>=0; yDegree--)
{
- if (xDegree > 0)
+ if (xDegree > 0)
{
gradient_x->parameters[parameterPosDx] = xDegree * parameters[parameterPosDx];
parameterPosDx++;
}
- if (yDegree > 0)
+ if (yDegree > 0)
{
gradient_y->parameters[parameterPosDy] = yDegree * parameters[ ( (degree+2-xDegree)* (degree+1-xDegree))/2 -
yDegree - 1];
unsigned int parametersSize = getNoOfParameters ();
real* tmpParameter = ¶meters[parametersSize-1];
real tmpX=1.0, tmpY, ret=0;
- for (int xDegree=0; xDegree<=degree; xDegree++)
+ for (int xDegree=0; xDegree<=degree; xDegree++)
{
tmpY = 1.0;
for (int yDegree=0; yDegree<=degree-xDegree; yDegree++)
x_values.clear ();
y_values.clear ();
types.clear ();
-
+
if (degree == 2)
{
real x = (real(2)*parameters[2]*parameters[3] - parameters[1]*parameters[4]) /
(parameters[1]*parameters[1] - real(4)*parameters[0]*parameters[3]),
y = (real(-2)*parameters[0]*x - parameters[2]) / parameters[1];
-
+
if (!std::isfinite(x) || !std::isfinite(y))
return;
-
+
int type = 2;
real det_H = real(4)*parameters[0]*parameters[3] - parameters[1]*parameters[1];
//std::cout << "det(H) = "<<det_H<<"\n";
real* tmpParameter = p.parameters;
bool first = true;
real currentParameter;
- for (int xDegree=p.degree; xDegree>=0; xDegree--)
+ for (int xDegree=p.degree; xDegree>=0; xDegree--)
{
- for (int yDegree=p.degree-xDegree; yDegree>=0; yDegree--)
+ for (int yDegree=p.degree-xDegree; yDegree>=0; yDegree--)
{
currentParameter = *tmpParameter;
- if (!first)
+ if (!first)
{
os << (currentParameter<0.0?" - ":" + ");
currentParameter = std::abs (currentParameter);
}
os << currentParameter;
- if (xDegree>0)
+ if (xDegree>0)
{
os << "x";
if (xDegree>1)
os<<"^"<<xDegree;
}
- if (yDegree>0)
+ if (yDegree>0)
{
os << "y";
if (yDegree>1)
os<<"^"<<yDegree;
}
-
+
first = false;
tmpParameter++;
}
template<typename real> void
pcl::BivariatePolynomialT<real>::writeBinary (std::ostream& os) const
{
- os.write (reinterpret_cast<char*> (°ree), sizeof (int));
+ os.write (reinterpret_cast<const char*> (°ree), sizeof (int));
unsigned int paramCnt = getNoOfParametersFromDegree (this->degree);
- os.write (reinterpret_cast<char*> (this->parameters), paramCnt * sizeof (real));
+ os.write (reinterpret_cast<const char*> (this->parameters), paramCnt * sizeof (real));
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}
#endif
-
{
float area = 0.0f;
int num_points = polygon.size ();
- int j = 0;
Eigen::Vector3f va,vb,res;
res(0) = res(1) = res(2) = 0.0f;
for (int i = 0; i < num_points; ++i)
{
- j = (i + 1) % num_points;
+ int j = (i + 1) % num_points;
va = polygon[i].getVector3fMap ();
vb = polygon[j].getVector3fMap ();
res += va.cross (vb);
#include <array>
#include <algorithm>
-
+#include <cmath>
#include <pcl/console/print.h>
//////////////////////////////////////////////////////////////////////////////////////////
}
//////////////////////////////////////////////////////////////////////////////////////////
-void
-pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
- const Eigen::Vector3f& y_direction,
+void
+pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
+ const Eigen::Vector3f& y_direction,
Eigen::Affine3f& transformation)
{
Eigen::Vector3f tmp0 = (y_direction.cross(z_axis)).normalized();
Eigen::Vector3f tmp1 = (z_axis.cross(tmp0)).normalized();
Eigen::Vector3f tmp2 = z_axis.normalized();
-
+
transformation(0,0)=tmp0[0]; transformation(0,1)=tmp0[1]; transformation(0,2)=tmp0[2]; transformation(0,3)=0.0f;
transformation(1,0)=tmp1[0]; transformation(1,1)=tmp1[1]; transformation(1,2)=tmp1[2]; transformation(1,3)=0.0f;
transformation(2,0)=tmp2[0]; transformation(2,1)=tmp2[1]; transformation(2,2)=tmp2[2]; transformation(2,3)=0.0f;
}
//////////////////////////////////////////////////////////////////////////////////////////
-Eigen::Affine3f
-pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
+Eigen::Affine3f
+pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
const Eigen::Vector3f& y_direction)
{
Eigen::Affine3f transformation;
}
//////////////////////////////////////////////////////////////////////////////////////////
-void
-pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
- const Eigen::Vector3f& y_direction,
+void
+pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
+ const Eigen::Vector3f& y_direction,
Eigen::Affine3f& transformation)
{
Eigen::Vector3f tmp2 = (x_axis.cross(y_direction)).normalized();
Eigen::Vector3f tmp1 = (tmp2.cross(x_axis)).normalized();
Eigen::Vector3f tmp0 = x_axis.normalized();
-
+
transformation(0,0)=tmp0[0]; transformation(0,1)=tmp0[1]; transformation(0,2)=tmp0[2]; transformation(0,3)=0.0f;
transformation(1,0)=tmp1[0]; transformation(1,1)=tmp1[1]; transformation(1,2)=tmp1[2]; transformation(1,3)=0.0f;
transformation(2,0)=tmp2[0]; transformation(2,1)=tmp2[1]; transformation(2,2)=tmp2[2]; transformation(2,3)=0.0f;
}
//////////////////////////////////////////////////////////////////////////////////////////
-Eigen::Affine3f
-pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
+Eigen::Affine3f
+pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
const Eigen::Vector3f& y_direction)
{
Eigen::Affine3f transformation;
}
//////////////////////////////////////////////////////////////////////////////////////////
-void
-pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
- const Eigen::Vector3f& z_axis,
+void
+pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
+ const Eigen::Vector3f& z_axis,
Eigen::Affine3f& transformation)
{
getTransFromUnitVectorsZY (z_axis, y_direction, transformation);
}
//////////////////////////////////////////////////////////////////////////////////////////
-Eigen::Affine3f
-pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
+Eigen::Affine3f
+pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
const Eigen::Vector3f& z_axis)
{
Eigen::Affine3f transformation;
return (transformation);
}
-void
-pcl::getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction,
+void
+pcl::getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction,
const Eigen::Vector3f& z_axis,
- const Eigen::Vector3f& origin,
+ const Eigen::Vector3f& origin,
Eigen::Affine3f& transformation)
{
getTransformationFromTwoUnitVectors(y_direction, z_axis, transformation);
}
//////////////////////////////////////////////////////////////////////////////////////////
-template <typename Scalar> void
-pcl::getTransformation (Scalar x, Scalar y, Scalar z,
- Scalar roll, Scalar pitch, Scalar yaw,
+template <typename Scalar> void
+pcl::getTransformation (Scalar x, Scalar y, Scalar z,
+ Scalar roll, Scalar pitch, Scalar yaw,
Eigen::Transform<Scalar, 3, Eigen::Affine> &t)
{
Scalar A = std::cos (yaw), B = sin (yaw), C = std::cos (pitch), D = sin (pitch),
}
//////////////////////////////////////////////////////////////////////////////////////////
-template <typename Derived> void
+template <typename Derived> void
pcl::saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file)
{
std::uint32_t rows = static_cast<std::uint32_t> (matrix.rows ()), cols = static_cast<std::uint32_t> (matrix.cols ());
}
//////////////////////////////////////////////////////////////////////////////////////////
-template <typename Derived> void
+template <typename Derived> void
pcl::loadBinary (Eigen::MatrixBase<Derived> const & matrix_, std::istream& file)
{
Eigen::MatrixBase<Derived> &matrix = const_cast<Eigen::MatrixBase<Derived> &> (matrix_);
file.read (reinterpret_cast<char*> (&cols), sizeof (cols));
if (matrix.rows () != static_cast<int>(rows) || matrix.cols () != static_cast<int>(cols))
matrix.derived().resize(rows, cols);
-
+
for (std::uint32_t i = 0; i < rows; ++i)
for (std::uint32_t j = 0; j < cols; ++j)
{
}
//////////////////////////////////////////////////////////////////////////////////////////
-template <typename Derived, typename OtherDerived>
+template <typename Derived, typename OtherDerived>
typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
pcl::umeyama (const Eigen::MatrixBase<Derived>& src, const Eigen::MatrixBase<OtherDerived>& dst, bool with_scaling)
{
}
#endif //PCL_COMMON_EIGEN_IMPL_HPP_
-
#ifndef PCL_COMMON_FILE_IO_IMPL_HPP_
#define PCL_COMMON_FILE_IO_IMPL_HPP_
+#include <boost/filesystem.hpp>
+#include <boost/range/iterator_range.hpp>
+
+#include <algorithm>
+#include <cstddef>
+#include <iostream>
+#include <string>
+#include <vector>
+
namespace pcl
{
-#ifndef _WIN32
- void getAllPcdFilesInDirectory(const std::string& directory, std::vector<std::string>& file_names)
+void getAllPcdFilesInDirectory(const std::string& directory, std::vector<std::string>& file_names)
+{
+ boost::filesystem::path p(directory);
+ if(boost::filesystem::is_directory(p))
{
- DIR *dp;
- struct dirent *dirp;
- if((dp = opendir(directory.c_str())) == nullptr) {
- std::cerr << "Could not open directory.\n";
- return;
- }
- while ((dirp = readdir(dp)) != nullptr) {
- if (dirp->d_type == DT_REG) // Only regular files
+ for(const auto& entry : boost::make_iterator_range(boost::filesystem::directory_iterator(p), {}))
+ {
+ if (boost::filesystem::is_regular_file(entry))
{
- std::string file_name = dirp->d_name;
- if (file_name.substr(file_name.size()-4, 4)==".pcd")
- file_names.emplace_back(dirp->d_name);
+ if (entry.path().extension() == ".pcd")
+ file_names.emplace_back(entry.path().filename().string());
}
}
- closedir(dp);
- std::sort(file_names.begin(), file_names.end());
- //for (unsigned int i=0; i<file_names.size(); ++i)
- //std::cout << file_names[i]<<"\n";
}
-#endif
+ else
+ {
+ std::cerr << "Given path is not a directory\n";
+ return;
+ }
+ std::sort(file_names.begin(), file_names.end());
+}
std::string getFilenameWithoutPath(const std::string& input)
{
} // namespace end
#endif
-
/* \author Bastian Steder */
+#include <algorithm>
+#include <cmath>
+// #include <iostream>
+
namespace pcl {
return;
}
- double root1, root2, root3, root4,
- a2 = a*a,
+ double a2 = a*a,
a3 = a2*a,
a4 = a2*a2,
b2 = b*b,
}
else if (quadraticRoot > 0.0)
{
- root1 = sqrt (quadraticRoot);
+ double root1 = sqrt (quadraticRoot);
roots.push_back (root1 - resubValue);
roots.push_back (-root1 - resubValue);
}
if (tmp1 > 0)
{
tmp1 = sqrt (tmp1);
- root1 = - (b/ (4.0*a)) + 0.5* (w+tmp1);
- root2 = - (b/ (4.0*a)) + 0.5* (w-tmp1);
+ double root1 = - (b/ (4.0*a)) + 0.5* (w+tmp1);
+ double root2 = - (b/ (4.0*a)) + 0.5* (w-tmp1);
roots.push_back (root1);
roots.push_back (root2);
}
else if (isNearlyZero (tmp1))
{
- root1 = - (b/ (4.0*a)) + 0.5*w;
+ double root1 = - (b/ (4.0*a)) + 0.5*w;
roots.push_back (root1);
}
if (tmp2 > 0)
{
tmp2 = sqrt (tmp2);
- root3 = - (b/ (4.0*a)) + 0.5* (-w+tmp2);
- root4 = - (b/ (4.0*a)) + 0.5* (-w-tmp2);
+ double root3 = - (b/ (4.0*a)) + 0.5* (-w+tmp2);
+ double root4 = - (b/ (4.0*a)) + 0.5* (-w-tmp2);
roots.push_back (root3);
roots.push_back (root4);
}
else if (isNearlyZero (tmp2))
{
- root3 = - (b/ (4.0*a)) - 0.5*w;
+ double root3 = - (b/ (4.0*a)) - 0.5*w;
roots.push_back (root3);
}
#include <immintrin.h>
#endif
+#include <algorithm>
+#include <cmath>
+#include <cstddef>
+#include <vector>
+
namespace pcl
{
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename Scalar> void
-pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
bool copy_all_fields)
// otherwise we get errors during the multiplication (?)
for (std::size_t i = 0; i < cloud_out.points.size (); ++i)
{
- if (!std::isfinite (cloud_in.points[i].x) ||
- !std::isfinite (cloud_in.points[i].y) ||
+ if (!std::isfinite (cloud_in.points[i].x) ||
+ !std::isfinite (cloud_in.points[i].y) ||
!std::isfinite (cloud_in.points[i].z))
continue;
tf.se3 (cloud_in[i].data, cloud_out[i].data);
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename Scalar> void
-pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
- const std::vector<int> &indices,
+pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+ const std::vector<int> &indices,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
bool copy_all_fields)
{
if (copy_all_fields)
cloud_out.points[i] = cloud_in.points[indices[i]];
- if (!std::isfinite (cloud_in.points[indices[i]].x) ||
- !std::isfinite (cloud_in.points[indices[i]].y) ||
+ if (!std::isfinite (cloud_in.points[indices[i]].x) ||
+ !std::isfinite (cloud_in.points[indices[i]].y) ||
!std::isfinite (cloud_in.points[indices[i]].z))
continue;
tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename Scalar> void
-pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
+pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
bool copy_all_fields)
{
for (std::size_t i = 0; i < cloud_out.points.size (); ++i)
{
- if (!std::isfinite (cloud_in.points[i].x) ||
- !std::isfinite (cloud_in.points[i].y) ||
+ if (!std::isfinite (cloud_in.points[i].x) ||
+ !std::isfinite (cloud_in.points[i].y) ||
!std::isfinite (cloud_in.points[i].z))
continue;
tf.se3 (cloud_in[i].data, cloud_out[i].data);
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename Scalar> void
-pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
- const std::vector<int> &indices,
+pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
+ const std::vector<int> &indices,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
bool copy_all_fields)
if (copy_all_fields)
cloud_out.points[i] = cloud_in.points[indices[i]];
- if (!std::isfinite (cloud_in.points[indices[i]].x) ||
- !std::isfinite (cloud_in.points[indices[i]].y) ||
+ if (!std::isfinite (cloud_in.points[indices[i]].x) ||
+ !std::isfinite (cloud_in.points[indices[i]].y) ||
!std::isfinite (cloud_in.points[indices[i]].z))
continue;
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename Scalar> inline void
-pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Matrix<Scalar, 3, 1> &offset,
+ const Eigen::Matrix<Scalar, 3, 1> &offset,
const Eigen::Quaternion<Scalar> &rotation,
bool copy_all_fields)
{
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename Scalar> inline void
-pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
+pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Matrix<Scalar, 3, 1> &offset,
+ const Eigen::Matrix<Scalar, 3, 1> &offset,
const Eigen::Quaternion<Scalar> &rotation,
bool copy_all_fields)
{
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename Scalar> inline PointT
-pcl::transformPoint (const PointT &point,
+pcl::transformPoint (const PointT &point,
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
PointT ret = point;
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename Scalar> inline PointT
-pcl::transformPointWithNormal (const PointT &point,
+pcl::transformPointWithNormal (const PointT &point,
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
PointT ret = point;
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename Scalar> double
-pcl::getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud,
+pcl::getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud,
Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
Eigen::Matrix<Scalar, 4, 1> centroid;
-
+
pcl::computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid);
EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> eigen_vects;
double rel1 = eigen_vals.coeff (0) / eigen_vals.coeff (1);
double rel2 = eigen_vals.coeff (1) / eigen_vals.coeff (2);
-
+
transform.translation () = centroid.head (3);
transform.linear () = eigen_vects;
-
+
return (std::min (rel1, rel2));
}
#include <pcl/PointIndices.h>
#include <pcl/conversions.h>
#include <pcl/exceptions.h>
+#include <pcl/pcl_macros.h>
#include <pcl/PolygonMesh.h>
#include <locale>
* \ingroup common
*/
template <typename PointT>
- [[deprecated("use getFieldIndex<PointT> (field_name, fields) instead")]]
+ PCL_DEPRECATED("use getFieldIndex<PointT> (field_name, fields) instead")
inline int
getFieldIndex (const pcl::PointCloud<PointT> &cloud, const std::string &field_name,
std::vector<pcl::PCLPointField> &fields);
* \ingroup common
*/
template <typename PointT>
- [[deprecated("use getFields<PointT> () with return value instead")]]
+ PCL_DEPRECATED("use getFields<PointT> () with return value instead")
inline void
getFields (const pcl::PointCloud<PointT> &cloud, std::vector<pcl::PCLPointField> &fields);
* \ingroup common
*/
template <typename PointT>
- [[deprecated("use getFields<PointT> () with return value instead")]]
+ PCL_DEPRECATED("use getFields<PointT> () with return value instead")
inline void
getFields (std::vector<pcl::PCLPointField> &fields);
* \return true if successful, false otherwise (e.g., name/number of fields differs)
* \ingroup common
*/
- [[deprecated("use pcl::concatenate() instead, but beware of subtle difference in behavior (see documentation)")]]
+ PCL_DEPRECATED("use pcl::concatenate() instead, but beware of subtle difference in behavior (see documentation)")
PCL_EXPORTS bool
concatenatePointCloud (const pcl::PCLPointCloud2 &cloud1,
const pcl::PCLPointCloud2 &cloud2,
template<> inline bool isFinite<pcl::Axis>(const pcl::Axis&) { return (true); }
template<> inline bool isFinite<pcl::BRISKSignature512>(const pcl::BRISKSignature512&) { return (true); }
- template<> inline bool isFinite<pcl::BorderDescription>(const pcl::BorderDescription &p) { return true; }
+ template<> inline bool isFinite<pcl::BorderDescription>(const pcl::BorderDescription &) { return true; }
template<> inline bool isFinite<pcl::Boundary>(const pcl::Boundary&) { return (true); }
template<> inline bool isFinite<pcl::ESFSignature640>(const pcl::ESFSignature640&) { return (true); }
template<> inline bool isFinite<pcl::FPFHSignature33>(const pcl::FPFHSignature33&) { return (true); }
#pragma once
+#include <deque>
#include <functional>
+#include <map>
#include <mutex>
+#include <utility>
namespace pcl
{
#pragma once
+#include <cmath>
#include <limits>
namespace pcl
* \param[in] eps epsilon
* \return true if val1 is equal to val2, false otherwise.
*/
- template<typename T> bool
+ template<typename T> bool
equal (T val1, T val2, T eps = std::numeric_limits<T>::min ())
{
return (std::fabs (val1 - val2) < eps);
PCL_EXPORTS int
parse_argument (int argc, const char * const * argv, const char * str, unsigned int &val);
+ /** \brief Parse for a specific given command line argument.
+ * \param[in] argc the number of command line arguments
+ * \param[in] argv the command line arguments
+ * \param[in] str the string value to search for
+ * \param[out] val the resultant value
+ * \return index of found argument or -1 if arguments do not appear in list
+ */
+ PCL_EXPORTS int
+ parse_argument (int argc, const char * const * argv, const char * str, long int &val) noexcept;
+
/** \brief Parse for a specific given command line argument.
* \param[in] argc the number of command line arguments
* \param[in] argv the command line arguments
#ifndef PCL_IMPL_POINT_TYPES_HPP_
#define PCL_IMPL_POINT_TYPES_HPP_
+#include <cstdint>
#if defined __GNUC__
# pragma GCC system_header
#endif
*/
struct EIGEN_ALIGN16 PointXYZ : public _PointXYZ
{
- inline PointXYZ (const _PointXYZ &p)
- {
- x = p.x; y = p.y; z = p.z; data[3] = 1.0f;
- }
+ inline PointXYZ (const _PointXYZ &p): PointXYZ(p.x, p.y, p.z) {}
- inline PointXYZ ()
- {
- x = y = z = 0.0f;
- data[3] = 1.0f;
- }
+ inline PointXYZ (): PointXYZ(0.f, 0.f, 0.f) {}
inline PointXYZ (float _x, float _y, float _z)
{
{
inline RGB (const _RGB &p)
{
- rgba = p.rgba;
+ rgba = p.rgba;
}
- inline RGB ()
- {
- r = g = b = 0;
- a = 255;
- }
+ inline RGB (): RGB(0, 0, 0) {}
inline RGB (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b)
{
- r = _r;
- g = _g;
- b = _b;
+ r = _r; g = _g; b = _b;
a = 255;
}
intensity = p.intensity;
}
- inline Intensity ()
+ inline Intensity (float _intensity = 0.f)
{
- intensity = 0.0f;
+ intensity = _intensity;
}
-
+
friend std::ostream& operator << (std::ostream& os, const Intensity& p);
};
-
+
struct _Intensity8u
{
intensity = p.intensity;
}
- inline Intensity8u ()
+ inline Intensity8u (std::uint8_t _intensity = 0)
{
- intensity = 0;
+ intensity = _intensity;
}
-
+
#if defined(_LIBCPP_VERSION) && _LIBCPP_VERSION <= 1101
operator unsigned char() const
{
intensity = p.intensity;
}
- inline Intensity32u ()
+ inline Intensity32u (std::uint32_t _intensity = 0)
{
- intensity = 0;
+ intensity = _intensity;
}
friend std::ostream& operator << (std::ostream& os, const Intensity32u& p);
intensity = p.intensity;
}
- inline PointXYZI ()
- {
- x = y = z = 0.0f;
- data[3] = 1.0f;
- intensity = 0.0f;
- }
- inline PointXYZI (float _intensity)
+ inline PointXYZI (float _intensity = 0.f): PointXYZI(0.f, 0.f, 0.f, _intensity) {}
+
+ inline PointXYZI (float _x, float _y, float _z, float _intensity = 0.f)
{
- x = y = z = 0.0f;
+ x = _x; y = _y; z = _z;
data[3] = 1.0f;
intensity = _intensity;
}
+
friend std::ostream& operator << (std::ostream& os, const PointXYZI& p);
};
-
+
struct EIGEN_ALIGN16 _PointXYZL
{
label = p.label;
}
- inline PointXYZL ()
+ inline PointXYZL (std::uint32_t _label = 0): PointXYZL(0.f, 0.f, 0.f, _label) {}
+
+ inline PointXYZL (float _x, float _y, float _z, std::uint32_t _label = 0)
{
- x = y = z = 0.0f;
+ x = _x; y = _y; z = _z;
data[3] = 1.0f;
- label = 0;
+ label = _label;
}
-
+
friend std::ostream& operator << (std::ostream& os, const PointXYZL& p);
};
PCL_EXPORTS std::ostream& operator << (std::ostream& os, const Label& p);
struct Label
{
- std::uint32_t label;
-
+ std::uint32_t label = 0;
+
+ Label (std::uint32_t _label = 0): label(_label) {}
+
friend std::ostream& operator << (std::ostream& os, const Label& p);
};
rgba = p.rgba;
}
- inline PointXYZRGBA ()
+ inline PointXYZRGBA (): PointXYZRGBA (0, 0, 0, 0) {}
+
+ inline PointXYZRGBA (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, std::uint8_t _a):
+ PointXYZRGBA (0.f, 0.f, 0.f, _r, _g, _b, _a) {}
+
+ inline PointXYZRGBA (float _x, float _y, float _z):
+ PointXYZRGBA (_x, _y, _z, 0, 0, 0, 0) {}
+
+ inline PointXYZRGBA (float _x, float _y, float _z, std::uint8_t _r,
+ std::uint8_t _g, std::uint8_t _b, std::uint8_t _a)
{
- x = y = z = 0.0f;
+ x = _x; y = _y; z = _z;
data[3] = 1.0f;
- r = g = b = 0;
- a = 255;
+ r = _r; g = _g; b = _b; a = _a;
}
friend std::ostream& operator << (std::ostream& os, const PointXYZRGBA& p);
rgb = p.rgb;
}
- inline PointXYZRGB ()
- {
- x = y = z = 0.0f;
- data[3] = 1.0f;
- r = g = b = 0;
- a = 255;
- }
- inline PointXYZRGB (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b)
+ inline PointXYZRGB (): PointXYZRGB (0.f, 0.f, 0.f) {}
+
+ inline PointXYZRGB (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b):
+ PointXYZRGB (0.f, 0.f, 0.f, _r, _g, _b) {}
+
+ inline PointXYZRGB (float _x, float _y, float _z):
+ PointXYZRGB (_x, _y, _z, 0, 0, 0) {}
+
+ inline PointXYZRGB (float _x, float _y, float _z,
+ std::uint8_t _r, std::uint8_t _g, std::uint8_t _b)
{
- x = y = z = 0.0f;
+ x = _x; y = _y; z = _z;
data[3] = 1.0f;
- r = _r;
- g = _g;
- b = _b;
+ r = _r; g = _g; b = _b;
a = 255;
}
label = p.label;
}
- inline PointXYZRGBL ()
- {
- x = y = z = 0.0f;
- data[3] = 1.0f;
- r = g = b = 0;
- a = 255;
- label = 0;
- }
- inline PointXYZRGBL (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, std::uint32_t _label)
+ inline PointXYZRGBL (std::uint32_t _label = 0):
+ PointXYZRGBL (0.f, 0.f, 0.f, 0, 0, 0, _label) {}
+
+ inline PointXYZRGBL (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b):
+ PointXYZRGBL (0.f, 0.f, 0.f, _r, _g, _b) {}
+
+ inline PointXYZRGBL (float _x, float _y, float _z):
+ PointXYZRGBL (_x, _y, _z, 0, 0, 0) {}
+
+ inline PointXYZRGBL (float _x, float _y, float _z,
+ std::uint8_t _r, std::uint8_t _g, std::uint8_t _b,
+ std::uint32_t _label = 0)
{
- x = y = z = 0.0f;
+ x = _x; y = _y; z = _z;
data[3] = 1.0f;
- r = _r;
- g = _g;
- b = _b;
+ r = _r; g = _g; b = _b;
a = 255;
label = _label;
}
-
+
friend std::ostream& operator << (std::ostream& os, const PointXYZRGBL& p);
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
h = p.h; s = p.s; v = p.v;
}
- inline PointXYZHSV ()
- {
- x = y = z = 0.0f;
- data[3] = 1.0f;
- h = s = v = data_c[3] = 0;
- }
- inline PointXYZHSV (float _h, float _v, float _s)
+ inline PointXYZHSV (): PointXYZHSV (0.f, 0.f, 0.f) {}
+
+ // @TODO: Use strong types??
+ // This is a dangerous type, doesn't behave like others
+ inline PointXYZHSV (float _h, float _s, float _v):
+ PointXYZHSV (0.f, 0.f, 0.f, _h, _s, _v) {}
+
+ inline PointXYZHSV (float _x, float _y, float _z,
+ float _h, float _s, float _v)
{
- x = y = z = 0.0f;
+ x = _x; y = _y; z = _z;
data[3] = 1.0f;
- h = _h; v = _v; s = _s;
+ h = _h; s = _s; v = _v;
data_c[3] = 0;
}
-
+
friend std::ostream& operator << (std::ostream& os, const PointXYZHSV& p);
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
*/
struct PointXY
{
- float x;
- float y;
-
+ float x = 0.f;
+ float y = 0.f;
+
+ inline PointXY() = default;
+
+ inline PointXY(float _x, float _y): x(_x), y(_y) {}
+
friend std::ostream& operator << (std::ostream& os, const PointXY& p);
};
*/
struct PointUV
{
- float u;
- float v;
-
+ float u = 0.f;
+ float v = 0.f;
+
+ inline PointUV() = default;
+
+ inline PointUV(float _u, float _v): u(_u), v(_v) {}
+
friend std::ostream& operator << (std::ostream& os, const PointUV& p);
};
/** \brief A point structure representing an interest point with Euclidean xyz coordinates, and an interest value.
* \ingroup common
*/
+ // @TODO: inheritance trick like on other PointTypes
struct EIGEN_ALIGN16 InterestPoint
{
PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
float data_c[4];
};
PCL_MAKE_ALIGNED_OPERATOR_NEW
-
+
friend std::ostream& operator << (std::ostream& os, const InterestPoint& p);
};
curvature = p.curvature;
}
- inline Normal ()
- {
- normal_x = normal_y = normal_z = data_n[3] = 0.0f;
- curvature = 0;
- }
+ inline Normal (float _curvature = 0.f): Normal (0.f, 0.f, 0.f, _curvature) {}
- inline Normal (float n_x, float n_y, float n_z)
+ inline Normal (float n_x, float n_y, float n_z, float _curvature = 0.f)
{
normal_x = n_x; normal_y = n_y; normal_z = n_z;
- curvature = 0;
data_n[3] = 0.0f;
+ curvature = _curvature;
}
friend std::ostream& operator << (std::ostream& os, const Normal& p);
data_n[3] = 0.0f;
}
- inline Axis ()
- {
- normal_x = normal_y = normal_z = data_n[3] = 0.0f;
- }
+ inline Axis (): Axis (0.f, 0.f, 0.f) {}
inline Axis (float n_x, float n_y, float n_z)
{
curvature = p.curvature;
}
- inline PointNormal ()
+ inline PointNormal (float _curvature = 0.f): PointNormal (0.f, 0.f, 0.f, 0.f, 0.f, 0.f, _curvature) {}
+
+ inline PointNormal (float _x, float _y, float _z):
+ PointNormal (_x, _y, _z, 0.f, 0.f, 0.f, 0.f) {}
+
+ inline PointNormal (float _x, float _y, float _z, float n_x, float n_y, float n_z, float _curvature = 0.f)
{
- x = y = z = 0.0f;
+ x = _x; y = _y; z = _z;
data[3] = 1.0f;
- normal_x = normal_y = normal_z = data_n[3] = 0.0f;
- curvature = 0.f;
+ normal_x = n_x; normal_y = n_y; normal_z = n_z;
+ data_n[3] = 0.0f;
+ curvature = _curvature;
}
-
+
friend std::ostream& operator << (std::ostream& os, const PointNormal& p);
};
rgba = p.rgba;
}
- inline PointXYZRGBNormal ()
+ inline PointXYZRGBNormal (float _curvature = 0.f):
+ PointXYZRGBNormal (0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, _curvature) {}
+
+ inline PointXYZRGBNormal (float _x, float _y, float _z):
+ PointXYZRGBNormal (_x, _y, _z, 0, 0, 0) {}
+
+ inline PointXYZRGBNormal (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b):
+ PointXYZRGBNormal (0.f, 0.f, 0.f, _r, _g, _b) {}
+
+ inline PointXYZRGBNormal (float _x, float _y, float _z, std::uint8_t _r, std::uint8_t _g, std::uint8_t _b):
+ PointXYZRGBNormal (_x, _y, _z, _r, _g, _b, 0.f, 0.f, 0.f) {}
+
+ inline PointXYZRGBNormal (float _x, float _y, float _z, std::uint8_t _r, std::uint8_t _g, std::uint8_t _b,
+ float n_x, float n_y, float n_z, float _curvature = 0.f)
{
- x = y = z = 0.0f;
+ x = _x; y = _y; z = _z;
data[3] = 1.0f;
- r = g = b = 0;
+ r = _r; g = _g; b = _b;
a = 255;
- normal_x = normal_y = normal_z = data_n[3] = 0.0f;
- curvature = 0;
+ normal_x = n_x; normal_y = n_y; normal_z = n_z;
+ data_n[3] = 0.f;
+ curvature = _curvature;
}
friend std::ostream& operator << (std::ostream& os, const PointXYZRGBNormal& p);
};
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
-
+
PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZINormal& p);
/** \brief A point structure representing Euclidean xyz coordinates, intensity, together with normal coordinates and the surface curvature estimate.
* \ingroup common
curvature = p.curvature;
intensity = p.intensity;
}
+
+ inline PointXYZINormal (float _intensity = 0.f): PointXYZINormal (0.f, 0.f, 0.f, _intensity) {}
+
+ inline PointXYZINormal (float _x, float _y, float _z, float _intensity = 0.f):
+ PointXYZINormal (_x, _y, _z, _intensity, 0.f, 0.f, 0.f) {}
- inline PointXYZINormal ()
+ inline PointXYZINormal (float _x, float _y, float _z, float _intensity,
+ float n_x, float n_y, float n_z, float _curvature = 0.f)
{
- x = y = z = 0.0f;
+ x = _x; y = _y; z = _z;
data[3] = 1.0f;
- normal_x = normal_y = normal_z = data_n[3] = 0.0f;
- intensity = 0.0f;
- curvature = 0;
+ intensity = _intensity;
+ normal_x = n_x; normal_y = n_y; normal_z = n_z;
+ data_n[3] = 0.f;
+ curvature = _curvature;
}
-
+
friend std::ostream& operator << (std::ostream& os, const PointXYZINormal& p);
};
label = p.label;
}
- inline PointXYZLNormal ()
+ inline PointXYZLNormal (std::uint32_t _label = 0): PointXYZLNormal (0.f, 0.f, 0.f, _label) {}
+
+ inline PointXYZLNormal (float _x, float _y, float _z, std::uint32_t _label = 0.f):
+ PointXYZLNormal (_x, _y, _z, _label, 0.f, 0.f, 0.f) {}
+
+ inline PointXYZLNormal (float _x, float _y, float _z, std::uint32_t _label,
+ float n_x, float n_y, float n_z, float _curvature = 0.f)
{
- x = y = z = 0.0f;
+ x = _x; y = _y; z = _z;
data[3] = 1.0f;
- normal_x = normal_y = normal_z = data_n[3] = 0.0f;
- label = 0;
- curvature = 0;
+ label = _label;
+ normal_x = n_x; normal_y = n_y; normal_z = n_z;
+ data_n[3] = 0.f;
+ curvature = _curvature;
}
friend std::ostream& operator << (std::ostream& os, const PointXYZLNormal& p);
range = p.range;
}
- inline PointWithRange ()
+ inline PointWithRange (float _range = 0.f): PointWithRange (0.f, 0.f, 0.f, _range) {}
+
+ inline PointWithRange (float _x, float _y, float _z, float _range = 0.f)
{
- x = y = z = 0.0f;
+ x = _x; y = _y; z = _z;
data[3] = 1.0f;
- range = 0.0f;
+ range = _range;
}
-
+
friend std::ostream& operator << (std::ostream& os, const PointWithRange& p);
};
vp_x = p.vp_x; vp_y = p.vp_y; vp_z = p.vp_z;
}
- inline PointWithViewpoint (float _x = 0.0f, float _y = 0.0f, float _z = 0.0f,
- float _vp_x = 0.0f, float _vp_y = 0.0f, float _vp_z = 0.0f)
+ inline PointWithViewpoint (): PointWithViewpoint (0.f, 0.f, 0.f) {}
+
+ PCL_DEPRECATED("Use ctor accepting all position (x, y, z) data")
+ inline PointWithViewpoint (float _x, float _y = 0.f):
+ PointWithViewpoint (_x, _y, 0.f) {}
+
+ PCL_DEPRECATED("Use ctor accepting all viewpoint (vp_x, vp_y, vp_z) data")
+ inline PointWithViewpoint (float _x, float _y, float _z, float _vp_x, float _vp_y = 0.f):
+ PointWithViewpoint (_x, _y, _z, _vp_x, _vp_y, 0.f) {}
+
+ inline PointWithViewpoint (float _x, float _y, float _z): PointWithViewpoint (_x, _y, _z, 0.f, 0.f, 0.f) {}
+
+ inline PointWithViewpoint (float _x, float _y, float _z, float _vp_x, float _vp_y, float _vp_z)
{
x = _x; y = _y; z = _z;
data[3] = 1.0f;
vp_x = _vp_x; vp_y = _vp_y; vp_z = _vp_z;
}
-
+
friend std::ostream& operator << (std::ostream& os, const PointWithViewpoint& p);
};
*/
struct MomentInvariants
{
- float j1, j2, j3;
-
+ float j1 = 0.f, j2 = 0.f, j3 = 0.f;
+
+ inline MomentInvariants () = default;
+
+ inline MomentInvariants (float _j1, float _j2, float _j3): j1 (_j1), j2 (_j2), j3 (_j3) {}
+
friend std::ostream& operator << (std::ostream& os, const MomentInvariants& p);
};
*/
struct PrincipalRadiiRSD
{
- float r_min, r_max;
-
+ float r_min = 0.f, r_max = 0.f;
+
+ inline PrincipalRadiiRSD () = default;
+
+ inline PrincipalRadiiRSD (float _r_min, float _r_max): r_min (_r_min), r_max (_r_max) {}
+
friend std::ostream& operator << (std::ostream& os, const PrincipalRadiiRSD& p);
};
*/
struct Boundary
{
- std::uint8_t boundary_point;
+ std::uint8_t boundary_point = 0;
#if defined(_LIBCPP_VERSION) && _LIBCPP_VERSION <= 1101
operator unsigned char() const
return boundary_point;
}
#endif
-
+
+ inline Boundary (std::uint8_t _boundary = 0): boundary_point (_boundary) {}
+
friend std::ostream& operator << (std::ostream& os, const Boundary& p);
};
float principal_curvature_z;
};
};
- float pc1;
- float pc2;
-
+ float pc1 = 0.f;
+ float pc2 = 0.f;
+
+ inline PrincipalCurvatures (): PrincipalCurvatures (0.f, 0.f) {}
+
+ inline PrincipalCurvatures (float _pc1, float _pc2): PrincipalCurvatures (0.f, 0.f, 0.f, _pc1, _pc2) {}
+
+ inline PrincipalCurvatures (float _x, float _y, float _z): PrincipalCurvatures (_x, _y, _z, 0.f, 0.f) {}
+
+ inline PrincipalCurvatures (float _x, float _y, float _z, float _pc1, float _pc2):
+ principal_curvature_x (_x), principal_curvature_y (_y), principal_curvature_z (_z), pc1 (_pc1), pc2 (_pc2) {}
+
friend std::ostream& operator << (std::ostream& os, const PrincipalCurvatures& p);
};
*/
struct PFHSignature125
{
- float histogram[125];
+ float histogram[125] = {0.f};
static int descriptorSize () { return 125; }
-
+
+ inline PFHSignature125 () = default;
+
friend std::ostream& operator << (std::ostream& os, const PFHSignature125& p);
};
*/
struct PFHRGBSignature250
{
- float histogram[250];
+ float histogram[250] = {0.f};
static int descriptorSize () { return 250; }
+ inline PFHRGBSignature250 () = default;
+
friend std::ostream& operator << (std::ostream& os, const PFHRGBSignature250& p);
};
*/
struct PPFSignature
{
- float f1, f2, f3, f4;
- float alpha_m;
-
+ float f1 = 0.f, f2 = 0.f, f3 = 0.f, f4 = 0.f;
+ float alpha_m = 0.f;
+
+ inline PPFSignature (float _alpha = 0.f): PPFSignature (0.f, 0.f, 0.f, 0.f, _alpha) {}
+
+ inline PPFSignature (float _f1, float _f2, float _f3, float _f4, float _alpha = 0.f):
+ f1 (_f1), f2 (_f2), f3 (_f3), f4 (_f4), alpha_m (_alpha) {}
+
friend std::ostream& operator << (std::ostream& os, const PPFSignature& p);
};
{
float f1, f2, f3, f4, f5, f6, f7, f8, f9, f10;
float alpha_m;
-
+
+ inline CPPFSignature (float _alpha = 0.f):
+ CPPFSignature (0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, _alpha) {}
+
+ inline CPPFSignature (float _f1, float _f2, float _f3, float _f4, float _f5, float _f6,
+ float _f7, float _f8, float _f9, float _f10, float _alpha = 0.f):
+ f1 (_f1), f2 (_f2), f3 (_f3), f4 (_f4), f5 (_f5), f6 (_f6),
+ f7 (_f7), f8 (_f8), f9 (_f9), f10 (_f10), alpha_m (_alpha) {}
+
friend std::ostream& operator << (std::ostream& os, const CPPFSignature& p);
};
*/
struct PPFRGBSignature
{
- float f1, f2, f3, f4;
- float r_ratio, g_ratio, b_ratio;
- float alpha_m;
-
+ float f1 = 0.f, f2 = 0.f, f3 = 0.f, f4 = 0.f;
+ float r_ratio = 0.f, g_ratio = 0.f, b_ratio = 0.f;
+ float alpha_m = 0.f;
+
+ inline PPFRGBSignature (float _alpha = 0.f): PPFRGBSignature (0.f, 0.f, 0.f, 0.f, _alpha) {}
+
+ inline PPFRGBSignature (float _f1, float _f2, float _f3, float _f4, float _alpha = 0.f):
+ PPFRGBSignature (_f1, _f2, _f3, _f4, _alpha, 0.f, 0.f, 0.f) {}
+
+ inline PPFRGBSignature (float _f1, float _f2, float _f3, float _f4, float _alpha, float _r, float _g, float _b):
+ f1 (_f1), f2 (_f2), f3 (_f3), f4 (_f4), r_ratio (_r), g_ratio (_g), b_ratio (_b), alpha_m (_alpha) {}
+
friend std::ostream& operator << (std::ostream& os, const PPFRGBSignature& p);
};
*/
struct NormalBasedSignature12
{
- float values[12];
-
+ float values[12] = {0.f};
+
+ inline NormalBasedSignature12 () = default;
+
friend std::ostream& operator << (std::ostream& os, const NormalBasedSignature12& p);
};
*/
struct ShapeContext1980
{
- float descriptor[1980];
- float rf[9];
+ float descriptor[1980] = {0.f};
+ float rf[9] = {0.f};
static int descriptorSize () { return 1980; }
+ inline ShapeContext1980 () = default;
+
friend std::ostream& operator << (std::ostream& os, const ShapeContext1980& p);
};
*/
struct UniqueShapeContext1960
{
- float descriptor[1960];
- float rf[9];
+ float descriptor[1960] = {0.f};
+ float rf[9] = {0.f};
static int descriptorSize () { return 1960; }
+ inline UniqueShapeContext1960 () = default;
+
friend std::ostream& operator << (std::ostream& os, const UniqueShapeContext1960& p);
};
*/
struct SHOT352
{
- float descriptor[352];
- float rf[9];
+ float descriptor[352] = {0.f};
+ float rf[9] = {0.f};
static int descriptorSize () { return 352; }
+ inline SHOT352 () = default;
+
friend std::ostream& operator << (std::ostream& os, const SHOT352& p);
};
*/
struct SHOT1344
{
- float descriptor[1344];
- float rf[9];
+ float descriptor[1344] = {0.f};
+ float rf[9] = {0.f};
static int descriptorSize () { return 1344; }
+ inline SHOT1344 () = default;
+
friend std::ostream& operator << (std::ostream& os, const SHOT1344& p);
};
std::fill_n(z_axis, 3, 0);
}
+ // @TODO: add other ctors
+
friend std::ostream& operator << (std::ostream& os, const ReferenceFrame& p);
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
*/
struct FPFHSignature33
{
- float histogram[33];
+ float histogram[33] = {0.f};
static int descriptorSize () { return 33; }
+ inline FPFHSignature33 () = default;
+
friend std::ostream& operator << (std::ostream& os, const FPFHSignature33& p);
};
*/
struct VFHSignature308
{
- float histogram[308];
+ float histogram[308] = {0.f};
static int descriptorSize () { return 308; }
+ inline VFHSignature308 () = default;
+
friend std::ostream& operator << (std::ostream& os, const VFHSignature308& p);
};
-
+
PCL_EXPORTS std::ostream& operator << (std::ostream& os, const GRSDSignature21& p);
/** \brief A point structure representing the Global Radius-based Surface Descriptor (GRSD).
* \ingroup common
*/
struct GRSDSignature21
{
- float histogram[21];
+ float histogram[21] = {0.f};
static int descriptorSize () { return 21; }
+ inline GRSDSignature21 () = default;
+
friend std::ostream& operator << (std::ostream& os, const GRSDSignature21& p);
};
*/
struct BRISKSignature512
{
- float scale;
- float orientation;
- unsigned char descriptor[64];
+ float scale = 0.f;
+ float orientation = 0.f;
+ unsigned char descriptor[64] = {0};
static int descriptorSize () { return 64; }
+ inline BRISKSignature512 () = default;
+
+ inline BRISKSignature512 (float _scale, float _orientation): scale (_scale), orientation (_orientation) {}
+
friend std::ostream& operator << (std::ostream& os, const BRISKSignature512& p);
};
*/
struct ESFSignature640
{
- float histogram[640];
+ float histogram[640] = {0.f};
static int descriptorSize () { return 640; }
+ inline ESFSignature640 () = default;
+
friend std::ostream& operator << (std::ostream& os, const ESFSignature640& p);
};
*/
struct GASDSignature512
{
- float histogram[512];
+ float histogram[512] = {0.f};
static int descriptorSize() { return 512; }
+ inline GASDSignature512 () = default;
+
friend std::ostream& operator << (std::ostream& os, const GASDSignature512& p);
};
*/
struct GASDSignature984
{
- float histogram[984];
+ float histogram[984] = {0.f};
static int descriptorSize() { return 984; }
+ inline GASDSignature984 () = default;
+
friend std::ostream& operator << (std::ostream& os, const GASDSignature984& p);
};
*/
struct GASDSignature7992
{
- float histogram[7992];
+ float histogram[7992] = {0.f};
static int descriptorSize() { return 7992; }
+ inline GASDSignature7992 () = default;
+
friend std::ostream& operator << (std::ostream& os, const GASDSignature7992& p);
};
*/
struct GFPFHSignature16
{
- float histogram[16];
+ float histogram[16] = {0.f};
static int descriptorSize () { return 16; }
-
+
+ inline GFPFHSignature16 () = default;
+
friend std::ostream& operator << (std::ostream& os, const GFPFHSignature16& p);
};
*/
struct Narf36
{
- float x, y, z, roll, pitch, yaw;
- float descriptor[36];
+ float x = 0.f, y = 0.f, z = 0.f, roll = 0.f, pitch = 0.f, yaw = 0.f;
+ float descriptor[36] = {0.f};
static int descriptorSize () { return 36; }
+ inline Narf36 () = default;
+
+ inline Narf36 (float _x, float _y, float _z): Narf36 (_x, _y, _z, 0.f, 0.f, 0.f) {}
+
+ inline Narf36 (float _x, float _y, float _z, float _roll, float _pitch, float _yaw):
+ x (_x), y (_y), z (_z), roll (_roll), pitch (_pitch), yaw (_yaw) {}
+
friend std::ostream& operator << (std::ostream& os, const Narf36& p);
};
*/
struct BorderDescription
{
- int x, y;
+ int x = 0.f, y = 0.f;
BorderTraits traits;
//std::vector<const BorderDescription*> neighbors;
-
+
+ inline BorderDescription () = default;
+
+ // TODO: provide other ctors
+
friend std::ostream& operator << (std::ostream& os, const BorderDescription& p);
};
float gradient_z;
};
};
-
+
+ inline IntensityGradient (): IntensityGradient (0.f, 0.f, 0.f) {}
+
+ inline IntensityGradient (float _x, float _y, float _z): gradient_x (_x), gradient_y (_y), gradient_z (_z) {}
+
friend std::ostream& operator << (std::ostream& os, const IntensityGradient& p);
};
+ // TODO: Maybe make other histogram based structs an alias for this
/** \brief A point structure representing an N-D histogram.
* \ingroup common
*/
octave = p.octave;
}
- inline PointWithScale ()
- {
- x = y = z = 0.0f;
- scale = 1.0f;
- angle = -1.0f;
- response = 0.0f;
- octave = 0;
- data[3] = 1.0f;
- }
+ inline PointWithScale (): PointWithScale (0.f, 0.f, 0.f) {}
- inline PointWithScale (float _x, float _y, float _z, float _scale)
+ inline PointWithScale (float _x, float _y, float _z, float _scale = 1.f,
+ float _angle = -1.f, float _response = 0.f, int _octave = 0)
{
- x = _x;
- y = _y;
- z = _z;
- scale = _scale;
- angle = -1.0f;
- response = 0.0f;
- octave = 0;
+ x = _x; y = _y; z = _z;
data[3] = 1.0f;
- }
-
- inline PointWithScale (float _x, float _y, float _z, float _scale, float _angle, float _response, int _octave)
- {
- x = _x;
- y = _y;
- z = _z;
scale = _scale;
angle = _angle;
response = _response;
octave = _octave;
- data[3] = 1.0f;
}
-
+
friend std::ostream& operator << (std::ostream& os, const PointWithScale& p);
};
a = 255;
radius = confidence = curvature = 0.0f;
}
-
+
+ // TODO: add other ctor to PointSurfel
+
friend std::ostream& operator << (std::ostream& os, const PointSurfel& p);
};
height_variance = p.height_variance;
}
- inline PointDEM ()
+ inline PointDEM (): PointDEM (0.f, 0.f, 0.f) {}
+
+ inline PointDEM (float _x, float _y, float _z): PointDEM (_x, _y, _z, 0.f, 0.f, 0.f) {}
+
+ inline PointDEM (float _x, float _y, float _z, float _intensity,
+ float _intensity_variance, float _height_variance)
{
- x = y = z = 0.0f; data[3] = 1.0f;
- intensity = 0.0f;
- intensity_variance = height_variance = 0.0f;
+ x = _x; y = _y; z = _z;
+ data[3] = 1.0f;
+ intensity = _intensity;
+ intensity_variance = _intensity_variance;
+ height_variance = _height_variance;
}
friend std::ostream& operator << (std::ostream& os, const PointDEM& p);
#include <pcl/pcl_config.h>
+// It seems that __has_cpp_attribute doesn't work correctly
+// when compiling with some versions of nvcc so we
+// additionally check if nvcc is used before setting the
+// PCL_DEPRECATED macro to [[deprecated]].
+#if defined(__has_cpp_attribute) && __has_cpp_attribute(deprecated) && !defined(__CUDACC__)
+ #define PCL_DEPRECATED(message) [[deprecated(message)]]
+#elif defined(__GNUC__) || defined(__clang__)
+ #define PCL_DEPRECATED(message) __attribute__((deprecated(message)))
+#elif defined(_MSC_VER)
+ // Until Visual Studio 2013 you had to use __declspec(deprecated).
+ // However, we decided to ignore the deprecation for these version because
+ // of simplicity reasons. See PR #3634 for the details.
+ #define PCL_DEPRECATED(message)
+#else
+ #warning "You need to implement PCL_DEPRECATED for this compiler"
+ #define PCL_DEPRECATED(message)
+#endif
+
namespace pcl
{
/**
template <typename T>
using shared_ptr = boost::shared_ptr<T>;
- using uint8_t [[deprecated("use std::uint8_t instead of pcl::uint8_t")]] = std::uint8_t;
- using int8_t [[deprecated("use std::int8_t instead of pcl::int8_t")]] = std::int8_t;
- using uint16_t [[deprecated("use std::uint16_t instead of pcl::uint16_t")]] = std::uint16_t;
- using int16_t [[deprecated("use std::uint16_t instead of pcl::int16_t")]] = std::int16_t;
- using uint32_t [[deprecated("use std::uint32_t instead of pcl::uint32_t")]] = std::uint32_t;
- using int32_t [[deprecated("use std::int32_t instead of pcl::int32_t")]] = std::int32_t;
- using uint64_t [[deprecated("use std::uint64_t instead of pcl::uint64_t")]] = std::uint64_t;
- using int64_t [[deprecated("use std::int64_t instead of pcl::int64_t")]] = std::int64_t;
- using int_fast16_t [[deprecated("use std::int_fast16_t instead of pcl::int_fast16_t")]] = std::int_fast16_t;
+ using uint8_t PCL_DEPRECATED("use std::uint8_t instead of pcl::uint8_t") = std::uint8_t;
+ using int8_t PCL_DEPRECATED("use std::int8_t instead of pcl::int8_t") = std::int8_t;
+ using uint16_t PCL_DEPRECATED("use std::uint16_t instead of pcl::uint16_t") = std::uint16_t;
+ using int16_t PCL_DEPRECATED("use std::uint16_t instead of pcl::int16_t") = std::int16_t;
+ using uint32_t PCL_DEPRECATED("use std::uint32_t instead of pcl::uint32_t") = std::uint32_t;
+ using int32_t PCL_DEPRECATED("use std::int32_t instead of pcl::int32_t") = std::int32_t;
+ using uint64_t PCL_DEPRECATED("use std::uint64_t instead of pcl::uint64_t") = std::uint64_t;
+ using int64_t PCL_DEPRECATED("use std::int64_t instead of pcl::int64_t") = std::int64_t;
+ using int_fast16_t PCL_DEPRECATED("use std::int_fast16_t instead of pcl::int_fast16_t") = std::int_fast16_t;
}
#if defined _WIN32 && defined _MSC_VER
template<typename T>
-[[deprecated("use std::isnan instead of pcl_isnan")]]
+PCL_DEPRECATED("use std::isnan instead of pcl_isnan")
bool pcl_isnan (T&& x) { return std::isnan (std::forward<T> (x)); }
template<typename T>
-[[deprecated("use std::isfinite instead of pcl_isfinite")]]
+PCL_DEPRECATED("use std::isfinite instead of pcl_isfinite")
bool pcl_isfinite (T&& x) { return std::isfinite (std::forward<T> (x)); }
template<typename T>
-[[deprecated("use std::isinf instead of pcl_isinf")]]
+PCL_DEPRECATED("use std::isinf instead of pcl_isinf")
bool pcl_isinf (T&& x) { return std::isinf (std::forward<T> (x)); }
namespace detail
{
template <typename PointT>
- [[deprecated("use createMapping() instead")]]
+ PCL_DEPRECATED("use createMapping() instead")
shared_ptr<pcl::MsgFieldMap>&
getMapping (pcl::PointCloud<PointT>& p);
} // namespace detail
protected:
// This is motivated by ROS integration. Users should not need to access mapping_.
- [[deprecated("rewrite your code to avoid using this protected field")]] shared_ptr<MsgFieldMap> mapping_;
+ PCL_DEPRECATED("rewrite your code to avoid using this protected field") shared_ptr<MsgFieldMap> mapping_;
friend shared_ptr<MsgFieldMap>& detail::getMapping<PointT>(pcl::PointCloud<PointT> &p);
#pragma once
#include <algorithm>
+#include <vector>
#include <pcl/point_types.h>
#include <pcl/pcl_macros.h>
return (field.name == traits::name<PointT, Tag>::value &&
field.datatype == traits::datatype<PointT, Tag>::value &&
(field.count == traits::datatype<PointT, Tag>::size ||
- field.count == 0 && traits::datatype<PointT, Tag>::size == 1 /* see bug #821 */));
+ ((field.count == 0) && (traits::datatype<PointT, Tag>::size == 1 /* see bug #821 */))));
}
};
// Must be used in global namespace with name fully qualified
#define POINT_CLOUD_REGISTER_POINT_STRUCT(name, fseq) \
POINT_CLOUD_REGISTER_POINT_STRUCT_I(name, \
- BOOST_PP_CAT(POINT_CLOUD_REGISTER_POINT_STRUCT_X fseq, 0)) \
+ BOOST_PP_CAT(POINT_CLOUD_REGISTER_POINT_STRUCT_X fseq, 0))
/***/
#define POINT_CLOUD_REGISTER_POINT_WRAPPER(wrapper, pod) \
namespace traits { \
template<> struct POD<wrapper> { using type = pod; }; \
} \
- } \
+ }
/***/
// These macros help transform the unusual data structure (type, name, tag)(type, name, tag)...
{
using type = std::remove_all_extents_t<T>;
static const std::uint32_t count = sizeof (T) / sizeof (type);
- for (int i = 0; i < count; ++i)
+ for (std::uint32_t i = 0; i < count; ++i)
l[i] += r[i];
}
{
using type = std::remove_all_extents_t<T1>;
static const std::uint32_t count = sizeof (T1) / sizeof (type);
- for (int i = 0; i < count; ++i)
+ for (std::uint32_t i = 0; i < count; ++i)
p[i] += scalar;
}
{
using type = std::remove_all_extents_t<T>;
static const std::uint32_t count = sizeof (T) / sizeof (type);
- for (int i = 0; i < count; ++i)
+ for (std::uint32_t i = 0; i < count; ++i)
l[i] -= r[i];
}
{
using type = std::remove_all_extents_t<T1>;
static const std::uint32_t count = sizeof (T1) / sizeof (type);
- for (int i = 0; i < count; ++i)
+ for (std::uint32_t i = 0; i < count; ++i)
p[i] -= scalar;
}
{
using type = std::remove_all_extents_t<T1>;
static const std::uint32_t count = sizeof (T1) / sizeof (type);
- for (int i = 0; i < count; ++i)
+ for (std::uint32_t i = 0; i < count; ++i)
p[i] *= scalar;
}
{
using type = std::remove_all_extents_t<T1>;
static const std::uint32_t count = sizeof (T1) / sizeof (type);
- for (int i = 0; i < count; ++i)
+ for (std::uint32_t i = 0; i < count; ++i)
p[i] /= scalar;
}
}
// Point operators
#define PCL_PLUSEQ_POINT_TAG(r, data, elem) \
pcl::traits::plus (lhs.BOOST_PP_TUPLE_ELEM(3, 1, elem), \
- rhs.BOOST_PP_TUPLE_ELEM(3, 1, elem)); \
+ rhs.BOOST_PP_TUPLE_ELEM(3, 1, elem));
/***/
#define PCL_PLUSEQSC_POINT_TAG(r, data, elem) \
pcl::traits::plusscalar (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \
- scalar); \
+ scalar);
/***/
- //p.BOOST_PP_TUPLE_ELEM(3, 1, elem) += scalar; \
+ //p.BOOST_PP_TUPLE_ELEM(3, 1, elem) += scalar;
#define PCL_MINUSEQ_POINT_TAG(r, data, elem) \
pcl::traits::minus (lhs.BOOST_PP_TUPLE_ELEM(3, 1, elem), \
- rhs.BOOST_PP_TUPLE_ELEM(3, 1, elem)); \
+ rhs.BOOST_PP_TUPLE_ELEM(3, 1, elem));
/***/
#define PCL_MINUSEQSC_POINT_TAG(r, data, elem) \
pcl::traits::minusscalar (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \
- scalar); \
+ scalar);
/***/
- //p.BOOST_PP_TUPLE_ELEM(3, 1, elem) -= scalar; \
+ //p.BOOST_PP_TUPLE_ELEM(3, 1, elem) -= scalar;
#define PCL_MULEQSC_POINT_TAG(r, data, elem) \
pcl::traits::mulscalar (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \
- scalar); \
+ scalar);
/***/
#define PCL_DIVEQSC_POINT_TAG(r, data, elem) \
pcl::traits::divscalar (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \
- scalar); \
+ scalar);
/***/
// Construct type traits given full sequence of (type, name, tag) triples
inline const name operator/ (const name& p, const float& scalar) \
{ name result = p; result /= scalar; return (result); } \
} \
- } \
+ }
/***/
#define POINT_CLOUD_REGISTER_FIELD_TAG(r, name, elem) \
const char name<point, \
pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem), \
dummy>::value[] = \
- BOOST_PP_STRINGIZE(BOOST_PP_TUPLE_ELEM(3, 2, elem)); \
+ BOOST_PP_STRINGIZE(BOOST_PP_TUPLE_ELEM(3, 2, elem));
/***/
#define POINT_CLOUD_REGISTER_FIELD_OFFSET(r, name, elem) \
template<> struct offset<name, pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem)> \
{ \
static const std::size_t value = offsetof(name, BOOST_PP_TUPLE_ELEM(3, 1, elem)); \
- }; \
+ };
/***/
#define POINT_CLOUD_REGISTER_FIELD_DATATYPE(r, name, elem) \
using decomposed = decomposeArray<type>; \
static const std::uint8_t value = asEnum<decomposed::type>::value; \
static const std::uint32_t size = decomposed::value; \
- }; \
+ };
/***/
#define POINT_CLOUD_TAG_OP(s, data, elem) pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem)
template<> struct fieldList<name> \
{ \
using type = boost::mpl::vector<BOOST_PP_SEQ_ENUM(seq)>; \
- }; \
+ };
/***/
#if defined _MSC_VER
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
- * Copyright (c) 2010-2012, Willow Garage, Inc.
- * Copyright (c) 2012-, Open Perception, Inc.
+ * Copyright (c) 2020-, Open Perception, Inc.
*
* All rights reserved.
*
*/
#include <cctype>
+#include <cerrno>
+#include <complex>
#include <cstdio>
+#include <limits>
+#include <type_traits>
+
#include <pcl/console/parse.h>
#include <pcl/console/print.h>
-#include <boost/algorithm/string.hpp>
-////////////////////////////////////////////////////////////////////////////////
-bool
-pcl::console::find_switch (int argc, const char * const * argv, const char * argument_name)
-{
- return (find_argument (argc, argv, argument_name) != -1);
-}
+#include <boost/algorithm/string.hpp>
////////////////////////////////////////////////////////////////////////////////
int
return (-1);
}
+////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::console::find_switch (int argc, const char * const * argv, const char * argument_name)
+{
+ return (find_argument (argc, argv, argument_name) != -1);
+}
+
////////////////////////////////////////////////////////////////////////////////
int
pcl::console::parse_argument (int argc, const char * const * argv, const char * str, std::string &val)
}
////////////////////////////////////////////////////////////////////////////////
-int
-pcl::console::parse_argument (int argc, const char * const * argv, const char * str, bool &val)
+namespace pcl
+{
+namespace console
{
+template <class T, class V = T(*)(const char*, const char**)> int
+parse_generic (V convert_func, int argc, const char* const* argv, const char* str, T& val)
+{
+ char *endptr = nullptr;
int index = find_argument (argc, argv, str) + 1;
+ errno = 0;
if (index > 0 && index < argc )
- val = atoi (argv[index]) == 1;
+ {
+ val = convert_func (argv[index], &endptr); // similar to strtol, strtod, strtof
+ // handle out-of-range, junk at the end and no conversion
+ if (errno == ERANGE || *endptr != '\0' || str == endptr)
+ {
+ return -1;
+ }
+ }
return (index - 1);
}
-////////////////////////////////////////////////////////////////////////////////
int
-pcl::console::parse_argument (int argc, const char * const * argv, const char * str, double &val)
+parse_argument (int argc, const char * const * argv, const char * str, long int &val) noexcept
{
- int index = find_argument (argc, argv, str) + 1;
-
- if (index > 0 && index < argc )
- val = atof (argv[index]);
+ const auto strtol_10 = [](const char *str, char **str_end){ return strtol(str, str_end, 10); };
+ return parse_generic(strtol_10, argc, argv, str, val);
+}
- return (index - 1);
+int
+parse_argument (int argc, const char * const * argv, const char * str, long long int &val) noexcept
+{
+ const auto strtoll_10 = [](const char *str, char **str_end){ return strtoll(str, str_end, 10); };
+ return parse_generic(strtoll_10, argc, argv, str, val);
}
-////////////////////////////////////////////////////////////////////////////////
int
-pcl::console::parse_argument (int argc, const char * const * argv, const char * str, float &val)
+parse_argument (int argc, const char * const * argv, const char * str, unsigned long long int &val) noexcept
{
- int index = find_argument (argc, argv, str) + 1;
+ long long int dummy;
+ const auto ret = parse_argument (argc, argv, str, dummy);
+ if ((ret == -1) || dummy < 0)
+ {
+ return -1;
+ }
+ val = dummy;
+ return ret;
+}
- if (index > 0 && index < argc )
- val = static_cast<float> (atof (argv[index]));
+namespace detail
+{
+template <typename T, typename U>
+constexpr auto legally_representable_v = (std::numeric_limits<T>::max () >= std::numeric_limits<U>::max ()) &&
+ (std::numeric_limits<T>::lowest () <= std::numeric_limits<U>::lowest ());
+template <typename T, typename U>
+struct legally_representable {
+ constexpr static bool value = legally_representable_v<T, U>;
+};
+
+// assumptions:
+// * either long int or long long int is a valid type for storing Integral
+// * unsigned long long int is handled specially
+template <typename Integral>
+using primary_legal_input_type = std::conditional_t<legally_representable_v<long int, Integral>,
+ long int, long long int>;
+
+// special handling if unsigned [long] int is of same size as long long int
+template <typename Integral>
+using legal_input_type = std::conditional_t<(std::is_unsigned<Integral>::value &&
+ (sizeof (Integral) == sizeof (long long int))),
+ unsigned long long int,
+ primary_legal_input_type<Integral>>;
+}
- return (index - 1);
+template <typename T>
+using IsIntegral = std::enable_if_t<std::is_integral<T>::value, bool>;
+
+template <typename T, IsIntegral<T> = true> int
+parse_argument (int argc, const char * const * argv, const char * str, T &val) noexcept
+{
+ using InputType = detail::legal_input_type<T>;
+ InputType dummy;
+ const auto ret = parse_argument (argc, argv, str, dummy);
+ if ((ret == -1) ||
+ (dummy < static_cast<InputType> (std::numeric_limits<T>::min ())) ||
+ (dummy > static_cast<InputType> (std::numeric_limits<T>::max ())))
+ {
+ return -1;
+ }
+
+ val = static_cast<T> (dummy);
+ return ret;
+}
+}
}
////////////////////////////////////////////////////////////////////////////////
int
-pcl::console::parse_argument (int argc, const char * const * argv, const char * str, int &val)
+pcl::console::parse_argument (int argc, const char * const * argv, const char * str, double &val)
{
- int index = find_argument (argc, argv, str) + 1;
-
- if (index > 0 && index < argc )
- val = atoi (argv[index]);
+ return parse_generic(strtod, argc, argv, str, val);
+}
- return (index - 1);
+////////////////////////////////////////////////////////////////////////////////
+int
+pcl::console::parse_argument (int argc, const char * const * argv, const char * str, float &val)
+{
+ return parse_generic(strtof, argc, argv, str, val);
}
////////////////////////////////////////////////////////////////////////////////
int
pcl::console::parse_argument (int argc, const char * const * argv, const char * str, unsigned int &val)
{
- int index = find_argument (argc, argv, str) + 1;
+ return parse_argument<unsigned int> (argc, argv, str, val);
+}
- if (index > 0 && index < argc )
- val = atoi (argv[index]);
+////////////////////////////////////////////////////////////////////////////////
+int
+pcl::console::parse_argument (int argc, const char * const * argv, const char * str, int &val)
+{
+ return parse_argument<int> (argc, argv, str, val);
+}
- return (index - 1);
+////////////////////////////////////////////////////////////////////////////////
+int
+pcl::console::parse_argument (int argc, const char * const * argv, const char * str, bool &val)
+{
+ long int dummy;
+ const auto ret = parse_argument (argc, argv, str, dummy);
+ if (ret != -1)
+ {
+ val = static_cast<bool> (dummy);
+ }
+ return ret;
}
////////////////////////////////////////////////////////////////////////////////
{
this->use_viewer = use_viewer;
//cudaDeviceSetCacheConfig (cudaFuncCachePreferL1);
- pcl::Grabber* interface = new pcl::OpenNIGrabber();
+ pcl::OpenNIGrabber interface {};
boost::signals2::connection c;
if (use_device)
{
std::cerr << "[RANSAC] Using GPU..." << std::endl;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&MultiRansac::cloud_cb<pcl_cuda::Device>, this, _1, _2, _3);
- c = interface->registerCallback (f);
+ c = interface.registerCallback (f);
}
else
{
std::cerr << "[RANSAC] Using CPU..." << std::endl;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&MultiRansac::cloud_cb<pcl_cuda::Host>, this, _1, _2, _3);
- c = interface->registerCallback (f);
+ c = interface.registerCallback (f);
}
if (use_viewer)
viewer.runOnVisualizationThread (std::bind(&MultiRansac::viz_cb, this, _1), "viz_cb");
- interface->start ();
+ interface.start ();
while (!viewer.wasStopped ())
{
sleep (1);
}
- interface->stop ();
+ interface.stop ();
}
pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr logo_cloud_;
{
if (use_file)
{
- pcl::Grabber* filegrabber = 0;
float frames_per_second = 1;
bool repeat = false;
std::string path = "./frame_0.pcd";
- filegrabber = new pcl::PCDGrabber<pcl::PointXYZRGB > (path, frames_per_second, repeat);
+ pcl::PCDGrabber<pcl::PointXYZRGB > filegrabber {path, frames_per_second, repeat};
if (use_device)
{
std::cerr << "[NormalEstimation] Using GPU..." << std::endl;
std::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = std::bind (&NormalEstimation::file_cloud_cb<Device>, this, _1);
- filegrabber->registerCallback (f);
+ filegrabber.registerCallback (f);
}
else
{
std::cerr << "[NormalEstimation] Using CPU..." << std::endl;
std::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = std::bind (&NormalEstimation::file_cloud_cb<Host>, this, _1);
- filegrabber->registerCallback (f);
+ filegrabber.registerCallback (f);
}
- filegrabber->start ();
+ filegrabber.start ();
while (go_on)//!viewer.wasStopped () && go_on)
{
pcl_sleep (1);
}
- filegrabber->stop ();
+ filegrabber.stop ();
}
else
{
- pcl::Grabber* grabber = new pcl::OpenNIGrabber();
+ pcl::OpenNIGrabber grabber {};
- boost::signals2::connection c;
if (use_device)
{
std::cerr << "[NormalEstimation] Using GPU..." << std::endl;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&NormalEstimation::cloud_cb<Device>, this, _1, _2, _3);
- c = grabber->registerCallback (f);
+ grabber.registerCallback (f);
}
else
{
std::cerr << "[NormalEstimation] Using CPU..." << std::endl;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&NormalEstimation::cloud_cb<Host>, this, _1, _2, _3);
- c = grabber->registerCallback (f);
+ grabber.registerCallback (f);
}
viewer.runOnVisualizationThread (std::bind(&NormalEstimation::viz_cb, this, _1), "viz_cb");
- grabber->start ();
+ grabber.start ();
while (!viewer.wasStopped ())
{
pcl_sleep (1);
}
- grabber->stop ();
+ grabber.stop ();
}
}
{
this->use_viewer = use_viewer;
//cudaDeviceSetCacheConfig (cudaFuncCachePreferL1);
- pcl::Grabber* interface = new pcl::OpenNIGrabber();
+ pcl::OpenNIGrabber interface {};
boost::signals2::connection c;
if (use_device)
{
std::cerr << "[RANSAC] Using GPU..." << std::endl;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&MultiRansac::cloud_cb<Device>, this, _1, _2, _3);
- c = interface->registerCallback (f);
+ c = interface.registerCallback (f);
}
else
{
std::cerr << "[RANSAC] Using CPU..." << std::endl;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&MultiRansac::cloud_cb<Host>, this, _1, _2, _3);
- c = interface->registerCallback (f);
+ c = interface.registerCallback (f);
}
if (use_viewer)
viewer.runOnVisualizationThread (std::bind(&MultiRansac::viz_cb, this, _1), "viz_cb");
- interface->start ();
+ interface.start ();
//--------------------- load pcl logo file
- //pcl::Grabber* filegrabber = 0;
-
//float frames_per_second = 1;
//bool repeat = false;
//std::string path = "./pcl_logo.pcd";
- //if (!path.empty() && boost::filesystem::exists (path))
+ //if (path.empty() || !boost::filesystem::exists (path))
//{
- // filegrabber = new pcl::PCDGrabber<pcl::PointXYZRGB > (path, frames_per_second, repeat);
- //}
- //else
// std::cerr << "did not find file" << std::endl;
+ //}
+ //pcl::PCDGrabber<pcl::PointXYZRGB> filegrabber {path, frames_per_second, repeat};
//
//std::function<void(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&) > f = std::bind (&MultiRansac::logo_cb, this, _1);
- //boost::signals2::connection c1 = filegrabber->registerCallback (f);
+ //boost::signals2::connection c1 = filegrabber.registerCallback (f);
- //filegrabber->start ();
+ //filegrabber.start ();
//------- END --------- load pcl logo file
//
while (!viewer.wasStopped ())
pcl_sleep (1);
}
- //filegrabber->stop ();
- interface->stop ();
+ //filegrabber.stop ();
+ interface.stop ();
}
pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr logo_cloud_;
bool repeat = false;
std::string path = "./frame_0.pcd";
- filegrabber = new pcl::PCDGrabber<pcl::PointXYZRGB > (path, frames_per_second, repeat);
+ pcl::PCDGrabber<pcl::PointXYZRGB > filegrabber (path, frames_per_second, repeat);
if (use_device)
{
std::cerr << "[RANSAC] Using GPU..." << std::endl;
std::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = std::bind (&SimpleKinectTool::file_cloud_cb<pcl_cuda::Device>, this, _1);
- filegrabber->registerCallback (f);
+ filegrabber.registerCallback (f);
}
else
{
std::cerr << "[RANSAC] Using CPU..." << std::endl;
std::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = std::bind (&SimpleKinectTool::file_cloud_cb<pcl_cuda::Host>, this, _1);
- filegrabber->registerCallback (f);
+ filegrabber.registerCallback (f);
}
- filegrabber->start ();
+ filegrabber.start ();
while (go_on)//!viewer.wasStopped () && go_on)
{
sleep (1);
}
- filegrabber->stop ();
+ filegrabber.stop ();
//------- END --------- load pcl logo file
#else
-
- pcl::Grabber* interface = new pcl::OpenNIGrabber();
-
+ pcl::OpenNIGrabber interface {};
boost::signals2::connection c;
if (use_device)
{
std::cerr << "[RANSAC] Using GPU..." << std::endl;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&SimpleKinectTool::cloud_cb<pcl_cuda::Device>, this, _1, _2, _3);
- c = interface->registerCallback (f);
+ c = interface.registerCallback (f);
}
else
{
std::cerr << "[RANSAC] Using CPU..." << std::endl;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&SimpleKinectTool::cloud_cb<pcl_cuda::Host>, this, _1, _2, _3);
- c = interface->registerCallback (f);
+ c = interface.registerCallback (f);
}
//viewer.runOnVisualizationThread (fn, "viz_cb");
- interface->start ();
+ interface.start ();
while (!viewer.wasStopped ())
{
sleep (1);
}
- interface->stop ();
+ interface.stop ();
#endif
}
bool repeat = false;
std::string path = "./frame_0.pcd";
- filegrabber = new pcl::PCDGrabber<pcl::PointXYZRGB > (path, frames_per_second, repeat);
+ pcl::PCDGrabber<pcl::PointXYZRGB > filegrabber {path, frames_per_second, repeat};
if (use_device)
{
std::cerr << "[Segmentation] Using GPU..." << std::endl;
std::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = std::bind (&Segmentation::file_cloud_cb<Device>, this, _1);
- filegrabber->registerCallback (f);
+ filegrabber.registerCallback (f);
}
else
{
// std::cerr << "[Segmentation] Using CPU..." << std::endl;
// std::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = std::bind (&Segmentation::file_cloud_cb<Host>, this, _1);
-// filegrabber->registerCallback (f);
+// filegrabber.registerCallback (f);
}
- filegrabber->start ();
+ filegrabber.start ();
while (go_on)//!viewer.wasStopped () && go_on)
{
pcl_sleep (1);
}
- filegrabber->stop ();
+ filegrabber.stop ();
}
else
{
- pcl::Grabber* grabber = new pcl::OpenNIGrabber();
+ pcl::OpenNIGrabber grabber {};
boost::signals2::connection c;
if (use_device)
{
std::cerr << "[Segmentation] Using GPU..." << std::endl;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&Segmentation::cloud_cb<Device>, this, _1, _2, _3);
- c = grabber->registerCallback (f);
+ c = grabber.registerCallback (f);
}
else
{
// std::cerr << "[Segmentation] Using CPU..." << std::endl;
// std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&Segmentation::cloud_cb<Host>, this, _1, _2, _3);
-// c = grabber->registerCallback (f);
+// c = grabber.registerCallback (f);
}
viewer.runOnVisualizationThread (std::bind(&Segmentation::viz_cb, this, _1), "viz_cb");
- grabber->start ();
+ grabber.start ();
while (!viewer.wasStopped ())
{
pcl_sleep (1);
}
- grabber->stop ();
+ grabber.stop ();
}
}
{
if (use_file)
{
- pcl::Grabber* filegrabber = 0;
-
float frames_per_second = 1;
bool repeat = false;
std::string path = "./frame_0.pcd";
- filegrabber = new pcl::PCDGrabber<pcl::PointXYZRGB > (path, frames_per_second, repeat);
+ pcl::PCDGrabber<pcl::PointXYZRGB > filegrabber {path, frames_per_second, repeat};
if (use_device)
{
std::cerr << "[Segmentation] Using GPU..." << std::endl;
std::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = std::bind (&Segmentation::file_cloud_cb<Device>, this, _1);
- filegrabber->registerCallback (f);
+ filegrabber.registerCallback (f);
}
else
{
// std::cerr << "[Segmentation] Using CPU..." << std::endl;
// std::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = std::bind (&Segmentation::file_cloud_cb<Host>, this, _1);
-// filegrabber->registerCallback (f);
+// filegrabber.registerCallback (f);
}
- filegrabber->start ();
+ filegrabber.start ();
while (go_on)//!viewer.wasStopped () && go_on)
{
pcl_sleep (1);
}
- filegrabber->stop ();
+ filegrabber.stop ();
}
else
{
- pcl::Grabber* grabber = new pcl::OpenNIGrabber();
+ pcl::OpenNIGrabber grabber {};
boost::signals2::connection c;
if (use_device)
{
std::cerr << "[Segmentation] Using GPU..." << std::endl;
std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&Segmentation::cloud_cb<Device>, this, _1, _2, _3);
- c = grabber->registerCallback (f);
+ c = grabber.registerCallback (f);
}
else
{
// std::cerr << "[Segmentation] Using CPU..." << std::endl;
// std::function<void (const openni_wrapper::Image::Ptr& image, const openni_wrapper::DepthImage::Ptr& depth_image, float)> f = std::bind (&Segmentation::cloud_cb<Host>, this, _1, _2, _3);
-// c = grabber->registerCallback (f);
+// c = grabber.registerCallback (f);
}
viewer.runOnVisualizationThread (std::bind(&Segmentation::viz_cb, this, _1), "viz_cb");
- grabber->start ();
+ grabber.start ();
while (!viewer.wasStopped ())
{
pcl_sleep (1);
}
- grabber->stop ();
+ grabber.stop ();
}
}
#include <pcl/cuda/point_types.h>
#include <pcl/cuda/thrust.h>
-#include <boost/shared_ptr.hpp>
+#include <pcl/make_shared.h>
namespace pcl
{
{
int x_pos, y_pos, x, y, idx;
- int leftX, rightX, leftY, rightY;
-
int radiusSearchPointCount;
- int maxSearchDistance;
double squaredMaxSearchRadius;
assert (k_arg>0);
// select point from organized pointcloud
x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
- radiusSearchLookup_Iterator++;
+ ++radiusSearchLookup_Iterator;
radiusSearchPointCount++;
if ((x >= 0) && (y >= 0) && (x < (int)input_->width) && (y < (int)input_->height))
squared_radius = std::min<double>(nearestNeighbors.back ().squared_distance_, squaredMaxSearchRadius);
+ int leftX, rightX, leftY, rightY;
this->getProjectedRadiusSearchBox(p_q_arg, squared_radius, leftX, rightX, leftY, rightY);
leftX *=leftX;
pointCountRadiusSearch = (rightX-leftX)*(rightY-leftY);
// search for maximum distance between search point to window borders in 2-D search window
- maxSearchDistance = 0;
+ int maxSearchDistance = 0;
maxSearchDistance = std::max<int> (maxSearchDistance, leftX + leftY);
maxSearchDistance = std::max<int> (maxSearchDistance, leftX + rightY);
maxSearchDistance = std::max<int> (maxSearchDistance, rightX + leftY);
// select point from organized point cloud
x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
- radiusSearchLookup_Iterator++;
+ ++radiusSearchLookup_Iterator;
if ((x >= 0) && (y >= 0) && (x < (int)input_->width) && (y < (int)input_->height))
{
--------------------------------------------------------
The pcl apps component contains a command line tool for point cloud compression
-and streaming: Simply execute "./openni_stream_compression -?" to see a full
+and streaming: Simply execute "./pcl_openni_octree_compression -?" to see a full
list of options (note: the output on screen may differ)::
PCL point cloud stream compression
- usage: ./openni_stream_compression [mode] [profile] [parameters]
+ usage: ./pcl_openni_octree_compression [mode] [profile] [parameters]
I/O:
-f file : file name
-e : show input cloud during encoding
example:
- ./openni_stream_compression -x -p highC -t -f pc_compressed.pcc
+ ./pcl_openni_octree_compression -x -p highC -t -f pc_compressed.pcc
In order to stream compressed point cloud via TCP/IP, you can start the server with::
- $ ./openni_stream_compression -s
+ $ ./pcl_openni_octree_compression -s
It will listen on port 6666 for incoming connections. Now start the client with::
- $ ./openni_stream_compression -c SERVER_NAME
+ $ ./pcl_openni_octree_compression -c SERVER_NAME
and remotely captured point clouds will be locally shown in the point cloud viewer.
You need a davidSDK to run the SDK on the server side, the official davidSDK does not come with a Makefile or a CMake project. An un-official fork provides a CMake project that enables to easily use the SDK under Linux (with minor tweaks)
* `Official davidSDK download page <http://www.david-3d.com/en/support/downloads>`_
- * `Victor Lamoine davidSDK fork <https://github.com/InstitutMaupertuis/davidSDK>`_
+ * `Victor Lamoine davidSDK fork <https://gitlab.com/InstitutMaupertuis/davidSDK>`_
-Please test `the example project <https://github.com/InstitutMaupertuis/davidSDK/blob/master/README.md#example-project-using-the-davidsdk>`_ before going further.
+Please test `the example project <https://gitlab.com/InstitutMaupertuis/davidSDK/blob/master/README.md#example-project-using-the-davidsdk>`_ before going further.
.. note:: If you use the trial version of the server, the only format available is OBJ (used by default)
using AdjacencyIterator = LCCPSegmentation<PointT>::AdjacencyIterator;
using EdgeID = LCCPSegmentation<PointT>::EdgeID;
- std::set<EdgeID> edge_drawn;
-
const unsigned char black_color [3] = {0, 0, 0};
const unsigned char white_color [3] = {255, 255, 255};
const unsigned char concave_color [3] = {255, 0, 0};
tree_ec->setInputCloud (cloud_ptr);
// Extracting Euclidean clusters using cloud and its normals
- std::vector<int> indices;
std::vector<pcl::PointIndices> cluster_indices;
const float tolerance = 0.5f; // 50cm tolerance in (x, y, z) coordinate system
const double eps_angle = 5 * (M_PI / 180.0); // 5degree tolerance in normals
using AdjacencyIterator = LCCPSegmentation<PointT>::AdjacencyIterator;
using EdgeID = LCCPSegmentation<PointT>::EdgeID;
- std::set<EdgeID> edge_drawn;
-
const unsigned char convex_color [3] = {255, 255, 255};
const unsigned char concave_color [3] = {255, 0, 0};
const unsigned char* color;
src/crh.cpp
src/don.cpp
src/fpfh.cpp
+ src/from_meshes.cpp
src/gasd.cpp
src/gfpfh.cpp
src/integral_image_normal.cpp
inline void
getCentroidClusters (std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > & centroids)
{
- for (const auto& centroid: centroids_dominant_orientations_)
- centroids.push_back (centroid);
+ centroids.insert (centroids.cend (), centroids_dominant_orientations_.cbegin (),
+ centroids_dominant_orientations_.cend ());
}
/** \brief Get the normal centroids used to compute different CVFH descriptors
}
- /** \brief Compute GICP-style covariance matrices given a point cloud and
+ /** \brief Compute GICP-style covariance matrices given a point cloud and
* the corresponding surface normals.
* \param[in] cloud Point cloud containing the XYZ coordinates,
* \param[in] normals Point cloud containing the corresponding surface normals.
* \param[in] Optional: Epsilon for the expected noise along the surface normal (default: 0.001)
*/
template <typename PointT, typename PointNT> inline void
- computeApproximateCovariances(const pcl::PointCloud<PointT>& cloud,
+ computeApproximateCovariances(const pcl::PointCloud<PointT>& cloud,
const pcl::PointCloud<PointNT>& normals,
std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> >& covariances,
double epsilon = 0.001)
covariances.reserve (nr_points);
for (const auto& point: normals.points)
{
- Eigen::Vector3d normal (normals.points[i].normal_x,
- normals.points[i].normal_y,
- normals.points[i].normal_z);
+ Eigen::Vector3d normal (point.normal_x,
+ point.normal_y,
+ point.normal_z);
// compute rotation matrix
Eigen::Matrix3d rot;
}
}
+
+#define PCL_INSTANTIATE_computeApproximateCovariances(T,NT) template PCL_EXPORTS void pcl::features::computeApproximateCovariances<T,NT> \
+ (const pcl::PointCloud<T>&, const pcl::PointCloud<NT>&, std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>>&, double);
radii_interval_[j] = static_cast<float> (std::exp (std::log (min_radius_) + ((static_cast<float> (j) / static_cast<float> (radius_bins_)) * std::log (search_radius_ / min_radius_))));
// Fill theta divisions of elevation
- theta_divisions_.resize (elevation_bins_ + 1);
- for (std::size_t k = 0; k < elevation_bins_ + 1; k++)
- theta_divisions_[k] = static_cast<float> (k) * elevation_interval;
+ theta_divisions_.resize (elevation_bins_ + 1, elevation_interval);
+ theta_divisions_[0] = 0.f;
+ std::partial_sum(theta_divisions_.begin (), theta_divisions_.end (), theta_divisions_.begin ());
// Fill phi didvisions of elevation
- phi_divisions_.resize (azimuth_bins_ + 1);
- for (std::size_t l = 0; l < azimuth_bins_ + 1; l++)
- phi_divisions_[l] = static_cast<float> (l) * azimuth_interval;
+ phi_divisions_.resize (azimuth_bins_ + 1, azimuth_interval);
+ phi_divisions_[0] = 0.f;
+ std::partial_sum(phi_divisions_.begin (), phi_divisions_.end (), phi_divisions_.begin ());
// LookUp Table that contains the volume of all the bins
// "phi" term of the volume integral
const std::size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
if (neighb_cnt == 0)
{
- for (float &descriptor : desc)
- descriptor = std::numeric_limits<float>::quiet_NaN ();
-
- memset (rf, 0, sizeof (rf[0]) * 9);
+ std::fill (desc.begin (), desc.end (), std::numeric_limits<float>::quiet_NaN ());
+ std::fill (rf, rf + 9, 0.f);
return (false);
}
- float minDist = std::numeric_limits<float>::max ();
- int minIndex = -1;
- for (std::size_t i = 0; i < nn_indices.size (); i++)
- {
- if (nn_dists[i] < minDist)
- {
- minDist = nn_dists[i];
- minIndex = nn_indices[i];
- }
- }
+ const auto minDistanceIt = std::min_element(nn_dists.begin (), nn_dists.end ());
+ const auto minIndex = nn_indices[std::distance (nn_dists.begin (), minDistanceIt)];
// Get origin point
Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
float theta = normal.dot (no);
theta = pcl::rad2deg (std::acos (std::min (1.0f, std::max (-1.0f, theta))));
- // Bin (j, k, l)
- std::size_t j = 0;
- std::size_t k = 0;
- std::size_t l = 0;
-
// Compute the Bin(j, k, l) coordinates of current neighbour
- for (std::size_t rad = 1; rad < radius_bins_+1; rad++)
- {
- if (r <= radii_interval_[rad])
- {
- j = rad-1;
- break;
- }
- }
-
- for (std::size_t ang = 1; ang < elevation_bins_+1; ang++)
- {
- if (theta <= theta_divisions_[ang])
- {
- k = ang-1;
- break;
- }
- }
+ const auto rad_min = std::lower_bound(std::next (radii_interval_.cbegin ()), radii_interval_.cend (), r);
+ const auto theta_min = std::lower_bound(std::next (theta_divisions_.cbegin ()), theta_divisions_.cend (), theta);
+ const auto phi_min = std::lower_bound(std::next (phi_divisions_.cbegin ()), phi_divisions_.cend (), phi);
- for (std::size_t ang = 1; ang < azimuth_bins_+1; ang++)
- {
- if (phi <= phi_divisions_[ang])
- {
- l = ang-1;
- break;
- }
- }
+ // Bin (j, k, l)
+ const auto j = std::distance(radii_interval_.cbegin (), std::prev(rad_min));
+ const auto k = std::distance(theta_divisions_.cbegin (), std::prev(theta_min));
+ const auto l = std::distance(phi_divisions_.cbegin (), std::prev(phi_min));
// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
std::vector<int> neighbour_indices;
} // end for each neighbour
// 3DSC does not define a repeatable local RF, we set it to zero to signal it to the user
- memset (rf, 0, sizeof (rf[0]) * 9);
+ std::fill (rf, rf + 9, 0);
return (true);
}
// If the point is not finite, set the descriptor to NaN and continue
if (!isFinite ((*input_)[(*indices_)[point_index]]))
{
- for (std::size_t i = 0; i < descriptor_length_; ++i)
- output[point_index].descriptor[i] = std::numeric_limits<float>::quiet_NaN ();
-
- memset (output[point_index].rf, 0, sizeof (output[point_index].rf[0]) * 9);
+ std::fill (output[point_index].descriptor, output[point_index].descriptor + descriptor_length_,
+ std::numeric_limits<float>::quiet_NaN ());
+ std::fill (output[point_index].rf, output[point_index].rf + 9, 0);
output.is_dense = false;
continue;
}
std::vector<float> descriptor (descriptor_length_);
if (!computePoint (point_index, *normals_, output[point_index].rf, descriptor))
output.is_dense = false;
- for (std::size_t j = 0; j < descriptor_length_; ++j)
- output[point_index].descriptor[j] = descriptor[j];
+ std::copy (descriptor.begin (), descriptor.end (), output[point_index].descriptor);
}
}
}
//find the center by averaging the points positions
- center.setZero ();
-
- for (int i = 0; i < n_points; ++i)
- {
- center += points.row (i);
- }
-
- center /= static_cast<float> (n_points);
+ center = points.colwise().mean().transpose();
//copy points - average (center)
- Eigen::Matrix<float, Eigen::Dynamic, 3> A (n_points, 3); //PointData
- for (int i = 0; i < n_points; ++i)
- {
- A (i, 0) = points (i, 0) - center.x ();
- A (i, 1) = points (i, 1) - center.y ();
- A (i, 2) = points (i, 2) - center.z ();
- }
+ const Eigen::Matrix<float, Eigen::Dynamic, 3> A = points.rowwise() - center.transpose();
Eigen::JacobiSVD<Eigen::MatrixXf> svd (A, Eigen::ComputeFullV);
norm = svd.matrixV ().col (2);
lrf.row (0).matrix () = x_axis;
- for (int i = 0; i < check_margin_array_size_; i++)
- {
- check_margin_array_[i] = false;
- margin_array_min_angle_[i] = std::numeric_limits<float>::max ();
- margin_array_max_angle_[i] = -std::numeric_limits<float>::max ();
- margin_array_min_angle_normal_[i] = -1.0;
- margin_array_max_angle_normal_[i] = -1.0;
- }
+ check_margin_array_.assign(check_margin_array_size_, false);
+ margin_array_min_angle_.assign(check_margin_array_size_, std::numeric_limits<float>::max ());
+ margin_array_max_angle_.assign(check_margin_array_size_, -std::numeric_limits<float>::max ());
+ margin_array_min_angle_normal_.assign(check_margin_array_size_, -1.0);
+ margin_array_max_angle_normal_.assign(check_margin_array_size_, -1.0);
+
max_boundary_angle = (2 * static_cast<float> (M_PI)) / static_cast<float> (check_margin_array_size_);
}
int hole_end;
int hole_first;
- //find first no border pie
- int first_no_border = -1;
- if (check_margin_array_[check_margin_array_size_ - 1])
- {
- first_no_border = 0;
- }
- else
- {
- for (int i = 0; i < check_margin_array_size_; i++)
+ const auto find_first_no_border_pie = [](const auto& array) -> std::size_t {
+ if (array.back())
{
- if (check_margin_array_[i])
- {
- first_no_border = i;
- break;
- }
+ return 0;
}
- }
+ const auto result = std::find_if(array.cbegin (), array.cend (),
+ [](const auto& x) -> bool { return x;});
+ return std::distance(array.cbegin (), result);
+ };
+ const auto first_no_border = find_first_no_border_pie(check_margin_array_);
//float steep_prob = 0.0;
float max_hole_prob = -std::numeric_limits<float>::max ();
size_list_[scale] = 0;
// generate the pattern points look-up
- double alpha, theta;
for (std::size_t rot = 0; rot < n_rot_; ++rot)
{
// this is the rotation of the feature
- theta = double (rot) * 2 * M_PI / double (n_rot_);
+ double theta = double (rot) * 2 * M_PI / double (n_rot_);
for (int ring = 0; ring < rings; ++ring)
{
for (int num = 0; num < number_list[ring]; ++num)
{
// the actual coordinates on the circle
- alpha = double (num) * 2 * M_PI / double (number_list[ring]);
+ double alpha = double (num) * 2 * M_PI / double (number_list[ring]);
// feature rotation plus angle of the point
pattern_iterator->x = scale_list_[scale] * radius_list[ring] * static_cast<float> (std::cos (alpha + theta));
// the initialization is made with () after the [nbins]
std::vector<kiss_fft_scalar> spatial_data(nbins);
- float sum_w = 0, w = 0;
- int bin = 0;
+ float sum_w = 0;
for (const auto &point : grid.points)
{
- bin = static_cast<int> ((((std::atan2 (point.normal_y, point.normal_x) + M_PI) * 180 / M_PI) / bin_angle)) % nbins;
- w = std::sqrt (point.normal_y * point.normal_y + point.normal_x * point.normal_x);
+ int bin = static_cast<int> ((((std::atan2 (point.normal_y, point.normal_x) + M_PI) * 180 / M_PI) / bin_angle)) % nbins;
+ float w = std::sqrt (point.normal_y * point.normal_y + point.normal_x * point.normal_x);
sum_w += w;
spatial_data[bin] += w;
}
srand (static_cast<unsigned int> (time (nullptr)));
int maxindex = static_cast<int> (pc.points.size ());
- int index1, index2, index3;
std::vector<float> d2v, d1v, d3v, wt_d3;
std::vector<int> wt_d2;
d1v.reserve (sample_size);
for (std::size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx)
{
// get a new random point
- index1 = rand()%maxindex;
- index2 = rand()%maxindex;
- index3 = rand()%maxindex;
+ int index1 = rand()%maxindex;
+ int index2 = rand()%maxindex;
+ int index3 = rand()%maxindex;
if (index1==index2 || index1 == index3 || index2 == index3)
{
template <typename PointInT, typename PointOutT> void
pcl::ESFEstimation<PointInT, PointOutT>::voxelize9 (PointCloudIn &cluster)
{
- int xi,yi,zi,xx,yy,zz;
for (std::size_t i = 0; i < cluster.points.size (); ++i)
{
- xx = cluster.points[i].x<0.0? static_cast<int>(std::floor(cluster.points[i].x)+GRIDSIZE_H) : static_cast<int>(std::ceil(cluster.points[i].x)+GRIDSIZE_H-1);
- yy = cluster.points[i].y<0.0? static_cast<int>(std::floor(cluster.points[i].y)+GRIDSIZE_H) : static_cast<int>(std::ceil(cluster.points[i].y)+GRIDSIZE_H-1);
- zz = cluster.points[i].z<0.0? static_cast<int>(std::floor(cluster.points[i].z)+GRIDSIZE_H) : static_cast<int>(std::ceil(cluster.points[i].z)+GRIDSIZE_H-1);
+ int xx = cluster.points[i].x<0.0? static_cast<int>(std::floor(cluster.points[i].x)+GRIDSIZE_H) : static_cast<int>(std::ceil(cluster.points[i].x)+GRIDSIZE_H-1);
+ int yy = cluster.points[i].y<0.0? static_cast<int>(std::floor(cluster.points[i].y)+GRIDSIZE_H) : static_cast<int>(std::ceil(cluster.points[i].y)+GRIDSIZE_H-1);
+ int zz = cluster.points[i].z<0.0? static_cast<int>(std::floor(cluster.points[i].z)+GRIDSIZE_H) : static_cast<int>(std::ceil(cluster.points[i].z)+GRIDSIZE_H-1);
for (int x = -1; x < 2; x++)
for (int y = -1; y < 2; y++)
for (int z = -1; z < 2; z++)
{
- xi = xx + x;
- yi = yy + y;
- zi = zz + z;
+ int xi = xx + x;
+ int yi = yy + y;
+ int zi = zz + z;
if (yi >= GRIDSIZE || xi >= GRIDSIZE || zi>=GRIDSIZE || yi < 0 || xi < 0 || zi < 0)
{
template <typename PointInT, typename PointOutT> void
pcl::ESFEstimation<PointInT, PointOutT>::cleanup9 (PointCloudIn &cluster)
{
- int xi,yi,zi,xx,yy,zz;
for (std::size_t i = 0; i < cluster.points.size (); ++i)
{
- xx = cluster.points[i].x<0.0? static_cast<int>(std::floor(cluster.points[i].x)+GRIDSIZE_H) : static_cast<int>(std::ceil(cluster.points[i].x)+GRIDSIZE_H-1);
- yy = cluster.points[i].y<0.0? static_cast<int>(std::floor(cluster.points[i].y)+GRIDSIZE_H) : static_cast<int>(std::ceil(cluster.points[i].y)+GRIDSIZE_H-1);
- zz = cluster.points[i].z<0.0? static_cast<int>(std::floor(cluster.points[i].z)+GRIDSIZE_H) : static_cast<int>(std::ceil(cluster.points[i].z)+GRIDSIZE_H-1);
+ int xx = cluster.points[i].x<0.0? static_cast<int>(std::floor(cluster.points[i].x)+GRIDSIZE_H) : static_cast<int>(std::ceil(cluster.points[i].x)+GRIDSIZE_H-1);
+ int yy = cluster.points[i].y<0.0? static_cast<int>(std::floor(cluster.points[i].y)+GRIDSIZE_H) : static_cast<int>(std::ceil(cluster.points[i].y)+GRIDSIZE_H-1);
+ int zz = cluster.points[i].z<0.0? static_cast<int>(std::floor(cluster.points[i].z)+GRIDSIZE_H) : static_cast<int>(std::ceil(cluster.points[i].z)+GRIDSIZE_H-1);
for (int x = -1; x < 2; x++)
for (int y = -1; y < 2; y++)
for (int z = -1; z < 2; z++)
{
- xi = xx + x;
- yi = yy + y;
- zi = zz + z;
+ int xi = xx + x;
+ int yi = yy + y;
+ int zi = zz + z;
if (yi >= GRIDSIZE || xi >= GRIDSIZE || zi>=GRIDSIZE || yi < 0 || xi < 0 || zi < 0)
{
pcl::compute3DCentroid (pc, centroid);
pcl::demeanPointCloud (pc, centroid, local_cloud_);
- float max_distance = 0, d;
+ float max_distance = 0;
pcl::PointXYZ cog (0, 0, 0);
for (std::size_t i = 0; i < local_cloud_.points.size (); ++i)
{
- d = pcl::euclideanDistance(cog,local_cloud_.points[i]);
+ float d = pcl::euclideanDistance(cog,local_cloud_.points[i]);
if (d > max_distance)
max_distance = d;
}
{
Eigen::Vector4f pfh_tuple;
// Get the number of bins from the histograms size
+ // @TODO: use arrays
int nr_bins_f1 = static_cast<int> (hist_f1.cols ());
int nr_bins_f2 = static_cast<int> (hist_f2.cols ());
int nr_bins_f3 = static_cast<int> (hist_f3.cols ());
const std::vector<int> &indices, const std::vector<float> &dists, Eigen::VectorXf &fpfh_histogram)
{
assert (indices.size () == dists.size ());
+ // @TODO: use arrays
double sum_f1 = 0.0, sum_f2 = 0.0, sum_f3 = 0.0;
float weight = 0.0, val_f1, val_f2, val_f3;
sum_f3 = 100.0 / sum_f3; // histogram values sum up to 100
// Adjust final FPFH values
- for (int f1_i = 0; f1_i < nr_bins_f1; ++f1_i)
- fpfh_histogram[f1_i] *= static_cast<float> (sum_f1);
- for (int f2_i = 0; f2_i < nr_bins_f2; ++f2_i)
- fpfh_histogram[f2_i + nr_bins_f1] *= static_cast<float> (sum_f2);
- for (int f3_i = 0; f3_i < nr_bins_f3; ++f3_i)
- fpfh_histogram[f3_i + nr_bins_f12] *= static_cast<float> (sum_f3);
+ const auto denormalize_with = [](auto factor)
+ {
+ return [=](const auto& data) { return data * factor; };
+ };
+
+ auto last = fpfh_histogram.data ();
+ last = std::transform(last, last + nr_bins_f1, last, denormalize_with (sum_f1));
+ last = std::transform(last, last + nr_bins_f2, last, denormalize_with (sum_f2));
+ std::transform(last, last + nr_bins_f3, last, denormalize_with (sum_f3));
}
//////////////////////////////////////////////////////////////////////////////////////////////
weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
// ...and copy it into the output cloud
- for (Eigen::Index d = 0; d < fpfh_histogram_.size (); ++d)
- output.points[idx].histogram[d] = fpfh_histogram_[d];
+ std::copy_n(fpfh_histogram_.data (), fpfh_histogram_.size (), output.points[idx].histogram);
}
}
else
weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
// ...and copy it into the output cloud
- for (Eigen::Index d = 0; d < fpfh_histogram_.size (); ++d)
- output.points[idx].histogram[d] = fpfh_histogram_[d];
+ std::copy_n(fpfh_histogram_.data (), fpfh_histogram_.size (), output.points[idx].histogram);
}
}
}
#ifndef PCL_FEATURES_IMPL_FPFH_OMP_H_
#define PCL_FEATURES_IMPL_FPFH_OMP_H_
+#include <numeric>
+
#include <pcl/features/fpfh_omp.h>
//////////////////////////////////////////////////////////////////////////////////////////////
spfh_indices_set.insert (nn_indices.begin (), nn_indices.end ());
}
spfh_indices_vec.resize (spfh_indices_set.size ());
- std::copy (spfh_indices_set.begin (), spfh_indices_set.end (), spfh_indices_vec.begin ());
+ std::copy (spfh_indices_set.cbegin (), spfh_indices_set.cend (), spfh_indices_vec.begin ());
}
else
{
// Special case: When a feature must be computed at every point, there is no need for a neighborhood search
spfh_indices_vec.resize (indices_->size ());
- for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
- spfh_indices_vec[idx] = idx;
+ std::iota(spfh_indices_vec.begin (), spfh_indices_vec.end (),
+ static_cast<decltype(spfh_indices_vec)::value_type>(0));
}
// Initialize the arrays that will store the SPFH signatures
hist_incr_ = 100.0f / static_cast<float> (shape_samples_.size () - 1);
// for each sample
- for (std::size_t i = 0; i < shape_samples_.size (); ++i)
+ for (const auto& sample: shape_samples_)
{
// compute shape histogram array coord based on distance between sample and centroid
- const Eigen::Vector4f p (shape_samples_[i].x, shape_samples_[i].y, shape_samples_[i].z, 0.0f);
+ const Eigen::Vector4f p (sample.x, sample.y, sample.z, 0.0f);
const float d = p.norm ();
const float shape_grid_step = distance_normalization_factor / shape_half_grid_size_;
Eigen::VectorXf::Zero (color_hists_size_ + 2));
// for each sample
- for (std::size_t i = 0; i < shape_samples_.size (); ++i)
+ for (const auto& sample: shape_samples_)
{
// compute shape histogram array coord based on distance between sample and centroid
- const Eigen::Vector4f p (shape_samples_[i].x, shape_samples_[i].y, shape_samples_[i].z, 0.0f);
+ const Eigen::Vector4f p (sample.x, sample.y, sample.z, 0.0f);
// compute hue value
float hue = 0.f;
- const unsigned char max = std::max (shape_samples_[i].r, std::max (shape_samples_[i].g, shape_samples_[i].b));
- const unsigned char min = std::min (shape_samples_[i].r, std::min (shape_samples_[i].g, shape_samples_[i].b));
+ const unsigned char max = std::max (sample.r, std::max (sample.g, sample.b));
+ const unsigned char min = std::min (sample.r, std::min (sample.g, sample.b));
const float diff_inv = 1.f / static_cast <float> (max - min);
if (std::isfinite (diff_inv))
{
- if (max == shape_samples_[i].r)
+ if (max == sample.r)
{
- hue = 60.f * (static_cast <float> (shape_samples_[i].g - shape_samples_[i].b) * diff_inv);
+ hue = 60.f * (static_cast <float> (sample.g - sample.b) * diff_inv);
}
- else if (max == shape_samples_[i].g)
+ else if (max == sample.g)
{
- hue = 60.f * (2.f + static_cast <float> (shape_samples_[i].b - shape_samples_[i].r) * diff_inv);
+ hue = 60.f * (2.f + static_cast <float> (sample.b - sample.r) * diff_inv);
}
else
{
- hue = 60.f * (4.f + static_cast <float> (shape_samples_[i].r - shape_samples_[i].g) * diff_inv); // max == b
+ hue = 60.f * (4.f + static_cast <float> (sample.r - sample.g) * diff_inv); // max == b
}
if (hue < 0.f)
output.width = 1;
output.height = 1;
output.points.resize (1);
- std::copy (gfpfh_histogram.begin (), gfpfh_histogram.end (), output.points[0].histogram);
+ std::copy (gfpfh_histogram.cbegin (), gfpfh_histogram.cend (), output.points[0].histogram);
}
//////////////////////////////////////////////////////////////////////////////////////////////
// Save the type of each point
int NR_CLASS = 5; // TODO make this nicer
std::vector<int> types (radii->points.size ());
- for (std::size_t idx = 0; idx < radii->points.size (); ++idx)
- types[idx] = getSimpleType (radii->points[idx].r_min, radii->points[idx].r_max);
+ std::transform(radii->points.cbegin (), radii->points.cend (), types.begin (),
+ [](const auto& point) {
+ // GCC 5.4 can't find unqualified getSimpleType
+ return GRSDEstimation<PointInT, PointNT, PointOutT>::getSimpleType(point.r_min, point.r_max); });
// Get the transitions between surface types between neighbors of occupied cells
Eigen::MatrixXi transition_matrix = Eigen::MatrixXi::Zero (NR_CLASS + 1, NR_CLASS + 1);
for (std::size_t idx = 0; idx < cloud_downsampled->points.size (); ++idx)
{
- int source_type = types[idx];
+ const int source_type = types[idx];
std::vector<int> neighbors = grid.getNeighborCentroidIndices (cloud_downsampled->points[idx], relative_coordinates_all_);
for (const int &neighbor : neighbors)
{
- int neighbor_type;
- if (neighbor == -1) // empty
- neighbor_type = NR_CLASS;
- else
+ int neighbor_type = NR_CLASS;
+ if (neighbor != -1) // not empty
neighbor_type = types[neighbor];
transition_matrix (source_type, neighbor_type)++;
}
continue;
}
- if (u < border || v > right)
+ if (u < border || u > right)
{
output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
output.points[idx].curvature = bad_point;
continue;
}
- if (u < border || v > right)
+ if (u < border || u > right)
{
output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
output.points[idx].curvature = bad_point;
}
else
{
- output [pt_index].getNormalVector3fMap ().setConstant (bad_point);
- output [pt_index].curvature = bad_point;
+ output [idx].getNormalVector3fMap ().setConstant (bad_point);
+ output [idx].curvature = bad_point;
}
}
}
float mu200 = 0, mu020 = 0, mu002 = 0, mu110 = 0, mu101 = 0, mu011 = 0;
// Iterate over the nearest neighbors set
- for (std::size_t nn_idx = 0; nn_idx < cloud.points.size (); ++nn_idx )
+ for (const auto& point: cloud.points)
{
// Demean the points
- temp_pt_[0] = cloud.points[nn_idx].x - xyz_centroid_[0];
- temp_pt_[1] = cloud.points[nn_idx].y - xyz_centroid_[1];
- temp_pt_[2] = cloud.points[nn_idx].z - xyz_centroid_[2];
+ temp_pt_[0] = point.x - xyz_centroid_[0];
+ temp_pt_[1] = point.y - xyz_centroid_[1];
+ temp_pt_[2] = point.z - xyz_centroid_[2];
mu200 += temp_pt_[0] * temp_pt_[0];
mu020 += temp_pt_[1] * temp_pt_[1];
#ifndef PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_
#define PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_
+#include <numeric>
#include <pcl/features/multiscale_feature_persistence.h>
//////////////////////////////////////////////////////////////////////////////////////////////
pcl::MultiscaleFeaturePersistence<PointSource, PointFeature>::calculateMeanFeature ()
{
// Reset mean feature
- for (int i = 0; i < feature_representation_->getNumberOfDimensions (); ++i)
- mean_feature_[i] = 0.0f;
+ std::fill_n(mean_feature_.begin (), mean_feature_.size (), 0.f);
- float normalization_factor = 0.0f;
+ std::size_t normalization_factor = 0;
for (const auto& scale: features_at_scale_vectorized_)
{
- normalization_factor += static_cast<float> (scale.size ());
+ normalization_factor += scale.size (); // not using accumulate for cache efficiency
for (const auto &feature : scale)
- for (int dim_i = 0; dim_i < feature_representation_->getNumberOfDimensions (); ++dim_i)
- mean_feature_[dim_i] += feature[dim_i];
+ std::transform(mean_feature_.cbegin (), mean_feature_.cend (),
+ feature.cbegin (), mean_feature_.begin (), std::plus<>{});
}
- for (int dim_i = 0; dim_i < feature_representation_->getNumberOfDimensions (); ++dim_i)
- mean_feature_[dim_i] /= normalization_factor;
+ const float factor = std::min<float>(1, normalization_factor);
+ std::transform(mean_feature_.cbegin(),
+ mean_feature_.cend(),
+ mean_feature_.begin(),
+ [factor](const auto& mean) {
+ return mean / factor;
+ });
}
if (input_->is_dense)
{
#ifdef _OPENMP
-#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
+#pragma omp parallel for shared (output) firstprivate (nn_indices, nn_dists) num_threads(threads_)
#endif
// Iterating over the entire index vector
for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
else
{
#ifdef _OPENMP
-#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
+#pragma omp parallel for shared (output) firstprivate (nn_indices, nn_dists) num_threads(threads_)
#endif
// Iterating over the entire index vector
for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
if (!found_invalid_neighbor)
{
// Every neighboring points are valid
- std::vector<float>::iterator min_itr = std::min_element (nghr_dist.begin (), nghr_dist.end ());
- std::vector<float>::iterator max_itr = std::max_element (nghr_dist.begin (), nghr_dist.end ());
+ std::vector<float>::const_iterator min_itr, max_itr;
+ std::tie (min_itr, max_itr) = std::minmax_element (nghr_dist.cbegin (), nghr_dist.cend ());
float nghr_dist_min = *min_itr;
float nghr_dist_max = *max_itr;
- float dist_dominant = std::abs (nghr_dist_min) > std::abs (nghr_dist_max) ? nghr_dist_min : nghr_dist_max;
+ float dist_dominant = std::max(std::abs (nghr_dist_min),std::abs (nghr_dist_max));
if (std::abs (dist_dominant) > th_depth_discon_*std::abs (curr_depth))
{
// Found a depth discontinuity
// Initialize to 0
covariance_matrix_.setZero ();
- double demean_xy, demean_xz, demean_yz;
// For each point in the cloud
for (std::size_t idx = 0; idx < indices.size (); ++idx)
{
demean_ = projected_normals_[idx] - xyz_centroid_;
- demean_xy = demean_[0] * demean_[1];
- demean_xz = demean_[0] * demean_[2];
- demean_yz = demean_[1] * demean_[2];
+ double demean_xy = demean_[0] * demean_[1];
+ double demean_xz = demean_[0] * demean_[2];
+ double demean_yz = demean_[1] * demean_[2];
covariance_matrix_(0, 0) += demean_[0] * demean_[0];
covariance_matrix_(0, 1) += static_cast<float> (demean_xy);
// Compute the RIFT descriptor
computeRIFT (*surface_, *gradient_, (*indices_)[idx], static_cast<float> (search_radius_), nn_indices, nn_dist_sqr, rift_descriptor);
- // Copy into the resultant cloud
- std::size_t bin = 0;
- for (Eigen::Index g_bin = 0; g_bin < rift_descriptor.cols (); ++g_bin)
- for (Eigen::Index d_bin = 0; d_bin < rift_descriptor.rows (); ++d_bin)
- output.points[idx].histogram[bin++] = rift_descriptor (d_bin, g_bin);
+ // Default layout is column major, copy elementwise
+ std::copy_n (rift_descriptor.data (), rift_descriptor.size (), output.points[idx].histogram);
}
}
} while (theta < 90.0f);
}
- float norm = 0.0f;
- for (std::size_t i_dim = 0; i_dim < feature_size; i_dim++)
- norm += std::abs (feature[i_dim]);
+ const float norm = std::accumulate(
+ feature.cbegin(), feature.cend(), 0.f, [](const auto& sum, const auto& val) {
+ return sum + std::abs(val);
+ });
+ float invert_norm;
if (norm < std::numeric_limits <float>::epsilon ())
- norm = 1.0f;
+ invert_norm = 1.0f;
else
- norm = 1.0f / norm;
+ invert_norm = 1.0f / norm;
output.points.emplace_back ();
for (std::size_t i_dim = 0; i_dim < feature_size; i_dim++)
- output.points.back ().histogram[i_dim] = feature[i_dim] * norm;
+ output.points.back().histogram[i_dim] = feature[i_dim] * invert_norm;
}
}
rotated_point.z = point (2);
rotated_cloud.points.emplace_back (rotated_point);
- if (min (0) > rotated_point.x) min (0) = rotated_point.x;
- if (min (1) > rotated_point.y) min (1) = rotated_point.y;
- if (min (2) > rotated_point.z) min (2) = rotated_point.z;
-
- if (max (0) < rotated_point.x) max (0) = rotated_point.x;
- if (max (1) < rotated_point.y) max (1) = rotated_point.y;
- if (max (2) < rotated_point.z) max (2) = rotated_point.z;
+ for (int i = 0; i < 3; ++i)
+ {
+ min(i) = std::min(min(i), point(i));
+ max(i) = std::max(max(i), point(i));
+ }
}
}
matrix (row, col) += 1.0f;
}
- matrix /= static_cast <float> (std::min<std::size_t> (1, cloud.points.size ()));
+ matrix /= std::max<float> (1, cloud.points.size ());
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}
// update min-max values for distance bins
- if (min_max_angle_by_dist[bin_d][0] > angle) min_max_angle_by_dist[bin_d][0] = angle;
- if (min_max_angle_by_dist[bin_d][1] < angle) min_max_angle_by_dist[bin_d][1] = angle;
+ min_max_angle_by_dist[bin_d][0] = std::min(angle, min_max_angle_by_dist[bin_d][0]);
+ min_max_angle_by_dist[bin_d][1] = std::max(angle, min_max_angle_by_dist[bin_d][1]);
}
// Estimate radius from min and max lines
for (std::size_t j = 0; j < radius_bins_ + 1; j++)
radii_interval_[j] = static_cast<float> (std::exp (std::log (min_radius_) + ((static_cast<float> (j) / static_cast<float> (radius_bins_)) * std::log (search_radius_/min_radius_))));
- // Fill theta didvisions of elevation
- theta_divisions_.resize (elevation_bins_+1);
- for (std::size_t k = 0; k < elevation_bins_+1; k++)
- theta_divisions_[k] = static_cast<float> (k) * elevation_interval;
+ // Fill theta divisions of elevation
+ theta_divisions_.resize (elevation_bins_ + 1, elevation_interval);
+ theta_divisions_[0] = 0;
+ std::partial_sum(theta_divisions_.begin (), theta_divisions_.end (), theta_divisions_.begin ());
- // Fill phi didvisions of elevation
- phi_divisions_.resize (azimuth_bins_+1);
- for (std::size_t l = 0; l < azimuth_bins_+1; l++)
- phi_divisions_[l] = static_cast<float> (l) * azimuth_interval;
+ // Fill phi divisions of elevation
+ phi_divisions_.resize (azimuth_bins_ + 1, azimuth_interval);
+ phi_divisions_[0] = 0;
+ std::partial_sum(phi_divisions_.begin (), phi_divisions_.end (), phi_divisions_.begin ());
// LookUp Table that contains the volume of all the bins
// "phi" term of the volume integral
float theta = normal.dot (no);
theta = pcl::rad2deg (std::acos (std::min (1.0f, std::max (-1.0f, theta))));
- /// Bin (j, k, l)
- std::size_t j = 0;
- std::size_t k = 0;
- std::size_t l = 0;
-
/// Compute the Bin(j, k, l) coordinates of current neighbour
- for (std::size_t rad = 1; rad < radius_bins_ + 1; rad++)
- {
- if (r <= radii_interval_[rad])
- {
- j = rad - 1;
- break;
- }
- }
+ const auto rad_min = std::lower_bound(std::next (radii_interval_.cbegin ()), radii_interval_.cend (), r);
+ const auto theta_min = std::lower_bound(std::next (theta_divisions_.cbegin ()), theta_divisions_.cend (), theta);
+ const auto phi_min = std::lower_bound(std::next (phi_divisions_.cbegin ()), phi_divisions_.cend (), phi);
- for (std::size_t ang = 1; ang < elevation_bins_ + 1; ang++)
- {
- if (theta <= theta_divisions_[ang])
- {
- k = ang - 1;
- break;
- }
- }
-
- for (std::size_t ang = 1; ang < azimuth_bins_ + 1; ang++)
- {
- if (phi <= phi_divisions_[ang])
- {
- l = ang - 1;
- break;
- }
- }
+ /// Bin (j, k, l)
+ const auto j = std::distance(radii_interval_.cbegin (), std::prev(rad_min));
+ const auto k = std::distance(theta_divisions_.cbegin (), std::prev(theta_min));
+ const auto l = std::distance(phi_divisions_.cbegin (), std::prev(phi_min));
/// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
std::vector<int> neighbour_indices;
!std::isfinite (current_frame.y_axis[0]) ||
!std::isfinite (current_frame.z_axis[0]) )
{
- for (std::size_t i = 0; i < descriptor_length_; ++i)
- output[point_index].descriptor[i] = std::numeric_limits<float>::quiet_NaN ();
-
- memset (output[point_index].rf, 0, sizeof (output[point_index].rf[0]) * 9);
+ std::fill (output.points[point_index].descriptor, output.points[point_index].descriptor + descriptor_length_,
+ std::numeric_limits<float>::quiet_NaN ());
+ std::fill (output.points[point_index].rf, output.points[point_index].rf + 9, 0);
output.is_dense = false;
continue;
}
std::vector<float> descriptor (descriptor_length_);
computePointDescriptor (point_index, descriptor);
- for (std::size_t j = 0; j < descriptor_length_; ++j)
- output [point_index].descriptor[j] = descriptor[j];
+ std::copy (descriptor.begin (), descriptor.end (), output.points[point_index].descriptor);
}
}
{
Eigen::Vector4f pfh_tuple;
// Reset the whole thing
- hist_f1_.setZero (nr_bins_f1_);
- hist_f2_.setZero (nr_bins_f2_);
- hist_f3_.setZero (nr_bins_f3_);
- hist_f4_.setZero (nr_bins_f4_);
+ for (int i = 0; i < 4; ++i)
+ {
+ hist_f_[i].setZero (nr_bins_f_[i]);
+ }
// Get the bounding box of the current cluster
//Eigen::Vector4f min_pt, max_pt;
continue;
// Normalize the f1, f2, f3, f4 features and push them in the histogram
- int h_index = static_cast<int> (std::floor (nr_bins_f1_ * ((pfh_tuple[0] + M_PI) * d_pi_)));
- if (h_index < 0)
- h_index = 0;
- if (h_index >= nr_bins_f1_)
- h_index = nr_bins_f1_ - 1;
- hist_f1_ (h_index) += hist_incr;
-
- h_index = static_cast<int> (std::floor (nr_bins_f2_ * ((pfh_tuple[1] + 1.0) * 0.5)));
- if (h_index < 0)
- h_index = 0;
- if (h_index >= nr_bins_f2_)
- h_index = nr_bins_f2_ - 1;
- hist_f2_ (h_index) += hist_incr;
-
- h_index = static_cast<int> (std::floor (nr_bins_f3_ * ((pfh_tuple[2] + 1.0) * 0.5)));
- if (h_index < 0)
- h_index = 0;
- if (h_index >= nr_bins_f3_)
- h_index = nr_bins_f3_ - 1;
- hist_f3_ (h_index) += hist_incr;
-
- if (normalize_distances_)
- h_index = static_cast<int> (std::floor (nr_bins_f4_ * (pfh_tuple[3] / distance_normalization_factor)));
- else
- h_index = static_cast<int> (pcl_round (pfh_tuple[3] * 100));
-
- if (h_index < 0)
- h_index = 0;
- if (h_index >= nr_bins_f4_)
- h_index = nr_bins_f4_ - 1;
+ for (int i = 0; i < 3; ++i)
+ {
+ const int raw_index = static_cast<int> (std::floor (nr_bins_f_[i] * ((pfh_tuple[i] + M_PI) * d_pi_)));
+ const int h_index = std::max(std::min(raw_index, nr_bins_f_[i] - 1), 0);
+ hist_f_[i] (h_index) += hist_incr;
+ }
- hist_f4_ (h_index) += hist_incr_size_component;
+ if (hist_incr_size_component)
+ {
+ int h_index;
+ if (normalize_distances_)
+ h_index = static_cast<int> (std::floor (nr_bins_f_[3] * (pfh_tuple[3] / distance_normalization_factor)));
+ else
+ h_index = static_cast<int> (pcl_round (pfh_tuple[3] * 100));
+
+ h_index = std::max (std::min (h_index, nr_bins_f_[3] - 1), 0);
+ hist_f_[3] (h_index) += hist_incr_size_component;
+ }
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
// ---[ Step 1b : compute the centroid in normal space
Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero ();
- std::size_t cp = 0;
// If the data is dense, we don't need to check for NaN
if (use_given_normal_)
normal_centroid = normal_to_use_;
else
{
+ std::size_t cp = 0;
if (normals_->is_dense)
{
for (const auto& index: *indices_)
normals_->points[index].normal[2], 0);
// Normalize
double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5;
- int fi = static_cast<int> (std::floor (alpha * static_cast<double> (hist_vp_.size ())));
- if (fi < 0)
- fi = 0;
- if (fi > (static_cast<int> (hist_vp_.size ()) - 1))
- fi = static_cast<int> (hist_vp_.size ()) - 1;
+ std::size_t fi = static_cast<std::size_t> (std::floor (alpha * hist_vp_.size ()));
+ fi = std::max<std::size_t> (0u, fi);
+ fi = std::min<std::size_t> (hist_vp_.size () - 1, fi);
// Bin into the histogram
hist_vp_ [fi] += hist_incr;
}
// Estimate the FPFH at nn_indices[0] using the entire cloud and copy the resultant signature
auto outPtr = std::begin (output.points[0].histogram);
- outPtr = std::copy_n (hist_f1_.data (), hist_f1_.size (), outPtr);
- outPtr = std::copy_n (hist_f2_.data (), hist_f2_.size (), outPtr);
- outPtr = std::copy_n (hist_f3_.data (), hist_f3_.size (), outPtr);
- outPtr = std::copy_n (hist_f4_.data (), hist_f4_.size (), outPtr);
+ for (int i = 0; i < 4; ++i)
+ {
+ outPtr = std::copy_n (hist_f_[i].data (), hist_f_[i].size (), outPtr);
+ }
outPtr = std::copy_n (hist_vp_.data (), hist_vp_.size (), outPtr);
}
int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram = false);
template <typename PointInT, typename PointNT, typename PointOutT>
- [[deprecated("use computeRSD() overload that takes input point clouds by const reference")]]
+ PCL_DEPRECATED("use computeRSD() overload that takes input point clouds by const reference")
Eigen::MatrixXf
computeRSD (typename pcl::PointCloud<PointInT>::ConstPtr &surface, typename pcl::PointCloud<PointNT>::ConstPtr &normals,
const std::vector<int> &indices, double max_dist,
int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram = false);
template <typename PointNT, typename PointOutT>
- [[deprecated("use computeRSD() overload that takes input point cloud by const reference")]]
+ PCL_DEPRECATED("use computeRSD() overload that takes input point cloud by const reference")
Eigen::MatrixXf
computeRSD (typename pcl::PointCloud<PointNT>::ConstPtr &normals,
const std::vector<int> &indices, const std::vector<float> &sqr_dists, double max_dist,
#pragma once
+#include <array>
+
#include <pcl/point_types.h>
#include <pcl/features/feature.h>
/** \brief Empty constructor. */
VFHEstimation () :
- nr_bins_f1_ (45), nr_bins_f2_ (45), nr_bins_f3_ (45), nr_bins_f4_ (45), nr_bins_vp_ (128),
+ nr_bins_f_ ({45, 45, 45, 45}), nr_bins_vp_ (128),
vpx_ (0), vpy_ (0), vpz_ (0),
use_given_normal_ (false), use_given_centroid_ (false),
normalize_bins_ (true), normalize_distances_ (false), size_component_ (false),
d_pi_ (1.0f / (2.0f * static_cast<float> (M_PI)))
{
- hist_f1_.setZero (nr_bins_f1_);
- hist_f2_.setZero (nr_bins_f2_);
- hist_f3_.setZero (nr_bins_f3_);
- hist_f4_.setZero (nr_bins_f4_);
+ for (int i = 0; i < 4; ++i)
+ {
+ hist_f_[i].setZero (nr_bins_f_[i]);
+ }
search_radius_ = 0;
k_ = 0;
feature_name_ = "VFHEstimation";
private:
/** \brief The number of subdivisions for each feature interval. */
- int nr_bins_f1_, nr_bins_f2_, nr_bins_f3_, nr_bins_f4_, nr_bins_vp_;
+ std::array<int, 4> nr_bins_f_;
+ int nr_bins_vp_;
/** \brief Values describing the viewpoint ("pinhole" camera model assumed). For per point viewpoints, inherit
* from VFHEstimation and provide your own computeFeature (). By default, the viewpoint is set to 0,0,0.
initCompute () override;
/** \brief Placeholder for the f1 histogram. */
- Eigen::VectorXf hist_f1_;
- /** \brief Placeholder for the f2 histogram. */
- Eigen::VectorXf hist_f2_;
- /** \brief Placeholder for the f3 histogram. */
- Eigen::VectorXf hist_f3_;
- /** \brief Placeholder for the f4 histogram. */
- Eigen::VectorXf hist_f4_;
+ std::array<Eigen::VectorXf, 4> hist_f_;
/** \brief Placeholder for the vp histogram. */
Eigen::VectorXf hist_vp_;
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/features/from_meshes.h>
+
+#ifndef PCL_NO_PRECOMPILE
+#include <pcl/point_types.h>
+#include <pcl/impl/instantiate.hpp>
+// Instantiations of specific point types
+#ifdef PCL_ONLY_CORE_POINT_TYPES
+ PCL_INSTANTIATE_PRODUCT(computeApproximateCovariances, ((pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA))((pcl::Normal)))
+#else
+ PCL_INSTANTIATE_PRODUCT(computeApproximateCovariances, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES))
+#endif
+#endif // PCL_NO_PRECOMPILE
getRotations(rotations, strengths);
if (rotations.empty())
return false;
- float best_rotation=rotations[0], best_strength=strengths[0];
- for (std::size_t i = 1; i < rotations.size(); ++i)
- {
- if (strengths[i] > best_strength)
- {
- best_rotation = rotations[i];
- best_strength = strengths[i];
- }
- }
-
+
+ const auto max_it = std::max_element(strengths.cbegin (), strengths.cend ());
+ const auto max_idx = std::distance(strengths.cbegin (), max_it);
+ const auto best_rotation = rotations[max_idx];
+
transformation_ = Eigen::AngleAxisf(-best_rotation, Eigen::Vector3f(0.0f, 0.0f, 1.0f))*transformation_;
surface_patch_rotation_ = best_rotation;
return extractDescriptor(descriptor_size_);
void
RangeImageBorderExtractor::extractBorderScoreImages ()
{
- if (border_scores_left_.empty())
+ if (!border_scores_left_.empty())
return;
extractLocalSurfaceStructure();
/**
* \brief virtual destructor
*/
- ~BoxClipper3D () throw ();
+ ~BoxClipper3D () noexcept;
bool
clipPoint3D (const PointT& point) const override;
/**
* \brief virtual destructor. Never throws an exception.
*/
- virtual ~Clipper3D () throw () {}
+ virtual ~Clipper3D () noexcept {}
/**
* \brief interface to clip a single point
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
-pcl::BoxClipper3D<PointT>::~BoxClipper3D () throw ()
+pcl::BoxClipper3D<PointT>::~BoxClipper3D () noexcept
{
}
output.points.resize (input_->points.size ());
removed_indices_->resize (input_->points.size ());
- int nr_p = 0;
int nr_removed_p = 0;
if (!keep_organized_)
{
+ int nr_p = 0;
for (std::size_t index: (*Filter<PointT>::indices_))
{
+
const PointT& point = input_->points[index];
// Check if the point is invalid
if (!std::isfinite (point.x)
#include <pcl/point_types.h>
#include <pcl/common/point_operators.h>
+#include <cmath>
+#include <cstdint>
+#include <limits>
+#include <vector>
+
///////////////////////////////////////////////////////////////////////////////////////////////////
namespace pcl
{
index.resize (cloud_in.points.size ());
std::size_t j = 0;
+ // Assume cloud is dense
+ cloud_out.is_dense = true;
+
for (std::size_t i = 0; i < cloud_in.points.size (); ++i)
{
if (!std::isfinite (cloud_in.points[i].normal_x) ||
!std::isfinite (cloud_in.points[i].normal_y) ||
!std::isfinite (cloud_in.points[i].normal_z))
continue;
+ if (cloud_out.is_dense && !pcl::isFinite(cloud_in.points[i]))
+ cloud_out.is_dense = false;
cloud_out.points[j] = cloud_in.points[i];
index[j] = static_cast<int>(i);
j++;
random_access[i].resize (normals_hg[i].size ());
std::size_t j = 0;
- for (std::list<int>::iterator itr = normals_hg[i].begin (); itr != normals_hg[i].end (); itr++, j++)
+ for (std::list<int>::iterator itr = normals_hg[i].begin (); itr != normals_hg[i].end (); ++itr, ++j)
random_access[i][j] = itr;
}
std::vector<std::size_t> start_index (normals_hg.size ());
}
template<typename PointT>
-pcl::PlaneClipper3D<PointT>::~PlaneClipper3D () throw ()
+pcl::PlaneClipper3D<PointT>::~PlaneClipper3D () noexcept
{
}
float t_delta_y = leaf_size_[1] / static_cast<float> (std::abs (direction[1]));
float t_delta_z = leaf_size_[2] / static_cast<float> (std::abs (direction[2]));
- // index of the point in the point cloud
- int index;
-
while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) &&
(ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) &&
(ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) )
if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2])
return 0;
+ // index of the point in the point cloud
+ int index = this->getCentroidIndexAt (ijk);
// check if voxel is occupied, if yes return 1 for occluded
- index = this->getCentroidIndexAt (ijk);
if (index != -1)
return 1;
float t_delta_z = leaf_size_[2] / static_cast<float> (std::abs (direction[2]));
// the index of the cloud (-1 if empty)
- int index = -1;
int result = 0;
while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) &&
break;
// check if voxel is occupied
- index = this->getCentroidIndexAt (ijk);
+ int index = this->getCentroidIndexAt (ijk);
if (index != -1)
result = 1;
#pragma once
+#include <pcl/pcl_macros.h>
#include <pcl/filters/filter_indices.h>
namespace pcl
* Default: false.
* \param[in] limit_negative return data inside the interval (false) or outside (true)
*/
- [[deprecated("use inherited FilterIndices::setNegative() instead")]]
+ PCL_DEPRECATED("use inherited FilterIndices::setNegative() instead")
inline void
setFilterLimitsNegative (const bool limit_negative)
{
/** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
* \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise
*/
- [[deprecated("use inherited FilterIndices::getNegative() instead")]]
+ PCL_DEPRECATED("use inherited FilterIndices::getNegative() instead")
inline void
getFilterLimitsNegative (bool &limit_negative) const
{
/** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
* \return true if data \b outside the interval [min; max] is to be returned, false otherwise
*/
- [[deprecated("use inherited FilterIndices::getNegative() instead")]]
+ PCL_DEPRECATED("use inherited FilterIndices::getNegative() instead")
inline bool
getFilterLimitsNegative () const
{
*/
PlaneClipper3D (const Eigen::Vector4f& plane_params);
- virtual ~PlaneClipper3D () throw ();
+ virtual ~PlaneClipper3D () noexcept;
/**
* \brief Set new plane parameters
* represents the underlying surface more accurately.
*
* \author Radu Bogdan Rusu
- * \ingroup keypoints
+ * \ingroup filters
*/
template <typename PointT>
class UniformSampling: public Filter<PointT>
void pcl::gpu::error(const char *error_string, const char *file, const int line, const char *func)
{
std::cout << "Error: " << error_string << "\t" << file << ":" << line << std::endl;
- exit(0);
+ exit(EXIT_FAILURE);
}
}
}
+// Test from issue:
+// - https://github.com/PointCloudLibrary/pcl/issues/2371#issuecomment-577727912
+TEST(PCL_FeaturesGPU, issue_2371)
+{
+ // This number is magic, do not set to lower value.
+ // It may affect error reproducibility.
+ const std::size_t N = 1000;
+ std::vector<pcl::PointXYZ> cloud_cpu(N, {0.0, 0.0, 0.0});
+
+ pcl::gpu::NormalEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(cloud_cpu);
+
+ pcl::gpu::NormalEstimation ne_gpu;
+ ne_gpu.setInputCloud(cloud_gpu);
+
+ const float radius_search = 2.0F;
+ const int max_results = 500;
+ ne_gpu.setRadiusSearch(radius_search, max_results);
+
+ pcl::gpu::NormalEstimation::Normals normals_gpu;
+ ne_gpu.compute(normals_gpu);
+}
+
+// See:
+// - https://github.com/PointCloudLibrary/pcl/pull/3627#discussion_r375826172
+TEST(PCL_FeaturesGPU, normals_nan_gpu)
+{
+ const std::size_t N = 5;
+
+ PointCloud<PointXYZ> cloud;
+ cloud.points.assign(N, {0.0, 0.0, 0.0});
+
+ const float radius_search = 2.0F;
+ const int max_results = 500;
+
+ pcl::gpu::NormalEstimation::PointCloud cloud_device;
+ cloud_device.upload(cloud.points);
+
+ pcl::gpu::NormalEstimation ne_device;
+ ne_device.setInputCloud(cloud_device);
+ ne_device.setRadiusSearch(radius_search, max_results);
+
+ pcl::gpu::NormalEstimation::Normals normals_device;
+ ne_device.compute(normals_device);
+
+ std::vector<PointXYZ> downloaded;
+ normals_device.download(downloaded);
+
+ ASSERT_EQ(downloaded.size(), N);
+
+ for(const auto& n : downloaded)
+ {
+ ASSERT_TRUE(std::isnan(n.x));
+ ASSERT_TRUE(std::isnan(n.y));
+ ASSERT_TRUE(std::isnan(n.z));
+ }
+}
int main (int argc, char** argv)
{
#include <pcl/point_types.h>
#include <pcl/gpu/containers/device_array.h>
#include <pcl/gpu/kinfu/pixel_rgb.h>
-#include <boost/shared_ptr.hpp>
+#include <pcl/make_shared.h>
#include <Eigen/Geometry>
namespace pcl
#include <Eigen/Geometry>
#include <fstream>
+#include <pcl/make_shared.h>
+
/**
* @brief The CameraPoseProcessor class is an interface to extract
* camera pose data generated by the pcl_kinfu_app program.
class CameraPoseProcessor
{
public:
- using Ptr = shared_ptr<CameraPoseProcessor>;
- using ConstPtr = shared_ptr<const CameraPoseProcessor>;
+ using Ptr = pcl::shared_ptr<CameraPoseProcessor>;
+ using ConstPtr = pcl::shared_ptr<const CameraPoseProcessor>;
virtual ~CameraPoseProcessor () {}
#include <pcl/gpu/containers/kernel_containers.h>
#include <pcl/gpu/kinfu/kinfu.h>
-#include <boost/shared_ptr.hpp>
+#include <pcl/make_shared.h>
#include <memory>
#include <string>
class Evaluation
{
public:
- using Ptr = shared_ptr<Evaluation>;
- using ConstPtr = shared_ptr<const Evaluation>;
+ using Ptr = pcl::shared_ptr<Evaluation>;
+ using ConstPtr = pcl::shared_ptr<const Evaluation>;
using RGB = pcl::gpu::KinfuTracker::PixelRGB;
Evaluation(const std::string& folder);
using RgbCloudConstPtr = pcl::PointCloud<RGB>::ConstPtr;
public:
- using Ptr = shared_ptr<PointCloudColorHandlerRGBCloud<PointT> >;
- using ConstPtr = shared_ptr<const PointCloudColorHandlerRGBCloud<PointT> >;
+ using Ptr =
pcl::shared_ptr<PointCloudColorHandlerRGBCloud<PointT> >;
+ using ConstPtr =
pcl::shared_ptr<const PointCloudColorHandlerRGBCloud<PointT> >;
/** \brief Constructor. */
PointCloudColorHandlerRGBCloud (const PointCloudConstPtr& cloud, const RgbCloudConstPtr& colors)
struct CurrentFrameCloudView
{
- using Ptr = shared_ptr<CurrentFrameCloudView>;
- using ConstPtr = shared_ptr<const CurrentFrameCloudView>;
+ using Ptr = pcl::shared_ptr<CurrentFrameCloudView>;
+ using ConstPtr = pcl::shared_ptr<const CurrentFrameCloudView>;
CurrentFrameCloudView() : cloud_device_ (480, 640), cloud_viewer_ ("Frame Cloud Viewer")
{
#include <Eigen/Dense>
#include <cmath>
#include <iostream>
-#include <boost/shared_ptr.hpp>
+#include <pcl/make_shared.h>
#ifdef _WIN32
# define WIN32_LEAN_AND_MEAN
# include <windows.h>
#include <pcl/gpu/containers/kernel_containers.h>
#include <pcl/gpu/kinfu_large_scale/kinfu.h>
-#include <boost/shared_ptr.hpp>
+#include <pcl/make_shared.h>
#include <memory>
#include <string>
class Evaluation
{
public:
- using Ptr = shared_ptr<Evaluation>;
- using ConstPtr = shared_ptr<const Evaluation>;
+ using Ptr = pcl::shared_ptr<Evaluation>;
+ using ConstPtr = pcl::shared_ptr<const Evaluation>;
using RGB = pcl::gpu::kinfuLS::PixelRGB;
Evaluation(const std::string& folder);
void
initRegistration ()
{
- registration_ =
- #ifdef HAVE_OPENNI
- capture_.providesCallback<pcl::ONIGrabber::sig_cb_openni_image_depth_image> ()
- #endif
- #if defined(HAVE_OPENNI) && defined(HAVE_OPENNI2)
- ||
- #endif
- #ifdef HAVE_OPENNI2
- capture_.providesCallback<pcl::io::OpenNI2Grabber::sig_cb_openni_image_depth_image> ();
- #endif
+ registration_ =
+ #if defined(HAVE_OPENNI) && !defined(HAVE_OPENNI2)
+ capture_.providesCallback<pcl::ONIGrabber::sig_cb_openni_image_depth_image> ();
+ #elif !defined(HAVE_OPENNI) && defined(HAVE_OPENNI2)
+ capture_.providesCallback<pcl::io::OpenNI2Grabber::sig_cb_openni_image_depth_image> ();
+ #elif defined(HAVE_OPENNI) && defined(HAVE_OPENNI2)
+ capture_.providesCallback<pcl::ONIGrabber::sig_cb_openni_image_depth_image> () ||
+ capture_.providesCallback<pcl::io::OpenNI2Grabber::sig_cb_openni_image_depth_image> ();
+ #endif
std::cout << "Registration mode: " << (registration_ ? "On" : "Off (not supported by source)") << std::endl;
}
{
#ifdef HAVE_OPENNI2
using namespace pcl::io;
- using DepthImagePtr = shared_ptr<DepthImage>;
- using ImagePtr = shared_ptr<Image>;
+ using DepthImagePtr = pcl::shared_ptr<DepthImage>;
+ using ImagePtr = pcl::shared_ptr<Image>;
std::function<void (const ImagePtr&, const DepthImagePtr&, float)> func1 =
[this] (const ImagePtr& img, const DepthImagePtr& depth, float constant)
{
#ifdef HAVE_OPENNI
using namespace openni_wrapper;
- using DepthImagePtr = shared_ptr<DepthImage>;
- using ImagePtr = shared_ptr<Image>;
+ using DepthImagePtr = pcl::shared_ptr<DepthImage>;
+ using ImagePtr = pcl::shared_ptr<Image>;
std::function<void (const ImagePtr&, const DepthImagePtr&, float)> func1 =
[this] (const ImagePtr& img, const DepthImagePtr& depth, float constant)
SceneCloudView scene_cloud_view_;
ImageView image_view_;
- shared_ptr<CurrentFrameCloudView> current_frame_cloud_view_;
+ pcl::shared_ptr<CurrentFrameCloudView> current_frame_cloud_view_;
KinfuTracker::DepthMap depth_device_;
// if (checkIfPreFermiGPU(device))
// return std::cout << std::endl << "Kinfu is supported only for Fermi and Kepler arhitectures. It is not even compiled for pre-Fermi by default. Exiting..." << std::endl, 1;
- shared_ptr<pcl::Grabber> capture;
+
pcl::shared_ptr<pcl::Grabber> capture;
bool triggered_capture = false;
bool pcd_input = false;
bool oni2_interface = false;
struct MapsRgb
{
- using Ptr = shared_ptr<MapsRgb>;
- using ConstPtr = shared_ptr<const MapsRgb>;
+ using Ptr = pcl::shared_ptr<MapsRgb>;
+ using ConstPtr = pcl::shared_ptr<const MapsRgb>;
pcl::gpu::PtrStepSz<const PixelRGB> rgb_;
pcl::gpu::PtrStepSz<const unsigned short> depth_;
__syncthreads();
- while (tasks_beg < tasks_end)
+ while (tasks_beg < tasks_end && level < Morton::levels)
{
int task_count = tasks_end - tasks_beg;
int iters = divUp(task_count, CTA_SIZE);
octree.begs [offset + i] = cell_begs[i];
octree.ends [offset + i] = cell_begs[i + 1];
octree.codes[offset + i] = parent_code_shifted + cell_code[i];
-
+ octree.nodes[offset + i] = 0;
octree.parent[offset + i] = task;
mask |= (1 << cell_code[i]);
}
# install include files
PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${hdrs} ${hdrs_cuda})
-add_subdirectory(tools)
+if(BUILD_tools)
+ add_subdirectory(tools)
+endif()
+set(SUBSYS_NAME tools)
+
if(NOT VTK_FOUND)
set(DEFAULT FALSE)
set(REASON "VTK was not found.")
#include <iostream>
namespace pc = pcl::console;
-using namespace pcl::visualization;
-using namespace pcl::gpu;
-using namespace pcl;
-using namespace std;
+using namespace std::chrono_literals;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-std::vector<string> getPcdFilesInDir(const string& directory)
+std::vector<std::string> getPcdFilesInDir(const std::string& directory)
{
namespace fs = boost::filesystem;
fs::path dir(directory);
if (!fs::exists(dir) || !fs::is_directory(dir))
PCL_THROW_EXCEPTION(pcl::IOException, "Wrong PCD directory");
- std::vector<string> result;
+ std::vector<std::string> result;
fs::directory_iterator pos(dir);
fs::directory_iterator end;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-struct SampledScopeTime : public StopWatch
+struct SampledScopeTime : public pcl::StopWatch
{
enum { EACH = 33 };
SampledScopeTime(int& time_ms) : time_ms_(time_ms) {}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-string
+std::string
make_name(int counter, const char* suffix)
{
char buf[4096];
rgba_host_.points.resize(COLS * ROWS);
rgb_host_.resize(COLS * ROWS * 3);
- people::uploadColorMap(color_map_);
-
+ pcl::gpu::people::uploadColorMap(color_map_);
}
void
visualizeAndWrite()
{
const PeopleDetector::Labels& labels = people_detector_.rdf_detector_->getLabels();
- people::colorizeLabels(color_map_, labels, cmap_device_);
+ pcl::gpu::people::colorizeLabels(color_map_, labels, cmap_device_);
//people::colorizeMixedLabels(
int c;
}
}
- void source_cb1(const PointCloud<PointXYZRGBA>::ConstPtr& cloud)
+ void source_cb1(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud)
{
{
std::lock_guard<std::mutex> lock(data_ready_mutex_);
for(std::size_t i = 0; i < rgba_host_.size(); ++i)
{
const unsigned char *pixel = &rgb_host_[i * 3];
- RGB& rgba = rgba_host_.points[i];
+ pcl::RGB& rgba = rgba_host_.points[i];
rgba.r = pixel[0];
rgba.g = pixel[1];
rgba.b = pixel[2];
{
cloud_cb_ = false;
-
PCDGrabberBase* ispcd = dynamic_cast<pcl::PCDGrabberBase*>(&capture_);
+
auto ispcd = dynamic_cast<pcl::PCDGrabberBase*>(&capture_);
if (ispcd)
cloud_cb_= true;
using DepthImagePtr = openni_wrapper::DepthImage::Ptr;
using ImagePtr = openni_wrapper::Image::Ptr;
- std::function<void (const PointCloud<PointXYZRGBA>::ConstPtr&)> func1 = [this] (const PointCloud<PointXYZRGBA>::ConstPtr& cloud) { source_cb1 (cloud); };
+ std::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> func1 = [this] (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud) { source_cb1 (cloud); };
std::function<void (const ImagePtr&, const DepthImagePtr&, float)> func2 = [this] (const ImagePtr& img, const DepthImagePtr& depth, float constant)
{
source_cb2 (img, depth, constant);
pcl::PointCloud<pcl::RGB> rgba_host_;
std::vector<unsigned char> rgb_host_;
- PointCloud<PointXYZRGBA> cloud_host_;
+ pcl::PointCloud<pcl::PointXYZRGBA> cloud_host_;
- ImageViewer final_view_;
- ImageViewer depth_view_;
+ pcl::visualization::ImageViewer final_view_;
+ pcl::visualization::ImageViewer depth_view_;
- DeviceArray<pcl::RGB> color_map_;
+
pcl::device::DeviceArray<pcl::RGB> color_map_;
};
void print_help()
pc::parse_argument (argc, argv, "-w", write);
// selecting data source
- shared_ptr<pcl::Grabber> capture;
- string openni_device, oni_file, pcd_file, pcd_folder;
+
pcl::shared_ptr<pcl::Grabber> capture;
+ std::string openni_device, oni_file, pcd_file, pcd_folder;
try
{
else
if (pc::parse_argument (argc, argv, "-pcd", pcd_file) > 0)
{
- capture.reset( new pcl::PCDGrabber<PointXYZRGBA>(vector<string>(31, pcd_file), 30, true) );
+ capture.reset( new pcl::PCDGrabber<pcl::PointXYZRGBA>(std::vector<std::string>(31, pcd_file), 30, true) );
}
else
if (pc::parse_argument (argc, argv, "-pcd_folder", pcd_folder) > 0)
{
- std::vector<string> pcd_files = getPcdFilesInDir(pcd_folder);
- capture.reset( new pcl::PCDGrabber<PointXYZRGBA>(pcd_files, 30, true) );
+ std::vector<std::string> pcd_files = getPcdFilesInDir(pcd_folder);
+ capture.reset( new pcl::PCDGrabber<pcl::PointXYZRGBA>(pcd_files, 30, true) );
}
else
{
capture.reset( new pcl::OpenNIGrabber() );
- //capture.reset( new pcl::ONIGrabber("d:/onis/20111013-224932.oni", true, true) );
-
- //vector<string> pcd_files(31, "d:/3/0008.pcd");
- //vector<string> pcd_files(31, "d:/git/pcl/gpu/people/tools/test.pcd");
- //vector<string> pcd_files = getPcdFilesInDir("d:/3/");
- //capture.reset( new pcl::PCDGrabber<PointXYZRGBA>(pcd_files, 30, true) );
}
}
catch (const pcl::PCLException& /*e*/) { return std::cout << "Can't open depth source" << std::endl, -1; }
//selecting tree files
- std::vector<string> tree_files;
+ std::vector<std::string> tree_files;
tree_files.emplace_back("Data/forest1/tree_20.txt");
tree_files.emplace_back("Data/forest2/tree_20.txt");
tree_files.emplace_back("Data/forest3/tree_20.txt");
#include <cstdio>
#include <cstdint>
+#include <pcl/pcl_macros.h>
+
namespace pcl
{
* \param n_arg: some value
* \return binary logarithm (log2) of argument n_arg
*/
- [[deprecated("use std::log2 instead")]]
+ PCL_DEPRECATED("use std::log2 instead")
inline double
Log2 (double n_arg)
{
// calculate amount of bytes per frequency table entry
std::uint8_t frequencyTableByteSize = static_cast<std::uint8_t> (std::ceil (
- std::log2 (static_cast<double> (cFreqTable_[static_cast<std::size_t> (frequencyTableSize - 1)])) / 8.0));
+ std::log2 (static_cast<double> (cFreqTable_[static_cast<std::size_t> (frequencyTableSize - 1)] + 1)) / 8.0));
// write size of frequency table to output stream
outputByteStream_arg.write (reinterpret_cast<const char*> (&frequencyTableSize), sizeof(frequencyTableSize));
OctreePointCloudCompression<PointT, LeafT, BranchT, OctreeT>::deserializeTreeCallback (LeafT&,
const OctreeKey& key_arg)
{
- double lowerVoxelCorner[3];
PointT newPoint;
std::size_t pointCount = 1;
output_->points.push_back (newPoint);
// calculcate position of lower voxel corner
+ double lowerVoxelCorner[3];
lowerVoxelCorner[0] = static_cast<double> (key_arg.x) * this->resolution_ + this->min_x_;
lowerVoxelCorner[1] = static_cast<double> (key_arg.y) * this->resolution_ + this->min_y_;
lowerVoxelCorner[2] = static_cast<double> (key_arg.z) * this->resolution_ + this->min_z_;
#pragma once
+#include <pcl/pcl_macros.h>
#include <pcl/io/file_io.h>
#include <pcl/PCLPointField.h>
#include <pcl/common/io.h>
* \param[in] p a point type
*/
template<typename PointT>
- [[deprecated("use parameterless setInputFields<PointT>() instead")]]
+ PCL_DEPRECATED("use parameterless setInputFields<PointT>() instead")
inline void setInputFields (const PointT p)
{
(void) p;
/** @brief Destructor inherited from the Grabber interface. It never throws. */
virtual
- ~DavidSDKGrabber () throw ();
+ ~DavidSDKGrabber () noexcept;
/** @brief [Connect](http://docs.david-3d.com/sdk/en/classdavid_1_1_client_json_rpc.html#a4b948e57a2e5e7f9cdcf1171c500aa24) client
* @param[in] address
DepthSenseGrabberImpl (DepthSenseGrabber* parent, const std::string& device_id);
- ~DepthSenseGrabberImpl () throw ();
+ ~DepthSenseGrabberImpl () noexcept;
void
start ();
DepthSenseGrabber (const std::string& device_id = "");
virtual
- ~DepthSenseGrabber () throw ();
+ ~DepthSenseGrabber () noexcept;
virtual void
start ();
DinastGrabber (const int device_position=1);
/** \brief Destructor. It never throws. */
- ~DinastGrabber () throw ();
+ ~DinastGrabber () noexcept;
/** \brief Check if the grabber is running
* \return true if grabber is running / streaming. False otherwise.
/** @brief Destructor inherited from the Grabber interface. It never throws. */
virtual
- ~EnsensoGrabber () throw ();
+ ~EnsensoGrabber () noexcept;
/** @brief Searches for available devices
* @returns The number of Ensenso devices connected */
class PCL_EXPORTS Grabber
{
public:
-
- /** \brief Constructor. */
- Grabber () {}
-
/** \brief virtual destructor. */
- virtual inline ~Grabber () throw ();
+ virtual inline ~Grabber () noexcept;
/** \brief registers a callback function/method to a signal with the corresponding signature
* \param[in] callback: the callback function/method
* \return Connection object, that can be used to disconnect the callback method from the signal again.
*/
template<typename T, template<typename> class FunctionT>
- [[deprecated ("please assign the callback to a std::function.")]]
+ PCL_DEPRECATED ("please assign the callback to a std::function.")
boost::signals2::connection
registerCallback (const FunctionT<T>& callback)
{
virtual void
stop () = 0;
+ /** \brief For devices that are streaming, stopped streams are started and running stream are stopped.
+ * For triggered devices, the behavior is not defined.
+ * \return true if grabber is running / streaming. False otherwise.
+ */
+ inline bool
+ toggle ();
+
/** \brief returns the name of the concrete subclass.
* \return the name of the concrete driver.
*/
std::map<std::string, std::vector<boost::signals2::shared_connection_block> > shared_connections_;
} ;
- Grabber::~Grabber () throw ()
+ Grabber::~Grabber () noexcept
{
for (auto &signal : signals_)
delete signal.second;
}
+ bool
+ Grabber::toggle ()
+ {
+ if (isRunning ())
+ {
+ stop ();
+ } else
+ {
+ start ();
+ }
+ return isRunning ();
+ }
+
template<typename T> boost::signals2::signal<T>*
Grabber::find_signal () const
{
#pragma once
#include "pcl/pcl_config.h"
+#include <pcl/pcl_macros.h>
#include <pcl/io/grabber.h>
#include <pcl/io/impl/synchronized_queue.hpp>
*/
using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba = void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &, float, float);
- using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb [[deprecated("use sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba instead")]]
+ using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb PCL_DEPRECATED("use sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba instead")
= sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba;
/** \brief Signal used for a single sector
*/
using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba = void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &);
- using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb [[deprecated("use sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba instead")]]
+ using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb PCL_DEPRECATED("use sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba instead")
= sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba;
/** \brief Constructor taking an optional path to an HDL corrections file. The Grabber will listen on the default IP/port for data packets [192.168.3.255/2368]
/** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
- ~HDLGrabber () throw ();
+ ~HDLGrabber () noexcept;
/** \brief Starts processing the Velodyne packets, either from the network or PCAP file. */
void
}
/** \brief Virtual destructor. */
- ~ImageGrabberBase () throw ();
+ ~ImageGrabberBase () noexcept;
/** \brief Starts playing the list of PCD files if frames_per_second is > 0. Otherwise it works as a trigger: publishes only the next PCD file in the list. */
void
bool repeat = false);
/** \brief Empty destructor */
- ~ImageGrabber () throw () {}
+ ~ImageGrabber () noexcept {}
// Inherited from FileGrabber
const typename pcl::PointCloud<PointT>::ConstPtr
IRImage (FrameWrapper::Ptr ir_metadata);
IRImage (FrameWrapper::Ptr ir_metadata, Timestamp time);
- ~IRImage () throw ()
+ ~IRImage () noexcept
{}
void
ImageRGB24 (FrameWrapper::Ptr image_metadata);
ImageRGB24 (FrameWrapper::Ptr image_metadata, Timestamp timestamp);
- ~ImageRGB24 () throw ();
+ ~ImageRGB24 () noexcept;
inline Encoding
getEncoding () const override
ImageYUV422 (FrameWrapper::Ptr image_metadata);
ImageYUV422 (FrameWrapper::Ptr image_metadata, Timestamp timestamp);
- ~ImageYUV422 () throw ();
+ ~ImageYUV422 () noexcept;
inline Encoding
getEncoding () const override
// Remove empty fields and adjust offset
int offset =0;
for (std::vector<pcl::PCLPointField>::iterator field_iter = fields_.begin ();
- field_iter != fields_.end (); field_iter++)
+ field_iter != fields_.end (); ++field_iter)
{
if (field_iter->name == "_")
field_iter = fields_.erase (field_iter);
#include <pcl/console/print.h>
#include <pcl/io/debayer.h>
+#include <cstddef>
+#include <cstdint>
+#include <limits>
+#include <string>
+#include <vector>
+
#define CLIP_CHAR(c) static_cast<unsigned char> ((c)>255?255:(c)<0?0:(c))
//////////////////////////////////////////////////////////////////////////////
oss.flush ();
data_idx = static_cast<int> (oss.tellp ());
-#if _WIN32
+#ifdef _WIN32
HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
if (h_native_file == INVALID_HANDLE_VALUE)
{
data_size = cloud.points.size () * fsize;
// Prepare the map
-#if _WIN32
+#ifdef _WIN32
HANDLE fm = CreateFileMappingA (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + data_size), NULL);
if (fm == NULL)
{
}
// If the user set the synchronization flag on, call msync
-#if !_WIN32
+#ifndef _WIN32
if (map_synchronization_)
msync (map, data_idx + data_size, MS_SYNC);
#endif
// Unmap the pages of memory
-#if _WIN32
+#ifdef _WIN32
UnmapViewOfFile (map);
#else
if (::munmap (map, (data_idx + data_size)) == -1)
}
#endif
// Close file
-#if _WIN32
+#ifdef _WIN32
CloseHandle (h_native_file);
#else
io::raw_close (fd);
oss.flush ();
data_idx = static_cast<int> (oss.tellp ());
-#if _WIN32
+#ifdef _WIN32
HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
if (h_native_file == INVALID_HANDLE_VALUE)
{
}
else
{
-#if !_WIN32
+#ifndef _WIN32
io::raw_close (fd);
#endif
resetLockingPermissions (file_name, file_lock);
}
// Prepare the map
-#if _WIN32
+#ifdef _WIN32
HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, compressed_final_size, NULL);
char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, compressed_final_size));
CloseHandle (fm);
// Copy the compressed data
memcpy (&map[data_idx], temp_buf, data_size);
-#if !_WIN32
+#ifndef _WIN32
// If the user set the synchronization flag on, call msync
if (map_synchronization_)
msync (map, compressed_final_size, MS_SYNC);
#endif
// Unmap the pages of memory
-#if _WIN32
+#ifdef _WIN32
UnmapViewOfFile (map);
#else
if (::munmap (map, (compressed_final_size)) == -1)
#endif
// Close file
-#if _WIN32
+#ifdef _WIN32
CloseHandle (h_native_file);
#else
io::raw_close (fd);
oss.flush ();
data_idx = static_cast<int> (oss.tellp ());
-#if _WIN32
+#ifdef _WIN32
HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
if (h_native_file == INVALID_HANDLE_VALUE)
{
data_size = indices.size () * fsize;
// Prepare the map
-#if _WIN32
+#ifdef _WIN32
HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, data_idx + data_size, NULL);
char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
CloseHandle (fm);
}
}
-#if !_WIN32
+#ifndef _WIN32
// If the user set the synchronization flag on, call msync
if (map_synchronization_)
msync (map, data_idx + data_size, MS_SYNC);
#endif
// Unmap the pages of memory
-#if _WIN32
+#ifdef _WIN32
UnmapViewOfFile (map);
#else
if (::munmap (map, (data_idx + data_size)) == -1)
}
#endif
// Close file
-#if _WIN32
+#ifdef _WIN32
CloseHandle(h_native_file);
#else
io::raw_close (fd);
unsigned line_number,
const std::string& message);
- ~IOException () throw ();
+ ~IOException () noexcept;
IOException&
operator= (const IOException& exception);
ONIGrabber (const std::string& file_name, bool repeat, bool stream);
/** \brief destructor never throws an exception */
- ~ONIGrabber () throw ();
+ ~ONIGrabber () noexcept;
/** \brief For devices that are streaming, the streams are started by calling this method.
* Trigger-based devices, just trigger the device once for each call of start.
const Mode& image_mode = OpenNI_Default_Mode);
/** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
- ~OpenNI2Grabber () throw ();
+ ~OpenNI2Grabber () noexcept;
/** \brief Start the data acquisition. */
void
* \param[in] no_sample_value defines which values in the depth data are indicating that no depth (disparity) could be determined .
* \attention The focal length may change, depending whether the depth stream is registered/mapped to the RGB stream or not.
*/
- inline DepthImage (pcl::shared_ptr<xn::DepthMetaData> depth_meta_data, float baseline, float focal_length, XnUInt64 shadow_value, XnUInt64 no_sample_value) throw ();
+ inline DepthImage (pcl::shared_ptr<xn::DepthMetaData> depth_meta_data, float baseline, float focal_length, XnUInt64 shadow_value, XnUInt64 no_sample_value) noexcept;
/** \brief Destructor. Never throws an exception. */
- inline virtual ~DepthImage () throw ();
+ inline virtual ~DepthImage () noexcept;
/** \brief method to access the internal data structure from OpenNI. If the data is accessed just read-only, then this method is faster than a fillXXX method
* \return the actual depth data of type xn::DepthMetaData.
XnUInt64 no_sample_value_;
} ;
- DepthImage::DepthImage (pcl::shared_ptr<xn::DepthMetaData> depth_meta_data, float baseline, float focal_length, XnUInt64 shadow_value, XnUInt64 no_sample_value) throw ()
+ DepthImage::DepthImage (pcl::shared_ptr<xn::DepthMetaData> depth_meta_data, float baseline, float focal_length, XnUInt64 shadow_value, XnUInt64 no_sample_value) noexcept
: depth_md_ (std::move(depth_meta_data))
, baseline_ (baseline)
, focal_length_ (focal_length)
, shadow_value_ (shadow_value)
, no_sample_value_ (no_sample_value) { }
- DepthImage::~DepthImage () throw () { }
+ DepthImage::~DepthImage () noexcept { }
const xn::DepthMetaData&
DepthImage::getDepthMetaData () const throw ()
public:
/** \brief virtual destructor. Never throws an exception. */
- virtual ~OpenNIDevice () throw ();
+ virtual ~OpenNIDevice () noexcept;
/** \brief finds an image output mode that can be used to retrieve images in desired output mode.
* e.g If device just supports VGA at 30Hz, then the desired mode QVGA at 30Hz would be possible by down sampling,
* \return a callback handler that can be used to remove the user callback from list of image-stream callbacks.
*/
CallbackHandle
- registerImageCallback (const ImageCallbackFunction& callback, void* cookie = nullptr) throw ();
+ registerImageCallback (const ImageCallbackFunction& callback, void* cookie = nullptr) noexcept;
/** \brief registers a callback function for the image stream with an optional user defined parameter.
* This version is used to register a member function of any class.
* \return a callback handler that can be used to remove the user callback from list of image-stream callbacks.
*/
template<typename T> CallbackHandle
- registerImageCallback (void (T::*callback)(Image::Ptr, void* cookie), T& instance, void* cookie = nullptr) throw ();
+ registerImageCallback (void (T::*callback)(Image::Ptr, void* cookie), T& instance, void* cookie = nullptr) noexcept;
/** \brief unregisters a callback function. i.e. removes that function from the list of image stream callbacks.
* \param[in] callbackHandle the handle of the callback to unregister.
* \return true, if callback was in list and could be unregistered, false otherwise.
*/
bool
- unregisterImageCallback (const CallbackHandle& callbackHandle) throw ();
+ unregisterImageCallback (const CallbackHandle& callbackHandle) noexcept;
/** \brief registers a callback function of std::function type for the depth stream with an optional user defined parameter.
* \return a callback handler that can be used to remove the user callback from list of depth-stream callbacks.
*/
CallbackHandle
- registerDepthCallback (const DepthImageCallbackFunction& callback, void* cookie = nullptr) throw ();
+ registerDepthCallback (const DepthImageCallbackFunction& callback, void* cookie = nullptr) noexcept;
/** \brief registers a callback function for the depth stream with an optional user defined parameter.
* This version is used to register a member function of any class.
* \return a callback handler that can be used to remove the user callback from list of depth-stream callbacks.
*/
template<typename T> CallbackHandle
- registerDepthCallback (void (T::*callback)(DepthImage::Ptr, void* cookie), T& instance, void* cookie = nullptr) throw ();
+ registerDepthCallback (void (T::*callback)(DepthImage::Ptr, void* cookie), T& instance, void* cookie = nullptr) noexcept;
/** \brief unregisters a callback function. i.e. removes that function from the list of depth stream callbacks.
* \param[in] callbackHandle the handle of the callback to unregister.
* \return true, if callback was in list and could be unregistered, false otherwise.
*/
bool
- unregisterDepthCallback (const CallbackHandle& callbackHandle) throw ();
+ unregisterDepthCallback (const CallbackHandle& callbackHandle) noexcept;
/** \brief registers a callback function of std::function type for the IR stream with an optional user defined parameter.
* The callback will always be called with a new IR image and the user data "cookie".
* \return a callback handler that can be used to remove the user callback from list of IR-stream callbacks.
*/
CallbackHandle
- registerIRCallback (const IRImageCallbackFunction& callback, void* cookie = nullptr) throw ();
+ registerIRCallback (const IRImageCallbackFunction& callback, void* cookie = nullptr) noexcept;
/** \brief registers a callback function for the IR stream with an optional user defined parameter.
* This version is used to register a member function of any class.
* \return a callback handler that can be used to remove the user callback from list of IR-stream callbacks.
*/
template<typename T> CallbackHandle
- registerIRCallback (void (T::*callback)(IRImage::Ptr, void* cookie), T& instance, void* cookie = nullptr) throw ();
+ registerIRCallback (void (T::*callback)(IRImage::Ptr, void* cookie), T& instance, void* cookie = nullptr) noexcept;
/** \brief unregisters a callback function. i.e. removes that function from the list of IR stream callbacks.
* \param[in] callbackHandle the handle of the callback to unregister.
* \return true, if callback was in list and could be unregistered, false otherwise.
*/
bool
- unregisterIRCallback (const CallbackHandle& callbackHandle) throw ();
+ unregisterIRCallback (const CallbackHandle& callbackHandle) noexcept;
/** \brief returns the serial number for device.
* \attention This might be an empty string!!!
OpenNIDevice (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& image_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node);
OpenNIDevice (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node);
OpenNIDevice (xn::Context& context);
- static void __stdcall NewDepthDataAvailable (xn::ProductionNode& node, void* cookie) throw ();
- static void __stdcall NewImageDataAvailable (xn::ProductionNode& node, void* cookie) throw ();
- static void __stdcall NewIRDataAvailable (xn::ProductionNode& node, void* cookie) throw ();
+ static void __stdcall NewDepthDataAvailable (xn::ProductionNode& node, void* cookie) noexcept;
+ static void __stdcall NewImageDataAvailable (xn::ProductionNode& node, void* cookie) noexcept;
+ static void __stdcall NewIRDataAvailable (xn::ProductionNode& node, void* cookie) noexcept;
// This is a workaround, since in the NewDepthDataAvailable function WaitAndUpdateData leads to a dead-lock behaviour
// and retrieving image data without WaitAndUpdateData leads to incomplete images!!!
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename T> OpenNIDevice::CallbackHandle
- OpenNIDevice::registerImageCallback (void (T::*callback)(Image::Ptr, void* cookie), T& instance, void* custom_data) throw ()
+ OpenNIDevice::registerImageCallback (void (T::*callback)(Image::Ptr, void* cookie), T& instance, void* custom_data) noexcept
{
image_callback_[image_callback_handle_counter_] = [=, &instance] (Image::Ptr img) { (instance.*callback) (img, custom_data); };
return (image_callback_handle_counter_++);
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename T> OpenNIDevice::CallbackHandle
- OpenNIDevice::registerDepthCallback (void (T::*callback)(DepthImage::Ptr, void* cookie), T& instance, void* custom_data) throw ()
+ OpenNIDevice::registerDepthCallback (void (T::*callback)(DepthImage::Ptr, void* cookie), T& instance, void* custom_data) noexcept
{
depth_callback_[depth_callback_handle_counter_] = [=, &instance] (DepthImage::Ptr img) { (instance.*callback) (img, custom_data); };
return (depth_callback_handle_counter_++);
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename T> OpenNIDevice::CallbackHandle
- OpenNIDevice::registerIRCallback (void (T::*callback)(IRImage::Ptr, void* cookie), T& instance, void* custom_data) throw ()
+ OpenNIDevice::registerIRCallback (void (T::*callback)(IRImage::Ptr, void* cookie), T& instance, void* custom_data) noexcept
{
ir_callback_[ir_callback_handle_counter_] = [=, &instance] (IRImage::Ptr img) { (instance.*callback) (img, custom_data); };
return (ir_callback_handle_counter_++);
friend class OpenNIDriver;
public:
DeviceKinect (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& image_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node);
- ~DeviceKinect () throw ();
+ ~DeviceKinect () noexcept;
- inline void setDebayeringMethod (const ImageBayerGRBG::DebayeringMethod& debayering_method) throw ();
+ inline void setDebayeringMethod (const ImageBayerGRBG::DebayeringMethod& debayering_method) noexcept;
inline const ImageBayerGRBG::DebayeringMethod& getDebayeringMethod () const throw ();
bool isSynchronizationSupported () const throw () override;
protected:
Image::Ptr getCurrentImage (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) const throw () override;
- void enumAvailableModes () throw ();
+ void enumAvailableModes () noexcept;
bool isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const throw () override;
ImageBayerGRBG::DebayeringMethod debayering_method_;
} ;
void
- DeviceKinect::setDebayeringMethod (const ImageBayerGRBG::DebayeringMethod& debayering_method) throw ()
+ DeviceKinect::setDebayeringMethod (const ImageBayerGRBG::DebayeringMethod& debayering_method) noexcept
{
debayering_method_ = debayering_method;
}
using ConstPtr = pcl::shared_ptr<const DeviceONI>;
DeviceONI (xn::Context& context, const std::string& file_name, bool repeat = false, bool streaming = true);
- ~DeviceONI () throw ();
+ ~DeviceONI () noexcept;
void startImageStream () override;
void stopImageStream () override;
Image::Ptr getCurrentImage (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) const throw () override;
void PlayerThreadFunction ();
- static void __stdcall NewONIDepthDataAvailable (xn::ProductionNode& node, void* cookie) throw ();
- static void __stdcall NewONIImageDataAvailable (xn::ProductionNode& node, void* cookie) throw ();
- static void __stdcall NewONIIRDataAvailable (xn::ProductionNode& node, void* cookie) throw ();
+ static void __stdcall NewONIDepthDataAvailable (xn::ProductionNode& node, void* cookie) noexcept;
+ static void __stdcall NewONIImageDataAvailable (xn::ProductionNode& node, void* cookie) noexcept;
+ static void __stdcall NewONIIRDataAvailable (xn::ProductionNode& node, void* cookie) noexcept;
xn::Player player_;
std::thread player_thread_;
friend class OpenNIDriver;
public:
DevicePrimesense (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& image_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node);
- ~DevicePrimesense () throw ();
+ ~DevicePrimesense () noexcept;
//virtual void setImageOutputMode (const XnMapOutputMode& output_mode);
protected:
Image::Ptr getCurrentImage (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) const throw () override;
- void enumAvailableModes () throw ();
+ void enumAvailableModes () noexcept;
bool isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const throw () override;
void startImageStream () override;
friend class OpenNIDriver;
public:
DeviceXtionPro (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node);
- ~DeviceXtionPro () throw ();
+ ~DeviceXtionPro () noexcept;
//virtual void setImageOutputMode (const XnMapOutputMode& output_mode);
protected:
Image::Ptr getCurrentImage (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) const throw () override;
- void enumAvailableModes () throw ();
+ void enumAvailableModes () noexcept;
bool isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const throw () override;
void startDepthStream () override;
* @author Suat Gedikli
* @brief virtual Destructor that never throws an exception
*/
- ~OpenNIDriver () throw ();
+ ~OpenNIDriver () noexcept;
/**
* @author Suat Gedikli
#ifndef _WIN32
// workaround to get additional device nformation like serial number, vendor and product name, until Primesense fix this
- void getDeviceInfos () throw ();
+ void getDeviceInfos () noexcept;
#endif
mutable std::vector<DeviceContext> device_context_;
* @param[in] line_number the line number where this exception was created.
* @param[in] message the message of the exception
*/
- OpenNIException (const std::string& function_name, const std::string& file_name, unsigned line_number, const std::string& message) throw ();
+ OpenNIException (const std::string& function_name, const std::string& file_name, unsigned line_number, const std::string& message) noexcept;
/**
* @brief virtual Destructor that never throws an exception
*/
- ~OpenNIException () throw ();
+ ~OpenNIException () noexcept;
/**
* @brief Assignment operator to allow copying the message of another exception variable.
* @param[in] exception left hand side
* @return
*/
- OpenNIException & operator= (const OpenNIException& exception) throw ();
+ OpenNIException & operator= (const OpenNIException& exception) noexcept;
/**
* @brief virtual method, derived from std::exception
* @brief Constructor
* @param[in] image_meta_data the actual image data from the OpenNI driver
*/
- inline Image (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) throw ();
+ inline Image (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) noexcept;
/**
* @author Suat Gedikli
* @brief virtual Destructor that never throws an exception.
*/
- inline virtual ~Image () throw ();
+ inline virtual ~Image () noexcept;
/**
* @author Suat Gedikli
pcl::shared_ptr<xn::ImageMetaData> image_md_;
} ;
- Image::Image (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) throw ()
+ Image::Image (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) noexcept
: image_md_ (std::move(image_meta_data))
{
}
- Image::~Image () throw () { }
+ Image::~Image () noexcept { }
unsigned
Image::getWidth () const throw ()
EdgeAwareWeighted
};
- ImageBayerGRBG (pcl::shared_ptr<xn::ImageMetaData> image_meta_data, DebayeringMethod method) throw ();
- ~ImageBayerGRBG () throw ();
+ ImageBayerGRBG (pcl::shared_ptr<xn::ImageMetaData> image_meta_data, DebayeringMethod method) noexcept;
+ ~ImageBayerGRBG () noexcept;
inline Encoding
getEncoding () const override
void fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const override;
void fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step = 0) const override;
bool isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const override;
- inline void setDebayeringMethod (const DebayeringMethod& method) throw ();
+ inline void setDebayeringMethod (const DebayeringMethod& method) noexcept;
inline DebayeringMethod getDebayeringMethod () const throw ();
inline static bool resizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height);
};
void
- ImageBayerGRBG::setDebayeringMethod (const ImageBayerGRBG::DebayeringMethod& method) throw ()
+ ImageBayerGRBG::setDebayeringMethod (const ImageBayerGRBG::DebayeringMethod& method) noexcept
{
debayering_method_ = method;
}
{
public:
- ImageRGB24 (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) throw ();
- ~ImageRGB24 () throw ();
+ ImageRGB24 (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) noexcept;
+ ~ImageRGB24 () noexcept;
inline Encoding
getEncoding () const override
class PCL_EXPORTS ImageYUV422 : public Image
{
public:
- ImageYUV422 (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) throw ();
- ~ImageYUV422 () throw ();
+ ImageYUV422 (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) noexcept;
+ ~ImageYUV422 () noexcept;
inline Encoding
getEncoding () const override
using Ptr = pcl::shared_ptr<IRImage>;
using ConstPtr = pcl::shared_ptr<const IRImage>;
- inline IRImage (pcl::shared_ptr<xn::IRMetaData> ir_meta_data) throw ();
- inline virtual ~IRImage () throw ();
+ inline IRImage (pcl::shared_ptr<xn::IRMetaData> ir_meta_data) noexcept;
+ inline virtual ~IRImage () noexcept;
void fillRaw (unsigned width, unsigned height, unsigned short* ir_buffer, unsigned line_step = 0) const;
pcl::shared_ptr<xn::IRMetaData> ir_md_;
};
-IRImage::IRImage (pcl::shared_ptr<xn::IRMetaData> ir_meta_data) throw ()
+IRImage::IRImage (pcl::shared_ptr<xn::IRMetaData> ir_meta_data) noexcept
: ir_md_ (std::move(ir_meta_data))
{
}
-IRImage::~IRImage () throw ()
+IRImage::~IRImage () noexcept
{
}
const Mode& image_mode = OpenNI_Default_Mode);
/** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
- ~OpenNIGrabber () throw ();
+ ~OpenNIGrabber () noexcept;
/** \brief Start the data acquisition. */
void
}
/** \brief Virtual destructor. */
- ~PCDGrabberBase () throw ();
+ ~PCDGrabberBase () noexcept;
/** \brief Starts playing the list of PCD files if frames_per_second is > 0. Otherwise it works as a trigger: publishes only the next PCD file in the list. */
void
PCDGrabber (const std::vector<std::string>& pcd_files, float frames_per_second = 0, bool repeat = false);
/** \brief Virtual destructor. */
- ~PCDGrabber () throw ()
+ ~PCDGrabber () noexcept
{
stop ();
}
#include <boost/predef/other/endian.h>
+#include <algorithm>
+#include <cstddef>
+
namespace pcl
{
namespace io
#include <pcl/point_types.h>
#include <pcl/common/time.h>
+#include <cstddef>
#include <memory>
+#include <mutex>
+#include <string>
#include <thread>
+#include <vector>
namespace pcl
{
RealSenseGrabber (const std::string& device_id = "", const Mode& mode = Mode (), bool strict = false);
virtual
- ~RealSenseGrabber () throw ();
+ ~RealSenseGrabber () noexcept;
virtual void
start ();
/// Depth buffer to perform temporal filtering of the depth images
std::shared_ptr<pcl::io::Buffer<unsigned short> > depth_buffer_;
-
};
-
}
RobotEyeGrabber (const boost::asio::ip::address& ipAddress, unsigned short port=443);
/** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
- ~RobotEyeGrabber () throw ();
+ ~RobotEyeGrabber () noexcept;
/** \brief Starts the RobotEye grabber.
* The grabber runs on a separate thread, this call will return without blocking. */
/** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
- ~VLPGrabber () throw ();
+ ~VLPGrabber () noexcept;
/** \brief Obtains the name of this I/O Grabber
* \return The name of the grabber
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-pcl::DavidSDKGrabber::~DavidSDKGrabber () throw ()
+pcl::DavidSDKGrabber::~DavidSDKGrabber () noexcept
{
try
{
point_cloud_rgba_signal_ = p_->createSignal<sig_cb_depth_sense_point_cloud_rgba> ();
}
-pcl::io::depth_sense::DepthSenseGrabberImpl::~DepthSenseGrabberImpl () throw ()
+pcl::io::depth_sense::DepthSenseGrabberImpl::~DepthSenseGrabberImpl () noexcept
{
stop ();
{
}
-pcl::DepthSenseGrabber::~DepthSenseGrabber () throw ()
+pcl::DepthSenseGrabber::~DepthSenseGrabber () noexcept
{
delete p_;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-pcl::DinastGrabber::~DinastGrabber () throw ()
+pcl::DinastGrabber::~DinastGrabber () noexcept
{
try
{
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-pcl::EnsensoGrabber::~EnsensoGrabber () throw ()
+pcl::EnsensoGrabber::~EnsensoGrabber () noexcept
{
try
{
}
/////////////////////////////////////////////////////////////////////////////
-pcl::HDLGrabber::~HDLGrabber () throw ()
+pcl::HDLGrabber::~HDLGrabber () noexcept
{
stop ();
std::uint32_t nr_points = 0;
std::ifstream fs;
- std::string line;
if (file_name.empty() || !boost::filesystem::exists (file_name))
{
}
///////////////////////////////////////////////////////////////////////////////////////////
-pcl::ImageGrabberBase::~ImageGrabberBase () throw ()
+pcl::ImageGrabberBase::~ImageGrabberBase () noexcept
{
stop ();
delete impl_;
{}
-pcl::io::ImageRGB24::~ImageRGB24 () throw ()
+pcl::io::ImageRGB24::~ImageRGB24 () noexcept
{}
bool
{}
-pcl::io::ImageYUV422::~ImageYUV422 () throw ()
+pcl::io::ImageYUV422::~ImageYUV422 () noexcept
{}
bool
message_long_ = sstream.str ();
}
-pcl::io::IOException::~IOException () throw ()
+pcl::io::IOException::~IOException () noexcept
{
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-ONIGrabber::~ONIGrabber() throw ()
+ONIGrabber::~ONIGrabber() noexcept
{
try
{
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-pcl::io::OpenNI2Grabber::~OpenNI2Grabber () throw ()
+pcl::io::OpenNI2Grabber::~OpenNI2Grabber () noexcept
{
try
{
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-openni_wrapper::OpenNIDevice::~OpenNIDevice () throw ()
+openni_wrapper::OpenNIDevice::~OpenNIDevice () noexcept
{
// stop streams
if (image_generator_.IsValid() && image_generator_.IsGenerating ())
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void __stdcall
-openni_wrapper::OpenNIDevice::NewDepthDataAvailable (xn::ProductionNode&, void* cookie) throw ()
+openni_wrapper::OpenNIDevice::NewDepthDataAvailable (xn::ProductionNode&, void* cookie) noexcept
{
OpenNIDevice* device = reinterpret_cast<OpenNIDevice*>(cookie);
device->depth_condition_.notify_all ();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void __stdcall
-openni_wrapper::OpenNIDevice::NewImageDataAvailable (xn::ProductionNode&, void* cookie) throw ()
+openni_wrapper::OpenNIDevice::NewImageDataAvailable (xn::ProductionNode&, void* cookie) noexcept
{
OpenNIDevice* device = reinterpret_cast<OpenNIDevice*>(cookie);
device->image_condition_.notify_all ();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void __stdcall
-openni_wrapper::OpenNIDevice::NewIRDataAvailable (xn::ProductionNode&, void* cookie) throw ()
+openni_wrapper::OpenNIDevice::NewIRDataAvailable (xn::ProductionNode&, void* cookie) noexcept
{
OpenNIDevice* device = reinterpret_cast<OpenNIDevice*>(cookie);
device->ir_condition_.notify_all ();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
openni_wrapper::OpenNIDevice::CallbackHandle
-openni_wrapper::OpenNIDevice::registerImageCallback (const ImageCallbackFunction& callback, void* custom_data) throw ()
+openni_wrapper::OpenNIDevice::registerImageCallback (const ImageCallbackFunction& callback, void* custom_data) noexcept
{
image_callback_[image_callback_handle_counter_] = [=] (const Image::Ptr& img) { callback (img, custom_data); };
return (image_callback_handle_counter_++);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool
-openni_wrapper::OpenNIDevice::unregisterImageCallback (const OpenNIDevice::CallbackHandle& callbackHandle) throw ()
+openni_wrapper::OpenNIDevice::unregisterImageCallback (const OpenNIDevice::CallbackHandle& callbackHandle) noexcept
{
return (image_callback_.erase (callbackHandle) != 0);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
openni_wrapper::OpenNIDevice::CallbackHandle
-openni_wrapper::OpenNIDevice::registerDepthCallback (const DepthImageCallbackFunction& callback, void* custom_data) throw ()
+openni_wrapper::OpenNIDevice::registerDepthCallback (const DepthImageCallbackFunction& callback, void* custom_data) noexcept
{
depth_callback_[depth_callback_handle_counter_] = [=] (const DepthImage::Ptr& img) { callback (img, custom_data); };
return (depth_callback_handle_counter_++);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool
-openni_wrapper::OpenNIDevice::unregisterDepthCallback (const OpenNIDevice::CallbackHandle& callbackHandle) throw ()
+openni_wrapper::OpenNIDevice::unregisterDepthCallback (const OpenNIDevice::CallbackHandle& callbackHandle) noexcept
{
return (depth_callback_.erase (callbackHandle) != 0);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
openni_wrapper::OpenNIDevice::CallbackHandle
-openni_wrapper::OpenNIDevice::registerIRCallback (const IRImageCallbackFunction& callback, void* custom_data) throw ()
+openni_wrapper::OpenNIDevice::registerIRCallback (const IRImageCallbackFunction& callback, void* custom_data) noexcept
{
ir_callback_[ir_callback_handle_counter_] = [=] (const IRImage::Ptr& img) { callback (img, custom_data); };
return (ir_callback_handle_counter_++);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool
-openni_wrapper::OpenNIDevice::unregisterIRCallback (const OpenNIDevice::CallbackHandle& callbackHandle) throw ()
+openni_wrapper::OpenNIDevice::unregisterIRCallback (const OpenNIDevice::CallbackHandle& callbackHandle) noexcept
{
return (ir_callback_.erase (callbackHandle) != 0);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-openni_wrapper::DeviceKinect::~DeviceKinect () throw ()
+openni_wrapper::DeviceKinect::~DeviceKinect () noexcept
{
depth_mutex_.lock ();
depth_generator_.UnregisterFromNewDataAvailable (depth_callback_handle_);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
-openni_wrapper::DeviceKinect::enumAvailableModes () throw ()
+openni_wrapper::DeviceKinect::enumAvailableModes () noexcept
{
XnMapOutputMode output_mode;
available_image_modes_.clear();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-openni_wrapper::DeviceONI::~DeviceONI() throw ()
+openni_wrapper::DeviceONI::~DeviceONI() noexcept
{
if (streaming_)
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void __stdcall
-openni_wrapper::DeviceONI::NewONIDepthDataAvailable (xn::ProductionNode&, void* cookie) throw ()
+openni_wrapper::DeviceONI::NewONIDepthDataAvailable (xn::ProductionNode&, void* cookie) noexcept
{
DeviceONI* device = reinterpret_cast<DeviceONI*>(cookie);
if (device->depth_stream_running_)
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void __stdcall
-openni_wrapper::DeviceONI::NewONIImageDataAvailable (xn::ProductionNode&, void* cookie) throw ()
+openni_wrapper::DeviceONI::NewONIImageDataAvailable (xn::ProductionNode&, void* cookie) noexcept
{
DeviceONI* device = reinterpret_cast<DeviceONI*> (cookie);
if (device->image_stream_running_)
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void __stdcall
-openni_wrapper::DeviceONI::NewONIIRDataAvailable (xn::ProductionNode&, void* cookie) throw ()
+openni_wrapper::DeviceONI::NewONIIRDataAvailable (xn::ProductionNode&, void* cookie) noexcept
{
DeviceONI* device = reinterpret_cast<DeviceONI*> (cookie);
if (device->ir_stream_running_)
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-openni_wrapper::DevicePrimesense::~DevicePrimesense () throw ()
+openni_wrapper::DevicePrimesense::~DevicePrimesense () noexcept
{
setDepthRegistration (false);
setSynchronization (false);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
-openni_wrapper::DevicePrimesense::enumAvailableModes () throw ()
+openni_wrapper::DevicePrimesense::enumAvailableModes () noexcept
{
XnMapOutputMode output_mode;
available_image_modes_.clear ();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-openni_wrapper::DeviceXtionPro::~DeviceXtionPro () throw ()
+openni_wrapper::DeviceXtionPro::~DeviceXtionPro () noexcept
{
depth_mutex_.lock ();
depth_generator_.UnregisterFromNewDataAvailable (depth_callback_handle_);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
-openni_wrapper::DeviceXtionPro::enumAvailableModes () throw ()
+openni_wrapper::DeviceXtionPro::enumAvailableModes () noexcept
{
XnMapOutputMode output_mode;
available_image_modes_.clear();
getDeviceType(device.device_node.GetCreationInfo (), vendor_id, product_id );
-#if _WIN32
+#ifdef _WIN32
if (vendor_id == 0x45e)
{
strcpy (const_cast<char*> (device_context_[device].device_node.GetDescription ().strVendor), "Microsoft");
THROW_OPENNI_EXCEPTION ("stopping all streams failed. Reason: %s", xnGetStatusString (status));
}
-openni_wrapper::OpenNIDriver::~OpenNIDriver () throw ()
+openni_wrapper::OpenNIDriver::~OpenNIDriver () noexcept
{
// no exception during destuctor
try
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
-openni_wrapper::OpenNIDriver::getDeviceInfos () throw ()
+openni_wrapper::OpenNIDriver::getDeviceInfos () noexcept
{
libusb_context *context = nullptr;
int result;
void
openni_wrapper::OpenNIDriver::getDeviceType (const std::string& connectionString, unsigned short& vendorId, unsigned short& productId)
{
-#if _WIN32
+#ifdef _WIN32
// expected format: "\\?\usb#vid_[ID]&pid_[ID]#[SERIAL]#{GUID}"
using tokenizer = boost::tokenizer<boost::char_separator<char> >;
boost::char_separator<char> separators("#&_");
namespace openni_wrapper
{
-OpenNIException::OpenNIException (const std::string& function_name, const std::string& file_name, unsigned line_number, const std::string& message) throw ()
+OpenNIException::OpenNIException (const std::string& function_name, const std::string& file_name, unsigned line_number, const std::string& message) noexcept
: function_name_ (function_name)
, file_name_ (file_name)
, line_number_ (line_number)
message_long_ = sstream.str();
}
-OpenNIException::~OpenNIException () throw ()
+OpenNIException::~OpenNIException () noexcept
{
}
-OpenNIException& OpenNIException::operator = (const OpenNIException& exception) throw ()
+OpenNIException& OpenNIException::operator = (const OpenNIException& exception) noexcept
{
message_ = exception.message_;
return *this;
using namespace std;
//////////////////////////////////////////////////////////////////////////////
-openni_wrapper::ImageBayerGRBG::ImageBayerGRBG (pcl::shared_ptr<xn::ImageMetaData> image_meta_data, DebayeringMethod method) throw ()
+openni_wrapper::ImageBayerGRBG::ImageBayerGRBG (pcl::shared_ptr<xn::ImageMetaData> image_meta_data, DebayeringMethod method) noexcept
: Image (std::move(image_meta_data))
, debayering_method_ (method)
{
}
//////////////////////////////////////////////////////////////////////////////
-openni_wrapper::ImageBayerGRBG::~ImageBayerGRBG () throw ()
+openni_wrapper::ImageBayerGRBG::~ImageBayerGRBG () noexcept
{
}
namespace openni_wrapper
{
-ImageRGB24::ImageRGB24 (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) throw ()
+ImageRGB24::ImageRGB24 (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) noexcept
: Image (std::move(image_meta_data))
{
}
-ImageRGB24::~ImageRGB24 () throw ()
+ImageRGB24::~ImageRGB24 () noexcept
{
}
namespace openni_wrapper
{
-ImageYUV422::ImageYUV422 (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) throw ()
+ImageYUV422::ImageYUV422 (pcl::shared_ptr<xn::ImageMetaData> image_meta_data) noexcept
: Image (std::move(image_meta_data))
{
}
-ImageYUV422::~ImageYUV422 () throw ()
+ImageYUV422::~ImageYUV422 () noexcept
{
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-pcl::OpenNIGrabber::~OpenNIGrabber () throw ()
+pcl::OpenNIGrabber::~OpenNIGrabber () noexcept
{
try
{
pcl::PCDGrabberBase::PCDGrabberImpl::readTARHeader ()
{
// Read in the header
-#if WIN32
+#ifdef _WIN32
int result = static_cast<int> (_read (tar_fd_, reinterpret_cast<char*> (&tar_header_), 512));
#else
int result = static_cast<int> (::read (tar_fd_, reinterpret_cast<char*> (&tar_header_), 512));
}
///////////////////////////////////////////////////////////////////////////////////////////
-pcl::PCDGrabberBase::~PCDGrabberBase () throw ()
+pcl::PCDGrabberBase::~PCDGrabberBase () noexcept
{
delete impl_;
}
return (impl_->numFrames ());
}
-
}
// Close file
- fs.close ();
+ fpout.close ();
return (0);
}
point_cloud_rgba_signal_ = createSignal<sig_cb_real_sense_point_cloud_rgba> ();
}
-pcl::RealSenseGrabber::~RealSenseGrabber () throw ()
+pcl::RealSenseGrabber::~RealSenseGrabber () noexcept
{
stop ();
}
/////////////////////////////////////////////////////////////////////////////
-pcl::RobotEyeGrabber::~RobotEyeGrabber () throw ()
+pcl::RobotEyeGrabber::~RobotEyeGrabber () noexcept
{
stop ();
disconnect_all_slots<sig_cb_robot_eye_point_cloud_xyzi> ();
}
/////////////////////////////////////////////////////////////////////////////
-pcl::VLPGrabber::~VLPGrabber () throw ()
+pcl::VLPGrabber::~VLPGrabber () noexcept
{
}
+set(SUBSYS_NAME tools)
+
if(WITH_OPENNI)
PCL_ADD_EXECUTABLE(pcl_openni_grabber_example COMPONENT ${SUBSYS_NAME} SOURCES openni_grabber_example.cpp)
target_link_libraries(pcl_openni_grabber_example pcl_common pcl_io)
do
{
key = static_cast<char> (getchar ());
- switch (key)
+ if (key == ' ')
{
- case ' ':
- if (interface.isRunning ())
- interface.stop ();
- else
- interface.start ();
+ interface.toggle ();
}
- } while (key != 27 && key != 'q' && key != 'Q');
+ } while ((key != 27) && (key != 'q') && (key != 'Q'));
// stop the grabber
interface.stop ();
// For each scale, compute the Gaussian "filter response" at the current point
float filter_response = 0.0f;
- float previous_filter_response;
for (std::size_t i_scale = 0; i_scale < scales.size (); ++i_scale)
{
float sigma_sqr = powf (scales[i_scale], 2.0f);
}
else break; // i.e. if dist > 3 standard deviations, then terminate early
}
- previous_filter_response = filter_response;
+ float previous_filter_response = filter_response;
filter_response = numerator / denominator;
// Compute the difference between adjacent scales
pcl::PointCloud<PointT> point;
std::vector<pcl::PCLPointField> fields;
- int user_index = -1;
// if no cluster field name is set, check for X Y Z
if (strcmp(cluster_field_name_.c_str(), "") == 0) {
int x_index = -1;
}
// if cluster field name is set, check if field name is valid
else {
- user_index = pcl::getFieldIndex<PointT>(cluster_field_name_.c_str(), fields);
+
int user_index = pcl::getFieldIndex<PointT>(cluster_field_name_.c_str(), fields);
if (user_index == -1) {
PCL_ERROR("Failed to find match for field '%s'\n", cluster_field_name_.c_str());
#pragma once
#ifdef __GNUC__
-#pragma GCC system_header
+#pragma GCC system_header
#endif
// Marking all Boost headers as system headers to remove warnings
#ifndef PCL_OCTREE_2BUF_BASE_HPP
#define PCL_OCTREE_2BUF_BASE_HPP
-namespace pcl
+namespace pcl {
+namespace octree {
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+Octree2BufBase<LeafContainerT, BranchContainerT>::Octree2BufBase()
+: leaf_count_(0)
+, branch_count_(1)
+, root_node_(new BranchNode())
+, depth_mask_(0)
+, buffer_selector_(0)
+, tree_dirty_flag_(false)
+, octree_depth_(0)
+, dynamic_depth_enabled_(false)
+{}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+Octree2BufBase<LeafContainerT, BranchContainerT>::~Octree2BufBase()
{
- namespace octree
- {
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- Octree2BufBase<LeafContainerT, BranchContainerT>::Octree2BufBase () :
- leaf_count_ (0),
- branch_count_ (1),
- root_node_ (new BranchNode ()),
- depth_mask_ (0),
- buffer_selector_ (0),
- tree_dirty_flag_ (false),
- octree_depth_ (0),
- dynamic_depth_enabled_(false)
- {
- }
+ // deallocate tree structure
+ deleteTree();
+ delete (root_node_);
+}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- Octree2BufBase<LeafContainerT, BranchContainerT>::~Octree2BufBase ()
- {
- // deallocate tree structure
- deleteTree ();
- delete (root_node_);
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+Octree2BufBase<LeafContainerT, BranchContainerT>::setMaxVoxelIndex(
+ unsigned int max_voxel_index_arg)
+{
+ unsigned int treeDepth;
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> void
- Octree2BufBase<LeafContainerT, BranchContainerT>::setMaxVoxelIndex (unsigned int max_voxel_index_arg)
- {
- unsigned int treeDepth;
+ assert(max_voxel_index_arg > 0);
- assert (max_voxel_index_arg > 0);
+ // tree depth == amount of bits of maxVoxels
+ treeDepth = std::max(
+ (std::min(static_cast<unsigned int>(OctreeKey::maxDepth),
+ static_cast<unsigned int>(std::ceil(std::log2(max_voxel_index_arg))))),
+ static_cast<unsigned int>(0));
- // tree depth == amount of bits of maxVoxels
- treeDepth = std::max ((std::min (static_cast<unsigned int> (OctreeKey::maxDepth),
- static_cast<unsigned int> (std::ceil (std::log2 (max_voxel_index_arg))))),
- static_cast<unsigned int> (0));
+ // define depthMask_ by setting a single bit to 1 at bit position == tree depth
+ depth_mask_ = (1 << (treeDepth - 1));
+}
- // define depthMask_ by setting a single bit to 1 at bit position == tree depth
- depth_mask_ = (1 << (treeDepth - 1));
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+Octree2BufBase<LeafContainerT, BranchContainerT>::setTreeDepth(unsigned int depth_arg)
+{
+ assert(depth_arg > 0);
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> void
- Octree2BufBase<LeafContainerT, BranchContainerT>::setTreeDepth (unsigned int depth_arg)
- {
- assert (depth_arg > 0);
+ // set octree depth
+ octree_depth_ = depth_arg;
- // set octree depth
- octree_depth_ = depth_arg;
+ // define depthMask_ by setting a single bit to 1 at bit position == tree depth
+ depth_mask_ = (1 << (depth_arg - 1));
- // define depthMask_ by setting a single bit to 1 at bit position == tree depth
- depth_mask_ = (1 << (depth_arg - 1));
+ // define max. keys
+ max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
+}
- // define max. keys
- max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+LeafContainerT*
+Octree2BufBase<LeafContainerT, BranchContainerT>::findLeaf(unsigned int idx_x_arg,
+ unsigned int idx_y_arg,
+ unsigned int idx_z_arg)
+{
+ // generate key
+ OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> LeafContainerT*
- Octree2BufBase<LeafContainerT, BranchContainerT>::findLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg)
- {
- // generate key
- OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
-
- // check if key exist in octree
- return ( findLeaf (key));
- }
-
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> LeafContainerT*
- Octree2BufBase<LeafContainerT, BranchContainerT>::createLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg)
- {
- // generate key
- OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
-
- // check if key exist in octree
- return ( createLeaf (key));
- }
-
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> bool
- Octree2BufBase<LeafContainerT, BranchContainerT>::existLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const
- {
- // generate key
- OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
-
- // check if key exist in octree
- return existLeaf (key);
- }
+ // check if key exist in octree
+ return (findLeaf(key));
+}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> void
- Octree2BufBase<LeafContainerT, BranchContainerT>::removeLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg)
- {
- // generate key
- OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+LeafContainerT*
+Octree2BufBase<LeafContainerT, BranchContainerT>::createLeaf(unsigned int idx_x_arg,
+ unsigned int idx_y_arg,
+ unsigned int idx_z_arg)
+{
+ // generate key
+ OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
- // free voxel at key
- return (this->removeLeaf (key));
- }
+ // check if key exist in octree
+ return (createLeaf(key));
+}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> void
- Octree2BufBase<LeafContainerT, BranchContainerT>::deleteTree ()
- {
- if (root_node_)
- {
- // reset octree
- deleteBranch (*root_node_);
- leaf_count_ = 0;
- branch_count_ = 1;
-
- tree_dirty_flag_ = false;
- depth_mask_ = 0;
- octree_depth_ = 0;
- }
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+bool
+Octree2BufBase<LeafContainerT, BranchContainerT>::existLeaf(
+ unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const
+{
+ // generate key
+ OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> void
- Octree2BufBase<LeafContainerT, BranchContainerT>::switchBuffers ()
- {
- if (tree_dirty_flag_)
- {
- // make sure that all unused branch nodes from previous buffer are deleted
- treeCleanUpRecursive (root_node_);
- }
+ // check if key exist in octree
+ return existLeaf(key);
+}
- // switch butter selector
- buffer_selector_ = !buffer_selector_;
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+Octree2BufBase<LeafContainerT, BranchContainerT>::removeLeaf(unsigned int idx_x_arg,
+ unsigned int idx_y_arg,
+ unsigned int idx_z_arg)
+{
+ // generate key
+ OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
- // reset flags
- tree_dirty_flag_ = true;
- leaf_count_ = 0;
- branch_count_ = 1;
+ // free voxel at key
+ return (this->removeLeaf(key));
+}
- // we can safely remove children references of root node
- for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
- {
- root_node_->setChildPtr(buffer_selector_, child_idx, nullptr);
- }
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+Octree2BufBase<LeafContainerT, BranchContainerT>::deleteTree()
+{
+ if (root_node_) {
+ // reset octree
+ deleteBranch(*root_node_);
+ leaf_count_ = 0;
+ branch_count_ = 1;
+
+ tree_dirty_flag_ = false;
+ depth_mask_ = 0;
+ octree_depth_ = 0;
+ }
+}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> void
- Octree2BufBase<LeafContainerT, BranchContainerT>::serializeTree (std::vector<char>& binary_tree_out_arg,
- bool do_XOR_encoding_arg)
- {
- OctreeKey new_key;
-
- // clear binary vector
- binary_tree_out_arg.clear ();
- binary_tree_out_arg.reserve (this->branch_count_);
-
- serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, nullptr, do_XOR_encoding_arg, false);
-
- // serializeTreeRecursive cleans-up unused octree nodes in previous octree
- tree_dirty_flag_ = false;
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+Octree2BufBase<LeafContainerT, BranchContainerT>::switchBuffers()
+{
+ if (tree_dirty_flag_) {
+ // make sure that all unused branch nodes from previous buffer are deleted
+ treeCleanUpRecursive(root_node_);
+ }
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> void
- Octree2BufBase<LeafContainerT, BranchContainerT>::serializeTree (std::vector<char>& binary_tree_out_arg,
- std::vector<LeafContainerT*>& leaf_container_vector_arg,
- bool do_XOR_encoding_arg)
- {
- OctreeKey new_key;
+ // switch butter selector
+ buffer_selector_ = !buffer_selector_;
- // clear output vectors
- binary_tree_out_arg.clear ();
- leaf_container_vector_arg.clear ();
+ // reset flags
+ tree_dirty_flag_ = true;
+ leaf_count_ = 0;
+ branch_count_ = 1;
- leaf_container_vector_arg.reserve (leaf_count_);
- binary_tree_out_arg.reserve (this->branch_count_);
+ // we can safely remove children references of root node
+ for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
+ root_node_->setChildPtr(buffer_selector_, child_idx, nullptr);
+ }
+}
- serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg, do_XOR_encoding_arg, false);
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+Octree2BufBase<LeafContainerT, BranchContainerT>::serializeTree(
+ std::vector<char>& binary_tree_out_arg, bool do_XOR_encoding_arg)
+{
+ OctreeKey new_key;
- // serializeTreeRecursive cleans-up unused octree nodes in previous octree
- tree_dirty_flag_ = false;
- }
+ // clear binary vector
+ binary_tree_out_arg.clear();
+ binary_tree_out_arg.reserve(this->branch_count_);
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> void
- Octree2BufBase<LeafContainerT, BranchContainerT>::serializeLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg)
- {
- OctreeKey new_key;
+ serializeTreeRecursive(
+ root_node_, new_key, &binary_tree_out_arg, nullptr, do_XOR_encoding_arg, false);
- // clear output vector
- leaf_container_vector_arg.clear ();
+ // serializeTreeRecursive cleans-up unused octree nodes in previous octree
+ tree_dirty_flag_ = false;
+}
- leaf_container_vector_arg.reserve (leaf_count_);
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+Octree2BufBase<LeafContainerT, BranchContainerT>::serializeTree(
+ std::vector<char>& binary_tree_out_arg,
+ std::vector<LeafContainerT*>& leaf_container_vector_arg,
+ bool do_XOR_encoding_arg)
+{
+ OctreeKey new_key;
- serializeTreeRecursive (root_node_, new_key, nullptr, &leaf_container_vector_arg, false, false);
+ // clear output vectors
+ binary_tree_out_arg.clear();
+ leaf_container_vector_arg.clear();
- // serializeLeafsRecursive cleans-up unused octree nodes in previous octree
- tree_dirty_flag_ = false;
- }
+ leaf_container_vector_arg.reserve(leaf_count_);
+ binary_tree_out_arg.reserve(this->branch_count_);
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> void
- Octree2BufBase<LeafContainerT, BranchContainerT>::deserializeTree (std::vector<char>& binary_tree_in_arg,
- bool do_XOR_decoding_arg)
- {
- OctreeKey new_key;
+ serializeTreeRecursive(root_node_,
+ new_key,
+ &binary_tree_out_arg,
+ &leaf_container_vector_arg,
+ do_XOR_encoding_arg,
+ false);
- // we will rebuild an octree -> reset leafCount
- leaf_count_ = 0;
+ // serializeTreeRecursive cleans-up unused octree nodes in previous octree
+ tree_dirty_flag_ = false;
+}
- // iterator for binary tree structure vector
- std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin ();
- std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end ();
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+Octree2BufBase<LeafContainerT, BranchContainerT>::serializeLeafs(
+ std::vector<LeafContainerT*>& leaf_container_vector_arg)
+{
+ OctreeKey new_key;
- deserializeTreeRecursive (root_node_, depth_mask_, new_key,
- binary_tree_in_it, binary_tree_in_it_end, nullptr, nullptr, false,
- do_XOR_decoding_arg);
+ // clear output vector
+ leaf_container_vector_arg.clear();
- // we modified the octree structure -> clean-up/tree-reset might be required
- tree_dirty_flag_ = false;
- }
+ leaf_container_vector_arg.reserve(leaf_count_);
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> void
- Octree2BufBase<LeafContainerT, BranchContainerT>::deserializeTree (std::vector<char>& binary_tree_in_arg,
- std::vector<LeafContainerT*>& leaf_container_vector_arg,
- bool do_XOR_decoding_arg)
- {
- OctreeKey new_key;
+ serializeTreeRecursive(
+ root_node_, new_key, nullptr, &leaf_container_vector_arg, false, false);
- // set data iterator to first element
- typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it = leaf_container_vector_arg.begin ();
+ // serializeLeafsRecursive cleans-up unused octree nodes in previous octree
+ tree_dirty_flag_ = false;
+}
- // set data iterator to last element
- typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it_end = leaf_container_vector_arg.end ();
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+Octree2BufBase<LeafContainerT, BranchContainerT>::deserializeTree(
+ std::vector<char>& binary_tree_in_arg, bool do_XOR_decoding_arg)
+{
+ OctreeKey new_key;
+
+ // we will rebuild an octree -> reset leafCount
+ leaf_count_ = 0;
+
+ // iterator for binary tree structure vector
+ std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin();
+ std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end();
+
+ deserializeTreeRecursive(root_node_,
+ depth_mask_,
+ new_key,
+ binary_tree_in_it,
+ binary_tree_in_it_end,
+ nullptr,
+ nullptr,
+ false,
+ do_XOR_decoding_arg);
+
+ // we modified the octree structure -> clean-up/tree-reset might be required
+ tree_dirty_flag_ = false;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+Octree2BufBase<LeafContainerT, BranchContainerT>::deserializeTree(
+ std::vector<char>& binary_tree_in_arg,
+ std::vector<LeafContainerT*>& leaf_container_vector_arg,
+ bool do_XOR_decoding_arg)
+{
+ OctreeKey new_key;
+
+ // set data iterator to first element
+ typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it =
+ leaf_container_vector_arg.begin();
+
+ // set data iterator to last element
+ typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it_end =
+ leaf_container_vector_arg.end();
+
+ // we will rebuild an octree -> reset leafCount
+ leaf_count_ = 0;
+
+ // iterator for binary tree structure vector
+ std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin();
+ std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end();
+
+ deserializeTreeRecursive(root_node_,
+ depth_mask_,
+ new_key,
+ binary_tree_in_it,
+ binary_tree_in_it_end,
+ &leaf_container_vector_it,
+ &leaf_container_vector_it_end,
+ false,
+ do_XOR_decoding_arg);
+
+ // we modified the octree structure -> clean-up/tree-reset might be required
+ tree_dirty_flag_ = false;
+}
- // we will rebuild an octree -> reset leafCount
- leaf_count_ = 0;
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+Octree2BufBase<LeafContainerT, BranchContainerT>::serializeNewLeafs(
+ std::vector<LeafContainerT*>& leaf_container_vector_arg)
+{
+ OctreeKey new_key;
- // iterator for binary tree structure vector
- std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin ();
- std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end ();
+ // clear output vector
+ leaf_container_vector_arg.clear();
+ leaf_container_vector_arg.reserve(leaf_count_);
- deserializeTreeRecursive (root_node_,
- depth_mask_,
- new_key,
- binary_tree_in_it,
- binary_tree_in_it_end,
- &leaf_container_vector_it,
- &leaf_container_vector_it_end,
- false,
- do_XOR_decoding_arg);
+ serializeTreeRecursive(
+ root_node_, new_key, nullptr, &leaf_container_vector_arg, false, true);
+ // serializeLeafsRecursive cleans-up unused octree nodes in previous octree buffer
+ tree_dirty_flag_ = false;
+}
- // we modified the octree structure -> clean-up/tree-reset might be required
- tree_dirty_flag_ = false;
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+unsigned int
+Octree2BufBase<LeafContainerT, BranchContainerT>::createLeafRecursive(
+ const OctreeKey& key_arg,
+ unsigned int depth_mask_arg,
+ BranchNode* branch_arg,
+ LeafNode*& return_leaf_arg,
+ BranchNode*& parent_of_leaf_arg,
+ bool branch_reset_arg)
+{
+ // branch reset -> this branch has been taken from previous buffer
+ if (branch_reset_arg) {
+ // we can safely remove children references
+ for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
+ branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr);
}
+ }
+ // find branch child from key
+ unsigned char child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> void
- Octree2BufBase<LeafContainerT, BranchContainerT>::serializeNewLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg)
- {
- OctreeKey new_key;
+ if (depth_mask_arg > 1) {
+ // we have not reached maximum tree depth
+ BranchNode* child_branch;
+ bool doNodeReset;
- // clear output vector
- leaf_container_vector_arg.clear ();
- leaf_container_vector_arg.reserve (leaf_count_);
+ doNodeReset = false;
- serializeTreeRecursive (root_node_, new_key, nullptr, &leaf_container_vector_arg, false, true);
+ // if required branch does not exist
+ if (!branch_arg->hasChild(buffer_selector_, child_idx)) {
+ // check if we find a branch node reference in previous buffer
- // serializeLeafsRecursive cleans-up unused octree nodes in previous octree buffer
- tree_dirty_flag_ = false;
- }
+ if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
+ OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_, child_idx);
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- unsigned int
- Octree2BufBase<LeafContainerT, BranchContainerT>::createLeafRecursive (const OctreeKey& key_arg,
- unsigned int depth_mask_arg,
- BranchNode* branch_arg,
- LeafNode*& return_leaf_arg,
- BranchNode*& parent_of_leaf_arg,
- bool branch_reset_arg)
- {
- // branch reset -> this branch has been taken from previous buffer
- if (branch_reset_arg)
- {
- // we can safely remove children references
- for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
- {
- branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr);
+ if (child_node->getNodeType() == BRANCH_NODE) {
+ child_branch = static_cast<BranchNode*>(child_node);
+ branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
+ }
+ else {
+ // depth has changed.. child in preceding buffer is a leaf node.
+ deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
+ child_branch = createBranchChild(*branch_arg, child_idx);
}
- }
- // find branch child from key
- unsigned char child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg);
+ // take child branch from previous buffer
+ doNodeReset = true; // reset the branch pointer array of stolen child node
+ }
+ else {
+ // if required branch does not exist -> create it
+ child_branch = createBranchChild(*branch_arg, child_idx);
+ }
- if (depth_mask_arg > 1)
- {
- // we have not reached maximum tree depth
- BranchNode* child_branch;
- bool doNodeReset;
+ branch_count_++;
+ }
+ // required branch node already exists - use it
+ else
+ child_branch = static_cast<BranchNode*>(
+ branch_arg->getChildPtr(buffer_selector_, child_idx));
+
+ // recursively proceed with indexed child branch
+ return createLeafRecursive(key_arg,
+ depth_mask_arg / 2,
+ child_branch,
+ return_leaf_arg,
+ parent_of_leaf_arg,
+ doNodeReset);
+ }
- doNodeReset = false;
+ // branch childs are leaf nodes
+ LeafNode* child_leaf;
+ if (!branch_arg->hasChild(buffer_selector_, child_idx)) {
+ // leaf node at child_idx does not exist
- // if required branch does not exist
- if (!branch_arg->hasChild(buffer_selector_, child_idx))
- {
- // check if we find a branch node reference in previous buffer
+ // check if we can take copy a reference from previous buffer
+ if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
- if (branch_arg->hasChild(!buffer_selector_, child_idx))
- {
- OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx);
+ OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_, child_idx);
+ if (child_node->getNodeType() == LEAF_NODE) {
+ child_leaf = static_cast<LeafNode*>(child_node);
+ branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
+ }
+ else {
+ // depth has changed.. child in preceding buffer is a leaf node.
+ deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
+ child_leaf = createLeafChild(*branch_arg, child_idx);
+ }
+ leaf_count_++;
+ }
+ else {
+ // if required leaf does not exist -> create it
+ child_leaf = createLeafChild(*branch_arg, child_idx);
+ leaf_count_++;
+ }
- if (child_node->getNodeType()==BRANCH_NODE) {
- child_branch = static_cast<BranchNode*> (child_node);
- branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
- } else {
- // depth has changed.. child in preceding buffer is a leaf node.
- deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
- child_branch = createBranchChild (*branch_arg, child_idx);
- }
+ // return leaf node
+ return_leaf_arg = child_leaf;
+ parent_of_leaf_arg = branch_arg;
+ }
+ else {
+ // leaf node already exist
+ return_leaf_arg =
+ static_cast<LeafNode*>(branch_arg->getChildPtr(buffer_selector_, child_idx));
+ parent_of_leaf_arg = branch_arg;
+ }
- // take child branch from previous buffer
- doNodeReset = true; // reset the branch pointer array of stolen child node
+ return depth_mask_arg;
+}
- }
- else
- {
- // if required branch does not exist -> create it
- child_branch = createBranchChild (*branch_arg, child_idx);
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+Octree2BufBase<LeafContainerT, BranchContainerT>::findLeafRecursive(
+ const OctreeKey& key_arg,
+ unsigned int depth_mask_arg,
+ BranchNode* branch_arg,
+ LeafContainerT*& result_arg) const
+{
+ // return leaf node
+ unsigned char child_idx;
- branch_count_++;
- }
- // required branch node already exists - use it
- else
- child_branch = static_cast<BranchNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx));
-
- // recursively proceed with indexed child branch
- return createLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, return_leaf_arg, parent_of_leaf_arg, doNodeReset);
- }
+ // find branch child from key
+ child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
- // branch childs are leaf nodes
- LeafNode* child_leaf;
- if (!branch_arg->hasChild(buffer_selector_, child_idx))
- {
- // leaf node at child_idx does not exist
-
- // check if we can take copy a reference from previous buffer
- if (branch_arg->hasChild(!buffer_selector_, child_idx))
- {
-
- OctreeNode * child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx);
- if (child_node->getNodeType () == LEAF_NODE)
- {
- child_leaf = static_cast<LeafNode*> (child_node);
- branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
- } else {
- // depth has changed.. child in preceding buffer is a leaf node.
- deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
- child_leaf = createLeafChild (*branch_arg, child_idx);
- }
- leaf_count_++;
- }
- else
- {
- // if required leaf does not exist -> create it
- child_leaf = createLeafChild (*branch_arg, child_idx);
- leaf_count_++;
- }
-
- // return leaf node
- return_leaf_arg = child_leaf;
- parent_of_leaf_arg = branch_arg;
- }
- else
- {
- // leaf node already exist
- return_leaf_arg = static_cast<LeafNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx));;
- parent_of_leaf_arg = branch_arg;
- }
+ if (depth_mask_arg > 1) {
+ // we have not reached maximum tree depth
+ BranchNode* child_branch;
+ child_branch =
+ static_cast<BranchNode*>(branch_arg->getChildPtr(buffer_selector_, child_idx));
- return depth_mask_arg;
+ if (child_branch)
+ // recursively proceed with indexed child branch
+ findLeafRecursive(key_arg, depth_mask_arg / 2, child_branch, result_arg);
+ }
+ else {
+ // we reached leaf node level
+ if (branch_arg->hasChild(buffer_selector_, child_idx)) {
+ // return existing leaf node
+ LeafNode* leaf_node =
+ static_cast<LeafNode*>(branch_arg->getChildPtr(buffer_selector_, child_idx));
+ result_arg = leaf_node->getContainerPtr();
}
+ }
+}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> void
- Octree2BufBase<LeafContainerT, BranchContainerT>::findLeafRecursive (const OctreeKey& key_arg,
- unsigned int depth_mask_arg,
- BranchNode* branch_arg,
- LeafContainerT*& result_arg) const
- {
- // return leaf node
- unsigned char child_idx;
-
- // find branch child from key
- child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg);
-
- if (depth_mask_arg > 1)
- {
- // we have not reached maximum tree depth
- BranchNode* child_branch;
- child_branch = static_cast<BranchNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx));
-
- if (child_branch)
- // recursively proceed with indexed child branch
- findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg);
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+bool
+Octree2BufBase<LeafContainerT, BranchContainerT>::deleteLeafRecursive(
+ const OctreeKey& key_arg, unsigned int depth_mask_arg, BranchNode* branch_arg)
+{
+ // index to branch child
+ unsigned char child_idx;
+ // indicates if branch is empty and can be safely removed
+ bool bNoChilds;
+
+ // find branch child from key
+ child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
+
+ if (depth_mask_arg > 1) {
+ // we have not reached maximum tree depth
+ BranchNode* child_branch;
+
+ // next branch child on our path through the tree
+ child_branch =
+ static_cast<BranchNode*>(branch_arg->getChildPtr(buffer_selector_, child_idx));
+
+ if (child_branch) {
+ // recursively explore the indexed child branch
+ bool bBranchOccupied =
+ deleteLeafRecursive(key_arg, depth_mask_arg / 2, child_branch);
+
+ if (!bBranchOccupied) {
+ // child branch does not own any sub-child nodes anymore -> delete child branch
+ deleteBranchChild(*branch_arg, buffer_selector_, child_idx);
+ branch_count_--;
}
- else
- {
- // we reached leaf node level
- if (branch_arg->hasChild(buffer_selector_, child_idx))
- {
- // return existing leaf node
- LeafNode* leaf_node = static_cast<LeafNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx));
- result_arg = leaf_node->getContainerPtr();;
- }
- }
}
+ }
+ else {
+ // our child is a leaf node -> delete it
+ deleteBranchChild(*branch_arg, buffer_selector_, child_idx);
+ leaf_count_--;
+ }
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> bool
- Octree2BufBase<LeafContainerT, BranchContainerT>::deleteLeafRecursive (const OctreeKey& key_arg,
- unsigned int depth_mask_arg,
- BranchNode* branch_arg)
- {
- // index to branch child
- unsigned char child_idx;
- // indicates if branch is empty and can be safely removed
- bool bNoChilds;
-
- // find branch child from key
- child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg);
-
- if (depth_mask_arg > 1)
- {
- // we have not reached maximum tree depth
- BranchNode* child_branch;
- bool bBranchOccupied;
-
- // next branch child on our path through the tree
- child_branch = static_cast<BranchNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx));
-
- if (child_branch)
- {
- // recursively explore the indexed child branch
- bBranchOccupied = deleteLeafRecursive (key_arg, depth_mask_arg / 2, child_branch);
-
- if (!bBranchOccupied)
- {
- // child branch does not own any sub-child nodes anymore -> delete child branch
- deleteBranchChild (*branch_arg, buffer_selector_, child_idx);
- branch_count_--;
- }
- }
- }
- else
- {
- // our child is a leaf node -> delete it
- deleteBranchChild (*branch_arg, buffer_selector_, child_idx);
- leaf_count_--;
- }
+ // check if current branch still owns childs
+ bNoChilds = false;
+ for (child_idx = 0; child_idx < 8; child_idx++) {
+ bNoChilds = branch_arg->hasChild(buffer_selector_, child_idx);
+ if (bNoChilds)
+ break;
+ }
- // check if current branch still owns childs
- bNoChilds = false;
- for (child_idx = 0; child_idx < 8; child_idx++)
- {
- bNoChilds = branch_arg->hasChild(buffer_selector_, child_idx);
- if (bNoChilds)
- break;
- }
+ // return true if current branch can be deleted
+ return (bNoChilds);
+}
- // return true if current branch can be deleted
- return (bNoChilds);
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+Octree2BufBase<LeafContainerT, BranchContainerT>::serializeTreeRecursive(
+ BranchNode* branch_arg,
+ OctreeKey& key_arg,
+ std::vector<char>* binary_tree_out_arg,
+ typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
+ bool do_XOR_encoding_arg,
+ bool new_leafs_filter_arg)
+{
+ // bit pattern
+ char branch_bit_pattern_curr_buffer;
+ char branch_bit_pattern_prev_buffer;
+ char node_XOR_bit_pattern;
+
+ // occupancy bit patterns of branch node (current and previous octree buffer)
+ branch_bit_pattern_curr_buffer = getBranchBitPattern(*branch_arg, buffer_selector_);
+ branch_bit_pattern_prev_buffer = getBranchBitPattern(*branch_arg, !buffer_selector_);
+
+ // XOR of current and previous occupancy bit patterns
+ node_XOR_bit_pattern =
+ branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer;
+
+ if (binary_tree_out_arg) {
+ if (do_XOR_encoding_arg) {
+ // write XOR bit pattern to output vector
+ binary_tree_out_arg->push_back(node_XOR_bit_pattern);
}
+ else {
+ // write bit pattern of current buffer to output vector
+ binary_tree_out_arg->push_back(branch_bit_pattern_curr_buffer);
+ }
+ }
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> void Octree2BufBase<
- LeafContainerT, BranchContainerT>::serializeTreeRecursive (BranchNode* branch_arg,
- OctreeKey& key_arg,
- std::vector<char>* binary_tree_out_arg,
- typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
- bool do_XOR_encoding_arg,
- bool new_leafs_filter_arg)
- {
- // bit pattern
- char branch_bit_pattern_curr_buffer;
- char branch_bit_pattern_prev_buffer;
- char node_XOR_bit_pattern;
-
- // occupancy bit patterns of branch node (current and previous octree buffer)
- branch_bit_pattern_curr_buffer = getBranchBitPattern (*branch_arg, buffer_selector_);
- branch_bit_pattern_prev_buffer = getBranchBitPattern (*branch_arg, !buffer_selector_);
-
- // XOR of current and previous occupancy bit patterns
- node_XOR_bit_pattern = branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer;
-
- if (binary_tree_out_arg)
- {
- if (do_XOR_encoding_arg)
- {
- // write XOR bit pattern to output vector
- binary_tree_out_arg->push_back (node_XOR_bit_pattern);
- }
- else
- {
- // write bit pattern of current buffer to output vector
- binary_tree_out_arg->push_back (branch_bit_pattern_curr_buffer);
- }
+ // iterate over all children
+ for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
+ if (branch_arg->hasChild(buffer_selector_, child_idx)) {
+ // add current branch voxel to key
+ key_arg.pushBranch(child_idx);
+
+ OctreeNode* child_node = branch_arg->getChildPtr(buffer_selector_, child_idx);
+
+ switch (child_node->getNodeType()) {
+ case BRANCH_NODE: {
+ // recursively proceed with indexed child branch
+ serializeTreeRecursive(static_cast<BranchNode*>(child_node),
+ key_arg,
+ binary_tree_out_arg,
+ leaf_container_vector_arg,
+ do_XOR_encoding_arg,
+ new_leafs_filter_arg);
+ break;
}
+ case LEAF_NODE: {
+ LeafNode* child_leaf = static_cast<LeafNode*>(child_node);
- // iterate over all children
- for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
- {
- if (branch_arg->hasChild(buffer_selector_, child_idx))
- {
- // add current branch voxel to key
- key_arg.pushBranch(child_idx);
-
- OctreeNode *child_node = branch_arg->getChildPtr(buffer_selector_,child_idx);
-
- switch (child_node->getNodeType ())
- {
- case BRANCH_NODE:
- {
- // recursively proceed with indexed child branch
- serializeTreeRecursive (static_cast<BranchNode*> (child_node), key_arg, binary_tree_out_arg,
- leaf_container_vector_arg, do_XOR_encoding_arg, new_leafs_filter_arg);
- break;
- }
- case LEAF_NODE:
- {
- LeafNode* child_leaf = static_cast<LeafNode*> (child_node);
-
- if (new_leafs_filter_arg)
- {
- if (!branch_arg->hasChild (!buffer_selector_, child_idx))
- {
- if (leaf_container_vector_arg)
- leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
-
- serializeTreeCallback (**child_leaf, key_arg);
- }
- } else
- {
-
- if (leaf_container_vector_arg)
- leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
-
- serializeTreeCallback (**child_leaf, key_arg);
- }
+ if (new_leafs_filter_arg) {
+ if (!branch_arg->hasChild(!buffer_selector_, child_idx)) {
+ if (leaf_container_vector_arg)
+ leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
- break;
- }
- default:
- break;
+ serializeTreeCallback(**child_leaf, key_arg);
}
-
- // pop current branch voxel from key
- key_arg.popBranch();
}
- else if (branch_arg->hasChild (!buffer_selector_, child_idx))
- {
- // delete branch, free memory
- deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
+ else {
+
+ if (leaf_container_vector_arg)
+ leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
+ serializeTreeCallback(**child_leaf, key_arg);
}
+ break;
+ }
+ default:
+ break;
}
+
+ // pop current branch voxel from key
+ key_arg.popBranch();
+ }
+ else if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
+ // delete branch, free memory
+ deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
}
+ }
+}
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+Octree2BufBase<LeafContainerT, BranchContainerT>::deserializeTreeRecursive(
+ BranchNode* branch_arg,
+ unsigned int depth_mask_arg,
+ OctreeKey& key_arg,
+ typename std::vector<char>::const_iterator& binaryTreeIT_arg,
+ typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
+ typename std::vector<LeafContainerT*>::const_iterator* dataVectorIterator_arg,
+ typename std::vector<LeafContainerT*>::const_iterator* dataVectorEndIterator_arg,
+ bool branch_reset_arg,
+ bool do_XOR_decoding_arg)
+{
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> void
- Octree2BufBase<LeafContainerT, BranchContainerT>::deserializeTreeRecursive (BranchNode* branch_arg,
- unsigned int depth_mask_arg, OctreeKey& key_arg,
- typename std::vector<char>::const_iterator& binaryTreeIT_arg,
- typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
- typename std::vector<LeafContainerT*>::const_iterator* dataVectorIterator_arg,
- typename std::vector<LeafContainerT*>::const_iterator* dataVectorEndIterator_arg,
- bool branch_reset_arg, bool do_XOR_decoding_arg)
- {
- // node bits
- char nodeBits;
- char recoveredNodeBits;
-
- // branch reset -> this branch has been taken from previous buffer
- if (branch_reset_arg)
- {
- // we can safely remove children references
- for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
- {
- branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr);
- }
- }
+ // branch reset -> this branch has been taken from previous buffer
+ if (branch_reset_arg) {
+ // we can safely remove children references
+ for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
+ branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr);
+ }
+ }
- if (binaryTreeIT_arg!=binaryTreeIT_End_arg) {
- // read branch occupancy bit pattern from vector
- nodeBits = *binaryTreeIT_arg++;
+ if (binaryTreeIT_arg != binaryTreeIT_End_arg) {
+ // read branch occupancy bit pattern from vector
+ char nodeBits = *binaryTreeIT_arg++;
+ // recover branch occupancy bit pattern
+ char recoveredNodeBits;
+ if (do_XOR_decoding_arg) {
+ recoveredNodeBits =
+ getBranchBitPattern(*branch_arg, !buffer_selector_) ^ nodeBits;
+ }
+ else {
+ recoveredNodeBits = nodeBits;
+ }
- // recover branch occupancy bit pattern
- if (do_XOR_decoding_arg)
- {
- recoveredNodeBits = getBranchBitPattern (*branch_arg, !buffer_selector_) ^ nodeBits;
- }
- else
- {
- recoveredNodeBits = nodeBits;
- }
+ // iterate over all children
+ for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
+ // if occupancy bit for child_idx is set..
+ if (recoveredNodeBits & (1 << child_idx)) {
+ // add current branch voxel to key
+ key_arg.pushBranch(child_idx);
- // iterate over all children
- for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
- {
- // if occupancy bit for child_idx is set..
- if (recoveredNodeBits & (1 << child_idx))
- {
- // add current branch voxel to key
- key_arg.pushBranch(child_idx);
-
- bool doNodeReset;
-
- doNodeReset = false;
-
- if (depth_mask_arg > 1)
- {
- // we have not reached maximum tree depth
-
- BranchNode* child_branch;
-
- // check if we find a branch node reference in previous buffer
- if (!branch_arg->hasChild(buffer_selector_, child_idx))
- {
-
- if (branch_arg->hasChild(!buffer_selector_, child_idx))
- {
- OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx);
-
- if (child_node->getNodeType()==BRANCH_NODE) {
- child_branch = static_cast<BranchNode*> (child_node);
- branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
- } else {
- // depth has changed.. child in preceding buffer is a leaf node.
- deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
- child_branch = createBranchChild (*branch_arg, child_idx);
- }
-
- // take child branch from previous buffer
- doNodeReset = true; // reset the branch pointer array of stolen child node
- }
- else
- {
- // if required branch does not exist -> create it
- child_branch = createBranchChild (*branch_arg, child_idx);
- }
-
- branch_count_++;
+ if (depth_mask_arg > 1) {
+ // we have not reached maximum tree depth
- }
- else
- {
- // required branch node already exists - use it
- child_branch = static_cast<BranchNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx));
- }
+ bool doNodeReset = false;
- // recursively proceed with indexed child branch
- deserializeTreeRecursive (child_branch, depth_mask_arg / 2, key_arg,
- binaryTreeIT_arg, binaryTreeIT_End_arg,
- dataVectorIterator_arg, dataVectorEndIterator_arg,
- doNodeReset, do_XOR_decoding_arg);
+ BranchNode* child_branch;
- }
- else
- {
- // branch childs are leaf nodes
- LeafNode* child_leaf;
-
- // check if we can take copy a reference pointer from previous buffer
- if (branch_arg->hasChild(!buffer_selector_, child_idx))
- {
- // take child leaf node from previous buffer
- OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx);
- if (child_node->getNodeType()==LEAF_NODE) {
- child_leaf = static_cast<LeafNode*> (child_node);
- branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
- } else {
- // depth has changed.. child in preceding buffer is a leaf node.
- deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
- child_leaf = createLeafChild (*branch_arg, child_idx);
- }
- }
- else
- {
- // if required leaf does not exist -> create it
- child_leaf = createLeafChild (*branch_arg, child_idx);
- }
+ // check if we find a branch node reference in previous buffer
+ if (!branch_arg->hasChild(buffer_selector_, child_idx)) {
- // we reached leaf node level
+ if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
+ OctreeNode* child_node =
+ branch_arg->getChildPtr(!buffer_selector_, child_idx);
- if (dataVectorIterator_arg
- && (*dataVectorIterator_arg != *dataVectorEndIterator_arg))
- {
- LeafContainerT& container = **child_leaf;
- container = ***dataVectorIterator_arg;
- ++*dataVectorIterator_arg;
+ if (child_node->getNodeType() == BRANCH_NODE) {
+ child_branch = static_cast<BranchNode*>(child_node);
+ branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
+ }
+ else {
+ // depth has changed.. child in preceding buffer is a leaf node.
+ deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
+ child_branch = createBranchChild(*branch_arg, child_idx);
}
- leaf_count_++;
+ // take child branch from previous buffer
+ doNodeReset = true; // reset the branch pointer array of stolen child node
+ }
+ else {
+ // if required branch does not exist -> create it
+ child_branch = createBranchChild(*branch_arg, child_idx);
+ }
- // execute deserialization callback
- deserializeTreeCallback (**child_leaf, key_arg);
+ branch_count_++;
+ }
+ else {
+ // required branch node already exists - use it
+ child_branch = static_cast<BranchNode*>(
+ branch_arg->getChildPtr(buffer_selector_, child_idx));
+ }
+ // recursively proceed with indexed child branch
+ deserializeTreeRecursive(child_branch,
+ depth_mask_arg / 2,
+ key_arg,
+ binaryTreeIT_arg,
+ binaryTreeIT_End_arg,
+ dataVectorIterator_arg,
+ dataVectorEndIterator_arg,
+ doNodeReset,
+ do_XOR_decoding_arg);
+ }
+ else {
+ // branch childs are leaf nodes
+ LeafNode* child_leaf;
+
+ // check if we can take copy a reference pointer from previous buffer
+ if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
+ // take child leaf node from previous buffer
+ OctreeNode* child_node =
+ branch_arg->getChildPtr(!buffer_selector_, child_idx);
+ if (child_node->getNodeType() == LEAF_NODE) {
+ child_leaf = static_cast<LeafNode*>(child_node);
+ branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
}
-
- // pop current branch voxel from key
- key_arg.popBranch();
+ else {
+ // depth has changed.. child in preceding buffer is a leaf node.
+ deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
+ child_leaf = createLeafChild(*branch_arg, child_idx);
+ }
+ }
+ else {
+ // if required leaf does not exist -> create it
+ child_leaf = createLeafChild(*branch_arg, child_idx);
}
- else if (branch_arg->hasChild (!buffer_selector_, child_idx))
- {
- // remove old branch pointer information in current branch
- branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr);
-
- // remove unused branches in previous buffer
- deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
+
+ // we reached leaf node level
+
+ if (dataVectorIterator_arg &&
+ (*dataVectorIterator_arg != *dataVectorEndIterator_arg)) {
+ LeafContainerT& container = **child_leaf;
+ container = ***dataVectorIterator_arg;
+ ++*dataVectorIterator_arg;
}
+
+ leaf_count_++;
+
+ // execute deserialization callback
+ deserializeTreeCallback(**child_leaf, key_arg);
}
+
+ // pop current branch voxel from key
+ key_arg.popBranch();
}
+ else if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
+ // remove old branch pointer information in current branch
+ branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr);
+ // remove unused branches in previous buffer
+ deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
+ }
}
-
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT> void
- Octree2BufBase<LeafContainerT, BranchContainerT>::treeCleanUpRecursive (BranchNode* branch_arg)
- {
- // occupancy bit pattern of branch node (previous octree buffer)
- char occupied_children_bit_pattern_prev_buffer = getBranchBitPattern (*branch_arg, !buffer_selector_);
-
- // XOR of current and previous occupancy bit patterns
- char node_XOR_bit_pattern = getBranchXORBitPattern (*branch_arg);
-
- // bit pattern indicating unused octree nodes in previous branch
- char unused_branches_bit_pattern = node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer;
-
- // iterate over all children
- for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
- {
- if (branch_arg->hasChild(buffer_selector_, child_idx))
- {
- OctreeNode *child_node = branch_arg->getChildPtr(buffer_selector_,child_idx);
-
- switch (child_node->getNodeType ())
- {
- case BRANCH_NODE:
- {
- // recursively proceed with indexed child branch
- treeCleanUpRecursive (static_cast<BranchNode*> (child_node));
- break;
- }
- case LEAF_NODE:
- // leaf level - nothing to do..
- break;
- default:
- break;
- }
- }
+ }
+}
- // check for unused branches in previous buffer
- if (unused_branches_bit_pattern & (1 << child_idx))
- {
- // delete branch, free memory
- deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+Octree2BufBase<LeafContainerT, BranchContainerT>::treeCleanUpRecursive(
+ BranchNode* branch_arg)
+{
+ // occupancy bit pattern of branch node (previous octree buffer)
+ char occupied_children_bit_pattern_prev_buffer =
+ getBranchBitPattern(*branch_arg, !buffer_selector_);
+
+ // XOR of current and previous occupancy bit patterns
+ char node_XOR_bit_pattern = getBranchXORBitPattern(*branch_arg);
+
+ // bit pattern indicating unused octree nodes in previous branch
+ char unused_branches_bit_pattern =
+ node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer;
+
+ // iterate over all children
+ for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
+ if (branch_arg->hasChild(buffer_selector_, child_idx)) {
+ OctreeNode* child_node = branch_arg->getChildPtr(buffer_selector_, child_idx);
+
+ switch (child_node->getNodeType()) {
+ case BRANCH_NODE: {
+ // recursively proceed with indexed child branch
+ treeCleanUpRecursive(static_cast<BranchNode*>(child_node));
+ break;
+ }
+ case LEAF_NODE:
+ // leaf level - nothing to do..
+ break;
+ default:
+ break;
}
}
+
+ // check for unused branches in previous buffer
+ if (unused_branches_bit_pattern & (1 << child_idx)) {
+ // delete branch, free memory
+ deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
+ }
}
}
+} // namespace octree
+} // namespace pcl
-#define PCL_INSTANTIATE_Octree2BufBase(T) template class PCL_EXPORTS pcl::octree::Octree2BufBase<T>;
+#define PCL_INSTANTIATE_Octree2BufBase(T) \
+ template class PCL_EXPORTS pcl::octree::Octree2BufBase<T>;
#endif
-
#include <pcl/impl/instantiate.hpp>
-namespace pcl
+namespace pcl {
+namespace octree {
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+OctreeBase<LeafContainerT, BranchContainerT>::OctreeBase()
+: leaf_count_(0)
+, branch_count_(1)
+, root_node_(new BranchNode())
+, depth_mask_(0)
+, octree_depth_(0)
+, dynamic_depth_enabled_(false)
+{}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+OctreeBase<LeafContainerT, BranchContainerT>::~OctreeBase()
{
- namespace octree
- {
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- OctreeBase<LeafContainerT, BranchContainerT>::OctreeBase () :
- leaf_count_ (0),
- branch_count_ (1),
- root_node_ (new BranchNode ()),
- depth_mask_ (0),
- octree_depth_ (0),
- dynamic_depth_enabled_ (false)
- {
- }
+ // deallocate tree structure
+ deleteTree();
+ delete (root_node_);
+}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- OctreeBase<LeafContainerT, BranchContainerT>::~OctreeBase ()
- {
- // deallocate tree structure
- deleteTree ();
- delete (root_node_);
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+OctreeBase<LeafContainerT, BranchContainerT>::setMaxVoxelIndex(
+ unsigned int max_voxel_index_arg)
+{
+ unsigned int tree_depth;
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- void
- OctreeBase<LeafContainerT, BranchContainerT>::setMaxVoxelIndex (unsigned int max_voxel_index_arg)
- {
- unsigned int tree_depth;
+ assert(max_voxel_index_arg > 0);
- assert(max_voxel_index_arg>0);
+ // tree depth == bitlength of maxVoxels
+ tree_depth =
+ std::min(static_cast<unsigned int>(OctreeKey::maxDepth),
+ static_cast<unsigned int>(std::ceil(std::log2(max_voxel_index_arg))));
- // tree depth == bitlength of maxVoxels
- tree_depth = std::min (static_cast<unsigned int> (OctreeKey::maxDepth), static_cast<unsigned int> (std::ceil (std::log2 (max_voxel_index_arg))));
+ // define depthMask_ by setting a single bit to 1 at bit position == tree depth
+ depth_mask_ = (1 << (tree_depth - 1));
+}
- // define depthMask_ by setting a single bit to 1 at bit position == tree depth
- depth_mask_ = (1 << (tree_depth - 1));
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+OctreeBase<LeafContainerT, BranchContainerT>::setTreeDepth(unsigned int depth_arg)
+{
+ assert(depth_arg > 0);
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- void
- OctreeBase<LeafContainerT, BranchContainerT>::setTreeDepth (unsigned int depth_arg)
- {
- assert(depth_arg>0);
+ // set octree depth
+ octree_depth_ = depth_arg;
- // set octree depth
- octree_depth_ = depth_arg;
+ // define depthMask_ by setting a single bit to 1 at bit position == tree depth
+ depth_mask_ = (1 << (depth_arg - 1));
- // define depthMask_ by setting a single bit to 1 at bit position == tree depth
- depth_mask_ = (1 << (depth_arg - 1));
+ // define max. keys
+ max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
+}
- // define max. keys
- max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+LeafContainerT*
+OctreeBase<LeafContainerT, BranchContainerT>::findLeaf(unsigned int idx_x_arg,
+ unsigned int idx_y_arg,
+ unsigned int idx_z_arg)
+{
+ // generate key
+ OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- LeafContainerT*
- OctreeBase<LeafContainerT, BranchContainerT>::findLeaf (unsigned int idx_x_arg,
- unsigned int idx_y_arg,
- unsigned int idx_z_arg)
- {
- // generate key
- OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
-
- // check if key exist in octree
- return (findLeaf (key));
- }
+ // check if key exist in octree
+ return (findLeaf(key));
+}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- LeafContainerT*
- OctreeBase<LeafContainerT, BranchContainerT>::createLeaf (unsigned int idx_x_arg,
- unsigned int idx_y_arg,
- unsigned int idx_z_arg)
- {
- // generate key
- OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
-
- // check if key exist in octree
- return (createLeaf (key));
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+LeafContainerT*
+OctreeBase<LeafContainerT, BranchContainerT>::createLeaf(unsigned int idx_x_arg,
+ unsigned int idx_y_arg,
+ unsigned int idx_z_arg)
+{
+ // generate key
+ OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- bool
- OctreeBase<LeafContainerT, BranchContainerT>::existLeaf (unsigned int idx_x_arg,
- unsigned int idx_y_arg,
- unsigned int idx_z_arg) const
- {
- // generate key
- OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
-
- // check if key exist in octree
- return (existLeaf (key));
- }
+ // check if key exist in octree
+ return (createLeaf(key));
+}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- void
- OctreeBase<LeafContainerT, BranchContainerT>::removeLeaf (unsigned int idx_x_arg,
- unsigned int idx_y_arg,
- unsigned int idx_z_arg)
- {
- // generate key
- OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
-
- // check if key exist in octree
- deleteLeafRecursive (key, depth_mask_, root_node_);
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+bool
+OctreeBase<LeafContainerT, BranchContainerT>::existLeaf(unsigned int idx_x_arg,
+ unsigned int idx_y_arg,
+ unsigned int idx_z_arg) const
+{
+ // generate key
+ OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- void
- OctreeBase<LeafContainerT, BranchContainerT>::deleteTree ()
- {
-
- if (root_node_)
- {
- // reset octree
- deleteBranch (*root_node_);
- leaf_count_ = 0;
- branch_count_ = 1;
- }
+ // check if key exist in octree
+ return (existLeaf(key));
+}
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+OctreeBase<LeafContainerT, BranchContainerT>::removeLeaf(unsigned int idx_x_arg,
+ unsigned int idx_y_arg,
+ unsigned int idx_z_arg)
+{
+ // generate key
+ OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- void
- OctreeBase<LeafContainerT, BranchContainerT>::serializeTree (std::vector<char>& binary_tree_out_arg)
- {
+ // check if key exist in octree
+ deleteLeafRecursive(key, depth_mask_, root_node_);
+}
- OctreeKey new_key;
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+OctreeBase<LeafContainerT, BranchContainerT>::deleteTree()
+{
- // clear binary vector
- binary_tree_out_arg.clear ();
- binary_tree_out_arg.reserve (this->branch_count_);
+ if (root_node_) {
+ // reset octree
+ deleteBranch(*root_node_);
+ leaf_count_ = 0;
+ branch_count_ = 1;
+ }
+}
- serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, nullptr);
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+OctreeBase<LeafContainerT, BranchContainerT>::serializeTree(
+ std::vector<char>& binary_tree_out_arg)
+{
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- void
- OctreeBase<LeafContainerT, BranchContainerT>::serializeTree (std::vector<char>& binary_tree_out_arg,
- std::vector<LeafContainerT*>& leaf_container_vector_arg)
- {
+ OctreeKey new_key;
- OctreeKey new_key;
+ // clear binary vector
+ binary_tree_out_arg.clear();
+ binary_tree_out_arg.reserve(this->branch_count_);
- // clear output vectors
- binary_tree_out_arg.clear ();
- leaf_container_vector_arg.clear ();
+ serializeTreeRecursive(root_node_, new_key, &binary_tree_out_arg, nullptr);
+}
- binary_tree_out_arg.reserve (this->branch_count_);
- leaf_container_vector_arg.reserve (this->leaf_count_);
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+OctreeBase<LeafContainerT, BranchContainerT>::serializeTree(
+ std::vector<char>& binary_tree_out_arg,
+ std::vector<LeafContainerT*>& leaf_container_vector_arg)
+{
- serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg);
- }
+ OctreeKey new_key;
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- void
- OctreeBase<LeafContainerT, BranchContainerT>::serializeLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg)
- {
- OctreeKey new_key;
+ // clear output vectors
+ binary_tree_out_arg.clear();
+ leaf_container_vector_arg.clear();
- // clear output vector
- leaf_container_vector_arg.clear ();
+ binary_tree_out_arg.reserve(this->branch_count_);
+ leaf_container_vector_arg.reserve(this->leaf_count_);
- leaf_container_vector_arg.reserve (this->leaf_count_);
+ serializeTreeRecursive(
+ root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg);
+}
- serializeTreeRecursive (root_node_, new_key, nullptr, &leaf_container_vector_arg);
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+OctreeBase<LeafContainerT, BranchContainerT>::serializeLeafs(
+ std::vector<LeafContainerT*>& leaf_container_vector_arg)
+{
+ OctreeKey new_key;
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- void
- OctreeBase<LeafContainerT, BranchContainerT>::deserializeTree (std::vector<char>& binary_tree_out_arg)
- {
- OctreeKey new_key;
+ // clear output vector
+ leaf_container_vector_arg.clear();
- // free existing tree before tree rebuild
- deleteTree ();
+ leaf_container_vector_arg.reserve(this->leaf_count_);
- //iterator for binary tree structure vector
- std::vector<char>::const_iterator binary_tree_out_it = binary_tree_out_arg.begin ();
- std::vector<char>::const_iterator binary_tree_out_it_end = binary_tree_out_arg.end ();
+ serializeTreeRecursive(root_node_, new_key, nullptr, &leaf_container_vector_arg);
+}
- deserializeTreeRecursive (root_node_,
- depth_mask_,
- new_key,
- binary_tree_out_it,
- binary_tree_out_it_end,
- nullptr,
- nullptr);
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+OctreeBase<LeafContainerT, BranchContainerT>::deserializeTree(
+ std::vector<char>& binary_tree_out_arg)
+{
+ OctreeKey new_key;
+
+ // free existing tree before tree rebuild
+ deleteTree();
+
+ // iterator for binary tree structure vector
+ std::vector<char>::const_iterator binary_tree_out_it = binary_tree_out_arg.begin();
+ std::vector<char>::const_iterator binary_tree_out_it_end = binary_tree_out_arg.end();
+
+ deserializeTreeRecursive(root_node_,
+ depth_mask_,
+ new_key,
+ binary_tree_out_it,
+ binary_tree_out_it_end,
+ nullptr,
+ nullptr);
+}
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+OctreeBase<LeafContainerT, BranchContainerT>::deserializeTree(
+ std::vector<char>& binary_tree_in_arg,
+ std::vector<LeafContainerT*>& leaf_container_vector_arg)
+{
+ OctreeKey new_key;
+
+ // set data iterator to first element
+ typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it =
+ leaf_container_vector_arg.begin();
+
+ // set data iterator to last element
+ typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it_end =
+ leaf_container_vector_arg.end();
+
+ // free existing tree before tree rebuild
+ deleteTree();
+
+ // iterator for binary tree structure vector
+ std::vector<char>::const_iterator binary_tree_input_it = binary_tree_in_arg.begin();
+ std::vector<char>::const_iterator binary_tree_input_it_end = binary_tree_in_arg.end();
+
+ deserializeTreeRecursive(root_node_,
+ depth_mask_,
+ new_key,
+ binary_tree_input_it,
+ binary_tree_input_it_end,
+ &leaf_vector_it,
+ &leaf_vector_it_end);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+unsigned int
+OctreeBase<LeafContainerT, BranchContainerT>::createLeafRecursive(
+ const OctreeKey& key_arg,
+ unsigned int depth_mask_arg,
+ BranchNode* branch_arg,
+ LeafNode*& return_leaf_arg,
+ BranchNode*& parent_of_leaf_arg)
+{
+ // index to branch child
+ unsigned char child_idx;
+
+ // find branch child from key
+ child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
+
+ OctreeNode* child_node = (*branch_arg)[child_idx];
+
+ if (!child_node) {
+ if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1)) {
+ // if required branch does not exist -> create it
+ BranchNode* childBranch = createBranchChild(*branch_arg, child_idx);
+
+ branch_count_++;
+
+ // recursively proceed with indexed child branch
+ return createLeafRecursive(key_arg,
+ depth_mask_arg / 2,
+ childBranch,
+ return_leaf_arg,
+ parent_of_leaf_arg);
+ }
+ // if leaf node at child_idx does not exist
+ LeafNode* leaf_node = createLeafChild(*branch_arg, child_idx);
+ return_leaf_arg = leaf_node;
+ parent_of_leaf_arg = branch_arg;
+ this->leaf_count_++;
+ }
+ else {
+ // Node exists already
+ switch (child_node->getNodeType()) {
+ case BRANCH_NODE:
+ // recursively proceed with indexed child branch
+ return createLeafRecursive(key_arg,
+ depth_mask_arg / 2,
+ static_cast<BranchNode*>(child_node),
+ return_leaf_arg,
+ parent_of_leaf_arg);
+ break;
+
+ case LEAF_NODE:
+ return_leaf_arg = static_cast<LeafNode*>(child_node);
+ parent_of_leaf_arg = branch_arg;
+ break;
+ }
+ }
+
+ return (depth_mask_arg >> 1);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+OctreeBase<LeafContainerT, BranchContainerT>::findLeafRecursive(
+ const OctreeKey& key_arg,
+ unsigned int depth_mask_arg,
+ BranchNode* branch_arg,
+ LeafContainerT*& result_arg) const
+{
+ // index to branch child
+ unsigned char child_idx;
+
+ // find branch child from key
+ child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
+
+ OctreeNode* child_node = (*branch_arg)[child_idx];
+
+ if (child_node) {
+ switch (child_node->getNodeType()) {
+ case BRANCH_NODE:
+ // we have not reached maximum tree depth
+ BranchNode* child_branch;
+ child_branch = static_cast<BranchNode*>(child_node);
+
+ findLeafRecursive(key_arg, depth_mask_arg / 2, child_branch, result_arg);
+ break;
+
+ case LEAF_NODE:
+ // return existing leaf node
+ LeafNode* child_leaf;
+ child_leaf = static_cast<LeafNode*>(child_node);
+
+ result_arg = child_leaf->getContainerPtr();
+ break;
+ }
+ }
+}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- void
- OctreeBase<LeafContainerT, BranchContainerT>::deserializeTree (std::vector<char>& binary_tree_in_arg,
- std::vector<LeafContainerT*>& leaf_container_vector_arg)
- {
- OctreeKey new_key;
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+bool
+OctreeBase<LeafContainerT, BranchContainerT>::deleteLeafRecursive(
+ const OctreeKey& key_arg, unsigned int depth_mask_arg, BranchNode* branch_arg)
+{
+ // index to branch child
+ unsigned char child_idx;
+ // indicates if branch is empty and can be safely removed
+ bool b_no_children;
- // set data iterator to first element
- typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it = leaf_container_vector_arg.begin ();
+ // find branch child from key
+ child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
- // set data iterator to last element
- typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it_end = leaf_container_vector_arg.end ();
+ OctreeNode* child_node = (*branch_arg)[child_idx];
- // free existing tree before tree rebuild
- deleteTree ();
+ if (child_node) {
+ switch (child_node->getNodeType()) {
- //iterator for binary tree structure vector
- std::vector<char>::const_iterator binary_tree_input_it = binary_tree_in_arg.begin ();
- std::vector<char>::const_iterator binary_tree_input_it_end = binary_tree_in_arg.end ();
+ case BRANCH_NODE:
+ BranchNode* child_branch;
+ child_branch = static_cast<BranchNode*>(child_node);
- deserializeTreeRecursive (root_node_,
- depth_mask_,
- new_key,
- binary_tree_input_it,
- binary_tree_input_it_end,
- &leaf_vector_it,
- &leaf_vector_it_end);
+ // recursively explore the indexed child branch
+ b_no_children = deleteLeafRecursive(key_arg, depth_mask_arg / 2, child_branch);
+ if (!b_no_children) {
+ // child branch does not own any sub-child nodes anymore -> delete child branch
+ deleteBranchChild(*branch_arg, child_idx);
+ branch_count_--;
}
+ break;
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- unsigned int
- OctreeBase<LeafContainerT, BranchContainerT>::createLeafRecursive (const OctreeKey& key_arg,
- unsigned int depth_mask_arg,
- BranchNode* branch_arg,
- LeafNode*& return_leaf_arg,
- BranchNode*& parent_of_leaf_arg)
- {
- // index to branch child
- unsigned char child_idx;
+ case LEAF_NODE:
+ // return existing leaf node
- // find branch child from key
- child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg);
+ // our child is a leaf node -> delete it
+ deleteBranchChild(*branch_arg, child_idx);
+ this->leaf_count_--;
+ break;
+ }
+ }
- OctreeNode* child_node = (*branch_arg)[child_idx];
+ // check if current branch still owns children
+ b_no_children = false;
+ for (child_idx = 0; (!b_no_children) && (child_idx < 8); child_idx++) {
+ b_no_children = branch_arg->hasChild(child_idx);
+ }
+ // return true if current branch can be deleted
+ return (b_no_children);
+}
- if (!child_node)
- {
- if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1))
- {
- // if required branch does not exist -> create it
- BranchNode* childBranch = createBranchChild (*branch_arg, child_idx);
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+OctreeBase<LeafContainerT, BranchContainerT>::serializeTreeRecursive(
+ const BranchNode* branch_arg,
+ OctreeKey& key_arg,
+ std::vector<char>* binary_tree_out_arg,
+ typename std::vector<LeafContainerT*>* leaf_container_vector_arg) const
+{
+ char node_bit_pattern;
- branch_count_++;
+ // branch occupancy bit pattern
+ node_bit_pattern = getBranchBitPattern(*branch_arg);
- // recursively proceed with indexed child branch
- return createLeafRecursive (key_arg, depth_mask_arg / 2, childBranch, return_leaf_arg, parent_of_leaf_arg);
+ // write bit pattern to output vector
+ if (binary_tree_out_arg)
+ binary_tree_out_arg->push_back(node_bit_pattern);
- }
- // if leaf node at child_idx does not exist
- LeafNode* leaf_node = createLeafChild (*branch_arg, child_idx);
- return_leaf_arg = leaf_node;
- parent_of_leaf_arg = branch_arg;
- this->leaf_count_++;
- }
- else
- {
- // Node exists already
- switch (child_node->getNodeType ())
- {
- case BRANCH_NODE:
- // recursively proceed with indexed child branch
- return createLeafRecursive (key_arg, depth_mask_arg / 2, static_cast<BranchNode*> (child_node),
- return_leaf_arg, parent_of_leaf_arg);
- break;
-
- case LEAF_NODE:
- return_leaf_arg = static_cast<LeafNode*> (child_node);;
- parent_of_leaf_arg = branch_arg;
- break;
- }
+ // iterate over all children
+ for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
- }
+ // if child exist
+ if (branch_arg->hasChild(child_idx)) {
+ // add current branch voxel to key
+ key_arg.pushBranch(child_idx);
- return (depth_mask_arg >> 1);
- }
+ OctreeNode* childNode = branch_arg->getChildPtr(child_idx);
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- void
- OctreeBase<LeafContainerT, BranchContainerT>::findLeafRecursive (const OctreeKey& key_arg,
- unsigned int depth_mask_arg,
- BranchNode* branch_arg,
- LeafContainerT*& result_arg) const
- {
- // index to branch child
- unsigned char child_idx;
-
- // find branch child from key
- child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg);
-
- OctreeNode* child_node = (*branch_arg)[child_idx];
-
- if (child_node)
- {
- switch (child_node->getNodeType ())
- {
- case BRANCH_NODE:
- // we have not reached maximum tree depth
- BranchNode* child_branch;
- child_branch = static_cast<BranchNode*> (child_node);
-
- findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg);
- break;
-
- case LEAF_NODE:
- // return existing leaf node
- LeafNode* child_leaf;
- child_leaf = static_cast<LeafNode*> (child_node);
-
- result_arg = child_leaf->getContainerPtr ();
- break;
- }
- }
+ switch (childNode->getNodeType()) {
+ case BRANCH_NODE: {
+ // recursively proceed with indexed child branch
+ serializeTreeRecursive(static_cast<const BranchNode*>(childNode),
+ key_arg,
+ binary_tree_out_arg,
+ leaf_container_vector_arg);
+ break;
}
+ case LEAF_NODE: {
+ LeafNode* child_leaf = static_cast<LeafNode*>(childNode);
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- bool
- OctreeBase<LeafContainerT, BranchContainerT>::deleteLeafRecursive (const OctreeKey& key_arg,
- unsigned int depth_mask_arg,
- BranchNode* branch_arg)
- {
- // index to branch child
- unsigned char child_idx;
- // indicates if branch is empty and can be safely removed
- bool b_no_children;
-
- // find branch child from key
- child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg);
-
- OctreeNode* child_node = (*branch_arg)[child_idx];
-
- if (child_node)
- {
- switch (child_node->getNodeType ())
- {
-
- case BRANCH_NODE:
- BranchNode* child_branch;
- child_branch = static_cast<BranchNode*> (child_node);
-
- // recursively explore the indexed child branch
- b_no_children = deleteLeafRecursive (key_arg, depth_mask_arg / 2, child_branch);
-
- if (!b_no_children)
- {
- // child branch does not own any sub-child nodes anymore -> delete child branch
- deleteBranchChild (*branch_arg, child_idx);
- branch_count_--;
- }
- break;
-
- case LEAF_NODE:
- // return existing leaf node
-
- // our child is a leaf node -> delete it
- deleteBranchChild (*branch_arg, child_idx);
- this->leaf_count_--;
- break;
- }
- }
+ if (leaf_container_vector_arg)
+ leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
- // check if current branch still owns children
- b_no_children = false;
- for (child_idx = 0; (!b_no_children) && (child_idx < 8); child_idx++)
- {
- b_no_children = branch_arg->hasChild (child_idx);
- }
- // return true if current branch can be deleted
- return (b_no_children);
+ // we reached a leaf node -> execute serialization callback
+ serializeTreeCallback(**child_leaf, key_arg);
+ break;
+ }
+ default:
+ break;
}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- void
- OctreeBase<LeafContainerT, BranchContainerT>::serializeTreeRecursive (const BranchNode* branch_arg,
- OctreeKey& key_arg,
- std::vector<char>* binary_tree_out_arg,
- typename std::vector<LeafContainerT*>* leaf_container_vector_arg) const
- {
- char node_bit_pattern;
-
- // branch occupancy bit pattern
- node_bit_pattern = getBranchBitPattern (*branch_arg);
-
- // write bit pattern to output vector
- if (binary_tree_out_arg)
- binary_tree_out_arg->push_back (node_bit_pattern);
-
- // iterate over all children
- for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
- {
-
- // if child exist
- if (branch_arg->hasChild (child_idx))
- {
- // add current branch voxel to key
- key_arg.pushBranch (child_idx);
-
- OctreeNode *childNode = branch_arg->getChildPtr (child_idx);
-
- switch (childNode->getNodeType ())
- {
- case BRANCH_NODE:
- {
- // recursively proceed with indexed child branch
- serializeTreeRecursive (static_cast<const BranchNode*> (childNode), key_arg, binary_tree_out_arg,
- leaf_container_vector_arg);
- break;
- }
- case LEAF_NODE:
- {
- LeafNode* child_leaf = static_cast<LeafNode*> (childNode);
-
- if (leaf_container_vector_arg)
- leaf_container_vector_arg->push_back (child_leaf->getContainerPtr ());
-
- // we reached a leaf node -> execute serialization callback
- serializeTreeCallback (**child_leaf, key_arg);
- break;
- }
- default:
- break;
- }
-
- // pop current branch voxel from key
- key_arg.popBranch ();
- }
+ // pop current branch voxel from key
+ key_arg.popBranch();
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename LeafContainerT, typename BranchContainerT>
+void
+OctreeBase<LeafContainerT, BranchContainerT>::deserializeTreeRecursive(
+ BranchNode* branch_arg,
+ unsigned int depth_mask_arg,
+ OctreeKey& key_arg,
+ typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
+ typename std::vector<char>::const_iterator& binary_tree_input_it_end_arg,
+ typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg,
+ typename std::vector<LeafContainerT*>::const_iterator*
+ leaf_container_vector_it_end_arg)
+{
+
+ if (binary_tree_input_it_arg != binary_tree_input_it_end_arg) {
+ // read branch occupancy bit pattern from input vector
+ char node_bits = (*binary_tree_input_it_arg);
+ binary_tree_input_it_arg++;
+
+ // iterate over all children
+ for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
+ // if occupancy bit for child_idx is set..
+ if (node_bits & (1 << child_idx)) {
+ // add current branch voxel to key
+ key_arg.pushBranch(child_idx);
+
+ if (depth_mask_arg > 1) {
+ // we have not reached maximum tree depth
+ BranchNode* newBranch = createBranchChild(*branch_arg, child_idx);
+
+ branch_count_++;
+
+ // recursively proceed with new child branch
+ deserializeTreeRecursive(newBranch,
+ depth_mask_arg / 2,
+ key_arg,
+ binary_tree_input_it_arg,
+ binary_tree_input_it_end_arg,
+ leaf_container_vector_it_arg,
+ leaf_container_vector_it_end_arg);
}
- }
+ else {
+ // we reached leaf node level
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename LeafContainerT, typename BranchContainerT>
- void
- OctreeBase<LeafContainerT, BranchContainerT>::deserializeTreeRecursive (BranchNode* branch_arg, unsigned int depth_mask_arg, OctreeKey& key_arg,
- typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
- typename std::vector<char>::const_iterator& binary_tree_input_it_end_arg,
- typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg,
- typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_end_arg)
- {
- char node_bits;
-
- if (binary_tree_input_it_arg != binary_tree_input_it_end_arg)
- {
- // read branch occupancy bit pattern from input vector
- node_bits = (*binary_tree_input_it_arg);
- binary_tree_input_it_arg++;
-
- // iterate over all children
- for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
- {
- // if occupancy bit for child_idx is set..
- if (node_bits & (1 << child_idx))
- {
- // add current branch voxel to key
- key_arg.pushBranch (child_idx);
-
- if (depth_mask_arg > 1)
- {
- // we have not reached maximum tree depth
- BranchNode * newBranch = createBranchChild (*branch_arg, child_idx);
-
- branch_count_++;
-
- // recursively proceed with new child branch
- deserializeTreeRecursive (newBranch, depth_mask_arg / 2, key_arg,
- binary_tree_input_it_arg, binary_tree_input_it_end_arg,
- leaf_container_vector_it_arg, leaf_container_vector_it_end_arg);
- }
- else
- {
- // we reached leaf node level
-
- LeafNode* child_leaf = createLeafChild (*branch_arg, child_idx);
-
- if (leaf_container_vector_it_arg && (*leaf_container_vector_it_arg != *leaf_container_vector_it_end_arg))
- {
- LeafContainerT& container = **child_leaf;
- container = ***leaf_container_vector_it_arg;
- ++*leaf_container_vector_it_arg;
- }
-
- leaf_count_++;
-
- // execute deserialization callback
- deserializeTreeCallback (**child_leaf, key_arg);
- }
-
- // pop current branch voxel from key
- key_arg.popBranch ();
- }
+ LeafNode* child_leaf = createLeafChild(*branch_arg, child_idx);
+
+ if (leaf_container_vector_it_arg &&
+ (*leaf_container_vector_it_arg != *leaf_container_vector_it_end_arg)) {
+ LeafContainerT& container = **child_leaf;
+ container = ***leaf_container_vector_it_arg;
+ ++*leaf_container_vector_it_arg;
}
+
+ leaf_count_++;
+
+ // execute deserialization callback
+ deserializeTreeCallback(**child_leaf, key_arg);
}
- }
+ // pop current branch voxel from key
+ key_arg.popBranch();
+ }
+ }
}
}
-#define PCL_INSTANTIATE_OctreeBase(T) template class PCL_EXPORTS pcl::octree::OctreeBase<T>;
+} // namespace octree
+} // namespace pcl
-#endif
+#define PCL_INSTANTIATE_OctreeBase(T) \
+ template class PCL_EXPORTS pcl::octree::OctreeBase<T>;
+#endif
#include <pcl/console/print.h>
-namespace pcl
+namespace pcl {
+namespace octree {
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+OctreeDepthFirstIterator<OctreeT>::OctreeDepthFirstIterator(unsigned int max_depth_arg)
+: OctreeIteratorBase<OctreeT>(max_depth_arg), stack_()
{
- namespace octree
- {
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- OctreeDepthFirstIterator<OctreeT>::OctreeDepthFirstIterator (unsigned int max_depth_arg) :
- OctreeIteratorBase<OctreeT> (max_depth_arg), stack_ ()
- {
- // initialize iterator
- this->reset ();
- }
+ // initialize iterator
+ this->reset();
+}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- OctreeDepthFirstIterator<OctreeT>::OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) :
- OctreeIteratorBase<OctreeT> (octree_arg, max_depth_arg), stack_ ()
- {
- // initialize iterator
- this->reset ();
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+OctreeDepthFirstIterator<OctreeT>::OctreeDepthFirstIterator(OctreeT* octree_arg,
+ unsigned int max_depth_arg)
+: OctreeIteratorBase<OctreeT>(octree_arg, max_depth_arg), stack_()
+{
+ // initialize iterator
+ this->reset();
+}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- void OctreeDepthFirstIterator<OctreeT>::reset ()
- {
- OctreeIteratorBase<OctreeT>::reset ();
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+void
+OctreeDepthFirstIterator<OctreeT>::reset()
+{
+ OctreeIteratorBase<OctreeT>::reset();
- if (this->octree_)
- {
- // allocate stack
- stack_.reserve (this->max_octree_depth_);
+ if (this->octree_) {
+ // allocate stack
+ stack_.reserve(this->max_octree_depth_);
- // empty stack
- stack_.clear ();
+ // empty stack
+ stack_.clear();
- // pushing root node to stack
- IteratorState stack_entry;
- stack_entry.node_ = this->octree_->getRootNode ();
- stack_entry.depth_ = 0;
- stack_entry.key_.x = stack_entry.key_.y = stack_entry.key_.z = 0;
+ // pushing root node to stack
+ IteratorState stack_entry;
+ stack_entry.node_ = this->octree_->getRootNode();
+ stack_entry.depth_ = 0;
+ stack_entry.key_.x = stack_entry.key_.y = stack_entry.key_.z = 0;
- stack_.push_back(stack_entry);
+ stack_.push_back(stack_entry);
- this->current_state_ = &stack_.back();
- }
+ this->current_state_ = &stack_.back();
+ }
+}
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+void
+OctreeDepthFirstIterator<OctreeT>::skipChildVoxels()
+{
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- void OctreeDepthFirstIterator<OctreeT>::skipChildVoxels ()
- {
-
- if (stack_.size ())
- {
- // current depth
- unsigned char current_depth = stack_.back ().depth_;
-
- // pop from stack as long as we find stack elements with depth >= current depth
- while (stack_.size () && (stack_.back ().depth_ >= current_depth))
- stack_.pop_back ();
-
- if (stack_.size ())
- {
- this->current_state_ = &stack_.back();
- } else
- {
- this->current_state_ = 0;
- }
- }
+ if (stack_.size()) {
+ // current depth
+ unsigned char current_depth = stack_.back().depth_;
+
+ // pop from stack as long as we find stack elements with depth >= current depth
+ while (stack_.size() && (stack_.back().depth_ >= current_depth))
+ stack_.pop_back();
+ if (stack_.size()) {
+ this->current_state_ = &stack_.back();
}
+ else {
+ this->current_state_ = 0;
+ }
+ }
+}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- OctreeDepthFirstIterator<OctreeT>&
- OctreeDepthFirstIterator<OctreeT>::operator++ ()
- {
-
- if (stack_.size ())
- {
- // get stack element
- IteratorState stack_entry = stack_.back ();
- stack_.pop_back ();
-
- stack_entry.depth_ ++;
- OctreeKey& current_key = stack_entry.key_;
-
- if ( (this->max_octree_depth_>=stack_entry.depth_) &&
- (stack_entry.node_->getNodeType () == BRANCH_NODE) )
- {
- // current node is a branch node
- BranchNode* current_branch =
- static_cast<BranchNode*> (stack_entry.node_);
-
- // add all children to stack
- for (std::int8_t i = 7; i >= 0; --i)
- {
- const unsigned char child_idx = (unsigned char) i;
-
- // if child exist
- if (this->octree_->branchHasChild(*current_branch, child_idx))
- {
- // add child to stack
- current_key.pushBranch (child_idx);
-
- stack_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx);
-
- stack_.push_back(stack_entry);
-
- current_key.popBranch();
- }
- }
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+OctreeDepthFirstIterator<OctreeT>&
+OctreeDepthFirstIterator<OctreeT>::operator++()
+{
+
+ if (stack_.size()) {
+ // get stack element
+ IteratorState stack_entry = stack_.back();
+ stack_.pop_back();
+
+ stack_entry.depth_++;
- if (stack_.size ())
- {
- this->current_state_ = &stack_.back();
- } else
- {
- this->current_state_ = 0;
+ if ((this->max_octree_depth_ >= stack_entry.depth_) &&
+ (stack_entry.node_->getNodeType() == BRANCH_NODE)) {
+ // current node is a branch node
+ BranchNode* current_branch = static_cast<BranchNode*>(stack_entry.node_);
+
+ OctreeKey& current_key = stack_entry.key_;
+
+ // add all children to stack
+ for (std::int8_t i = 7; i >= 0; --i) {
+ const unsigned char child_idx = (unsigned char)i;
+
+ // if child exist
+ if (this->octree_->branchHasChild(*current_branch, child_idx)) {
+ // add child to stack
+ current_key.pushBranch(child_idx);
+
+ stack_entry.node_ =
+ this->octree_->getBranchChildPtr(*current_branch, child_idx);
+
+ stack_.push_back(stack_entry);
+
+ current_key.popBranch();
}
}
-
- return (*this);
}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator (unsigned int max_depth_arg) :
- OctreeIteratorBase<OctreeT> (max_depth_arg), FIFO_ ()
- {
- OctreeIteratorBase<OctreeT>::reset ();
-
- // initialize iterator
- this->reset ();
+ if (stack_.size()) {
+ this->current_state_ = &stack_.back();
+ }
+ else {
+ this->current_state_ = 0;
}
+ }
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) :
- OctreeIteratorBase<OctreeT> (octree_arg, max_depth_arg), FIFO_ ()
- {
- OctreeIteratorBase<OctreeT>::reset ();
+ return (*this);
+}
- // initialize iterator
- this->reset ();
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator(
+ unsigned int max_depth_arg)
+: OctreeIteratorBase<OctreeT>(max_depth_arg), FIFO_()
+{
+ OctreeIteratorBase<OctreeT>::reset();
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- void OctreeBreadthFirstIterator<OctreeT>::reset ()
- {
- OctreeIteratorBase<OctreeT>::reset ();
+ // initialize iterator
+ this->reset();
+}
- // init FIFO
- FIFO_.clear ();
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator(
+ OctreeT* octree_arg, unsigned int max_depth_arg)
+: OctreeIteratorBase<OctreeT>(octree_arg, max_depth_arg), FIFO_()
+{
+ OctreeIteratorBase<OctreeT>::reset();
- if (this->octree_)
- {
- // pushing root node to stack
- IteratorState FIFO_entry;
- FIFO_entry.node_ = this->octree_->getRootNode ();
- FIFO_entry.depth_ = 0;
- FIFO_entry.key_.x = FIFO_entry.key_.y = FIFO_entry.key_.z = 0;
+ // initialize iterator
+ this->reset();
+}
- FIFO_.push_back(FIFO_entry);
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+void
+OctreeBreadthFirstIterator<OctreeT>::reset()
+{
+ OctreeIteratorBase<OctreeT>::reset();
- this->current_state_ = &FIFO_.front();
- }
- }
+ // init FIFO
+ FIFO_.clear();
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- OctreeBreadthFirstIterator<OctreeT>&
- OctreeBreadthFirstIterator<OctreeT>::operator++ ()
- {
-
- if (FIFO_.size ())
- {
- // get stack element
- IteratorState FIFO_entry = FIFO_.front ();
- FIFO_.pop_front ();
-
- FIFO_entry.depth_ ++;
- OctreeKey& current_key = FIFO_entry.key_;
-
- if ( (this->max_octree_depth_>=FIFO_entry.depth_) &&
- (FIFO_entry.node_->getNodeType () == BRANCH_NODE) )
- {
- // current node is a branch node
- BranchNode* current_branch =
- static_cast<BranchNode*> (FIFO_entry.node_);
-
- // iterate over all children
- for (unsigned char child_idx = 0; child_idx < 8 ; ++child_idx)
- {
-
- // if child exist
- if (this->octree_->branchHasChild(*current_branch, child_idx))
- {
- // add child to stack
- current_key.pushBranch (static_cast<unsigned char> (child_idx));
-
- FIFO_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx);
-
- FIFO_.push_back(FIFO_entry);
-
- current_key.popBranch();
- }
- }
- }
+ if (this->octree_) {
+ // pushing root node to stack
+ IteratorState FIFO_entry;
+ FIFO_entry.node_ = this->octree_->getRootNode();
+ FIFO_entry.depth_ = 0;
+ FIFO_entry.key_.x = FIFO_entry.key_.y = FIFO_entry.key_.z = 0;
- if (FIFO_.size ())
- {
- this->current_state_ = &FIFO_.front();
- } else
- {
- this->current_state_ = 0;
- }
+ FIFO_.push_back(FIFO_entry);
- }
+ this->current_state_ = &FIFO_.front();
+ }
+}
- return (*this);
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+OctreeBreadthFirstIterator<OctreeT>&
+OctreeBreadthFirstIterator<OctreeT>::operator++()
+{
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- OctreeFixedDepthIterator<OctreeT>::OctreeFixedDepthIterator () :
- OctreeBreadthFirstIterator<OctreeT> (0u), fixed_depth_ (0u)
- {}
-
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- OctreeFixedDepthIterator<OctreeT>::OctreeFixedDepthIterator (OctreeT* octree_arg, unsigned int fixed_depth_arg) :
- OctreeBreadthFirstIterator<OctreeT> (octree_arg, fixed_depth_arg), fixed_depth_ (fixed_depth_arg)
- {
- this->reset (fixed_depth_arg);
- }
+ if (FIFO_.size()) {
+ // get stack element
+ IteratorState FIFO_entry = FIFO_.front();
+ FIFO_.pop_front();
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- void OctreeFixedDepthIterator<OctreeT>::reset (unsigned int fixed_depth_arg)
- {
- // Set the desired depth to walk through
- fixed_depth_ = fixed_depth_arg;
+ FIFO_entry.depth_++;
- if (!this->octree_)
- {
- return;
- }
+ if ((this->max_octree_depth_ >= FIFO_entry.depth_) &&
+ (FIFO_entry.node_->getNodeType() == BRANCH_NODE)) {
+ // current node is a branch node
+ BranchNode* current_branch = static_cast<BranchNode*>(FIFO_entry.node_);
- // If I'm nowhere, reset
- // If I'm somewhere and I can't guarantee I'm before the first node, reset
- if ((!this->current_state_) || (fixed_depth_ <= this->getCurrentOctreeDepth ()))
- OctreeBreadthFirstIterator<OctreeT>::reset ();
+ // iterate over all children
+ for (unsigned char child_idx = 0; child_idx < 8; ++child_idx) {
- if (this->octree_->getTreeDepth () < fixed_depth_)
- {
- PCL_WARN ("[pcl::octree::FixedDepthIterator] The requested fixed depth was bigger than the octree's depth.\n");
- PCL_WARN ("[pcl::octree::FixedDepthIterator] fixed_depth = %d (instead of %d)\n", this->octree_->getTreeDepth (), fixed_depth_);
- }
+ // if child exist
+ if (this->octree_->branchHasChild(*current_branch, child_idx)) {
+ // add child to stack
+ OctreeKey& current_key = FIFO_entry.key_;
+ current_key.pushBranch(static_cast<unsigned char>(child_idx));
- // By default for the parent class OctreeBreadthFirstIterator, if the
- // depth argument is equal to 0, the iterator would run over every node.
- // For the OctreeFixedDepthIterator, whatever the depth ask, set the
- // max_octree_depth_ accordingly
- this->max_octree_depth_ = std::min (fixed_depth_, this->octree_->getTreeDepth ());
+ FIFO_entry.node_ =
+ this->octree_->getBranchChildPtr(*current_branch, child_idx);
- // Restore previous state in case breath first iterator had child nodes already set up
- if (FIFO_.size ())
- this->current_state_ = &FIFO_.front ();
+ FIFO_.push_back(FIFO_entry);
- // Iterate all the way to the desired level
- while (this->current_state_ && (this->getCurrentOctreeDepth () != fixed_depth_))
- OctreeBreadthFirstIterator<OctreeT>::operator++ ();
+ current_key.popBranch();
+ }
+ }
}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- OctreeLeafNodeBreadthFirstIterator<OctreeT>::OctreeLeafNodeBreadthFirstIterator (unsigned int max_depth_arg) :
- OctreeBreadthFirstIterator<OctreeT> (max_depth_arg)
- {
- reset ();
+ if (FIFO_.size()) {
+ this->current_state_ = &FIFO_.front();
}
-
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- OctreeLeafNodeBreadthFirstIterator<OctreeT>::OctreeLeafNodeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) :
- OctreeBreadthFirstIterator<OctreeT> (octree_arg, max_depth_arg)
- {
- reset ();
+ else {
+ this->current_state_ = 0;
}
+ }
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- OctreeLeafNodeBreadthFirstIterator<OctreeT>::OctreeLeafNodeBreadthFirstIterator (OctreeT* octree_arg,
- unsigned int max_depth_arg,
- IteratorState* current_state,
- const std::deque<IteratorState>& fifo)
- : OctreeBreadthFirstIterator<OctreeT> (octree_arg,
- max_depth_arg,
- current_state,
- fifo)
- {}
-
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- void OctreeLeafNodeBreadthFirstIterator<OctreeT>::reset ()
- {
- OctreeBreadthFirstIterator<OctreeT>::reset ();
- ++*this;
- }
+ return (*this);
+}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- OctreeLeafNodeBreadthFirstIterator<OctreeT>&
- OctreeLeafNodeBreadthFirstIterator<OctreeT>::operator++ ()
- {
- do
- {
- OctreeBreadthFirstIterator<OctreeT>::operator++ ();
- } while ((this->current_state_) && (this->current_state_->node_->getNodeType () != LEAF_NODE));
-
- return (*this);
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+OctreeFixedDepthIterator<OctreeT>::OctreeFixedDepthIterator()
+: OctreeBreadthFirstIterator<OctreeT>(0u), fixed_depth_(0u)
+{}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+OctreeFixedDepthIterator<OctreeT>::OctreeFixedDepthIterator(
+ OctreeT* octree_arg, unsigned int fixed_depth_arg)
+: OctreeBreadthFirstIterator<OctreeT>(octree_arg, fixed_depth_arg)
+, fixed_depth_(fixed_depth_arg)
+{
+ this->reset(fixed_depth_arg);
+}
- //////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- OctreeLeafNodeBreadthFirstIterator<OctreeT>
- OctreeLeafNodeBreadthFirstIterator<OctreeT>::operator++ (int)
- {
- OctreeLeafNodeBreadthFirstIterator _Tmp = *this;
- ++*this;
- return (_Tmp);
- }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+void
+OctreeFixedDepthIterator<OctreeT>::reset(unsigned int fixed_depth_arg)
+{
+ // Set the desired depth to walk through
+ fixed_depth_ = fixed_depth_arg;
+
+ if (!this->octree_) {
+ return;
}
+
+ // If I'm nowhere, reset
+ // If I'm somewhere and I can't guarantee I'm before the first node, reset
+ if ((!this->current_state_) || (fixed_depth_ <= this->getCurrentOctreeDepth()))
+ OctreeBreadthFirstIterator<OctreeT>::reset();
+
+ if (this->octree_->getTreeDepth() < fixed_depth_) {
+ PCL_WARN("[pcl::octree::FixedDepthIterator] The requested fixed depth was bigger "
+ "than the octree's depth.\n");
+ PCL_WARN("[pcl::octree::FixedDepthIterator] fixed_depth = %d (instead of %d)\n",
+ this->octree_->getTreeDepth(),
+ fixed_depth_);
+ }
+
+ // By default for the parent class OctreeBreadthFirstIterator, if the
+ // depth argument is equal to 0, the iterator would run over every node.
+ // For the OctreeFixedDepthIterator, whatever the depth ask, set the
+ // max_octree_depth_ accordingly
+ this->max_octree_depth_ = std::min(fixed_depth_, this->octree_->getTreeDepth());
+
+ // Restore previous state in case breath first iterator had child nodes already set up
+ if (FIFO_.size())
+ this->current_state_ = &FIFO_.front();
+
+ // Iterate all the way to the desired level
+ while (this->current_state_ && (this->getCurrentOctreeDepth() != fixed_depth_))
+ OctreeBreadthFirstIterator<OctreeT>::operator++();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+OctreeLeafNodeBreadthFirstIterator<OctreeT>::OctreeLeafNodeBreadthFirstIterator(
+ unsigned int max_depth_arg)
+: OctreeBreadthFirstIterator<OctreeT>(max_depth_arg)
+{
+ reset();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+OctreeLeafNodeBreadthFirstIterator<OctreeT>::OctreeLeafNodeBreadthFirstIterator(
+ OctreeT* octree_arg, unsigned int max_depth_arg)
+: OctreeBreadthFirstIterator<OctreeT>(octree_arg, max_depth_arg)
+{
+ reset();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+OctreeLeafNodeBreadthFirstIterator<OctreeT>::OctreeLeafNodeBreadthFirstIterator(
+ OctreeT* octree_arg,
+ unsigned int max_depth_arg,
+ IteratorState* current_state,
+ const std::deque<IteratorState>& fifo)
+: OctreeBreadthFirstIterator<OctreeT>(octree_arg, max_depth_arg, current_state, fifo)
+{}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+void
+OctreeLeafNodeBreadthFirstIterator<OctreeT>::reset()
+{
+ OctreeBreadthFirstIterator<OctreeT>::reset();
+ ++*this;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+OctreeLeafNodeBreadthFirstIterator<OctreeT>&
+OctreeLeafNodeBreadthFirstIterator<OctreeT>::operator++()
+{
+ do {
+ OctreeBreadthFirstIterator<OctreeT>::operator++();
+ } while ((this->current_state_) &&
+ (this->current_state_->node_->getNodeType() != LEAF_NODE));
+
+ return (*this);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+OctreeLeafNodeBreadthFirstIterator<OctreeT>
+OctreeLeafNodeBreadthFirstIterator<OctreeT>::operator++(int)
+{
+ OctreeLeafNodeBreadthFirstIterator _Tmp = *this;
+ ++*this;
+ return (_Tmp);
}
+} // namespace octree
+} // namespace pcl
#endif
#include <pcl/octree/impl/octree_base.hpp>
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT>
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::OctreePointCloud (const double resolution) :
- OctreeT (), input_ (PointCloudConstPtr ()), indices_ (IndicesConstPtr ()),
- epsilon_ (0), resolution_ (resolution), min_x_ (0.0f), max_x_ (resolution), min_y_ (0.0f),
- max_y_ (resolution), min_z_ (0.0f), max_z_ (resolution), bounding_box_defined_ (false), max_objs_per_leaf_(0)
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ OctreePointCloud(const double resolution)
+: OctreeT()
+, input_(PointCloudConstPtr())
+, indices_(IndicesConstPtr())
+, epsilon_(0)
+, resolution_(resolution)
+, min_x_(0.0f)
+, max_x_(resolution)
+, min_y_(0.0f)
+, max_y_(resolution)
+, min_z_(0.0f)
+, max_z_(resolution)
+, bounding_box_defined_(false)
+, max_objs_per_leaf_(0)
{
- assert (resolution > 0.0f);
+ assert(resolution > 0.0f);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::addPointsFromInputCloud ()
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ addPointsFromInputCloud()
{
- if (indices_)
- {
- for (const int &index : *indices_)
- {
- assert( (index >= 0) && (index < static_cast<int> (input_->points.size ())));
-
- if (isFinite (input_->points[index]))
- {
+ if (indices_) {
+ for (const int& index : *indices_) {
+ assert((index >= 0) && (index < static_cast<int>(input_->points.size())));
+
+ if (isFinite(input_->points[index])) {
// add points to octree
- this->addPointIdx (index);
+ this->addPointIdx(index);
}
}
}
- else
- {
- for (std::size_t i = 0; i < input_->points.size (); i++)
- {
- if (isFinite (input_->points[i]))
- {
+ else {
+ for (std::size_t i = 0; i < input_->points.size(); i++) {
+ if (isFinite(input_->points[i])) {
// add points to octree
- this->addPointIdx (static_cast<unsigned int> (i));
+ this->addPointIdx(static_cast<unsigned int>(i));
}
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::addPointFromCloud (const int point_idx_arg, IndicesPtr indices_arg)
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg)
{
- this->addPointIdx (point_idx_arg);
+ this->addPointIdx(point_idx_arg);
if (indices_arg)
- indices_arg->push_back (point_idx_arg);
+ indices_arg->push_back(point_idx_arg);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg)
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ addPointToCloud(const PointT& point_arg, PointCloudPtr cloud_arg)
{
- assert (cloud_arg==input_);
+ assert(cloud_arg == input_);
- cloud_arg->push_back (point_arg);
+ cloud_arg->push_back(point_arg);
- this->addPointIdx (static_cast<const int> (cloud_arg->points.size ()) - 1);
+ this->addPointIdx(static_cast<const int>(cloud_arg->points.size()) - 1);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg,
- IndicesPtr indices_arg)
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ addPointToCloud(const PointT& point_arg,
+ PointCloudPtr cloud_arg,
+ IndicesPtr indices_arg)
{
- assert (cloud_arg==input_);
- assert (indices_arg==indices_);
+ assert(cloud_arg == input_);
+ assert(indices_arg == indices_);
- cloud_arg->push_back (point_arg);
+ cloud_arg->push_back(point_arg);
- this->addPointFromCloud (static_cast<const int> (cloud_arg->points.size ()) - 1, indices_arg);
+ this->addPointFromCloud(static_cast<const int>(cloud_arg->points.size()) - 1,
+ indices_arg);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::isVoxelOccupiedAtPoint (const PointT& point_arg) const
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+bool
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ isVoxelOccupiedAtPoint(const PointT& point_arg) const
{
- if (!isPointWithinBoundingBox (point_arg))
- {
+ if (!isPointWithinBoundingBox(point_arg)) {
return false;
}
OctreeKey key;
// generate key for point
- this->genOctreeKeyforPoint (point_arg, key);
+ this->genOctreeKeyforPoint(point_arg, key);
// search for key in octree
- return (this->existLeaf (key));
+ return (this->existLeaf(key));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::isVoxelOccupiedAtPoint (const int& point_idx_arg) const
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+bool
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ isVoxelOccupiedAtPoint(const int& point_idx_arg) const
{
// retrieve point from input cloud
const PointT& point = this->input_->points[point_idx_arg];
// search for voxel at point in octree
- return (this->isVoxelOccupiedAtPoint (point));
+ return (this->isVoxelOccupiedAtPoint(point));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::isVoxelOccupiedAtPoint (
- const double point_x_arg, const double point_y_arg, const double point_z_arg) const
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+bool
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ isVoxelOccupiedAtPoint(const double point_x_arg,
+ const double point_y_arg,
+ const double point_z_arg) const
{
// create a new point with the argument coordinates
PointT point;
point.z = point_z_arg;
// search for voxel at point in octree
- return (this->isVoxelOccupiedAtPoint (point));
+ return (this->isVoxelOccupiedAtPoint(point));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::deleteVoxelAtPoint (const PointT& point_arg)
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ deleteVoxelAtPoint(const PointT& point_arg)
{
- if (!isPointWithinBoundingBox (point_arg))
- {
+ if (!isPointWithinBoundingBox(point_arg)) {
return;
}
OctreeKey key;
// generate key for point
- this->genOctreeKeyforPoint (point_arg, key);
+ this->genOctreeKeyforPoint(point_arg, key);
- this->removeLeaf (key);
+ this->removeLeaf(key);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::deleteVoxelAtPoint (const int& point_idx_arg)
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ deleteVoxelAtPoint(const int& point_idx_arg)
{
// retrieve point from input cloud
const PointT& point = this->input_->points[point_idx_arg];
// delete leaf at point
- this->deleteVoxelAtPoint (point);
+ this->deleteVoxelAtPoint(point);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> int
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::getOccupiedVoxelCenters (
- AlignedPointTVector &voxel_center_list_arg) const
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+int
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ getOccupiedVoxelCenters(AlignedPointTVector& voxel_center_list_arg) const
{
OctreeKey key;
key.x = key.y = key.z = 0;
- voxel_center_list_arg.clear ();
-
- return getOccupiedVoxelCentersRecursive (this->root_node_, key, voxel_center_list_arg);
+ voxel_center_list_arg.clear();
+ return getOccupiedVoxelCentersRecursive(this->root_node_, key, voxel_center_list_arg);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> int
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::getApproxIntersectedVoxelCentersBySegment (
- const Eigen::Vector3f& origin,
- const Eigen::Vector3f& end,
- AlignedPointTVector &voxel_center_list,
- float precision)
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+int
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f& origin,
+ const Eigen::Vector3f& end,
+ AlignedPointTVector& voxel_center_list,
+ float precision)
{
Eigen::Vector3f direction = end - origin;
- float norm = direction.norm ();
- direction.normalize ();
+ float norm = direction.norm();
+ direction.normalize();
- const float step_size = static_cast<float> (resolution_) * precision;
+ const float step_size = static_cast<float>(resolution_) * precision;
// Ensure we get at least one step for the first voxel.
- const int nsteps = std::max (1, static_cast<int> (norm / step_size));
+ const int nsteps = std::max(1, static_cast<int>(norm / step_size));
OctreeKey prev_key;
bool bkeyDefined = false;
// Walk along the line segment with small steps.
- for (int i = 0; i < nsteps; ++i)
- {
- Eigen::Vector3f p = origin + (direction * step_size * static_cast<float> (i));
+ for (int i = 0; i < nsteps; ++i) {
+ Eigen::Vector3f p = origin + (direction * step_size * static_cast<float>(i));
PointT octree_p;
- octree_p.x = p.x ();
- octree_p.y = p.y ();
- octree_p.z = p.z ();
+ octree_p.x = p.x();
+ octree_p.y = p.y();
+ octree_p.z = p.z();
OctreeKey key;
- this->genOctreeKeyforPoint (octree_p, key);
+ this->genOctreeKeyforPoint(octree_p, key);
// Not a new key, still the same voxel.
- if ((key == prev_key) && (bkeyDefined) )
+ if ((key == prev_key) && (bkeyDefined))
continue;
prev_key = key;
bkeyDefined = true;
PointT center;
- genLeafNodeCenterFromOctreeKey (key, center);
- voxel_center_list.push_back (center);
+ genLeafNodeCenterFromOctreeKey(key, center);
+ voxel_center_list.push_back(center);
}
OctreeKey end_key;
PointT end_p;
- end_p.x = end.x ();
- end_p.y = end.y ();
- end_p.z = end.z ();
- this->genOctreeKeyforPoint (end_p, end_key);
- if (!(end_key == prev_key))
- {
+ end_p.x = end.x();
+ end_p.y = end.y();
+ end_p.z = end.z();
+ this->genOctreeKeyforPoint(end_p, end_key);
+ if (!(end_key == prev_key)) {
PointT center;
- genLeafNodeCenterFromOctreeKey (end_key, center);
- voxel_center_list.push_back (center);
+ genLeafNodeCenterFromOctreeKey(end_key, center);
+ voxel_center_list.push_back(center);
}
- return (static_cast<int> (voxel_center_list.size ()));
+ return (static_cast<int>(voxel_center_list.size()));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::defineBoundingBox ()
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ defineBoundingBox()
{
double minX, minY, minZ, maxX, maxY, maxZ;
PointT max_pt;
// bounding box cannot be changed once the octree contains elements
- assert (this->leaf_count_ == 0);
+ assert(this->leaf_count_ == 0);
- pcl::getMinMax3D (*input_, min_pt, max_pt);
+ pcl::getMinMax3D(*input_, min_pt, max_pt);
- float minValue = std::numeric_limits<float>::epsilon () * 512.0f;
+ float minValue = std::numeric_limits<float>::epsilon() * 512.0f;
minX = min_pt.x;
minY = min_pt.y;
maxZ = max_pt.z + minValue;
// generate bit masks for octree
- defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
+ defineBoundingBox(minX, minY, minZ, maxX, maxY, maxZ);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::defineBoundingBox (const double min_x_arg,
- const double min_y_arg,
- const double min_z_arg,
- const double max_x_arg,
- const double max_y_arg,
- const double max_z_arg)
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ defineBoundingBox(const double min_x_arg,
+ const double min_y_arg,
+ const double min_z_arg,
+ const double max_x_arg,
+ const double max_y_arg,
+ const double max_z_arg)
{
// bounding box cannot be changed once the octree contains elements
- assert (this->leaf_count_ == 0);
+ assert(this->leaf_count_ == 0);
- assert (max_x_arg >= min_x_arg);
- assert (max_y_arg >= min_y_arg);
- assert (max_z_arg >= min_z_arg);
+ assert(max_x_arg >= min_x_arg);
+ assert(max_y_arg >= min_y_arg);
+ assert(max_z_arg >= min_z_arg);
min_x_ = min_x_arg;
max_x_ = max_x_arg;
min_z_ = min_z_arg;
max_z_ = max_z_arg;
- min_x_ = std::min (min_x_, max_x_);
- min_y_ = std::min (min_y_, max_y_);
- min_z_ = std::min (min_z_, max_z_);
+ min_x_ = std::min(min_x_, max_x_);
+ min_y_ = std::min(min_y_, max_y_);
+ min_z_ = std::min(min_z_, max_z_);
- max_x_ = std::max (min_x_, max_x_);
- max_y_ = std::max (min_y_, max_y_);
- max_z_ = std::max (min_z_, max_z_);
+ max_x_ = std::max(min_x_, max_x_);
+ max_y_ = std::max(min_y_, max_y_);
+ max_z_ = std::max(min_z_, max_z_);
// generate bit masks for octree
- getKeyBitSize ();
+ getKeyBitSize();
bounding_box_defined_ = true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::defineBoundingBox (
- const double max_x_arg, const double max_y_arg, const double max_z_arg)
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ defineBoundingBox(const double max_x_arg,
+ const double max_y_arg,
+ const double max_z_arg)
{
// bounding box cannot be changed once the octree contains elements
- assert (this->leaf_count_ == 0);
+ assert(this->leaf_count_ == 0);
- assert (max_x_arg >= 0.0f);
- assert (max_y_arg >= 0.0f);
- assert (max_z_arg >= 0.0f);
+ assert(max_x_arg >= 0.0f);
+ assert(max_y_arg >= 0.0f);
+ assert(max_z_arg >= 0.0f);
min_x_ = 0.0f;
max_x_ = max_x_arg;
min_z_ = 0.0f;
max_z_ = max_z_arg;
- min_x_ = std::min (min_x_, max_x_);
- min_y_ = std::min (min_y_, max_y_);
- min_z_ = std::min (min_z_, max_z_);
+ min_x_ = std::min(min_x_, max_x_);
+ min_y_ = std::min(min_y_, max_y_);
+ min_z_ = std::min(min_z_, max_z_);
- max_x_ = std::max (min_x_, max_x_);
- max_y_ = std::max (min_y_, max_y_);
- max_z_ = std::max (min_z_, max_z_);
+ max_x_ = std::max(min_x_, max_x_);
+ max_y_ = std::max(min_y_, max_y_);
+ max_z_ = std::max(min_z_, max_z_);
// generate bit masks for octree
- getKeyBitSize ();
+ getKeyBitSize();
bounding_box_defined_ = true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::defineBoundingBox (const double cubeLen_arg)
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ defineBoundingBox(const double cubeLen_arg)
{
// bounding box cannot be changed once the octree contains elements
- assert (this->leaf_count_ == 0);
+ assert(this->leaf_count_ == 0);
- assert (cubeLen_arg >= 0.0f);
+ assert(cubeLen_arg >= 0.0f);
min_x_ = 0.0f;
max_x_ = cubeLen_arg;
min_z_ = 0.0f;
max_z_ = cubeLen_arg;
- min_x_ = std::min (min_x_, max_x_);
- min_y_ = std::min (min_y_, max_y_);
- min_z_ = std::min (min_z_, max_z_);
+ min_x_ = std::min(min_x_, max_x_);
+ min_y_ = std::min(min_y_, max_y_);
+ min_z_ = std::min(min_z_, max_z_);
- max_x_ = std::max (min_x_, max_x_);
- max_y_ = std::max (min_y_, max_y_);
- max_z_ = std::max (min_z_, max_z_);
+ max_x_ = std::max(min_x_, max_x_);
+ max_y_ = std::max(min_y_, max_y_);
+ max_z_ = std::max(min_z_, max_z_);
// generate bit masks for octree
- getKeyBitSize ();
+ getKeyBitSize();
bounding_box_defined_ = true;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::getBoundingBox (
- double& min_x_arg, double& min_y_arg, double& min_z_arg,
- double& max_x_arg, double& max_y_arg, double& max_z_arg) const
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ getBoundingBox(double& min_x_arg,
+ double& min_y_arg,
+ double& min_z_arg,
+ double& max_x_arg,
+ double& max_y_arg,
+ double& max_z_arg) const
{
min_x_arg = min_x_;
min_y_arg = min_y_;
max_z_arg = max_z_;
}
-
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT>
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::adoptBoundingBoxToPoint (const PointT& point_idx_arg)
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ adoptBoundingBoxToPoint(const PointT& point_idx_arg)
{
- const float minValue = std::numeric_limits<float>::epsilon ();
+ const float minValue = std::numeric_limits<float>::epsilon();
// increase octree size until point fits into bounding box
- while (true)
- {
+ while (true) {
bool bLowerBoundViolationX = (point_idx_arg.x < min_x_);
bool bLowerBoundViolationY = (point_idx_arg.y < min_y_);
bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_);
bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_);
// do we violate any bounds?
- if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || bUpperBoundViolationX
- || bUpperBoundViolationY || bUpperBoundViolationZ || (!bounding_box_defined_) )
- {
+ if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ ||
+ bUpperBoundViolationX || bUpperBoundViolationY || bUpperBoundViolationZ ||
+ (!bounding_box_defined_)) {
- if (bounding_box_defined_)
- {
+ if (bounding_box_defined_) {
double octreeSideLen;
unsigned char child_idx;
- // octree not empty - we add another tree level and thus increase its size by a factor of 2*2*2
- child_idx = static_cast<unsigned char> (((!bUpperBoundViolationX) << 2) | ((!bUpperBoundViolationY) << 1)
- | ((!bUpperBoundViolationZ)));
+ // octree not empty - we add another tree level and thus increase its size by a
+ // factor of 2*2*2
+ child_idx = static_cast<unsigned char>(((!bUpperBoundViolationX) << 2) |
+ ((!bUpperBoundViolationY) << 1) |
+ ((!bUpperBoundViolationZ)));
BranchNode* newRootBranch;
newRootBranch = new BranchNode();
this->branch_count_++;
- this->setBranchChildPtr (*newRootBranch, child_idx, this->root_node_);
+ this->setBranchChildPtr(*newRootBranch, child_idx, this->root_node_);
this->root_node_ = newRootBranch;
- octreeSideLen = static_cast<double> (1 << this->octree_depth_) * resolution_;
+ octreeSideLen = static_cast<double>(1 << this->octree_depth_) * resolution_;
if (!bUpperBoundViolationX)
min_x_ -= octreeSideLen;
// configure tree depth of octree
this->octree_depth_++;
- this->setTreeDepth (this->octree_depth_);
+ this->setTreeDepth(this->octree_depth_);
// recalculate bounding box width
- octreeSideLen = static_cast<double> (1 << this->octree_depth_) * resolution_ - minValue;
+ octreeSideLen =
+ static_cast<double>(1 << this->octree_depth_) * resolution_ - minValue;
// increase octree bounding box
max_x_ = min_x_ + octreeSideLen;
max_y_ = min_y_ + octreeSideLen;
max_z_ = min_z_ + octreeSideLen;
-
}
// bounding box is not defined - set it to point position
- else
- {
+ else {
// octree is empty - we set the center of the bounding box to our first pixel
this->min_x_ = point_idx_arg.x - this->resolution_ / 2;
this->min_y_ = point_idx_arg.y - this->resolution_ / 2;
this->max_y_ = point_idx_arg.y + this->resolution_ / 2;
this->max_z_ = point_idx_arg.z + this->resolution_ / 2;
- getKeyBitSize ();
+ getKeyBitSize();
bounding_box_defined_ = true;
}
-
}
else
// no bound violations anymore - leave while loop
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::expandLeafNode (LeafNode* leaf_node, BranchNode* parent_branch, unsigned char child_idx, unsigned int depth_mask)
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ expandLeafNode(LeafNode* leaf_node,
+ BranchNode* parent_branch,
+ unsigned char child_idx,
+ unsigned int depth_mask)
{
- if (depth_mask)
- {
+ if (depth_mask) {
// get amount of objects in leaf container
- std::size_t leaf_obj_count = (*leaf_node)->getSize ();
+ std::size_t leaf_obj_count = (*leaf_node)->getSize();
- // copy leaf data
+ // copy leaf data
std::vector<int> leafIndices;
leafIndices.reserve(leaf_obj_count);
// delete current leaf node
this->deleteBranchChild(*parent_branch, child_idx);
- this->leaf_count_ --;
+ this->leaf_count_--;
// create new branch node
- BranchNode* childBranch = this->createBranchChild (*parent_branch, child_idx);
- this->branch_count_ ++;
+ BranchNode* childBranch = this->createBranchChild(*parent_branch, child_idx);
+ this->branch_count_++;
// add data to new branch
OctreeKey new_index_key;
- for (const int &leafIndex : leafIndices)
- {
+ for (const int& leafIndex : leafIndices) {
const PointT& point_from_index = input_->points[leafIndex];
// generate key
- genOctreeKeyforPoint (point_from_index, new_index_key);
+ genOctreeKeyforPoint(point_from_index, new_index_key);
LeafNode* newLeaf;
BranchNode* newBranchParent;
- this->createLeafRecursive (new_index_key, depth_mask, childBranch, newLeaf, newBranchParent);
+ this->createLeafRecursive(
+ new_index_key, depth_mask, childBranch, newLeaf, newBranchParent);
(*newLeaf)->addPointIndex(leafIndex);
}
}
-
-
}
-
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::addPointIdx (const int point_idx_arg)
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ addPointIdx(const int point_idx_arg)
{
OctreeKey key;
- assert (point_idx_arg < static_cast<int> (input_->points.size ()));
+ assert(point_idx_arg < static_cast<int>(input_->points.size()));
const PointT& point = input_->points[point_idx_arg];
// make sure bounding box is big enough
- adoptBoundingBoxToPoint (point);
+ adoptBoundingBoxToPoint(point);
// generate key
- genOctreeKeyforPoint (point, key);
+ genOctreeKeyforPoint(point, key);
LeafNode* leaf_node;
BranchNode* parent_branch_of_leaf_node;
- unsigned int depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node);
+ unsigned int depth_mask = this->createLeafRecursive(
+ key, this->depth_mask_, this->root_node_, leaf_node, parent_branch_of_leaf_node);
- if (this->dynamic_depth_enabled_ && depth_mask)
- {
+ if (this->dynamic_depth_enabled_ && depth_mask) {
// get amount of objects in leaf container
- std::size_t leaf_obj_count = (*leaf_node)->getSize ();
+ std::size_t leaf_obj_count = (*leaf_node)->getSize();
- while (leaf_obj_count>=max_objs_per_leaf_ && depth_mask)
- {
+ while (leaf_obj_count >= max_objs_per_leaf_ && depth_mask) {
// index to branch child
- unsigned char child_idx = key.getChildIdxWithDepthMask (depth_mask*2);
+ unsigned char child_idx = key.getChildIdxWithDepthMask(depth_mask * 2);
- expandLeafNode (leaf_node,
- parent_branch_of_leaf_node,
- child_idx,
- depth_mask);
+ expandLeafNode(leaf_node, parent_branch_of_leaf_node, child_idx, depth_mask);
- depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node);
- leaf_obj_count = (*leaf_node)->getSize ();
+ depth_mask = this->createLeafRecursive(key,
+ this->depth_mask_,
+ this->root_node_,
+ leaf_node,
+ parent_branch_of_leaf_node);
+ leaf_obj_count = (*leaf_node)->getSize();
}
-
}
- (*leaf_node)->addPointIndex (point_idx_arg);
+ (*leaf_node)->addPointIndex(point_idx_arg);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> const PointT&
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::getPointByIndex (const unsigned int index_arg) const
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+const PointT&
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ getPointByIndex(const unsigned int index_arg) const
{
// retrieve point from input cloud
- assert (index_arg < static_cast<unsigned int> (input_->points.size ()));
+ assert(index_arg < static_cast<unsigned int>(input_->points.size()));
return (this->input_->points[index_arg]);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::getKeyBitSize ()
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ getKeyBitSize()
{
unsigned int max_voxels;
const float minValue = std::numeric_limits<float>::epsilon();
// find maximum key values for x, y, z
- max_key_x = static_cast<unsigned int> (std::ceil ((max_x_ - min_x_ - minValue) / resolution_));
- max_key_y = static_cast<unsigned int> (std::ceil ((max_y_ - min_y_ - minValue) / resolution_));
- max_key_z = static_cast<unsigned int> (std::ceil ((max_z_ - min_z_ - minValue) / resolution_));
+ max_key_x =
+ static_cast<unsigned int>(std::ceil((max_x_ - min_x_ - minValue) / resolution_));
+ max_key_y =
+ static_cast<unsigned int>(std::ceil((max_y_ - min_y_ - minValue) / resolution_));
+ max_key_z =
+ static_cast<unsigned int>(std::ceil((max_z_ - min_z_ - minValue) / resolution_));
// find maximum amount of keys
- max_voxels = std::max (std::max (std::max (max_key_x, max_key_y), max_key_z), static_cast<unsigned int> (2));
-
+ max_voxels = std::max(std::max(std::max(max_key_x, max_key_y), max_key_z),
+ static_cast<unsigned int>(2));
// tree depth == amount of bits of max_voxels
- this->octree_depth_ = std::max ((std::min (static_cast<unsigned int> (OctreeKey::maxDepth), static_cast<unsigned int> (std::ceil (std::log2 (max_voxels) - minValue)))),
- static_cast<unsigned int> (0));
+ this->octree_depth_ =
+ std::max((std::min(static_cast<unsigned int>(OctreeKey::maxDepth),
+ static_cast<unsigned int>(
+ std::ceil(std::log2(max_voxels) - minValue)))),
+ static_cast<unsigned int>(0));
- octree_side_len = static_cast<double> (1 << this->octree_depth_) * resolution_;
+ octree_side_len = static_cast<double>(1 << this->octree_depth_) * resolution_;
- if (this->leaf_count_ == 0)
- {
+ if (this->leaf_count_ == 0) {
double octree_oversize_x;
double octree_oversize_y;
double octree_oversize_z;
octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0;
octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0;
- assert (octree_oversize_x > -minValue);
- assert (octree_oversize_y > -minValue);
- assert (octree_oversize_z > -minValue);
+ assert(octree_oversize_x > -minValue);
+ assert(octree_oversize_y > -minValue);
+ assert(octree_oversize_z > -minValue);
- if (octree_oversize_x > minValue)
- {
+ if (octree_oversize_x > minValue) {
min_x_ -= octree_oversize_x;
max_x_ += octree_oversize_x;
}
- if (octree_oversize_y > minValue)
- {
+ if (octree_oversize_y > minValue) {
min_y_ -= octree_oversize_y;
max_y_ += octree_oversize_y;
}
- if (octree_oversize_z > minValue)
- {
+ if (octree_oversize_z > minValue) {
min_z_ -= octree_oversize_z;
max_z_ += octree_oversize_z;
}
}
- else
- {
+ else {
max_x_ = min_x_ + octree_side_len;
max_y_ = min_y_ + octree_side_len;
max_z_ = min_z_ + octree_side_len;
}
- // configure tree depth of octree
- this->setTreeDepth (this->octree_depth_);
-
+ // configure tree depth of octree
+ this->setTreeDepth(this->octree_depth_);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::genOctreeKeyforPoint (const PointT& point_arg,
- OctreeKey & key_arg) const
- {
- // calculate integer key for point coordinates
- key_arg.x = static_cast<unsigned int> ((point_arg.x - this->min_x_) / this->resolution_);
- key_arg.y = static_cast<unsigned int> ((point_arg.y - this->min_y_) / this->resolution_);
- key_arg.z = static_cast<unsigned int> ((point_arg.z - this->min_z_) / this->resolution_);
-
- assert (key_arg.x <= this->max_key_.x);
- assert (key_arg.y <= this->max_key_.y);
- assert (key_arg.z <= this->max_key_.z);
- }
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const
+{
+ // calculate integer key for point coordinates
+ key_arg.x =
+ static_cast<unsigned int>((point_arg.x - this->min_x_) / this->resolution_);
+ key_arg.y =
+ static_cast<unsigned int>((point_arg.y - this->min_y_) / this->resolution_);
+ key_arg.z =
+ static_cast<unsigned int>((point_arg.z - this->min_z_) / this->resolution_);
+
+ assert(key_arg.x <= this->max_key_.x);
+ assert(key_arg.y <= this->max_key_.y);
+ assert(key_arg.z <= this->max_key_.z);
+}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::genOctreeKeyforPoint (
- const double point_x_arg, const double point_y_arg,
- const double point_z_arg, OctreeKey & key_arg) const
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ genOctreeKeyforPoint(const double point_x_arg,
+ const double point_y_arg,
+ const double point_z_arg,
+ OctreeKey& key_arg) const
{
PointT temp_point;
- temp_point.x = static_cast<float> (point_x_arg);
- temp_point.y = static_cast<float> (point_y_arg);
- temp_point.z = static_cast<float> (point_z_arg);
+ temp_point.x = static_cast<float>(point_x_arg);
+ temp_point.y = static_cast<float>(point_y_arg);
+ temp_point.z = static_cast<float>(point_z_arg);
// generate key for point
- genOctreeKeyforPoint (temp_point, key_arg);
+ genOctreeKeyforPoint(temp_point, key_arg);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::genOctreeKeyForDataT (const int& data_arg, OctreeKey & key_arg) const
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+bool
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ genOctreeKeyForDataT(const int& data_arg, OctreeKey& key_arg) const
{
- const PointT temp_point = getPointByIndex (data_arg);
+ const PointT temp_point = getPointByIndex(data_arg);
// generate key for point
- genOctreeKeyforPoint (temp_point, key_arg);
+ genOctreeKeyforPoint(temp_point, key_arg);
return (true);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::genLeafNodeCenterFromOctreeKey (const OctreeKey & key, PointT & point) const
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ genLeafNodeCenterFromOctreeKey(const OctreeKey& key, PointT& point) const
{
// define point to leaf node voxel center
- point.x = static_cast<float> ((static_cast<double> (key.x) + 0.5f) * this->resolution_ + this->min_x_);
- point.y = static_cast<float> ((static_cast<double> (key.y) + 0.5f) * this->resolution_ + this->min_y_);
- point.z = static_cast<float> ((static_cast<double> (key.z) + 0.5f) * this->resolution_ + this->min_z_);
+ point.x = static_cast<float>((static_cast<double>(key.x) + 0.5f) * this->resolution_ +
+ this->min_x_);
+ point.y = static_cast<float>((static_cast<double>(key.y) + 0.5f) * this->resolution_ +
+ this->min_y_);
+ point.z = static_cast<float>((static_cast<double>(key.z) + 0.5f) * this->resolution_ +
+ this->min_z_);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::genVoxelCenterFromOctreeKey (
- const OctreeKey & key_arg,
- unsigned int tree_depth_arg,
- PointT& point_arg) const
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ genVoxelCenterFromOctreeKey(const OctreeKey& key_arg,
+ unsigned int tree_depth_arg,
+ PointT& point_arg) const
{
// generate point for voxel center defined by treedepth (bitLen) and key
- point_arg.x = static_cast<float> ((static_cast <double> (key_arg.x) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_x_);
- point_arg.y = static_cast<float> ((static_cast <double> (key_arg.y) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_y_);
- point_arg.z = static_cast<float> ((static_cast <double> (key_arg.z) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_z_);
+ point_arg.x = static_cast<float>(
+ (static_cast<double>(key_arg.x) + 0.5f) *
+ (this->resolution_ *
+ static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
+ this->min_x_);
+ point_arg.y = static_cast<float>(
+ (static_cast<double>(key_arg.y) + 0.5f) *
+ (this->resolution_ *
+ static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
+ this->min_y_);
+ point_arg.z = static_cast<float>(
+ (static_cast<double>(key_arg.z) + 0.5f) *
+ (this->resolution_ *
+ static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
+ this->min_z_);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::genVoxelBoundsFromOctreeKey (
- const OctreeKey & key_arg,
- unsigned int tree_depth_arg,
- Eigen::Vector3f &min_pt,
- Eigen::Vector3f &max_pt) const
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+void
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ genVoxelBoundsFromOctreeKey(const OctreeKey& key_arg,
+ unsigned int tree_depth_arg,
+ Eigen::Vector3f& min_pt,
+ Eigen::Vector3f& max_pt) const
{
// calculate voxel size of current tree depth
- double voxel_side_len = this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg));
+ double voxel_side_len =
+ this->resolution_ *
+ static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
// calculate voxel bounds
- min_pt (0) = static_cast<float> (static_cast<double> (key_arg.x) * voxel_side_len + this->min_x_);
- min_pt (1) = static_cast<float> (static_cast<double> (key_arg.y) * voxel_side_len + this->min_y_);
- min_pt (2) = static_cast<float> (static_cast<double> (key_arg.z) * voxel_side_len + this->min_z_);
-
- max_pt (0) = static_cast<float> (static_cast<double> (key_arg.x + 1) * voxel_side_len + this->min_x_);
- max_pt (1) = static_cast<float> (static_cast<double> (key_arg.y + 1) * voxel_side_len + this->min_y_);
- max_pt (2) = static_cast<float> (static_cast<double> (key_arg.z + 1) * voxel_side_len + this->min_z_);
+ min_pt(0) = static_cast<float>(static_cast<double>(key_arg.x) * voxel_side_len +
+ this->min_x_);
+ min_pt(1) = static_cast<float>(static_cast<double>(key_arg.y) * voxel_side_len +
+ this->min_y_);
+ min_pt(2) = static_cast<float>(static_cast<double>(key_arg.z) * voxel_side_len +
+ this->min_z_);
+
+ max_pt(0) = static_cast<float>(static_cast<double>(key_arg.x + 1) * voxel_side_len +
+ this->min_x_);
+ max_pt(1) = static_cast<float>(static_cast<double>(key_arg.y + 1) * voxel_side_len +
+ this->min_y_);
+ max_pt(2) = static_cast<float>(static_cast<double>(key_arg.z + 1) * voxel_side_len +
+ this->min_z_);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> double
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::getVoxelSquaredSideLen (unsigned int tree_depth_arg) const
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+double
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ getVoxelSquaredSideLen(unsigned int tree_depth_arg) const
{
double side_len;
// side length of the voxel cube increases exponentially with the octree depth
- side_len = this->resolution_ * static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
+ side_len = this->resolution_ *
+ static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
// squared voxel side length
side_len *= side_len;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> double
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::getVoxelSquaredDiameter (unsigned int tree_depth_arg) const
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+double
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ getVoxelSquaredDiameter(unsigned int tree_depth_arg) const
{
// return the squared side length of the voxel cube as a function of the octree depth
- return (getVoxelSquaredSideLen (tree_depth_arg) * 3);
+ return (getVoxelSquaredSideLen(tree_depth_arg) * 3);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> int
-pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::getOccupiedVoxelCentersRecursive (
- const BranchNode* node_arg,
- const OctreeKey& key_arg,
- AlignedPointTVector &voxel_center_list_arg) const
+template <typename PointT,
+ typename LeafContainerT,
+ typename BranchContainerT,
+ typename OctreeT>
+int
+pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
+ getOccupiedVoxelCentersRecursive(const BranchNode* node_arg,
+ const OctreeKey& key_arg,
+ AlignedPointTVector& voxel_center_list_arg) const
{
int voxel_count = 0;
// iterate over all children
- for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
- {
- if (!this->branchHasChild (*node_arg, child_idx))
+ for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
+ if (!this->branchHasChild(*node_arg, child_idx))
continue;
- const OctreeNode * child_node;
- child_node = this->getBranchChildPtr (*node_arg, child_idx);
+ const OctreeNode* child_node;
+ child_node = this->getBranchChildPtr(*node_arg, child_idx);
// generate new key for current branch voxel
OctreeKey new_key;
new_key.y = (key_arg.y << 1) | (!!(child_idx & (1 << 1)));
new_key.z = (key_arg.z << 1) | (!!(child_idx & (1 << 0)));
- switch (child_node->getNodeType ())
- {
- case BRANCH_NODE:
- {
- // recursively proceed with indexed child branch
- voxel_count += getOccupiedVoxelCentersRecursive (static_cast<const BranchNode*> (child_node), new_key, voxel_center_list_arg);
- break;
- }
- case LEAF_NODE:
- {
- PointT new_point;
+ switch (child_node->getNodeType()) {
+ case BRANCH_NODE: {
+ // recursively proceed with indexed child branch
+ voxel_count += getOccupiedVoxelCentersRecursive(
+ static_cast<const BranchNode*>(child_node), new_key, voxel_center_list_arg);
+ break;
+ }
+ case LEAF_NODE: {
+ PointT new_point;
- genLeafNodeCenterFromOctreeKey (new_key, new_point);
- voxel_center_list_arg.push_back (new_point);
+ genLeafNodeCenterFromOctreeKey(new_key, new_point);
+ voxel_center_list_arg.push_back(new_point);
- voxel_count++;
- break;
- }
- default:
- break;
+ voxel_count++;
+ break;
+ }
+ default:
+ break;
}
}
return (voxel_count);
}
-#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty > >;
-#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty > >;
-
-#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty > >;
-#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty > >;
-
-#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty > >;
-#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty > >;
+#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) \
+ template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
+ T, \
+ pcl::octree::OctreeContainerPointIndices, \
+ pcl::octree::OctreeContainerEmpty, \
+ pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndices, \
+ pcl::octree::OctreeContainerEmpty>>;
+#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) \
+ template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
+ T, \
+ pcl::octree::OctreeContainerPointIndices, \
+ pcl::octree::OctreeContainerEmpty, \
+ pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndices, \
+ pcl::octree::OctreeContainerEmpty>>;
+
+#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) \
+ template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
+ T, \
+ pcl::octree::OctreeContainerPointIndex, \
+ pcl::octree::OctreeContainerEmpty, \
+ pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndex, \
+ pcl::octree::OctreeContainerEmpty>>;
+#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) \
+ template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
+ T, \
+ pcl::octree::OctreeContainerPointIndex, \
+ pcl::octree::OctreeContainerEmpty, \
+ pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndex, \
+ pcl::octree::OctreeContainerEmpty>>;
+
+#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) \
+ template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
+ T, \
+ pcl::octree::OctreeContainerEmpty, \
+ pcl::octree::OctreeContainerEmpty, \
+ pcl::octree::OctreeBase<pcl::octree::OctreeContainerEmpty, \
+ pcl::octree::OctreeContainerEmpty>>;
+#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) \
+ template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
+ T, \
+ pcl::octree::OctreeContainerEmpty, \
+ pcl::octree::OctreeContainerEmpty, \
+ pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerEmpty, \
+ pcl::octree::OctreeContainerEmpty>>;
#endif /* OCTREE_POINTCLOUD_HPP_ */
#ifndef PCL_OCTREE_POINTCLOUD_ADJACENCY_HPP_
#define PCL_OCTREE_POINTCLOUD_ADJACENCY_HPP_
-#include <pcl/console/print.h>
#include <pcl/common/geometry.h>
+#include <pcl/console/print.h>
/*
* OctreePointCloudAdjacency is not precompiled, since it's used in other
* parts of PCL with custom LeafContainers. So if PCL_NO_PRECOMPILE is NOT
#include <pcl/octree/impl/octree_pointcloud.hpp>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT>
-pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::OctreePointCloudAdjacency (const double resolution_arg)
-: OctreePointCloud<PointT, LeafContainerT, BranchContainerT
-, OctreeBase<LeafContainerT, BranchContainerT> > (resolution_arg)
-{
-
-}
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::
+ OctreePointCloudAdjacency(const double resolution_arg)
+: OctreePointCloud<PointT,
+ LeafContainerT,
+ BranchContainerT,
+ OctreeBase<LeafContainerT, BranchContainerT>>(resolution_arg)
+{}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
-pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::addPointsFromInputCloud ()
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+void
+pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::
+ addPointsFromInputCloud()
{
- //double t1,t2;
- float minX = std::numeric_limits<float>::max (), minY = std::numeric_limits<float>::max (), minZ = std::numeric_limits<float>::max ();
- float maxX = -std::numeric_limits<float>::max(), maxY = -std::numeric_limits<float>::max(), maxZ = -std::numeric_limits<float>::max();
-
- for (std::size_t i = 0; i < input_->size (); ++i)
- {
- PointT temp (input_->points[i]);
- if (transform_func_) //Search for point with
- transform_func_ (temp);
- if (!pcl::isFinite (temp)) //Check to make sure transform didn't make point not finite
+ // double t1,t2;
+ float minX = std::numeric_limits<float>::max(),
+ minY = std::numeric_limits<float>::max(),
+ minZ = std::numeric_limits<float>::max();
+ float maxX = -std::numeric_limits<float>::max(),
+ maxY = -std::numeric_limits<float>::max(),
+ maxZ = -std::numeric_limits<float>::max();
+
+ for (std::size_t i = 0; i < input_->size(); ++i) {
+ PointT temp(input_->points[i]);
+ if (transform_func_) // Search for point with
+ transform_func_(temp);
+ if (!pcl::isFinite(
+ temp)) // Check to make sure transform didn't make point not finite
continue;
if (temp.x < minX)
minX = temp.x;
if (temp.z > maxZ)
maxZ = temp.z;
}
- this->defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
-
- OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::addPointsFromInputCloud ();
-
- leaf_vector_.reserve (this->getLeafCount ());
- for (auto leaf_itr = this->leaf_depth_begin () ; leaf_itr != this->leaf_depth_end (); ++leaf_itr)
- {
- OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey ();
- LeafContainerT *leaf_container = &(leaf_itr.getLeafContainer ());
-
- //Run the leaf's compute function
- leaf_container->computeData ();
-
- computeNeighbors (leaf_key, leaf_container);
-
- leaf_vector_.push_back (leaf_container);
+ this->defineBoundingBox(minX, minY, minZ, maxX, maxY, maxZ);
+
+ OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::addPointsFromInputCloud();
+
+ leaf_vector_.reserve(this->getLeafCount());
+ for (auto leaf_itr = this->leaf_depth_begin(); leaf_itr != this->leaf_depth_end();
+ ++leaf_itr) {
+ OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey();
+ LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer());
+
+ // Run the leaf's compute function
+ leaf_container->computeData();
+
+ computeNeighbors(leaf_key, leaf_container);
+
+ leaf_vector_.push_back(leaf_container);
}
- //Make sure our leaf vector is correctly sized
- assert (leaf_vector_.size () == this->getLeafCount ());
+ // Make sure our leaf vector is correctly sized
+ assert(leaf_vector_.size() == this->getLeafCount());
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
-pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::genOctreeKeyforPoint (const PointT& point_arg,OctreeKey & key_arg) const
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+void
+pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::
+ genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const
{
- if (transform_func_)
- {
- PointT temp (point_arg);
- transform_func_ (temp);
- // calculate integer key for transformed point coordinates
- if (pcl::isFinite (temp)) //Make sure transformed point is finite - if it is not, it gets default key
+ if (transform_func_) {
+ PointT temp(point_arg);
+ transform_func_(temp);
+ // calculate integer key for transformed point coordinates
+ if (pcl::isFinite(temp)) // Make sure transformed point is finite - if it is not, it
+ // gets default key
{
- key_arg.x = static_cast<unsigned int> ((temp.x - this->min_x_) / this->resolution_);
- key_arg.y = static_cast<unsigned int> ((temp.y - this->min_y_) / this->resolution_);
- key_arg.z = static_cast<unsigned int> ((temp.z - this->min_z_) / this->resolution_);
+ key_arg.x =
+ static_cast<unsigned int>((temp.x - this->min_x_) / this->resolution_);
+ key_arg.y =
+ static_cast<unsigned int>((temp.y - this->min_y_) / this->resolution_);
+ key_arg.z =
+ static_cast<unsigned int>((temp.z - this->min_z_) / this->resolution_);
}
- else
- {
- key_arg = OctreeKey ();
+ else {
+ key_arg = OctreeKey();
}
}
- else
- {
+ else {
// calculate integer key for point coordinates
- key_arg.x = static_cast<unsigned int> ((point_arg.x - this->min_x_) / this->resolution_);
- key_arg.y = static_cast<unsigned int> ((point_arg.y - this->min_y_) / this->resolution_);
- key_arg.z = static_cast<unsigned int> ((point_arg.z - this->min_z_) / this->resolution_);
+ key_arg.x =
+ static_cast<unsigned int>((point_arg.x - this->min_x_) / this->resolution_);
+ key_arg.y =
+ static_cast<unsigned int>((point_arg.y - this->min_y_) / this->resolution_);
+ key_arg.z =
+ static_cast<unsigned int>((point_arg.z - this->min_z_) / this->resolution_);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
-pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::addPointIdx (const int pointIdx_arg)
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+void
+pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::
+ addPointIdx(const int pointIdx_arg)
{
OctreeKey key;
-
- assert (pointIdx_arg < static_cast<int> (this->input_->points.size ()));
-
+
+ assert(pointIdx_arg < static_cast<int>(this->input_->points.size()));
+
const PointT& point = this->input_->points[pointIdx_arg];
- if (!pcl::isFinite (point))
+ if (!pcl::isFinite(point))
return;
-
+
// generate key
- this->genOctreeKeyforPoint (point, key);
+ this->genOctreeKeyforPoint(point, key);
// add point to octree at key
LeafContainerT* container = this->createLeaf(key);
- container->addPoint (point);
+ container->addPoint(point);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
-pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container)
-{
- //Make sure requested key is valid
- if (key_arg.x > this->max_key_.x || key_arg.y > this->max_key_.y || key_arg.z > this->max_key_.z)
- {
- PCL_ERROR ("OctreePointCloudAdjacency::computeNeighbors Requested neighbors for invalid octree key\n");
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+void
+pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::
+ computeNeighbors(OctreeKey& key_arg, LeafContainerT* leaf_container)
+{
+ // Make sure requested key is valid
+ if (key_arg.x > this->max_key_.x || key_arg.y > this->max_key_.y ||
+ key_arg.z > this->max_key_.z) {
+ PCL_ERROR("OctreePointCloudAdjacency::computeNeighbors Requested neighbors for "
+ "invalid octree key\n");
return;
}
-
+
OctreeKey neighbor_key;
int dx_min = (key_arg.x > 0) ? -1 : 0;
int dy_min = (key_arg.y > 0) ? -1 : 0;
int dx_max = (key_arg.x == this->max_key_.x) ? 0 : 1;
int dy_max = (key_arg.y == this->max_key_.y) ? 0 : 1;
int dz_max = (key_arg.z == this->max_key_.z) ? 0 : 1;
-
- for (int dx = dx_min; dx <= dx_max; ++dx)
- {
- for (int dy = dy_min; dy <= dy_max; ++dy)
- {
- for (int dz = dz_min; dz <= dz_max; ++dz)
- {
- neighbor_key.x = static_cast<std::uint32_t> (key_arg.x + dx);
- neighbor_key.y = static_cast<std::uint32_t> (key_arg.y + dy);
- neighbor_key.z = static_cast<std::uint32_t> (key_arg.z + dz);
- LeafContainerT *neighbor = this->findLeaf (neighbor_key);
- if (neighbor)
- {
- leaf_container->addNeighbor (neighbor);
+
+ for (int dx = dx_min; dx <= dx_max; ++dx) {
+ for (int dy = dy_min; dy <= dy_max; ++dy) {
+ for (int dz = dz_min; dz <= dz_max; ++dz) {
+ neighbor_key.x = static_cast<std::uint32_t>(key_arg.x + dx);
+ neighbor_key.y = static_cast<std::uint32_t>(key_arg.y + dy);
+ neighbor_key.z = static_cast<std::uint32_t>(key_arg.z + dz);
+ LeafContainerT* neighbor = this->findLeaf(neighbor_key);
+ if (neighbor) {
+ leaf_container->addNeighbor(neighbor);
}
}
}
}
}
-
-
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> LeafContainerT*
-pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::getLeafContainerAtPoint (
- const PointT& point_arg) const
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+LeafContainerT*
+pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::
+ getLeafContainerAtPoint(const PointT& point_arg) const
{
OctreeKey key;
LeafContainerT* leaf = nullptr;
// generate key
- this->genOctreeKeyforPoint (point_arg, key);
-
- leaf = this->findLeaf (key);
-
+ this->genOctreeKeyforPoint(point_arg, key);
+
+ leaf = this->findLeaf(key);
+
return leaf;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
-pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::computeVoxelAdjacencyGraph (VoxelAdjacencyList &voxel_adjacency_graph)
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+void
+pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::
+ computeVoxelAdjacencyGraph(VoxelAdjacencyList& voxel_adjacency_graph)
{
- //TODO Change this to use leaf centers, not centroids!
-
- voxel_adjacency_graph.clear ();
- //Add a vertex for each voxel, store ids in map
- std::map <LeafContainerT*, VoxelID> leaf_vertex_id_map;
- for (typename OctreeAdjacencyT::LeafNodeDepthFirstIterator leaf_itr = this->leaf_depth_begin () ; leaf_itr != this->leaf_depth_end (); ++leaf_itr)
- {
- OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey ();
+ // TODO Change this to use leaf centers, not centroids!
+
+ voxel_adjacency_graph.clear();
+ // Add a vertex for each voxel, store ids in map
+ std::map<LeafContainerT*, VoxelID> leaf_vertex_id_map;
+ for (typename OctreeAdjacencyT::LeafNodeDepthFirstIterator leaf_itr =
+ this->leaf_depth_begin();
+ leaf_itr != this->leaf_depth_end();
+ ++leaf_itr) {
+ OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey();
PointT centroid_point;
- this->genLeafNodeCenterFromOctreeKey (leaf_key, centroid_point);
- VoxelID node_id = add_vertex (voxel_adjacency_graph);
-
+ this->genLeafNodeCenterFromOctreeKey(leaf_key, centroid_point);
+ VoxelID node_id = add_vertex(voxel_adjacency_graph);
+
voxel_adjacency_graph[node_id] = centroid_point;
- LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer ());
+ LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer());
leaf_vertex_id_map[leaf_container] = node_id;
}
-
- //Iterate through and add edges to adjacency graph
- for ( typename std::vector<LeafContainerT*>::iterator leaf_itr = leaf_vector_.begin (); leaf_itr != leaf_vector_.end (); ++leaf_itr)
- {
- VoxelID u = (leaf_vertex_id_map.find (*leaf_itr))->second;
+
+ // Iterate through and add edges to adjacency graph
+ for (typename std::vector<LeafContainerT*>::iterator leaf_itr = leaf_vector_.begin();
+ leaf_itr != leaf_vector_.end();
+ ++leaf_itr) {
+ VoxelID u = (leaf_vertex_id_map.find(*leaf_itr))->second;
PointT p_u = voxel_adjacency_graph[u];
- for (auto neighbor_itr = (*leaf_itr)->cbegin (), neighbor_end = (*leaf_itr)->cend (); neighbor_itr != neighbor_end; ++neighbor_itr)
- {
+ for (auto neighbor_itr = (*leaf_itr)->cbegin(), neighbor_end = (*leaf_itr)->cend();
+ neighbor_itr != neighbor_end;
+ ++neighbor_itr) {
LeafContainerT* neighbor_container = *neighbor_itr;
EdgeID edge;
bool edge_added;
- VoxelID v = (leaf_vertex_id_map.find (neighbor_container))->second;
- boost::tie (edge, edge_added) = add_edge (u,v,voxel_adjacency_graph);
-
+ VoxelID v = (leaf_vertex_id_map.find(neighbor_container))->second;
+ boost::tie(edge, edge_added) = add_edge(u, v, voxel_adjacency_graph);
+
PointT p_v = voxel_adjacency_graph[v];
- float dist = (p_v.getVector3fMap () - p_u.getVector3fMap ()).norm ();
+ float dist = (p_v.getVector3fMap() - p_u.getVector3fMap()).norm();
voxel_adjacency_graph[edge] = dist;
-
}
-
}
-
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> bool
-pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::testForOcclusion (const PointT& point_arg, const PointXYZ &camera_pos)
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+bool
+pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::
+ testForOcclusion(const PointT& point_arg, const PointXYZ& camera_pos)
{
OctreeKey key;
- this->genOctreeKeyforPoint (point_arg, key);
+ this->genOctreeKeyforPoint(point_arg, key);
// This code follows the method in Octree::PointCloud
- Eigen::Vector3f sensor(camera_pos.x,
- camera_pos.y,
- camera_pos.z);
-
- Eigen::Vector3f leaf_centroid(static_cast<float> ((static_cast<double> (key.x) + 0.5f) * this->resolution_ + this->min_x_),
- static_cast<float> ((static_cast<double> (key.y) + 0.5f) * this->resolution_ + this->min_y_),
- static_cast<float> ((static_cast<double> (key.z) + 0.5f) * this->resolution_ + this->min_z_));
+ Eigen::Vector3f sensor(camera_pos.x, camera_pos.y, camera_pos.z);
+
+ Eigen::Vector3f leaf_centroid(
+ static_cast<float>((static_cast<double>(key.x) + 0.5f) * this->resolution_ +
+ this->min_x_),
+ static_cast<float>((static_cast<double>(key.y) + 0.5f) * this->resolution_ +
+ this->min_y_),
+ static_cast<float>((static_cast<double>(key.z) + 0.5f) * this->resolution_ +
+ this->min_z_));
Eigen::Vector3f direction = sensor - leaf_centroid;
-
- float norm = direction.norm ();
- direction.normalize ();
+
+ float norm = direction.norm();
+ direction.normalize();
float precision = 1.0f;
- const float step_size = static_cast<const float> (resolution_) * precision;
- const int nsteps = std::max (1, static_cast<int> (norm / step_size));
-
+ const float step_size = static_cast<const float>(resolution_) * precision;
+ const int nsteps = std::max(1, static_cast<int>(norm / step_size));
+
OctreeKey prev_key = key;
// Walk along the line segment with small steps.
Eigen::Vector3f p = leaf_centroid;
PointT octree_p;
- for (int i = 0; i < nsteps; ++i)
- {
- //Start at the leaf voxel, and move back towards sensor.
+ for (int i = 0; i < nsteps; ++i) {
+ // Start at the leaf voxel, and move back towards sensor.
p += (direction * step_size);
-
- octree_p.x = p.x ();
- octree_p.y = p.y ();
- octree_p.z = p.z ();
+
+ octree_p.x = p.x();
+ octree_p.y = p.y();
+ octree_p.z = p.z();
// std::cout << octree_p<< "\n";
OctreeKey key;
- this->genOctreeKeyforPoint (octree_p, key);
-
+ this->genOctreeKeyforPoint(octree_p, key);
+
// Not a new key, still the same voxel (starts at self).
if ((key == prev_key))
continue;
-
+
prev_key = key;
-
- LeafContainerT *leaf = this->findLeaf (key);
- //If the voxel is occupied, there is a possible occlusion
- if (leaf)
- {
- return true;
+
+ LeafContainerT* leaf = this->findLeaf(key);
+ // If the voxel is occupied, there is a possible occlusion
+ if (leaf) {
+ return true;
}
}
-
- //If we didn't run into a voxel on the way to this camera, it can't be occluded.
+
+ // If we didn't run into a voxel on the way to this camera, it can't be occluded.
return false;
-
}
-#define PCL_INSTANTIATE_OctreePointCloudAdjacency(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudAdjacency<T>;
+#define PCL_INSTANTIATE_OctreePointCloudAdjacency(T) \
+ template class PCL_EXPORTS pcl::octree::OctreePointCloudAdjacency<T>;
#endif
-
#include <pcl/octree/impl/octree_pointcloud.hpp>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> bool
-pcl::octree::OctreePointCloudVoxelCentroid<PointT, LeafContainerT, BranchContainerT>::getVoxelCentroidAtPoint (
- const PointT& point_arg, PointT& voxel_centroid_arg) const
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+bool
+pcl::octree::OctreePointCloudVoxelCentroid<PointT, LeafContainerT, BranchContainerT>::
+ getVoxelCentroidAtPoint(const PointT& point_arg, PointT& voxel_centroid_arg) const
{
OctreeKey key;
LeafContainerT* leaf = NULL;
// generate key
- genOctreeKeyforPoint (point_arg, key);
+ genOctreeKeyforPoint(point_arg, key);
- leaf = this->findLeaf (key);
+ leaf = this->findLeaf(key);
if (leaf)
- leaf->getCentroid (voxel_centroid_arg);
+ leaf->getCentroid(voxel_centroid_arg);
return (leaf != NULL);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> std::size_t
-pcl::octree::OctreePointCloudVoxelCentroid<PointT, LeafContainerT, BranchContainerT>::getVoxelCentroids (
- typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::AlignedPointTVector &voxel_centroid_list_arg) const
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+std::size_t
+pcl::octree::OctreePointCloudVoxelCentroid<PointT, LeafContainerT, BranchContainerT>::
+ getVoxelCentroids(
+ typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::
+ AlignedPointTVector& voxel_centroid_list_arg) const
{
OctreeKey new_key;
// reset output vector
- voxel_centroid_list_arg.clear ();
- voxel_centroid_list_arg.reserve (this->leaf_count_);
+ voxel_centroid_list_arg.clear();
+ voxel_centroid_list_arg.reserve(this->leaf_count_);
- getVoxelCentroidsRecursive (this->root_node_, new_key, voxel_centroid_list_arg );
+ getVoxelCentroidsRecursive(this->root_node_, new_key, voxel_centroid_list_arg);
// return size of centroid vector
- return (voxel_centroid_list_arg.size ());
+ return (voxel_centroid_list_arg.size());
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
-pcl::octree::OctreePointCloudVoxelCentroid<PointT, LeafContainerT, BranchContainerT>::getVoxelCentroidsRecursive (
- const BranchNode* branch_arg, OctreeKey& key_arg,
- typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::AlignedPointTVector &voxel_centroid_list_arg) const
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+void
+pcl::octree::OctreePointCloudVoxelCentroid<PointT, LeafContainerT, BranchContainerT>::
+ getVoxelCentroidsRecursive(
+ const BranchNode* branch_arg,
+ OctreeKey& key_arg,
+ typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::
+ AlignedPointTVector& voxel_centroid_list_arg) const
{
// iterate over all children
- for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
- {
+ for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
// if child exist
- if (branch_arg->hasChild (child_idx))
- {
+ if (branch_arg->hasChild(child_idx)) {
// add current branch voxel to key
- key_arg.pushBranch (child_idx);
+ key_arg.pushBranch(child_idx);
- OctreeNode *child_node = branch_arg->getChildPtr (child_idx);
+ OctreeNode* child_node = branch_arg->getChildPtr(child_idx);
- switch (child_node->getNodeType ())
- {
- case BRANCH_NODE:
- {
- // recursively proceed with indexed child branch
- getVoxelCentroidsRecursive (static_cast<const BranchNode*> (child_node), key_arg, voxel_centroid_list_arg);
- break;
- }
- case LEAF_NODE:
- {
- PointT new_centroid;
+ switch (child_node->getNodeType()) {
+ case BRANCH_NODE: {
+ // recursively proceed with indexed child branch
+ getVoxelCentroidsRecursive(static_cast<const BranchNode*>(child_node),
+ key_arg,
+ voxel_centroid_list_arg);
+ break;
+ }
+ case LEAF_NODE: {
+ PointT new_centroid;
- LeafNode* container = static_cast<LeafNode*> (child_node);
+ LeafNode* container = static_cast<LeafNode*>(child_node);
- container->getContainer().getCentroid (new_centroid);
+ container->getContainer().getCentroid(new_centroid);
- voxel_centroid_list_arg.push_back (new_centroid);
- break;
- }
- default:
- break;
- }
+ voxel_centroid_list_arg.push_back(new_centroid);
+ break;
+ }
+ default:
+ break;
+ }
// pop current branch voxel from key
- key_arg.popBranch ();
+ key_arg.popBranch();
}
}
}
-
-#define PCL_INSTANTIATE_OctreePointCloudVoxelCentroid(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudVoxelCentroid<T>;
+#define PCL_INSTANTIATE_OctreePointCloudVoxelCentroid(T) \
+ template class PCL_EXPORTS pcl::octree::OctreePointCloudVoxelCentroid<T>;
#endif
-
#include <cassert>
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> bool
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::voxelSearch (const PointT& point,
- std::vector<int>& point_idx_data)
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+bool
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ voxelSearch(const PointT& point, std::vector<int>& point_idx_data)
{
- assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
+ assert(isFinite(point) &&
+ "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
OctreeKey key;
bool b_success = false;
// generate key
- this->genOctreeKeyforPoint (point, key);
+ this->genOctreeKeyforPoint(point, key);
- LeafContainerT* leaf = this->findLeaf (key);
+ LeafContainerT* leaf = this->findLeaf(key);
- if (leaf)
- {
- (*leaf).getPointIndices (point_idx_data);
+ if (leaf) {
+ (*leaf).getPointIndices(point_idx_data);
b_success = true;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> bool
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::voxelSearch (const int index,
- std::vector<int>& point_idx_data)
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+bool
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ voxelSearch(const int index, std::vector<int>& point_idx_data)
{
- const PointT search_point = this->getPointByIndex (index);
- return (this->voxelSearch (search_point, point_idx_data));
+ const PointT search_point = this->getPointByIndex(index);
+ return (this->voxelSearch(search_point, point_idx_data));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::nearestKSearch (const PointT &p_q, int k,
- std::vector<int> &k_indices,
- std::vector<float> &k_sqr_distances)
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+int
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ nearestKSearch(const PointT& p_q,
+ int k,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances)
{
- assert(this->leaf_count_>0);
- assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
+ assert(this->leaf_count_ > 0);
+ assert(isFinite(p_q) &&
+ "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
- k_indices.clear ();
- k_sqr_distances.clear ();
+ k_indices.clear();
+ k_sqr_distances.clear();
if (k < 1)
return 0;
-
+
prioPointQueueEntry point_entry;
std::vector<prioPointQueueEntry> point_candidates;
key.x = key.y = key.z = 0;
// initialize smallest point distance in search with high value
- double smallest_dist = std::numeric_limits<double>::max ();
+ double smallest_dist = std::numeric_limits<double>::max();
+
+ getKNearestNeighborRecursive(
+ p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates);
- getKNearestNeighborRecursive (p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates);
+ unsigned int result_count = static_cast<unsigned int>(point_candidates.size());
- unsigned int result_count = static_cast<unsigned int> (point_candidates.size ());
+ k_indices.resize(result_count);
+ k_sqr_distances.resize(result_count);
- k_indices.resize (result_count);
- k_sqr_distances.resize (result_count);
-
- for (unsigned int i = 0; i < result_count; ++i)
- {
- k_indices [i] = point_candidates [i].point_idx_;
- k_sqr_distances [i] = point_candidates [i].point_distance_;
+ for (unsigned int i = 0; i < result_count; ++i) {
+ k_indices[i] = point_candidates[i].point_idx_;
+ k_sqr_distances[i] = point_candidates[i].point_distance_;
}
- return static_cast<int> (k_indices.size ());
+ return static_cast<int>(k_indices.size());
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::nearestKSearch (int index, int k,
- std::vector<int> &k_indices,
- std::vector<float> &k_sqr_distances)
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+int
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ nearestKSearch(int index,
+ int k,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances)
{
- const PointT search_point = this->getPointByIndex (index);
- return (nearestKSearch (search_point, k, k_indices, k_sqr_distances));
+ const PointT search_point = this->getPointByIndex(index);
+ return (nearestKSearch(search_point, k, k_indices, k_sqr_distances));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::approxNearestSearch (const PointT &p_q,
- int &result_index,
- float &sqr_distance)
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+void
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ approxNearestSearch(const PointT& p_q, int& result_index, float& sqr_distance)
{
- assert(this->leaf_count_>0);
- assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
-
+ assert(this->leaf_count_ > 0);
+ assert(isFinite(p_q) &&
+ "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
+
OctreeKey key;
key.x = key.y = key.z = 0;
- approxNearestSearchRecursive (p_q, this->root_node_, key, 1, result_index, sqr_distance);
+ approxNearestSearchRecursive(
+ p_q, this->root_node_, key, 1, result_index, sqr_distance);
return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::approxNearestSearch (int query_index, int &result_index,
- float &sqr_distance)
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+void
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ approxNearestSearch(int query_index, int& result_index, float& sqr_distance)
{
- const PointT search_point = this->getPointByIndex (query_index);
+ const PointT search_point = this->getPointByIndex(query_index);
- return (approxNearestSearch (search_point, result_index, sqr_distance));
+ return (approxNearestSearch(search_point, result_index, sqr_distance));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::radiusSearch (const PointT &p_q, const double radius,
- std::vector<int> &k_indices,
- std::vector<float> &k_sqr_distances,
- unsigned int max_nn) const
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+int
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ radiusSearch(const PointT& p_q,
+ const double radius,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances,
+ unsigned int max_nn) const
{
- assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
+ assert(isFinite(p_q) &&
+ "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
OctreeKey key;
key.x = key.y = key.z = 0;
- k_indices.clear ();
- k_sqr_distances.clear ();
+ k_indices.clear();
+ k_sqr_distances.clear();
- getNeighborsWithinRadiusRecursive (p_q, radius * radius, this->root_node_, key, 1, k_indices, k_sqr_distances,
- max_nn);
+ getNeighborsWithinRadiusRecursive(p_q,
+ radius * radius,
+ this->root_node_,
+ key,
+ 1,
+ k_indices,
+ k_sqr_distances,
+ max_nn);
- return (static_cast<int> (k_indices.size ()));
+ return (static_cast<int>(k_indices.size()));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::radiusSearch (int index, const double radius,
- std::vector<int> &k_indices,
- std::vector<float> &k_sqr_distances,
- unsigned int max_nn) const
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+int
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ radiusSearch(int index,
+ const double radius,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances,
+ unsigned int max_nn) const
{
- const PointT search_point = this->getPointByIndex (index);
+ const PointT search_point = this->getPointByIndex(index);
- return (radiusSearch (search_point, radius, k_indices, k_sqr_distances, max_nn));
+ return (radiusSearch(search_point, radius, k_indices, k_sqr_distances, max_nn));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::boxSearch (const Eigen::Vector3f &min_pt,
- const Eigen::Vector3f &max_pt,
- std::vector<int> &k_indices) const
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+int
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ boxSearch(const Eigen::Vector3f& min_pt,
+ const Eigen::Vector3f& max_pt,
+ std::vector<int>& k_indices) const
{
OctreeKey key;
key.x = key.y = key.z = 0;
- k_indices.clear ();
-
- boxSearchRecursive (min_pt, max_pt, this->root_node_, key, 1, k_indices);
+ k_indices.clear();
- return (static_cast<int> (k_indices.size ()));
+ boxSearchRecursive(min_pt, max_pt, this->root_node_, key, 1, k_indices);
+ return (static_cast<int>(k_indices.size()));
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> double
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::getKNearestNeighborRecursive (
- const PointT & point, unsigned int K, const BranchNode* node, const OctreeKey& key, unsigned int tree_depth,
- const double squared_search_radius, std::vector<prioPointQueueEntry>& point_candidates) const
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+double
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ getKNearestNeighborRecursive(
+ const PointT& point,
+ unsigned int K,
+ const BranchNode* node,
+ const OctreeKey& key,
+ unsigned int tree_depth,
+ const double squared_search_radius,
+ std::vector<prioPointQueueEntry>& point_candidates) const
{
std::vector<prioBranchQueueEntry> search_heap;
- search_heap.resize (8);
+ search_heap.resize(8);
OctreeKey new_key;
double smallest_squared_dist = squared_search_radius;
// get spatial voxel information
- double voxelSquaredDiameter = this->getVoxelSquaredDiameter (tree_depth);
+ double voxelSquaredDiameter = this->getVoxelSquaredDiameter(tree_depth);
// iterate over all children
- for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
- {
- if (this->branchHasChild (*node, child_idx))
- {
+ for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
+ if (this->branchHasChild(*node, child_idx)) {
PointT voxel_center;
search_heap[child_idx].key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
search_heap[child_idx].key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
// generate voxel center point for voxel at key
- this->genVoxelCenterFromOctreeKey (search_heap[child_idx].key, tree_depth, voxel_center);
+ this->genVoxelCenterFromOctreeKey(
+ search_heap[child_idx].key, tree_depth, voxel_center);
// generate new priority queue element
- search_heap[child_idx].node = this->getBranchChildPtr (*node, child_idx);
- search_heap[child_idx].point_distance = pointSquaredDist (voxel_center, point);
+ search_heap[child_idx].node = this->getBranchChildPtr(*node, child_idx);
+ search_heap[child_idx].point_distance = pointSquaredDist(voxel_center, point);
}
- else
- {
- search_heap[child_idx].point_distance = std::numeric_limits<float>::infinity ();
+ else {
+ search_heap[child_idx].point_distance = std::numeric_limits<float>::infinity();
}
}
- std::sort (search_heap.begin (), search_heap.end ());
+ std::sort(search_heap.begin(), search_heap.end());
// iterate over all children in priority queue
- // check if the distance to search candidate is smaller than the best point distance (smallest_squared_dist)
- while ((!search_heap.empty ()) && (search_heap.back ().point_distance <
- smallest_squared_dist + voxelSquaredDiameter / 4.0 + sqrt (smallest_squared_dist * voxelSquaredDiameter) - this->epsilon_))
- {
+ // check if the distance to search candidate is smaller than the best point distance
+ // (smallest_squared_dist)
+ while ((!search_heap.empty()) &&
+ (search_heap.back().point_distance <
+ smallest_squared_dist + voxelSquaredDiameter / 4.0 +
+ sqrt(smallest_squared_dist * voxelSquaredDiameter) - this->epsilon_)) {
const OctreeNode* child_node;
// read from priority queue element
- child_node = search_heap.back ().node;
- new_key = search_heap.back ().key;
+ child_node = search_heap.back().node;
+ new_key = search_heap.back().key;
- if (tree_depth < this->octree_depth_)
- {
+ if (tree_depth < this->octree_depth_) {
// we have not reached maximum tree depth
- smallest_squared_dist = getKNearestNeighborRecursive (point, K, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1,
- smallest_squared_dist, point_candidates);
+ smallest_squared_dist =
+ getKNearestNeighborRecursive(point,
+ K,
+ static_cast<const BranchNode*>(child_node),
+ new_key,
+ tree_depth + 1,
+ smallest_squared_dist,
+ point_candidates);
}
- else
- {
+ else {
// we reached leaf node level
std::vector<int> decoded_point_vector;
- const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
+ const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
// decode leaf node into decoded_point_vector
- (*child_leaf)->getPointIndices (decoded_point_vector);
+ (*child_leaf)->getPointIndices(decoded_point_vector);
// Linearly iterate over all decoded (unsorted) points
- for (const int &point_index : decoded_point_vector)
- {
+ for (const int& point_index : decoded_point_vector) {
- const PointT& candidate_point = this->getPointByIndex (point_index);
+ const PointT& candidate_point = this->getPointByIndex(point_index);
// calculate point distance to search point
- float squared_dist = pointSquaredDist (candidate_point, point);
+ float squared_dist = pointSquaredDist(candidate_point, point);
// check if a closer match is found
- if (squared_dist < smallest_squared_dist)
- {
+ if (squared_dist < smallest_squared_dist) {
prioPointQueueEntry point_entry;
point_entry.point_distance_ = squared_dist;
point_entry.point_idx_ = point_index;
- point_candidates.push_back (point_entry);
+ point_candidates.push_back(point_entry);
}
}
- std::sort (point_candidates.begin (), point_candidates.end ());
+ std::sort(point_candidates.begin(), point_candidates.end());
- if (point_candidates.size () > K)
- point_candidates.resize (K);
+ if (point_candidates.size() > K)
+ point_candidates.resize(K);
- if (point_candidates.size () == K)
- smallest_squared_dist = point_candidates.back ().point_distance_;
+ if (point_candidates.size() == K)
+ smallest_squared_dist = point_candidates.back().point_distance_;
}
// pop element from priority queue
- search_heap.pop_back ();
+ search_heap.pop_back();
}
return (smallest_squared_dist);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::getNeighborsWithinRadiusRecursive (
- const PointT & point, const double radiusSquared, const BranchNode* node, const OctreeKey& key,
- unsigned int tree_depth, std::vector<int>& k_indices, std::vector<float>& k_sqr_distances,
- unsigned int max_nn) const
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+void
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ getNeighborsWithinRadiusRecursive(const PointT& point,
+ const double radiusSquared,
+ const BranchNode* node,
+ const OctreeKey& key,
+ unsigned int tree_depth,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances,
+ unsigned int max_nn) const
{
// get spatial voxel information
- double voxel_squared_diameter = this->getVoxelSquaredDiameter (tree_depth);
+ double voxel_squared_diameter = this->getVoxelSquaredDiameter(tree_depth);
// iterate over all children
- for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
- {
- if (!this->branchHasChild (*node, child_idx))
+ for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
+ if (!this->branchHasChild(*node, child_idx))
continue;
const OctreeNode* child_node;
- child_node = this->getBranchChildPtr (*node, child_idx);
+ child_node = this->getBranchChildPtr(*node, child_idx);
OctreeKey new_key;
PointT voxel_center;
new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
// generate voxel center point for voxel at key
- this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center);
+ this->genVoxelCenterFromOctreeKey(new_key, tree_depth, voxel_center);
// calculate distance to search point
- squared_dist = pointSquaredDist (static_cast<const PointT&> (voxel_center), point);
+ squared_dist = pointSquaredDist(static_cast<const PointT&>(voxel_center), point);
// if distance is smaller than search radius
- if (squared_dist + this->epsilon_
- <= voxel_squared_diameter / 4.0 + radiusSquared + sqrt (voxel_squared_diameter * radiusSquared))
- {
+ if (squared_dist + this->epsilon_ <=
+ voxel_squared_diameter / 4.0 + radiusSquared +
+ sqrt(voxel_squared_diameter * radiusSquared)) {
- if (tree_depth < this->octree_depth_)
- {
+ if (tree_depth < this->octree_depth_) {
// we have not reached maximum tree depth
- getNeighborsWithinRadiusRecursive (point, radiusSquared, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1,
- k_indices, k_sqr_distances, max_nn);
- if (max_nn != 0 && k_indices.size () == static_cast<unsigned int> (max_nn))
+ getNeighborsWithinRadiusRecursive(point,
+ radiusSquared,
+ static_cast<const BranchNode*>(child_node),
+ new_key,
+ tree_depth + 1,
+ k_indices,
+ k_sqr_distances,
+ max_nn);
+ if (max_nn != 0 && k_indices.size() == static_cast<unsigned int>(max_nn))
return;
}
- else
- {
+ else {
// we reached leaf node level
- const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
+ const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
std::vector<int> decoded_point_vector;
// decode leaf node into decoded_point_vector
- (*child_leaf)->getPointIndices (decoded_point_vector);
+ (*child_leaf)->getPointIndices(decoded_point_vector);
// Linearly iterate over all decoded (unsorted) points
- for (const int &index : decoded_point_vector)
- {
- const PointT& candidate_point = this->getPointByIndex (index);
+ for (const int& index : decoded_point_vector) {
+ const PointT& candidate_point = this->getPointByIndex(index);
// calculate point distance to search point
- squared_dist = pointSquaredDist (candidate_point, point);
+ squared_dist = pointSquaredDist(candidate_point, point);
// check if a match is found
if (squared_dist > radiusSquared)
continue;
// add point to result vector
- k_indices.push_back (index);
- k_sqr_distances.push_back (squared_dist);
+ k_indices.push_back(index);
+ k_sqr_distances.push_back(squared_dist);
- if (max_nn != 0 && k_indices.size () == static_cast<unsigned int> (max_nn))
+ if (max_nn != 0 && k_indices.size() == static_cast<unsigned int>(max_nn))
return;
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::approxNearestSearchRecursive (const PointT & point,
- const BranchNode* node,
- const OctreeKey& key,
- unsigned int tree_depth,
- int& result_index,
- float& sqr_distance)
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+void
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ approxNearestSearchRecursive(const PointT& point,
+ const BranchNode* node,
+ const OctreeKey& key,
+ unsigned int tree_depth,
+ int& result_index,
+ float& sqr_distance)
{
OctreeKey minChildKey;
OctreeKey new_key;
const OctreeNode* child_node;
// set minimum voxel distance to maximum value
- double min_voxel_center_distance = std::numeric_limits<double>::max ();
+ double min_voxel_center_distance = std::numeric_limits<double>::max();
unsigned char min_child_idx = 0xFF;
// iterate over all children
- for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
- {
- if (!this->branchHasChild (*node, child_idx))
+ for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
+ if (!this->branchHasChild(*node, child_idx))
continue;
PointT voxel_center;
new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
// generate voxel center point for voxel at key
- this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center);
+ this->genVoxelCenterFromOctreeKey(new_key, tree_depth, voxel_center);
- voxelPointDist = pointSquaredDist (voxel_center, point);
+ voxelPointDist = pointSquaredDist(voxel_center, point);
// search for child voxel with shortest distance to search point
if (voxelPointDist >= min_voxel_center_distance)
}
// make sure we found at least one branch child
- assert(min_child_idx<8);
+ assert(min_child_idx < 8);
- child_node = this->getBranchChildPtr (*node, min_child_idx);
+ child_node = this->getBranchChildPtr(*node, min_child_idx);
- if (tree_depth < this->octree_depth_)
- {
+ if (tree_depth < this->octree_depth_) {
// we have not reached maximum tree depth
- approxNearestSearchRecursive (point, static_cast<const BranchNode*> (child_node), minChildKey, tree_depth + 1, result_index,
- sqr_distance);
+ approxNearestSearchRecursive(point,
+ static_cast<const BranchNode*>(child_node),
+ minChildKey,
+ tree_depth + 1,
+ result_index,
+ sqr_distance);
}
- else
- {
+ else {
// we reached leaf node level
std::vector<int> decoded_point_vector;
- const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
+ const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
- double smallest_squared_dist = std::numeric_limits<double>::max ();
+ double smallest_squared_dist = std::numeric_limits<double>::max();
// decode leaf node into decoded_point_vector
- (**child_leaf).getPointIndices (decoded_point_vector);
+ (**child_leaf).getPointIndices(decoded_point_vector);
// Linearly iterate over all decoded (unsorted) points
- for (const int &index : decoded_point_vector)
- {
- const PointT& candidate_point = this->getPointByIndex (index);
+ for (const int& index : decoded_point_vector) {
+ const PointT& candidate_point = this->getPointByIndex(index);
// calculate point distance to search point
- double squared_dist = pointSquaredDist (candidate_point, point);
+ double squared_dist = pointSquaredDist(candidate_point, point);
// check if a closer match is found
if (squared_dist >= smallest_squared_dist)
result_index = index;
smallest_squared_dist = squared_dist;
- sqr_distance = static_cast<float> (squared_dist);
+ sqr_distance = static_cast<float>(squared_dist);
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> float
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::pointSquaredDist (const PointT & point_a,
- const PointT & point_b) const
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+float
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ pointSquaredDist(const PointT& point_a, const PointT& point_b) const
{
- return (point_a.getVector3fMap () - point_b.getVector3fMap ()).squaredNorm ();
+ return (point_a.getVector3fMap() - point_b.getVector3fMap()).squaredNorm();
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::boxSearchRecursive (const Eigen::Vector3f &min_pt,
- const Eigen::Vector3f &max_pt,
- const BranchNode* node,
- const OctreeKey& key,
- unsigned int tree_depth,
- std::vector<int>& k_indices) const
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+void
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ boxSearchRecursive(const Eigen::Vector3f& min_pt,
+ const Eigen::Vector3f& max_pt,
+ const BranchNode* node,
+ const OctreeKey& key,
+ unsigned int tree_depth,
+ std::vector<int>& k_indices) const
{
// iterate over all children
- for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
- {
+ for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
const OctreeNode* child_node;
- child_node = this->getBranchChildPtr (*node, child_idx);
+ child_node = this->getBranchChildPtr(*node, child_idx);
if (!child_node)
continue;
Eigen::Vector3f lower_voxel_corner;
Eigen::Vector3f upper_voxel_corner;
// get voxel coordinates
- this->genVoxelBoundsFromOctreeKey (new_key, tree_depth, lower_voxel_corner, upper_voxel_corner);
+ this->genVoxelBoundsFromOctreeKey(
+ new_key, tree_depth, lower_voxel_corner, upper_voxel_corner);
// test if search region overlap with voxel space
- if ( !( (lower_voxel_corner (0) > max_pt (0)) || (min_pt (0) > upper_voxel_corner(0)) ||
- (lower_voxel_corner (1) > max_pt (1)) || (min_pt (1) > upper_voxel_corner(1)) ||
- (lower_voxel_corner (2) > max_pt (2)) || (min_pt (2) > upper_voxel_corner(2)) ) )
- {
+ if (!((lower_voxel_corner(0) > max_pt(0)) || (min_pt(0) > upper_voxel_corner(0)) ||
+ (lower_voxel_corner(1) > max_pt(1)) || (min_pt(1) > upper_voxel_corner(1)) ||
+ (lower_voxel_corner(2) > max_pt(2)) || (min_pt(2) > upper_voxel_corner(2)))) {
- if (tree_depth < this->octree_depth_)
- {
+ if (tree_depth < this->octree_depth_) {
// we have not reached maximum tree depth
- boxSearchRecursive (min_pt, max_pt, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1, k_indices);
+ boxSearchRecursive(min_pt,
+ max_pt,
+ static_cast<const BranchNode*>(child_node),
+ new_key,
+ tree_depth + 1,
+ k_indices);
}
- else
- {
+ else {
// we reached leaf node level
std::vector<int> decoded_point_vector;
- bool bInBox;
- const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
+ const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
// decode leaf node into decoded_point_vector
- (**child_leaf).getPointIndices (decoded_point_vector);
+ (**child_leaf).getPointIndices(decoded_point_vector);
// Linearly iterate over all decoded (unsorted) points
- for (const int &index : decoded_point_vector)
- {
- const PointT& candidate_point = this->getPointByIndex (index);
+ for (const int& index : decoded_point_vector) {
+ const PointT& candidate_point = this->getPointByIndex(index);
// check if point falls within search box
- bInBox = ( (candidate_point.x >= min_pt (0)) && (candidate_point.x <= max_pt (0)) &&
- (candidate_point.y >= min_pt (1)) && (candidate_point.y <= max_pt (1)) &&
- (candidate_point.z >= min_pt (2)) && (candidate_point.z <= max_pt (2)) );
+ bool bInBox =
+ ((candidate_point.x >= min_pt(0)) && (candidate_point.x <= max_pt(0)) &&
+ (candidate_point.y >= min_pt(1)) && (candidate_point.y <= max_pt(1)) &&
+ (candidate_point.z >= min_pt(2)) && (candidate_point.z <= max_pt(2)));
if (bInBox)
// add to result vector
- k_indices.push_back (index);
+ k_indices.push_back(index);
}
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::getIntersectedVoxelCenters (
- Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list,
- int max_voxel_count) const
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+int
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ getIntersectedVoxelCenters(Eigen::Vector3f origin,
+ Eigen::Vector3f direction,
+ AlignedPointTVector& voxel_center_list,
+ int max_voxel_count) const
{
OctreeKey key;
key.x = key.y = key.z = 0;
- voxel_center_list.clear ();
+ voxel_center_list.clear();
// Voxel child_idx remapping
unsigned char a = 0;
double min_x, min_y, min_z, max_x, max_y, max_z;
- initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
-
- if (std::max (std::max (min_x, min_y), min_z) < std::min (std::min (max_x, max_y), max_z))
- return getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
- voxel_center_list, max_voxel_count);
+ initIntersectedVoxel(origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
+
+ if (std::max(std::max(min_x, min_y), min_z) < std::min(std::min(max_x, max_y), max_z))
+ return getIntersectedVoxelCentersRecursive(min_x,
+ min_y,
+ min_z,
+ max_x,
+ max_y,
+ max_z,
+ a,
+ this->root_node_,
+ key,
+ voxel_center_list,
+ max_voxel_count);
return (0);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::getIntersectedVoxelIndices (
- Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector<int> &k_indices,
- int max_voxel_count) const
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+int
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ getIntersectedVoxelIndices(Eigen::Vector3f origin,
+ Eigen::Vector3f direction,
+ std::vector<int>& k_indices,
+ int max_voxel_count) const
{
OctreeKey key;
key.x = key.y = key.z = 0;
- k_indices.clear ();
+ k_indices.clear();
// Voxel child_idx remapping
unsigned char a = 0;
double min_x, min_y, min_z, max_x, max_y, max_z;
- initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
-
- if (std::max (std::max (min_x, min_y), min_z) < std::min (std::min (max_x, max_y), max_z))
- return getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
- k_indices, max_voxel_count);
+ initIntersectedVoxel(origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
+
+ if (std::max(std::max(min_x, min_y), min_z) < std::min(std::min(max_x, max_y), max_z))
+ return getIntersectedVoxelIndicesRecursive(min_x,
+ min_y,
+ min_z,
+ max_x,
+ max_y,
+ max_z,
+ a,
+ this->root_node_,
+ key,
+ k_indices,
+ max_voxel_count);
return (0);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::getIntersectedVoxelCentersRecursive (
- double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a,
- const OctreeNode* node, const OctreeKey& key, AlignedPointTVector &voxel_center_list, int max_voxel_count) const
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+int
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ getIntersectedVoxelCentersRecursive(double min_x,
+ double min_y,
+ double min_z,
+ double max_x,
+ double max_y,
+ double max_z,
+ unsigned char a,
+ const OctreeNode* node,
+ const OctreeKey& key,
+ AlignedPointTVector& voxel_center_list,
+ int max_voxel_count) const
{
if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
return (0);
// If leaf node, get voxel center and increment intersection count
- if (node->getNodeType () == LEAF_NODE)
- {
+ if (node->getNodeType() == LEAF_NODE) {
PointT newPoint;
- this->genLeafNodeCenterFromOctreeKey (key, newPoint);
+ this->genLeafNodeCenterFromOctreeKey(key, newPoint);
- voxel_center_list.push_back (newPoint);
+ voxel_center_list.push_back(newPoint);
return (1);
}
double mid_z = 0.5 * (min_z + max_z);
// First voxel node ray will intersect
- int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z);
+ int curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z);
// Child index, node and key
unsigned char child_idx;
- const OctreeNode *child_node;
OctreeKey child_key;
- do
- {
+ do {
if (curr_node != 0)
- child_idx = static_cast<unsigned char> (curr_node ^ a);
+ child_idx = static_cast<unsigned char>(curr_node ^ a);
else
child_idx = a;
// child_node == 0 if child_node doesn't exist
- child_node = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), child_idx);
+ const OctreeNode* child_node =
+ this->getBranchChildPtr(static_cast<const BranchNode&>(*node), child_idx);
// Generate new key for current branch voxel
child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2)));
// node intersected by the ray. Children that do not intersect will
// not be traversed.
- switch (curr_node)
- {
- case 0:
- if (child_node)
- voxel_count += getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, mid_x, mid_y, mid_z, a, child_node,
- child_key, voxel_center_list, max_voxel_count);
- curr_node = getNextIntersectedNode (mid_x, mid_y, mid_z, 4, 2, 1);
- break;
-
- case 1:
- if (child_node)
- voxel_count += getIntersectedVoxelCentersRecursive (min_x, min_y, mid_z, mid_x, mid_y, max_z, a, child_node,
- child_key, voxel_center_list, max_voxel_count);
- curr_node = getNextIntersectedNode (mid_x, mid_y, max_z, 5, 3, 8);
- break;
-
- case 2:
- if (child_node)
- voxel_count += getIntersectedVoxelCentersRecursive (min_x, mid_y, min_z, mid_x, max_y, mid_z, a, child_node,
- child_key, voxel_center_list, max_voxel_count);
- curr_node = getNextIntersectedNode (mid_x, max_y, mid_z, 6, 8, 3);
- break;
-
- case 3:
- if (child_node)
- voxel_count += getIntersectedVoxelCentersRecursive (min_x, mid_y, mid_z, mid_x, max_y, max_z, a, child_node,
- child_key, voxel_center_list, max_voxel_count);
- curr_node = getNextIntersectedNode (mid_x, max_y, max_z, 7, 8, 8);
- break;
-
- case 4:
- if (child_node)
- voxel_count += getIntersectedVoxelCentersRecursive (mid_x, min_y, min_z, max_x, mid_y, mid_z, a, child_node,
- child_key, voxel_center_list, max_voxel_count);
- curr_node = getNextIntersectedNode (max_x, mid_y, mid_z, 8, 6, 5);
- break;
-
- case 5:
- if (child_node)
- voxel_count += getIntersectedVoxelCentersRecursive (mid_x, min_y, mid_z, max_x, mid_y, max_z, a, child_node,
- child_key, voxel_center_list, max_voxel_count);
- curr_node = getNextIntersectedNode (max_x, mid_y, max_z, 8, 7, 8);
- break;
-
- case 6:
- if (child_node)
- voxel_count += getIntersectedVoxelCentersRecursive (mid_x, mid_y, min_z, max_x, max_y, mid_z, a, child_node,
- child_key, voxel_center_list, max_voxel_count);
- curr_node = getNextIntersectedNode (max_x, max_y, mid_z, 8, 8, 7);
- break;
-
- case 7:
- if (child_node)
- voxel_count += getIntersectedVoxelCentersRecursive (mid_x, mid_y, mid_z, max_x, max_y, max_z, a, child_node,
- child_key, voxel_center_list, max_voxel_count);
- curr_node = 8;
- break;
+ switch (curr_node) {
+ case 0:
+ if (child_node)
+ voxel_count += getIntersectedVoxelCentersRecursive(min_x,
+ min_y,
+ min_z,
+ mid_x,
+ mid_y,
+ mid_z,
+ a,
+ child_node,
+ child_key,
+ voxel_center_list,
+ max_voxel_count);
+ curr_node = getNextIntersectedNode(mid_x, mid_y, mid_z, 4, 2, 1);
+ break;
+
+ case 1:
+ if (child_node)
+ voxel_count += getIntersectedVoxelCentersRecursive(min_x,
+ min_y,
+ mid_z,
+ mid_x,
+ mid_y,
+ max_z,
+ a,
+ child_node,
+ child_key,
+ voxel_center_list,
+ max_voxel_count);
+ curr_node = getNextIntersectedNode(mid_x, mid_y, max_z, 5, 3, 8);
+ break;
+
+ case 2:
+ if (child_node)
+ voxel_count += getIntersectedVoxelCentersRecursive(min_x,
+ mid_y,
+ min_z,
+ mid_x,
+ max_y,
+ mid_z,
+ a,
+ child_node,
+ child_key,
+ voxel_center_list,
+ max_voxel_count);
+ curr_node = getNextIntersectedNode(mid_x, max_y, mid_z, 6, 8, 3);
+ break;
+
+ case 3:
+ if (child_node)
+ voxel_count += getIntersectedVoxelCentersRecursive(min_x,
+ mid_y,
+ mid_z,
+ mid_x,
+ max_y,
+ max_z,
+ a,
+ child_node,
+ child_key,
+ voxel_center_list,
+ max_voxel_count);
+ curr_node = getNextIntersectedNode(mid_x, max_y, max_z, 7, 8, 8);
+ break;
+
+ case 4:
+ if (child_node)
+ voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
+ min_y,
+ min_z,
+ max_x,
+ mid_y,
+ mid_z,
+ a,
+ child_node,
+ child_key,
+ voxel_center_list,
+ max_voxel_count);
+ curr_node = getNextIntersectedNode(max_x, mid_y, mid_z, 8, 6, 5);
+ break;
+
+ case 5:
+ if (child_node)
+ voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
+ min_y,
+ mid_z,
+ max_x,
+ mid_y,
+ max_z,
+ a,
+ child_node,
+ child_key,
+ voxel_center_list,
+ max_voxel_count);
+ curr_node = getNextIntersectedNode(max_x, mid_y, max_z, 8, 7, 8);
+ break;
+
+ case 6:
+ if (child_node)
+ voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
+ mid_y,
+ min_z,
+ max_x,
+ max_y,
+ mid_z,
+ a,
+ child_node,
+ child_key,
+ voxel_center_list,
+ max_voxel_count);
+ curr_node = getNextIntersectedNode(max_x, max_y, mid_z, 8, 8, 7);
+ break;
+
+ case 7:
+ if (child_node)
+ voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
+ mid_y,
+ mid_z,
+ max_x,
+ max_y,
+ max_z,
+ a,
+ child_node,
+ child_key,
+ voxel_center_list,
+ max_voxel_count);
+ curr_node = 8;
+ break;
}
} while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
return (voxel_count);
}
//////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointT, typename LeafContainerT, typename BranchContainerT> int
-pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::getIntersectedVoxelIndicesRecursive (
- double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a,
- const OctreeNode* node, const OctreeKey& key, std::vector<int> &k_indices, int max_voxel_count) const
+template <typename PointT, typename LeafContainerT, typename BranchContainerT>
+int
+pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
+ getIntersectedVoxelIndicesRecursive(double min_x,
+ double min_y,
+ double min_z,
+ double max_x,
+ double max_y,
+ double max_z,
+ unsigned char a,
+ const OctreeNode* node,
+ const OctreeKey& key,
+ std::vector<int>& k_indices,
+ int max_voxel_count) const
{
if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
return (0);
// If leaf node, get voxel center and increment intersection count
- if (node->getNodeType () == LEAF_NODE)
- {
- const LeafNode* leaf = static_cast<const LeafNode*> (node);
+ if (node->getNodeType() == LEAF_NODE) {
+ const LeafNode* leaf = static_cast<const LeafNode*>(node);
// decode leaf node into k_indices
- (*leaf)->getPointIndices (k_indices);
+ (*leaf)->getPointIndices(k_indices);
return (1);
}
double mid_z = 0.5 * (min_z + max_z);
// First voxel node ray will intersect
- int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z);
+ int curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z);
// Child index, node and key
unsigned char child_idx;
- const OctreeNode *child_node;
OctreeKey child_key;
- do
- {
+ do {
if (curr_node != 0)
- child_idx = static_cast<unsigned char> (curr_node ^ a);
+ child_idx = static_cast<unsigned char>(curr_node ^ a);
else
child_idx = a;
// child_node == 0 if child_node doesn't exist
- child_node = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), child_idx);
+ const OctreeNode* child_node =
+ this->getBranchChildPtr(static_cast<const BranchNode&>(*node), child_idx);
// Generate new key for current branch voxel
child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2)));
child_key.y = (key.y << 1) | (!!(child_idx & (1 << 1)));
// Recursively call each intersected child node, selecting the next
// node intersected by the ray. Children that do not intersect will
// not be traversed.
- switch (curr_node)
- {
- case 0:
- if (child_node)
- voxel_count += getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, mid_x, mid_y, mid_z, a, child_node,
- child_key, k_indices, max_voxel_count);
- curr_node = getNextIntersectedNode (mid_x, mid_y, mid_z, 4, 2, 1);
- break;
-
- case 1:
- if (child_node)
- voxel_count += getIntersectedVoxelIndicesRecursive (min_x, min_y, mid_z, mid_x, mid_y, max_z, a, child_node,
- child_key, k_indices, max_voxel_count);
- curr_node = getNextIntersectedNode (mid_x, mid_y, max_z, 5, 3, 8);
- break;
-
- case 2:
- if (child_node)
- voxel_count += getIntersectedVoxelIndicesRecursive (min_x, mid_y, min_z, mid_x, max_y, mid_z, a, child_node,
- child_key, k_indices, max_voxel_count);
- curr_node = getNextIntersectedNode (mid_x, max_y, mid_z, 6, 8, 3);
- break;
-
- case 3:
- if (child_node)
- voxel_count += getIntersectedVoxelIndicesRecursive (min_x, mid_y, mid_z, mid_x, max_y, max_z, a, child_node,
- child_key, k_indices, max_voxel_count);
- curr_node = getNextIntersectedNode (mid_x, max_y, max_z, 7, 8, 8);
- break;
-
- case 4:
- if (child_node)
- voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, min_y, min_z, max_x, mid_y, mid_z, a, child_node,
- child_key, k_indices, max_voxel_count);
- curr_node = getNextIntersectedNode (max_x, mid_y, mid_z, 8, 6, 5);
- break;
-
- case 5:
- if (child_node)
- voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, min_y, mid_z, max_x, mid_y, max_z, a, child_node,
- child_key, k_indices, max_voxel_count);
- curr_node = getNextIntersectedNode (max_x, mid_y, max_z, 8, 7, 8);
- break;
-
- case 6:
- if (child_node)
- voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, mid_y, min_z, max_x, max_y, mid_z, a, child_node,
- child_key, k_indices, max_voxel_count);
- curr_node = getNextIntersectedNode (max_x, max_y, mid_z, 8, 8, 7);
- break;
-
- case 7:
- if (child_node)
- voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, mid_y, mid_z, max_x, max_y, max_z, a, child_node,
- child_key, k_indices, max_voxel_count);
- curr_node = 8;
- break;
+ switch (curr_node) {
+ case 0:
+ if (child_node)
+ voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
+ min_y,
+ min_z,
+ mid_x,
+ mid_y,
+ mid_z,
+ a,
+ child_node,
+ child_key,
+ k_indices,
+ max_voxel_count);
+ curr_node = getNextIntersectedNode(mid_x, mid_y, mid_z, 4, 2, 1);
+ break;
+
+ case 1:
+ if (child_node)
+ voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
+ min_y,
+ mid_z,
+ mid_x,
+ mid_y,
+ max_z,
+ a,
+ child_node,
+ child_key,
+ k_indices,
+ max_voxel_count);
+ curr_node = getNextIntersectedNode(mid_x, mid_y, max_z, 5, 3, 8);
+ break;
+
+ case 2:
+ if (child_node)
+ voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
+ mid_y,
+ min_z,
+ mid_x,
+ max_y,
+ mid_z,
+ a,
+ child_node,
+ child_key,
+ k_indices,
+ max_voxel_count);
+ curr_node = getNextIntersectedNode(mid_x, max_y, mid_z, 6, 8, 3);
+ break;
+
+ case 3:
+ if (child_node)
+ voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
+ mid_y,
+ mid_z,
+ mid_x,
+ max_y,
+ max_z,
+ a,
+ child_node,
+ child_key,
+ k_indices,
+ max_voxel_count);
+ curr_node = getNextIntersectedNode(mid_x, max_y, max_z, 7, 8, 8);
+ break;
+
+ case 4:
+ if (child_node)
+ voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
+ min_y,
+ min_z,
+ max_x,
+ mid_y,
+ mid_z,
+ a,
+ child_node,
+ child_key,
+ k_indices,
+ max_voxel_count);
+ curr_node = getNextIntersectedNode(max_x, mid_y, mid_z, 8, 6, 5);
+ break;
+
+ case 5:
+ if (child_node)
+ voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
+ min_y,
+ mid_z,
+ max_x,
+ mid_y,
+ max_z,
+ a,
+ child_node,
+ child_key,
+ k_indices,
+ max_voxel_count);
+ curr_node = getNextIntersectedNode(max_x, mid_y, max_z, 8, 7, 8);
+ break;
+
+ case 6:
+ if (child_node)
+ voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
+ mid_y,
+ min_z,
+ max_x,
+ max_y,
+ mid_z,
+ a,
+ child_node,
+ child_key,
+ k_indices,
+ max_voxel_count);
+ curr_node = getNextIntersectedNode(max_x, max_y, mid_z, 8, 8, 7);
+ break;
+
+ case 7:
+ if (child_node)
+ voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
+ mid_y,
+ mid_z,
+ max_x,
+ max_y,
+ max_z,
+ a,
+ child_node,
+ child_key,
+ k_indices,
+ max_voxel_count);
+ curr_node = 8;
+ break;
}
} while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
return (voxel_count);
}
-#define PCL_INSTANTIATE_OctreePointCloudSearch(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
+#define PCL_INSTANTIATE_OctreePointCloudSearch(T) \
+ template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
-#endif // PCL_OCTREE_SEARCH_IMPL_H_
+#endif // PCL_OCTREE_SEARCH_IMPL_H_
#pragma once
-#include <pcl/octree/octree_base.h>
#include <pcl/octree/octree2buf_base.h>
+#include <pcl/octree/octree_base.h>
#include <pcl/octree/octree_iterator.h>
#include <pcl/octree/octree_pointcloud.h>
+#include <pcl/octree/octree_pointcloud_adjacency.h>
+#include <pcl/octree/octree_pointcloud_changedetector.h>
#include <pcl/octree/octree_pointcloud_density.h>
#include <pcl/octree/octree_pointcloud_occupancy.h>
-#include <pcl/octree/octree_pointcloud_singlepoint.h>
#include <pcl/octree/octree_pointcloud_pointvector.h>
-#include <pcl/octree/octree_pointcloud_changedetector.h>
+#include <pcl/octree/octree_pointcloud_singlepoint.h>
#include <pcl/octree/octree_pointcloud_voxelcentroid.h>
-#include <pcl/octree/octree_pointcloud_adjacency.h>
#include <pcl/octree/octree_search.h>
#include <vector>
-#include <pcl/octree/octree_nodes.h>
#include <pcl/octree/octree_container.h>
-#include <pcl/octree/octree_key.h>
#include <pcl/octree/octree_iterator.h>
+#include <pcl/octree/octree_key.h>
+#include <pcl/octree/octree_nodes.h>
+#include <pcl/pcl_macros.h>
+
+namespace pcl {
+namespace octree {
+
+template <typename ContainerT>
+class BufferedBranchNode : public OctreeNode {
+
+public:
+ /** \brief Empty constructor. */
+ BufferedBranchNode() : OctreeNode() { reset(); }
+
+ /** \brief Copy constructor. */
+ BufferedBranchNode(const BufferedBranchNode& source) : OctreeNode()
+ {
+ *this = source;
+ }
+
+ /** \brief Copy operator. */
+ inline BufferedBranchNode&
+ operator=(const BufferedBranchNode& source_arg)
+ {
+ memset(child_node_array_, 0, sizeof(child_node_array_));
+
+ for (unsigned char b = 0; b < 2; ++b)
+ for (unsigned char i = 0; i < 8; ++i)
+ if (source_arg.child_node_array_[b][i])
+ child_node_array_[b][i] = source_arg.child_node_array_[b][i]->deepCopy();
+
+ return (*this);
+ }
+
+ /** \brief Empty constructor. */
+ ~BufferedBranchNode() {}
+
+ /** \brief Method to perform a deep copy of the octree */
+ BufferedBranchNode*
+ deepCopy() const override
+ {
+ return new BufferedBranchNode(*this);
+ }
+
+ /** \brief Get child pointer in current branch node
+ * \param buffer_arg: buffer selector
+ * \param index_arg: index of child in node
+ * \return pointer to child node
+ */
+ inline OctreeNode*
+ getChildPtr(unsigned char buffer_arg, unsigned char index_arg) const
+ {
+ assert((buffer_arg < 2) && (index_arg < 8));
+ return child_node_array_[buffer_arg][index_arg];
+ }
+
+ /** \brief Set child pointer in current branch node
+ * \param buffer_arg: buffer selector
+ * \param index_arg: index of child in node
+ * \param newNode_arg: pointer to new child node
+ */
+ inline void
+ setChildPtr(unsigned char buffer_arg,
+ unsigned char index_arg,
+ OctreeNode* newNode_arg)
+ {
+ assert((buffer_arg < 2) && (index_arg < 8));
+ child_node_array_[buffer_arg][index_arg] = newNode_arg;
+ }
+
+ /** \brief Check if branch is pointing to a particular child node
+ * \param buffer_arg: buffer selector
+ * \param index_arg: index of child in node
+ * \return "true" if pointer to child node exists; "false" otherwise
+ */
+ inline bool
+ hasChild(unsigned char buffer_arg, unsigned char index_arg) const
+ {
+ assert((buffer_arg < 2) && (index_arg < 8));
+ return (child_node_array_[buffer_arg][index_arg] != nullptr);
+ }
+
+ /** \brief Get the type of octree node. Returns LEAVE_NODE type */
+ node_type_t
+ getNodeType() const override
+ {
+ return BRANCH_NODE;
+ }
+
+ /** \brief Reset branch node container for every branch buffer. */
+ inline void
+ reset()
+ {
+ memset(&child_node_array_[0][0], 0, sizeof(OctreeNode*) * 8 * 2);
+ }
+
+ /** \brief Get const pointer to container */
+ const ContainerT* operator->() const { return &container_; }
+
+ /** \brief Get pointer to container */
+ ContainerT* operator->() { return &container_; }
+
+ /** \brief Get const reference to container */
+ const ContainerT& operator*() const { return container_; }
+
+ /** \brief Get reference to container */
+ ContainerT& operator*() { return container_; }
+
+ /** \brief Get const reference to container */
+ const ContainerT&
+ getContainer() const
+ {
+ return container_;
+ }
+
+ /** \brief Get reference to container */
+ ContainerT&
+ getContainer()
+ {
+ return container_;
+ }
+
+ /** \brief Get const pointer to container */
+ const ContainerT*
+ getContainerPtr() const
+ {
+ return &container_;
+ }
+
+ /** \brief Get pointer to container */
+ ContainerT*
+ getContainerPtr()
+ {
+ return &container_;
+ }
+
+protected:
+ ContainerT container_;
+
+ OctreeNode* child_node_array_[2][8];
+};
+
+/** \brief @b Octree double buffer class
+ *
+ * This octree implementation keeps two separate octree structures in memory
+ * which allows for differentially comparison of the octree structures (change
+ * detection, differential encoding).
+ * \note The tree depth defines the maximum amount of octree voxels / leaf nodes (should
+ * be initially defined).
+ * \note All leaf nodes are addressed by integer indices.
+ * \note The tree depth equates to the bit length of the voxel indices.
+ * \ingroup octree
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+template <typename LeafContainerT = int,
+ typename BranchContainerT = OctreeContainerEmpty>
+class Octree2BufBase {
+
+public:
+ using OctreeT = Octree2BufBase<LeafContainerT, BranchContainerT>;
+
+ // iterators are friends
+ friend class OctreeIteratorBase<OctreeT>;
+ friend class OctreeDepthFirstIterator<OctreeT>;
+ friend class OctreeBreadthFirstIterator<OctreeT>;
+ friend class OctreeLeafNodeDepthFirstIterator<OctreeT>;
+ friend class OctreeLeafNodeBreadthFirstIterator<OctreeT>;
+
+ using BranchNode = BufferedBranchNode<BranchContainerT>;
+ using LeafNode = OctreeLeafNode<LeafContainerT>;
+
+ using BranchContainer = BranchContainerT;
+ using LeafContainer = LeafContainerT;
+
+ // Octree default iterators
+ using Iterator = OctreeDepthFirstIterator<OctreeT>;
+ using ConstIterator = const OctreeDepthFirstIterator<OctreeT>;
+ Iterator
+ begin(unsigned int max_depth_arg = 0)
+ {
+ return Iterator(this, max_depth_arg);
+ };
+ const Iterator
+ end()
+ {
+ return Iterator();
+ };
+
+ // Octree leaf node iterators
+ // The previous deprecated names
+ // LeafNodeIterator and ConstLeafNodeIterator are deprecated.
+ // Please use LeafNodeDepthFirstIterator and ConstLeafNodeDepthFirstIterator instead.
+ using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator<OctreeT>;
+ using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator<OctreeT>;
+
+ PCL_DEPRECATED("use leaf_depth_begin() instead")
+ LeafNodeIterator
+ leaf_begin(unsigned int max_depth_arg = 0)
+ {
+ return LeafNodeIterator(this, max_depth_arg);
+ };
+
+ PCL_DEPRECATED("use leaf_depth_end() instead") const LeafNodeIterator leaf_end()
+ {
+ return LeafNodeIterator();
+ };
+
+ // The currently valide names
+ using LeafNodeDepthFirstIterator = OctreeLeafNodeDepthFirstIterator<OctreeT>;
+ using ConstLeafNodeDepthFirstIterator =
+ const OctreeLeafNodeDepthFirstIterator<OctreeT>;
+ LeafNodeDepthFirstIterator
+ leaf_depth_begin(unsigned int max_depth_arg = 0)
+ {
+ return LeafNodeDepthFirstIterator(this, max_depth_arg);
+ };
+
+ const LeafNodeDepthFirstIterator
+ leaf_depth_end()
+ {
+ return LeafNodeDepthFirstIterator();
+ };
+
+ // Octree depth-first iterators
+ using DepthFirstIterator = OctreeDepthFirstIterator<OctreeT>;
+ using ConstDepthFirstIterator = const OctreeDepthFirstIterator<OctreeT>;
+ DepthFirstIterator
+ depth_begin(unsigned int maxDepth_arg = 0)
+ {
+ return DepthFirstIterator(this, maxDepth_arg);
+ };
+ const DepthFirstIterator
+ depth_end()
+ {
+ return DepthFirstIterator();
+ };
+
+ // Octree breadth-first iterators
+ using BreadthFirstIterator = OctreeBreadthFirstIterator<OctreeT>;
+ using ConstBreadthFirstIterator = const OctreeBreadthFirstIterator<OctreeT>;
+ BreadthFirstIterator
+ breadth_begin(unsigned int max_depth_arg = 0)
+ {
+ return BreadthFirstIterator(this, max_depth_arg);
+ };
+ const BreadthFirstIterator
+ breadth_end()
+ {
+ return BreadthFirstIterator();
+ };
+
+ // Octree leaf node iterators
+ using LeafNodeBreadthIterator = OctreeLeafNodeBreadthFirstIterator<OctreeT>;
+ using ConstLeafNodeBreadthIterator =
+ const OctreeLeafNodeBreadthFirstIterator<OctreeT>;
+
+ LeafNodeBreadthIterator
+ leaf_breadth_begin(unsigned int max_depth_arg = 0u)
+ {
+ return LeafNodeBreadthIterator(this,
+ max_depth_arg ? max_depth_arg : this->octree_depth_);
+ };
+
+ const LeafNodeBreadthIterator
+ leaf_breadth_end()
+ {
+ return LeafNodeBreadthIterator(this, 0, nullptr);
+ };
+
+ /** \brief Empty constructor. */
+ Octree2BufBase();
+
+ /** \brief Empty deconstructor. */
+ virtual ~Octree2BufBase();
+
+ /** \brief Copy constructor. */
+ Octree2BufBase(const Octree2BufBase& source)
+ : leaf_count_(source.leaf_count_)
+ , branch_count_(source.branch_count_)
+ , root_node_(new (BranchNode)(*(source.root_node_)))
+ , depth_mask_(source.depth_mask_)
+ , max_key_(source.max_key_)
+ , buffer_selector_(source.buffer_selector_)
+ , tree_dirty_flag_(source.tree_dirty_flag_)
+ , octree_depth_(source.octree_depth_)
+ , dynamic_depth_enabled_(source.dynamic_depth_enabled_)
+ {}
+
+ /** \brief Copy constructor. */
+ inline Octree2BufBase&
+ operator=(const Octree2BufBase& source)
+ {
+ leaf_count_ = source.leaf_count_;
+ branch_count_ = source.branch_count_;
+ root_node_ = new (BranchNode)(*(source.root_node_));
+ depth_mask_ = source.depth_mask_;
+ max_key_ = source.max_key_;
+ buffer_selector_ = source.buffer_selector_;
+ tree_dirty_flag_ = source.tree_dirty_flag_;
+ octree_depth_ = source.octree_depth_;
+ dynamic_depth_enabled_ = source.dynamic_depth_enabled_;
+ return (*this);
+ }
+
+ /** \brief Set the maximum amount of voxels per dimension.
+ * \param max_voxel_index_arg: maximum amount of voxels per dimension
+ */
+ void
+ setMaxVoxelIndex(unsigned int max_voxel_index_arg);
+
+ /** \brief Set the maximum depth of the octree.
+ * \param depth_arg: maximum depth of octree
+ */
+ void
+ setTreeDepth(unsigned int depth_arg);
+
+ /** \brief Get the maximum depth of the octree.
+ * \return depth_arg: maximum depth of octree
+ */
+ inline unsigned int
+ getTreeDepth() const
+ {
+ return this->octree_depth_;
+ }
+
+ /** \brief Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
+ * \note If leaf node already exist, this method returns the existing node
+ * \param idx_x_arg: index of leaf node in the X axis.
+ * \param idx_y_arg: index of leaf node in the Y axis.
+ * \param idx_z_arg: index of leaf node in the Z axis.
+ * \return pointer to new leaf node container.
+ */
+ LeafContainerT*
+ createLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
+
+ /** \brief Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
+ * \note If leaf node already exist, this method returns the existing node
+ * \param idx_x_arg: index of leaf node in the X axis.
+ * \param idx_y_arg: index of leaf node in the Y axis.
+ * \param idx_z_arg: index of leaf node in the Z axis.
+ * \return pointer to leaf node container if found, null pointer otherwise.
+ */
+ LeafContainerT*
+ findLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
+
+ /** \brief Check for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
+ * \param idx_x_arg: index of leaf node in the X axis.
+ * \param idx_y_arg: index of leaf node in the Y axis.
+ * \param idx_z_arg: index of leaf node in the Z axis.
+ * \return "true" if leaf node search is successful, otherwise it returns "false".
+ */
+ bool
+ existLeaf(unsigned int idx_x_arg,
+ unsigned int idx_y_arg,
+ unsigned int idx_z_arg) const;
+
+ /** \brief Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
+ * \param idx_x_arg: index of leaf node in the X axis.
+ * \param idx_y_arg: index of leaf node in the Y axis.
+ * \param idx_z_arg: index of leaf node in the Z axis.
+ */
+ void
+ removeLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
+
+ /** \brief Return the amount of existing leafs in the octree.
+ * \return amount of registered leaf nodes.
+ */
+ inline std::size_t
+ getLeafCount() const
+ {
+ return (leaf_count_);
+ }
+
+ /** \brief Return the amount of existing branches in the octree.
+ * \return amount of branch nodes.
+ */
+ inline std::size_t
+ getBranchCount() const
+ {
+ return (branch_count_);
+ }
+
+ /** \brief Delete the octree structure and its leaf nodes.
+ */
+ void
+ deleteTree();
+
+ /** \brief Delete octree structure of previous buffer. */
+ inline void
+ deletePreviousBuffer()
+ {
+ treeCleanUpRecursive(root_node_);
+ }
+
+ /** \brief Delete the octree structure in the current buffer. */
+ inline void
+ deleteCurrentBuffer()
+ {
+ buffer_selector_ = !buffer_selector_;
+ treeCleanUpRecursive(root_node_);
+ leaf_count_ = 0;
+ }
+
+ /** \brief Switch buffers and reset current octree structure. */
+ void
+ switchBuffers();
+
+ /** \brief Serialize octree into a binary output vector describing its branch node
+ * structure.
+ * \param binary_tree_out_arg: reference to output vector for writing binary
+ * tree structure.
+ * \param do_XOR_encoding_arg: select if binary tree structure should be generated
+ * based on current octree (false) of based on a XOR comparison between current and
+ * previous octree
+ **/
+ void
+ serializeTree(std::vector<char>& binary_tree_out_arg,
+ bool do_XOR_encoding_arg = false);
+
+ /** \brief Serialize octree into a binary output vector describing its branch node
+ * structure and and push all DataT elements stored in the octree to a vector.
+ * \param binary_tree_out_arg: reference to output vector for writing binary tree
+ * structure.
+ * \param leaf_container_vector_arg: pointer to all LeafContainerT objects in the
+ * octree
+ * \param do_XOR_encoding_arg: select if binary tree structure should be
+ * generated based on current octree (false) of based on a XOR comparison between
+ * current and previous octree
+ **/
+ void
+ serializeTree(std::vector<char>& binary_tree_out_arg,
+ std::vector<LeafContainerT*>& leaf_container_vector_arg,
+ bool do_XOR_encoding_arg = false);
+
+ /** \brief Outputs a vector of all DataT elements that are stored within the octree
+ * leaf nodes.
+ * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects
+ * in the octree
+ */
+ void
+ serializeLeafs(std::vector<LeafContainerT*>& leaf_container_vector_arg);
+
+ /** \brief Outputs a vector of all DataT elements from leaf nodes, that do not exist
+ * in the previous octree buffer.
+ * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects
+ * in the octree
+ */
+ void
+ serializeNewLeafs(std::vector<LeafContainerT*>& leaf_container_vector_arg);
+
+ /** \brief Deserialize a binary octree description vector and create a corresponding
+ * octree structure. Leaf nodes are initialized with getDataTByKey(..).
+ * \param binary_tree_in_arg: reference to input vector for reading binary tree
+ * structure.
+ * \param do_XOR_decoding_arg: select if binary tree structure is based on current
+ * octree (false) of based on a XOR comparison between current and previous octree
+ */
+ void
+ deserializeTree(std::vector<char>& binary_tree_in_arg,
+ bool do_XOR_decoding_arg = false);
+
+ /** \brief Deserialize a binary octree description and create a corresponding octree
+ * structure. Leaf nodes are initialized with DataT elements from the dataVector.
+ * \param binary_tree_in_arg: reference to inpvectoream for reading binary tree
+ * structure.
+ * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects
+ * in the octree
+ * \param do_XOR_decoding_arg: select if binary tree structure is based on current
+ * octree (false) of based on a XOR comparison between current and previous octree
+ */
+ void
+ deserializeTree(std::vector<char>& binary_tree_in_arg,
+ std::vector<LeafContainerT*>& leaf_container_vector_arg,
+ bool do_XOR_decoding_arg = false);
+
+protected:
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Protected octree methods based on octree keys
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /** \brief Retrieve root node */
+ OctreeNode*
+ getRootNode() const
+ {
+ return (this->root_node_);
+ }
+
+ /** \brief Find leaf node
+ * \param key_arg: octree key addressing a leaf node.
+ * \return pointer to leaf container. If leaf node is not found, this pointer returns
+ * 0.
+ */
+ inline LeafContainerT*
+ findLeaf(const OctreeKey& key_arg) const
+ {
+ LeafContainerT* result = nullptr;
+ findLeafRecursive(key_arg, depth_mask_, root_node_, result);
+ return result;
+ }
+
+ /** \brief Create a leaf node.
+ * \note If the leaf node at the given octree node does not exist, it will be created
+ * and added to the tree.
+ * \param key_arg: octree key addressing a leaf node.
+ * \return pointer to an existing or created leaf container.
+ */
+ inline LeafContainerT*
+ createLeaf(const OctreeKey& key_arg)
+ {
+ LeafNode* leaf_node;
+ BranchNode* leaf_node_parent;
+
+ createLeafRecursive(
+ key_arg, depth_mask_, root_node_, leaf_node, leaf_node_parent, false);
+
+ LeafContainerT* ret = leaf_node->getContainerPtr();
+
+ return ret;
+ }
+
+ /** \brief Check if leaf doesn't exist in the octree
+ * \param key_arg: octree key addressing a leaf node.
+ * \return "true" if leaf node is found; "false" otherwise
+ */
+ inline bool
+ existLeaf(const OctreeKey& key_arg) const
+ {
+ return (findLeaf(key_arg) != nullptr);
+ }
+
+ /** \brief Remove leaf node from octree
+ * \param key_arg: octree key addressing a leaf node.
+ */
+ inline void
+ removeLeaf(const OctreeKey& key_arg)
+ {
+ if (key_arg <= max_key_) {
+ deleteLeafRecursive(key_arg, depth_mask_, root_node_);
+
+ // we changed the octree structure -> dirty
+ tree_dirty_flag_ = true;
+ }
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Branch node accessor inline functions
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /** \brief Check if branch is pointing to a particular child node
+ * \param branch_arg: reference to octree branch class
+ * \param child_idx_arg: index to child node
+ * \return "true" if pointer to child node exists; "false" otherwise
+ */
+ inline bool
+ branchHasChild(const BranchNode& branch_arg, unsigned char child_idx_arg) const
+ {
+ // test occupancyByte for child existence
+ return (branch_arg.getChildPtr(buffer_selector_, child_idx_arg) != nullptr);
+ }
+
+ /** \brief Retrieve a child node pointer for child node at child_idx.
+ * \param branch_arg: reference to octree branch class
+ * \param child_idx_arg: index to child node
+ * \return pointer to octree child node class
+ */
+ inline OctreeNode*
+ getBranchChildPtr(const BranchNode& branch_arg, unsigned char child_idx_arg) const
+ {
+ return branch_arg.getChildPtr(buffer_selector_, child_idx_arg);
+ }
+
+ /** \brief Assign new child node to branch
+ * \param branch_arg: reference to octree branch class
+ * \param child_idx_arg: index to child node
+ * \param new_child_arg: pointer to new child node
+ */
+ inline void
+ setBranchChildPtr(BranchNode& branch_arg,
+ unsigned char child_idx_arg,
+ OctreeNode* new_child_arg)
+ {
+ branch_arg.setChildPtr(buffer_selector_, child_idx_arg, new_child_arg);
+ }
+
+ /** \brief Generate bit pattern reflecting the existence of child node pointers for
+ * current buffer
+ * \param branch_arg: reference to octree branch class
+ * \return a single byte with 8 bits of child node information
+ */
+ inline char
+ getBranchBitPattern(const BranchNode& branch_arg) const
+ {
+ char node_bits;
+
+ // create bit pattern
+ node_bits = 0;
+ for (unsigned char i = 0; i < 8; i++) {
+ const OctreeNode* child = branch_arg.getChildPtr(buffer_selector_, i);
+ node_bits |= static_cast<char>((!!child) << i);
+ }
+
+ return (node_bits);
+ }
+
+ /** \brief Generate bit pattern reflecting the existence of child node pointers in
+ * specific buffer
+ * \param branch_arg: reference to octree branch class
+ * \param bufferSelector_arg: buffer selector
+ * \return a single byte with 8 bits of child node information
+ */
+ inline char
+ getBranchBitPattern(const BranchNode& branch_arg,
+ unsigned char bufferSelector_arg) const
+ {
+ char node_bits;
+
+ // create bit pattern
+ node_bits = 0;
+ for (unsigned char i = 0; i < 8; i++) {
+ const OctreeNode* child = branch_arg.getChildPtr(bufferSelector_arg, i);
+ node_bits |= static_cast<char>((!!child) << i);
+ }
+
+ return (node_bits);
+ }
+
+ /** \brief Generate XOR bit pattern reflecting differences between the two octree
+ * buffers
+ * \param branch_arg: reference to octree branch class
+ * \return a single byte with 8 bits of child node XOR difference information
+ */
+ inline char
+ getBranchXORBitPattern(const BranchNode& branch_arg) const
+ {
+ char node_bits[2];
+
+ // create bit pattern for both buffers
+ node_bits[0] = node_bits[1] = 0;
+
+ for (unsigned char i = 0; i < 8; i++) {
+ const OctreeNode* childA = branch_arg.getChildPtr(0, i);
+ const OctreeNode* childB = branch_arg.getChildPtr(1, i);
+
+ node_bits[0] |= static_cast<char>((!!childA) << i);
+ node_bits[1] |= static_cast<char>((!!childB) << i);
+ }
+
+ return node_bits[0] ^ node_bits[1];
+ }
+
+ /** \brief Test if branch changed between previous and current buffer
+ * \param branch_arg: reference to octree branch class
+ * \return "true", if child node information differs between current and previous
+ * octree buffer
+ */
+ inline bool
+ hasBranchChanges(const BranchNode& branch_arg) const
+ {
+ return (getBranchXORBitPattern(branch_arg) > 0);
+ }
+
+ /** \brief Delete child node and all its subchilds from octree in specific buffer
+ * \param branch_arg: reference to octree branch class
+ * \param buffer_selector_arg: buffer selector
+ * \param child_idx_arg: index to child node
+ */
+ inline void
+ deleteBranchChild(BranchNode& branch_arg,
+ unsigned char buffer_selector_arg,
+ unsigned char child_idx_arg)
+ {
+ if (branch_arg.hasChild(buffer_selector_arg, child_idx_arg)) {
+ OctreeNode* branchChild =
+ branch_arg.getChildPtr(buffer_selector_arg, child_idx_arg);
+
+ switch (branchChild->getNodeType()) {
+ case BRANCH_NODE: {
+ // free child branch recursively
+ deleteBranch(*static_cast<BranchNode*>(branchChild));
+
+ // delete unused branch
+ delete (branchChild);
+ break;
+ }
+
+ case LEAF_NODE: {
+ // push unused leaf to branch pool
+ delete (branchChild);
+ break;
+ }
+ default:
+ break;
+ }
+
+ // set branch child pointer to 0
+ branch_arg.setChildPtr(buffer_selector_arg, child_idx_arg, nullptr);
+ }
+ }
+
+ /** \brief Delete child node and all its subchilds from octree in current buffer
+ * \param branch_arg: reference to octree branch class
+ * \param child_idx_arg: index to child node
+ */
+ inline void
+ deleteBranchChild(BranchNode& branch_arg, unsigned char child_idx_arg)
+ {
+ deleteBranchChild(branch_arg, buffer_selector_, child_idx_arg);
+ }
+
+ /** \brief Delete branch and all its subchilds from octree (both buffers)
+ * \param branch_arg: reference to octree branch class
+ */
+ inline void
+ deleteBranch(BranchNode& branch_arg)
+ {
+ // delete all branch node children
+ for (char i = 0; i < 8; i++) {
+
+ if (branch_arg.getChildPtr(0, i) == branch_arg.getChildPtr(1, i)) {
+ // reference was copied - there is only one child instance to be deleted
+ deleteBranchChild(branch_arg, 0, i);
+
+ // remove pointers from both buffers
+ branch_arg.setChildPtr(0, i, nullptr);
+ branch_arg.setChildPtr(1, i, nullptr);
+ }
+ else {
+ deleteBranchChild(branch_arg, 0, i);
+ deleteBranchChild(branch_arg, 1, i);
+ }
+ }
+ }
+
+ /** \brief Fetch and add a new branch child to a branch class in current buffer
+ * \param branch_arg: reference to octree branch class
+ * \param child_idx_arg: index to child node
+ * \return pointer of new branch child to this reference
+ */
+ inline BranchNode*
+ createBranchChild(BranchNode& branch_arg, unsigned char child_idx_arg)
+ {
+ BranchNode* new_branch_child = new BranchNode();
+
+ branch_arg.setChildPtr(
+ buffer_selector_, child_idx_arg, static_cast<OctreeNode*>(new_branch_child));
+
+ return new_branch_child;
+ }
+
+ /** \brief Fetch and add a new leaf child to a branch class
+ * \param branch_arg: reference to octree branch class
+ * \param child_idx_arg: index to child node
+ * \return pointer of new leaf child to this reference
+ */
+ inline LeafNode*
+ createLeafChild(BranchNode& branch_arg, unsigned char child_idx_arg)
+ {
+ LeafNode* new_leaf_child = new LeafNode();
+
+ branch_arg.setChildPtr(buffer_selector_, child_idx_arg, new_leaf_child);
+
+ return new_leaf_child;
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Recursive octree methods
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /** \brief Create a leaf node at octree key. If leaf node does already exist, it is
+ * returned.
+ * \param key_arg: reference to an octree key
+ * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth
+ * indicator
+ * \param branch_arg: current branch node
+ * \param return_leaf_arg: return pointer to leaf container
+ * \param parent_of_leaf_arg: return pointer to parent of leaf node
+ * \param branch_reset_arg: Reset pointer array of current branch
+ * \return depth mask at which leaf node was created/found
+ **/
+ unsigned int
+ createLeafRecursive(const OctreeKey& key_arg,
+ unsigned int depth_mask_arg,
+ BranchNode* branch_arg,
+ LeafNode*& return_leaf_arg,
+ BranchNode*& parent_of_leaf_arg,
+ bool branch_reset_arg = false);
+
+ /** \brief Recursively search for a given leaf node and return a pointer.
+ * \note If leaf node does not exist, a 0 pointer is returned.
+ * \param key_arg: reference to an octree key
+ * \param depth_mask_arg: depth mask used for octree key analysis and for branch
+ * depth indicator
+ * \param branch_arg: current branch node
+ * \param result_arg: pointer to leaf container class
+ **/
+ void
+ findLeafRecursive(const OctreeKey& key_arg,
+ unsigned int depth_mask_arg,
+ BranchNode* branch_arg,
+ LeafContainerT*& result_arg) const;
+
+ /** \brief Recursively search and delete leaf node
+ * \param key_arg: reference to an octree key
+ * \param depth_mask_arg: depth mask used for octree key analysis and branch depth
+ * indicator
+ * \param branch_arg: current branch node
+ * \return "true" if branch does not contain any childs; "false" otherwise. This
+ * indicates if current branch can be deleted.
+ **/
+ bool
+ deleteLeafRecursive(const OctreeKey& key_arg,
+ unsigned int depth_mask_arg,
+ BranchNode* branch_arg);
+
+ /** \brief Recursively explore the octree and output binary octree description
+ * together with a vector of leaf node DataT content.
+ * \param branch_arg: current branch node
+ * \param key_arg: reference to an octree key
+ * \param binary_tree_out_arg: binary output vector
+ * \param leaf_container_vector_arg: vector to return pointers to all leaf container
+ * in the tree.
+ * \param do_XOR_encoding_arg: select if binary tree structure should be generated
+ * based on current octree (false) of based on a XOR comparison between current and
+ * previous octree
+ * \param new_leafs_filter_arg: execute callback only for leaf nodes that did not
+ * exist in preceding buffer
+ **/
+ void
+ serializeTreeRecursive(
+ BranchNode* branch_arg,
+ OctreeKey& key_arg,
+ std::vector<char>* binary_tree_out_arg,
+ typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
+ bool do_XOR_encoding_arg = false,
+ bool new_leafs_filter_arg = false);
+
+ /** \brief Rebuild an octree based on binary XOR octree description and DataT objects
+ * for leaf node initialization.
+ * \param branch_arg: current branch node
+ * \param depth_mask_arg: depth mask used for octree key analysis and branch depth
+ * indicator
+ * \param key_arg: reference to an octree key
+ * \param binary_tree_in_it_arg iterator of binary input data
+ * \param binary_tree_in_it_end_arg
+ * \param leaf_container_vector_it_arg: iterator pointing to leaf container pointers
+ * to be added to a leaf node
+ * \param leaf_container_vector_it_end_arg: iterator pointing to leaf container
+ * pointers pointing to last object in input container.
+ * \param branch_reset_arg: Reset pointer array of current branch
+ * \param do_XOR_decoding_arg: select if binary tree structure is based on current
+ * octree (false) of based on a XOR comparison between current and previous octree
+ **/
+ void
+ deserializeTreeRecursive(
+ BranchNode* branch_arg,
+ unsigned int depth_mask_arg,
+ OctreeKey& key_arg,
+ typename std::vector<char>::const_iterator& binary_tree_in_it_arg,
+ typename std::vector<char>::const_iterator& binary_tree_in_it_end_arg,
+ typename std::vector<LeafContainerT*>::const_iterator*
+ leaf_container_vector_it_arg,
+ typename std::vector<LeafContainerT*>::const_iterator*
+ leaf_container_vector_it_end_arg,
+ bool branch_reset_arg = false,
+ bool do_XOR_decoding_arg = false);
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Serialization callbacks
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /** \brief Callback executed for every leaf node data during serialization
+ **/
+ virtual void
+ serializeTreeCallback(LeafContainerT&, const OctreeKey&)
+ {}
+
+ /** \brief Callback executed for every leaf node data during deserialization
+ **/
+ virtual void
+ deserializeTreeCallback(LeafContainerT&, const OctreeKey&)
+ {}
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Helpers
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /** \brief Recursively explore the octree and remove unused branch and leaf nodes
+ * \param branch_arg: current branch node
+ **/
+ void
+ treeCleanUpRecursive(BranchNode* branch_arg);
+
+ /** \brief Helper function to calculate the binary logarithm
+ * \param n_arg: some value
+ * \return binary logarithm (log2) of argument n_arg
+ */
+ PCL_DEPRECATED("use std::log2 instead") inline double Log2(double n_arg)
+ {
+ return std::log2(n_arg);
+ }
+
+ /** \brief Test if octree is able to dynamically change its depth. This is required
+ * for adaptive bounding box adjustment.
+ * \return "false" - not resizeable due to XOR serialization
+ **/
+ inline bool
+ octreeCanResize()
+ {
+ return (false);
+ }
+
+ /** \brief Prints binary representation of a byte - used for debugging
+ * \param data_arg - byte to be printed to stdout
+ **/
+ inline void
+ printBinary(char data_arg)
+ {
+ unsigned char mask = 1; // Bit mask
+
+ // Extract the bits
+ for (int i = 0; i < 8; i++) {
+ // Mask each bit in the byte and print it
+ std::cout << ((data_arg & (mask << i)) ? "1" : "0");
+ }
+ std::cout << std::endl;
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Globals
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /** \brief Amount of leaf nodes **/
+ std::size_t leaf_count_;
+
+ /** \brief Amount of branch nodes **/
+ std::size_t branch_count_;
+
+ /** \brief Pointer to root branch node of octree **/
+ BranchNode* root_node_;
+
+ /** \brief Depth mask based on octree depth **/
+ unsigned int depth_mask_;
+
+ /** \brief key range */
+ OctreeKey max_key_;
+
+ /** \brief Currently active octree buffer **/
+ unsigned char buffer_selector_;
+
+ // flags indicating if unused branches and leafs might exist in previous buffer
+ bool tree_dirty_flag_;
+ /** \brief Octree depth */
+ unsigned int octree_depth_;
-namespace pcl
-{
- namespace octree
- {
-
- template<typename ContainerT>
- class BufferedBranchNode : public OctreeNode
- {
-
- public:
- /** \brief Empty constructor. */
- BufferedBranchNode () : OctreeNode()
- {
- reset ();
- }
-
- /** \brief Copy constructor. */
- BufferedBranchNode (const BufferedBranchNode& source) : OctreeNode()
- {
- *this = source;
- }
-
- /** \brief Copy operator. */
- inline BufferedBranchNode&
- operator = (const BufferedBranchNode &source_arg)
- {
- memset (child_node_array_, 0, sizeof(child_node_array_));
-
- for (unsigned char b = 0; b < 2; ++b)
- for (unsigned char i = 0; i < 8; ++i)
- if (source_arg.child_node_array_[b][i])
- child_node_array_[b][i] = source_arg.child_node_array_[b][i]->deepCopy ();
-
- return (*this);
-
- }
-
- /** \brief Empty constructor. */
- ~BufferedBranchNode ()
- {
- }
-
- /** \brief Method to perform a deep copy of the octree */
- BufferedBranchNode*
- deepCopy () const override
- {
- return new BufferedBranchNode (*this);
- }
-
- /** \brief Get child pointer in current branch node
- * \param buffer_arg: buffer selector
- * \param index_arg: index of child in node
- * \return pointer to child node
- * */
- inline OctreeNode*
- getChildPtr (unsigned char buffer_arg, unsigned char index_arg) const
- {
- assert( (buffer_arg<2) && (index_arg<8));
- return child_node_array_[buffer_arg][index_arg];
- }
-
- /** \brief Set child pointer in current branch node
- * \param buffer_arg: buffer selector
- * \param index_arg: index of child in node
- * \param newNode_arg: pointer to new child node
- * */
- inline void setChildPtr (unsigned char buffer_arg, unsigned char index_arg,
- OctreeNode* newNode_arg)
- {
- assert( (buffer_arg<2) && (index_arg<8));
- child_node_array_[buffer_arg][index_arg] = newNode_arg;
- }
-
- /** \brief Check if branch is pointing to a particular child node
- * \param buffer_arg: buffer selector
- * \param index_arg: index of child in node
- * \return "true" if pointer to child node exists; "false" otherwise
- * */
- inline bool hasChild (unsigned char buffer_arg, unsigned char index_arg) const
- {
- assert( (buffer_arg<2) && (index_arg<8));
- return (child_node_array_[buffer_arg][index_arg] != nullptr);
- }
-
- /** \brief Get the type of octree node. Returns LEAVE_NODE type */
- node_type_t getNodeType () const override
- {
- return BRANCH_NODE;
- }
-
- /** \brief Reset branch node container for every branch buffer. */
- inline void reset ()
- {
- memset (&child_node_array_[0][0], 0, sizeof(OctreeNode*) * 8 * 2);
- }
-
- /** \brief Get const pointer to container */
- const ContainerT*
- operator->() const
- {
- return &container_;
- }
-
- /** \brief Get pointer to container */
- ContainerT*
- operator-> ()
- {
- return &container_;
- }
-
- /** \brief Get const reference to container */
- const ContainerT&
- operator* () const
- {
- return container_;
- }
-
- /** \brief Get reference to container */
- ContainerT&
- operator* ()
- {
- return container_;
- }
-
- /** \brief Get const reference to container */
- const ContainerT&
- getContainer () const
- {
- return container_;
- }
-
- /** \brief Get reference to container */
- ContainerT&
- getContainer ()
- {
- return container_;
- }
-
- /** \brief Get const pointer to container */
- const ContainerT*
- getContainerPtr() const
- {
- return &container_;
- }
-
- /** \brief Get pointer to container */
- ContainerT*
- getContainerPtr ()
- {
- return &container_;
- }
-
- protected:
- ContainerT container_;
-
- OctreeNode* child_node_array_[2][8];
- };
-
- /** \brief @b Octree double buffer class
- *
- * \note This octree implementation keeps two separate octree structures
- * in memory.
- *
- * \note This allows for differentially compare the octree structures (change detection, differential encoding).
- * \note The tree depth defines the maximum amount of octree voxels / leaf nodes (should be initially defined).
- * \note All leaf nodes are addressed by integer indices.
- * \note Note: The tree depth equates to the bit length of the voxel indices.
- * \ingroup octree
- * \author Julius Kammerl (julius@kammerl.de)
- */
- template<typename LeafContainerT = int,
- typename BranchContainerT = OctreeContainerEmpty >
- class Octree2BufBase
- {
-
- public:
-
- using OctreeT = Octree2BufBase<LeafContainerT, BranchContainerT>;
-
- // iterators are friends
- friend class OctreeIteratorBase<OctreeT> ;
- friend class OctreeDepthFirstIterator<OctreeT> ;
- friend class OctreeBreadthFirstIterator<OctreeT> ;
- friend class OctreeLeafNodeDepthFirstIterator<OctreeT> ;
- friend class OctreeLeafNodeBreadthFirstIterator<OctreeT> ;
-
- using BranchNode = BufferedBranchNode<BranchContainerT>;
- using LeafNode = OctreeLeafNode<LeafContainerT>;
-
- using BranchContainer = BranchContainerT;
- using LeafContainer = LeafContainerT;
-
- // Octree default iterators
- using Iterator = OctreeDepthFirstIterator<OctreeT>;
- using ConstIterator = const OctreeDepthFirstIterator<OctreeT>;
- Iterator begin(unsigned int max_depth_arg = 0) {return Iterator(this, max_depth_arg);};
- const Iterator end() {return Iterator();};
-
- // Octree leaf node iterators
- // The previous deprecated names
- // LeafNodeIterator and ConstLeafNodeIterator are deprecated.
- // Please use LeafNodeDepthFirstIterator and ConstLeafNodeDepthFirstIterator instead.
- using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator<OctreeT>;
- using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator<OctreeT>;
-
- [[deprecated("use leaf_depth_begin() instead")]]
- LeafNodeIterator leaf_begin (unsigned int max_depth_arg = 0)
- {
- return LeafNodeIterator (this, max_depth_arg);
- };
-
- [[deprecated("use leaf_depth_end() instead")]]
- const LeafNodeIterator leaf_end ()
- {
- return LeafNodeIterator ();
- };
-
- // The currently valide names
- using LeafNodeDepthFirstIterator = OctreeLeafNodeDepthFirstIterator<OctreeT>;
- using ConstLeafNodeDepthFirstIterator = const OctreeLeafNodeDepthFirstIterator<OctreeT>;
- LeafNodeDepthFirstIterator leaf_depth_begin (unsigned int max_depth_arg = 0)
- {
- return LeafNodeDepthFirstIterator (this, max_depth_arg);
- };
-
- const LeafNodeDepthFirstIterator leaf_depth_end ()
- {
- return LeafNodeDepthFirstIterator();
- };
-
- // Octree depth-first iterators
- using DepthFirstIterator = OctreeDepthFirstIterator<OctreeT>;
- using ConstDepthFirstIterator = const OctreeDepthFirstIterator<OctreeT>;
- DepthFirstIterator depth_begin(unsigned int maxDepth_arg = 0) {return DepthFirstIterator(this, maxDepth_arg);};
- const DepthFirstIterator depth_end() {return DepthFirstIterator();};
-
- // Octree breadth-first iterators
- using BreadthFirstIterator = OctreeBreadthFirstIterator<OctreeT>;
- using ConstBreadthFirstIterator = const OctreeBreadthFirstIterator<OctreeT>;
- BreadthFirstIterator breadth_begin(unsigned int max_depth_arg = 0) {return BreadthFirstIterator(this, max_depth_arg);};
- const BreadthFirstIterator breadth_end() {return BreadthFirstIterator();};
-
- // Octree leaf node iterators
- using LeafNodeBreadthIterator = OctreeLeafNodeBreadthFirstIterator<OctreeT>;
- using ConstLeafNodeBreadthIterator = const OctreeLeafNodeBreadthFirstIterator<OctreeT>;
-
- LeafNodeBreadthIterator leaf_breadth_begin (unsigned int max_depth_arg = 0u)
- {
- return LeafNodeBreadthIterator (this, max_depth_arg? max_depth_arg : this->octree_depth_);
- };
-
- const LeafNodeBreadthIterator leaf_breadth_end ()
- {
- return LeafNodeBreadthIterator (this, 0, nullptr);
- };
-
- /** \brief Empty constructor. */
- Octree2BufBase ();
-
- /** \brief Empty deconstructor. */
- virtual
- ~Octree2BufBase ();
-
- /** \brief Copy constructor. */
- Octree2BufBase (const Octree2BufBase& source) :
- leaf_count_ (source.leaf_count_),
- branch_count_ (source.branch_count_),
- root_node_ (new (BranchNode) (*(source.root_node_))),
- depth_mask_ (source.depth_mask_),
- max_key_ (source.max_key_),
- buffer_selector_ (source.buffer_selector_),
- tree_dirty_flag_ (source.tree_dirty_flag_),
- octree_depth_ (source.octree_depth_),
- dynamic_depth_enabled_(source.dynamic_depth_enabled_)
- {
- }
-
- /** \brief Copy constructor. */
- inline Octree2BufBase&
- operator = (const Octree2BufBase& source)
- {
- leaf_count_ = source.leaf_count_;
- branch_count_ = source.branch_count_;
- root_node_ = new (BranchNode) (* (source.root_node_));
- depth_mask_ = source.depth_mask_;
- max_key_ = source.max_key_;
- buffer_selector_ = source.buffer_selector_;
- tree_dirty_flag_ = source.tree_dirty_flag_;
- octree_depth_ = source.octree_depth_;
- dynamic_depth_enabled_ = source.dynamic_depth_enabled_;
- return (*this);
- }
-
- /** \brief Set the maximum amount of voxels per dimension.
- * \param max_voxel_index_arg: maximum amount of voxels per dimension
- * */
- void
- setMaxVoxelIndex (unsigned int max_voxel_index_arg);
-
- /** \brief Set the maximum depth of the octree.
- * \param depth_arg: maximum depth of octree
- * */
- void
- setTreeDepth (unsigned int depth_arg);
-
- /** \brief Get the maximum depth of the octree.
- * \return depth_arg: maximum depth of octree
- * */
- inline unsigned int getTreeDepth () const
- {
- return this->octree_depth_;
- }
-
- /** \brief Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
- * \note If leaf node already exist, this method returns the existing node
- * \param idx_x_arg: index of leaf node in the X axis.
- * \param idx_y_arg: index of leaf node in the Y axis.
- * \param idx_z_arg: index of leaf node in the Z axis.
- * \return pointer to new leaf node container.
- * */
- LeafContainerT*
- createLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
-
- /** \brief Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
- * \note If leaf node already exist, this method returns the existing node
- * \param idx_x_arg: index of leaf node in the X axis.
- * \param idx_y_arg: index of leaf node in the Y axis.
- * \param idx_z_arg: index of leaf node in the Z axis.
- * \return pointer to leaf node container if found, null pointer otherwise.
- * */
- LeafContainerT*
- findLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
-
- /** \brief Check for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
- * \param idx_x_arg: index of leaf node in the X axis.
- * \param idx_y_arg: index of leaf node in the Y axis.
- * \param idx_z_arg: index of leaf node in the Z axis.
- * \return "true" if leaf node search is successful, otherwise it returns "false".
- * */
- bool
- existLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const;
-
- /** \brief Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
- * \param idx_x_arg: index of leaf node in the X axis.
- * \param idx_y_arg: index of leaf node in the Y axis.
- * \param idx_z_arg: index of leaf node in the Z axis.
- * */
- void
- removeLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
-
- /** \brief Return the amount of existing leafs in the octree.
- * \return amount of registered leaf nodes.
- * */
- inline std::size_t getLeafCount () const
- {
- return (leaf_count_);
- }
-
- /** \brief Return the amount of existing branches in the octree.
- * \return amount of branch nodes.
- * */
- inline std::size_t getBranchCount () const
- {
- return (branch_count_);
- }
-
- /** \brief Delete the octree structure and its leaf nodes.
- * */
- void
- deleteTree ();
-
- /** \brief Delete octree structure of previous buffer. */
- inline void deletePreviousBuffer ()
- {
- treeCleanUpRecursive (root_node_);
- }
-
- /** \brief Delete the octree structure in the current buffer. */
- inline void deleteCurrentBuffer ()
- {
- buffer_selector_ = !buffer_selector_;
- treeCleanUpRecursive (root_node_);
- leaf_count_ = 0;
- }
-
- /** \brief Switch buffers and reset current octree structure. */
- void
- switchBuffers ();
-
- /** \brief Serialize octree into a binary output vector describing its branch node structure.
- * \param binary_tree_out_arg: reference to output vector for writing binary tree structure.
- * \param do_XOR_encoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree
- * */
- void
- serializeTree (std::vector<char>& binary_tree_out_arg,
- bool do_XOR_encoding_arg = false);
-
- /** \brief Serialize octree into a binary output vector describing its branch node structure and and push all DataT elements stored in the octree to a vector.
- * \param binary_tree_out_arg: reference to output vector for writing binary tree structure.
- * \param leaf_container_vector_arg: pointer to all LeafContainerT objects in the octree
- * \param do_XOR_encoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree
- * */
- void
- serializeTree (std::vector<char>& binary_tree_out_arg,
- std::vector<LeafContainerT*>& leaf_container_vector_arg,
- bool do_XOR_encoding_arg = false);
-
- /** \brief Outputs a vector of all DataT elements that are stored within the octree leaf nodes.
- * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree
- * */
- void
- serializeLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg);
-
- /** \brief Outputs a vector of all DataT elements from leaf nodes, that do not exist in the previous octree buffer.
- * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree
- * */
- void
- serializeNewLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg);
-
- /** \brief Deserialize a binary octree description vector and create a corresponding octree structure. Leaf nodes are initialized with getDataTByKey(..).
- * \param binary_tree_in_arg: reference to input vector for reading binary tree structure.
- * \param do_XOR_decoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree
- * */
- void
- deserializeTree (std::vector<char>& binary_tree_in_arg,
- bool do_XOR_decoding_arg = false);
-
- /** \brief Deserialize a binary octree description and create a corresponding octree structure. Leaf nodes are initialized with DataT elements from the dataVector.
- * \param binary_tree_in_arg: reference to inpvectoream for reading binary tree structure.
- * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree
- * \param do_XOR_decoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree
- * */
- void
- deserializeTree (std::vector<char>& binary_tree_in_arg,
- std::vector<LeafContainerT*>& leaf_container_vector_arg,
- bool do_XOR_decoding_arg = false);
-
- protected:
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // Protected octree methods based on octree keys
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- /** \brief Retrieve root node */
- OctreeNode*
- getRootNode () const
- {
- return (this->root_node_);
- }
-
- /** \brief Find leaf node
- * \param key_arg: octree key addressing a leaf node.
- * \return pointer to leaf container. If leaf node is not found, this pointer returns 0.
- * */
- inline LeafContainerT*
- findLeaf (const OctreeKey& key_arg) const
- {
- LeafContainerT* result = nullptr;
- findLeafRecursive (key_arg, depth_mask_, root_node_, result);
- return result;
- }
-
- /** \brief Create a leaf node.
- * \note If the leaf node at the given octree node does not exist, it will be created and added to the tree.
- * \param key_arg: octree key addressing a leaf node.
- * \return pointer to an existing or created leaf container.
- * */
- inline LeafContainerT*
- createLeaf (const OctreeKey& key_arg)
- {
- LeafNode* leaf_node;
- BranchNode* leaf_node_parent;
-
- createLeafRecursive (key_arg, depth_mask_ ,root_node_, leaf_node, leaf_node_parent, false);
-
- LeafContainerT* ret = leaf_node->getContainerPtr();
-
- return ret;
- }
-
- /** \brief Check if leaf doesn't exist in the octree
- * \param key_arg: octree key addressing a leaf node.
- * \return "true" if leaf node is found; "false" otherwise
- * */
- inline bool existLeaf (const OctreeKey& key_arg) const
- {
- return (findLeaf(key_arg) != nullptr);
- }
-
- /** \brief Remove leaf node from octree
- * \param key_arg: octree key addressing a leaf node.
- * */
- inline void removeLeaf (const OctreeKey& key_arg)
- {
- if (key_arg <= max_key_)
- {
- deleteLeafRecursive (key_arg, depth_mask_, root_node_);
-
- // we changed the octree structure -> dirty
- tree_dirty_flag_ = true;
- }
- }
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // Branch node accessor inline functions
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- /** \brief Check if branch is pointing to a particular child node
- * \param branch_arg: reference to octree branch class
- * \param child_idx_arg: index to child node
- * \return "true" if pointer to child node exists; "false" otherwise
- * */
- inline bool
- branchHasChild (const BranchNode& branch_arg, unsigned char child_idx_arg) const
- {
- // test occupancyByte for child existence
- return (branch_arg.getChildPtr(buffer_selector_, child_idx_arg) != nullptr);
- }
-
- /** \brief Retrieve a child node pointer for child node at child_idx.
- * \param branch_arg: reference to octree branch class
- * \param child_idx_arg: index to child node
- * \return pointer to octree child node class
- */
- inline OctreeNode*
- getBranchChildPtr (const BranchNode& branch_arg,
- unsigned char child_idx_arg) const
- {
- return branch_arg.getChildPtr(buffer_selector_, child_idx_arg);
- }
-
- /** \brief Assign new child node to branch
- * \param branch_arg: reference to octree branch class
- * \param child_idx_arg: index to child node
- * \param new_child_arg: pointer to new child node
- * */
- inline void
- setBranchChildPtr (BranchNode& branch_arg, unsigned char child_idx_arg, OctreeNode* new_child_arg)
- {
- branch_arg.setChildPtr (buffer_selector_, child_idx_arg, new_child_arg);
- }
-
- /** \brief Generate bit pattern reflecting the existence of child node pointers for current buffer
- * \param branch_arg: reference to octree branch class
- * \return a single byte with 8 bits of child node information
- * */
- inline char getBranchBitPattern (const BranchNode& branch_arg) const
- {
- char node_bits;
-
- // create bit pattern
- node_bits = 0;
- for (unsigned char i = 0; i < 8; i++)
- {
- const OctreeNode* child = branch_arg.getChildPtr(buffer_selector_, i);
- node_bits |= static_cast<char> ( (!!child) << i);
- }
-
- return (node_bits);
- }
-
- /** \brief Generate bit pattern reflecting the existence of child node pointers in specific buffer
- * \param branch_arg: reference to octree branch class
- * \param bufferSelector_arg: buffer selector
- * \return a single byte with 8 bits of child node information
- * */
- inline char getBranchBitPattern (const BranchNode& branch_arg,
- unsigned char bufferSelector_arg) const
- {
- char node_bits;
-
- // create bit pattern
- node_bits = 0;
- for (unsigned char i = 0; i < 8; i++)
- {
- const OctreeNode* child = branch_arg.getChildPtr(bufferSelector_arg, i);
- node_bits |= static_cast<char> ( (!!child) << i);
- }
-
- return (node_bits);
- }
-
- /** \brief Generate XOR bit pattern reflecting differences between the two octree buffers
- * \param branch_arg: reference to octree branch class
- * \return a single byte with 8 bits of child node XOR difference information
- * */
- inline char getBranchXORBitPattern (
- const BranchNode& branch_arg) const
- {
- char node_bits[2];
-
- // create bit pattern for both buffers
- node_bits[0] = node_bits[1] = 0;
-
- for (unsigned char i = 0; i < 8; i++)
- {
- const OctreeNode* childA = branch_arg.getChildPtr(0, i);
- const OctreeNode* childB = branch_arg.getChildPtr(1, i);
-
- node_bits[0] |= static_cast<char> ( (!!childA) << i);
- node_bits[1] |= static_cast<char> ( (!!childB) << i);
- }
-
- return node_bits[0] ^ node_bits[1];
- }
-
- /** \brief Test if branch changed between previous and current buffer
- * \param branch_arg: reference to octree branch class
- * \return "true", if child node information differs between current and previous octree buffer
- * */
- inline bool hasBranchChanges (const BranchNode& branch_arg) const
- {
- return (getBranchXORBitPattern (branch_arg) > 0);
- }
-
- /** \brief Delete child node and all its subchilds from octree in specific buffer
- * \param branch_arg: reference to octree branch class
- * \param buffer_selector_arg: buffer selector
- * \param child_idx_arg: index to child node
- * */
- inline void deleteBranchChild (BranchNode& branch_arg,
- unsigned char buffer_selector_arg,
- unsigned char child_idx_arg)
- {
- if (branch_arg.hasChild(buffer_selector_arg, child_idx_arg))
- {
- OctreeNode* branchChild = branch_arg.getChildPtr(buffer_selector_arg, child_idx_arg);
-
- switch (branchChild->getNodeType ())
- {
- case BRANCH_NODE:
- {
- // free child branch recursively
- deleteBranch (*static_cast<BranchNode*> (branchChild));
-
- // delete unused branch
- delete (branchChild);
- break;
- }
-
- case LEAF_NODE:
- {
- // push unused leaf to branch pool
- delete (branchChild);
- break;
- }
- default:
- break;
- }
-
- // set branch child pointer to 0
- branch_arg.setChildPtr(buffer_selector_arg, child_idx_arg, nullptr);
- }
- }
-
- /** \brief Delete child node and all its subchilds from octree in current buffer
- * \param branch_arg: reference to octree branch class
- * \param child_idx_arg: index to child node
- * */
- inline void deleteBranchChild (BranchNode& branch_arg, unsigned char child_idx_arg)
- {
- deleteBranchChild(branch_arg, buffer_selector_, child_idx_arg);
- }
-
- /** \brief Delete branch and all its subchilds from octree (both buffers)
- * \param branch_arg: reference to octree branch class
- * */
- inline void deleteBranch (BranchNode& branch_arg)
- {
- // delete all branch node children
- for (char i = 0; i < 8; i++)
- {
-
- if (branch_arg.getChildPtr(0, i) == branch_arg.getChildPtr(1, i))
- {
- // reference was copied - there is only one child instance to be deleted
- deleteBranchChild (branch_arg, 0, i);
-
- // remove pointers from both buffers
- branch_arg.setChildPtr(0, i, nullptr);
- branch_arg.setChildPtr(1, i, nullptr);
- }
- else
- {
- deleteBranchChild (branch_arg, 0, i);
- deleteBranchChild (branch_arg, 1, i);
- }
- }
- }
-
- /** \brief Fetch and add a new branch child to a branch class in current buffer
- * \param branch_arg: reference to octree branch class
- * \param child_idx_arg: index to child node
- * \return pointer of new branch child to this reference
- * */
- inline BranchNode* createBranchChild (BranchNode& branch_arg,
- unsigned char child_idx_arg)
- {
- BranchNode* new_branch_child = new BranchNode();
-
- branch_arg.setChildPtr (buffer_selector_, child_idx_arg,
- static_cast<OctreeNode*> (new_branch_child));
-
- return new_branch_child;
- }
-
- /** \brief Fetch and add a new leaf child to a branch class
- * \param branch_arg: reference to octree branch class
- * \param child_idx_arg: index to child node
- * \return pointer of new leaf child to this reference
- * */
- inline LeafNode*
- createLeafChild (BranchNode& branch_arg, unsigned char child_idx_arg)
- {
- LeafNode* new_leaf_child = new LeafNode();
-
- branch_arg.setChildPtr(buffer_selector_, child_idx_arg, new_leaf_child);
-
- return new_leaf_child;
- }
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // Recursive octree methods
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- /** \brief Create a leaf node at octree key. If leaf node does already exist, it is returned.
- * \param key_arg: reference to an octree key
- * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth indicator
- * \param branch_arg: current branch node
- * \param return_leaf_arg: return pointer to leaf container
- * \param parent_of_leaf_arg: return pointer to parent of leaf node
- * \param branch_reset_arg: Reset pointer array of current branch
- * \return depth mask at which leaf node was created/found
- **/
- unsigned int
- createLeafRecursive (const OctreeKey& key_arg,
- unsigned int depth_mask_arg,
- BranchNode* branch_arg,
- LeafNode*& return_leaf_arg,
- BranchNode*& parent_of_leaf_arg,
- bool branch_reset_arg = false);
-
-
- /** \brief Recursively search for a given leaf node and return a pointer.
- * \note If leaf node does not exist, a 0 pointer is returned.
- * \param key_arg: reference to an octree key
- * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth indicator
- * \param branch_arg: current branch node
- * \param result_arg: pointer to leaf container class
- **/
- void
- findLeafRecursive (const OctreeKey& key_arg,
- unsigned int depth_mask_arg,
- BranchNode* branch_arg,
- LeafContainerT*& result_arg) const;
-
-
- /** \brief Recursively search and delete leaf node
- * \param key_arg: reference to an octree key
- * \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator
- * \param branch_arg: current branch node
- * \return "true" if branch does not contain any childs; "false" otherwise. This indicates if current branch can be deleted.
- **/
- bool
- deleteLeafRecursive (const OctreeKey& key_arg,
- unsigned int depth_mask_arg,
- BranchNode* branch_arg);
-
- /** \brief Recursively explore the octree and output binary octree description together with a vector of leaf node DataT content.
- * \param branch_arg: current branch node
- * \param key_arg: reference to an octree key
- * \param binary_tree_out_arg: binary output vector
- * \param leaf_container_vector_arg: vector to return pointers to all leaf container in the tree.
- * \param do_XOR_encoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree
- * \param new_leafs_filter_arg: execute callback only for leaf nodes that did not exist in preceding buffer
- **/
- void
- serializeTreeRecursive (BranchNode* branch_arg,
- OctreeKey& key_arg,
- std::vector<char>* binary_tree_out_arg,
- typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
- bool do_XOR_encoding_arg = false,
- bool new_leafs_filter_arg = false);
-
- /** \brief Rebuild an octree based on binary XOR octree description and DataT objects for leaf node initialization.
- * \param branch_arg: current branch node
- * \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator
- * \param key_arg: reference to an octree key
- * \param binary_tree_in_it_arg iterator of binary input data
- * \param binary_tree_in_it_end_arg
- * \param leaf_container_vector_it_arg: iterator pointing to leaf container pointers to be added to a leaf node
- * \param leaf_container_vector_it_end_arg: iterator pointing to leaf container pointers pointing to last object in input container.
- * \param branch_reset_arg: Reset pointer array of current branch
- * \param do_XOR_decoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree
- **/
- void
- deserializeTreeRecursive (BranchNode* branch_arg,
- unsigned int depth_mask_arg,
- OctreeKey& key_arg,
- typename std::vector<char>::const_iterator& binary_tree_in_it_arg,
- typename std::vector<char>::const_iterator& binary_tree_in_it_end_arg,
- typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg,
- typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_end_arg,
- bool branch_reset_arg = false,
- bool do_XOR_decoding_arg = false);
-
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // Serialization callbacks
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- /** \brief Callback executed for every leaf node data during serialization
- **/
- virtual void serializeTreeCallback (LeafContainerT &, const OctreeKey &)
- {
-
- }
-
- /** \brief Callback executed for every leaf node data during deserialization
- **/
- virtual void deserializeTreeCallback (LeafContainerT&, const OctreeKey&)
- {
-
- }
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // Helpers
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- /** \brief Recursively explore the octree and remove unused branch and leaf nodes
- * \param branch_arg: current branch node
- **/
- void
- treeCleanUpRecursive (BranchNode* branch_arg);
-
- /** \brief Helper function to calculate the binary logarithm
- * \param n_arg: some value
- * \return binary logarithm (log2) of argument n_arg
- */
- [[deprecated("use std::log2 instead")]]
- inline double Log2 (double n_arg)
- {
- return std::log2 (n_arg);
- }
-
- /** \brief Test if octree is able to dynamically change its depth. This is required for adaptive bounding box adjustment.
- * \return "false" - not resizeable due to XOR serialization
- **/
- inline bool octreeCanResize ()
- {
- return (false);
- }
-
- /** \brief Prints binary representation of a byte - used for debugging
- * \param data_arg - byte to be printed to stdout
- **/
- inline void printBinary (char data_arg)
- {
- unsigned char mask = 1; // Bit mask
-
- // Extract the bits
- for (int i = 0; i < 8; i++)
- {
- // Mask each bit in the byte and print it
- std::cout << ((data_arg & (mask << i)) ? "1" : "0");
- }
- std::cout << std::endl;
- }
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // Globals
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- /** \brief Amount of leaf nodes **/
- std::size_t leaf_count_;
-
- /** \brief Amount of branch nodes **/
- std::size_t branch_count_;
-
- /** \brief Pointer to root branch node of octree **/
- BranchNode* root_node_;
-
- /** \brief Depth mask based on octree depth **/
- unsigned int depth_mask_;
-
- /** \brief key range */
- OctreeKey max_key_;
-
- /** \brief Currently active octree buffer **/
- unsigned char buffer_selector_;
-
- // flags indicating if unused branches and leafs might exist in previous buffer
- bool tree_dirty_flag_;
-
- /** \brief Octree depth */
- unsigned int octree_depth_;
-
- /** \brief Enable dynamic_depth
- * \note Note that this parameter is ignored in octree2buf! */
- bool dynamic_depth_enabled_;
-
- };
- }
-}
+ /** \brief Enable dynamic_depth
+ * \note Note that this parameter is ignored in octree2buf! */
+ bool dynamic_depth_enabled_;
+};
+} // namespace octree
+} // namespace pcl
#ifdef PCL_NO_PRECOMPILE
#include <pcl/octree/impl/octree2buf_base.hpp>
#include <vector>
-#include <pcl/octree/octree_nodes.h>
#include <pcl/octree/octree_container.h>
-#include <pcl/octree/octree_key.h>
#include <pcl/octree/octree_iterator.h>
+#include <pcl/octree/octree_key.h>
+#include <pcl/octree/octree_nodes.h>
+#include <pcl/pcl_macros.h>
+
+namespace pcl {
+namespace octree {
+/** \brief Octree class
+ * \note The tree depth defines the maximum amount of octree voxels / leaf nodes (should
+ * be initially defined).
+ * \note All leaf nodes are addressed by integer indices.
+ * \note The tree depth equates to the bit length of the voxel indices.
+ * \ingroup octree
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+template <typename LeafContainerT = int,
+ typename BranchContainerT = OctreeContainerEmpty>
+class OctreeBase {
+public:
+ using OctreeT = OctreeBase<LeafContainerT, BranchContainerT>;
+
+ using BranchNode = OctreeBranchNode<BranchContainerT>;
+ using LeafNode = OctreeLeafNode<LeafContainerT>;
+
+ using BranchContainer = BranchContainerT;
+ using LeafContainer = LeafContainerT;
+
+protected:
+ ///////////////////////////////////////////////////////////////////////
+ // Members
+ ///////////////////////////////////////////////////////////////////////
+
+ /** \brief Amount of leaf nodes **/
+ std::size_t leaf_count_;
+
+ /** \brief Amount of branch nodes **/
+ std::size_t branch_count_;
+
+ /** \brief Pointer to root branch node of octree **/
+ BranchNode* root_node_;
+
+ /** \brief Depth mask based on octree depth **/
+ unsigned int depth_mask_;
+
+ /** \brief Octree depth */
+ unsigned int octree_depth_;
+
+ /** \brief Enable dynamic_depth **/
+ bool dynamic_depth_enabled_;
+
+ /** \brief key range */
+ OctreeKey max_key_;
+
+public:
+ // iterators are friends
+ friend class OctreeIteratorBase<OctreeT>;
+ friend class OctreeDepthFirstIterator<OctreeT>;
+ friend class OctreeBreadthFirstIterator<OctreeT>;
+ friend class OctreeFixedDepthIterator<OctreeT>;
+ friend class OctreeLeafNodeDepthFirstIterator<OctreeT>;
+ friend class OctreeLeafNodeBreadthFirstIterator<OctreeT>;
+
+ // Octree default iterators
+ using Iterator = OctreeDepthFirstIterator<OctreeT>;
+ using ConstIterator = const OctreeDepthFirstIterator<OctreeT>;
+
+ Iterator
+ begin(unsigned int max_depth_arg = 0u)
+ {
+ return Iterator(this, max_depth_arg ? max_depth_arg : this->octree_depth_);
+ };
+
+ const Iterator
+ end()
+ {
+ return Iterator(this, 0, nullptr);
+ };
+
+ // Octree leaf node iterators
+ // The previous deprecated names
+ // LeafNodeIterator and ConstLeafNodeIterator are deprecated.
+ // Please use LeafNodeDepthFirstIterator and ConstLeafNodeDepthFirstIterator instead.
+ using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator<OctreeT>;
+ using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator<OctreeT>;
+
+ PCL_DEPRECATED("use leaf_depth_begin() instead")
+ LeafNodeIterator
+ leaf_begin(unsigned int max_depth_arg = 0u)
+ {
+ return LeafNodeIterator(this, max_depth_arg ? max_depth_arg : this->octree_depth_);
+ };
+
+ PCL_DEPRECATED("use leaf_depth_end() instead") const LeafNodeIterator leaf_end()
+ {
+ return LeafNodeIterator(this, 0, nullptr);
+ };
+
+ // The currently valide names
+ using LeafNodeDepthFirstIterator = OctreeLeafNodeDepthFirstIterator<OctreeT>;
+ using ConstLeafNodeDepthFirstIterator =
+ const OctreeLeafNodeDepthFirstIterator<OctreeT>;
+
+ LeafNodeDepthFirstIterator
+ leaf_depth_begin(unsigned int max_depth_arg = 0u)
+ {
+ return LeafNodeDepthFirstIterator(
+ this, max_depth_arg ? max_depth_arg : this->octree_depth_);
+ };
+
+ const LeafNodeDepthFirstIterator
+ leaf_depth_end()
+ {
+ return LeafNodeDepthFirstIterator(this, 0, nullptr);
+ };
+
+ // Octree depth-first iterators
+ using DepthFirstIterator = OctreeDepthFirstIterator<OctreeT>;
+ using ConstDepthFirstIterator = const OctreeDepthFirstIterator<OctreeT>;
+
+ DepthFirstIterator
+ depth_begin(unsigned int max_depth_arg = 0u)
+ {
+ return DepthFirstIterator(this,
+ max_depth_arg ? max_depth_arg : this->octree_depth_);
+ };
+
+ const DepthFirstIterator
+ depth_end()
+ {
+ return DepthFirstIterator(this, 0, nullptr);
+ };
+
+ // Octree breadth-first iterators
+ using BreadthFirstIterator = OctreeBreadthFirstIterator<OctreeT>;
+ using ConstBreadthFirstIterator = const OctreeBreadthFirstIterator<OctreeT>;
+
+ BreadthFirstIterator
+ breadth_begin(unsigned int max_depth_arg = 0u)
+ {
+ return BreadthFirstIterator(this,
+ max_depth_arg ? max_depth_arg : this->octree_depth_);
+ };
+
+ const BreadthFirstIterator
+ breadth_end()
+ {
+ return BreadthFirstIterator(this, 0, nullptr);
+ };
+
+ // Octree breadth iterators at a given depth
+ using FixedDepthIterator = OctreeFixedDepthIterator<OctreeT>;
+ using ConstFixedDepthIterator = const OctreeFixedDepthIterator<OctreeT>;
+
+ FixedDepthIterator
+ fixed_depth_begin(unsigned int fixed_depth_arg = 0u)
+ {
+ return FixedDepthIterator(this, fixed_depth_arg);
+ };
+
+ const FixedDepthIterator
+ fixed_depth_end()
+ {
+ return FixedDepthIterator(this, 0, nullptr);
+ };
+
+ // Octree leaf node iterators
+ using LeafNodeBreadthFirstIterator = OctreeLeafNodeBreadthFirstIterator<OctreeT>;
+ using ConstLeafNodeBreadthFirstIterator =
+ const OctreeLeafNodeBreadthFirstIterator<OctreeT>;
+
+ LeafNodeBreadthFirstIterator
+ leaf_breadth_begin(unsigned int max_depth_arg = 0u)
+ {
+ return LeafNodeBreadthFirstIterator(
+ this, max_depth_arg ? max_depth_arg : this->octree_depth_);
+ };
+
+ const LeafNodeBreadthFirstIterator
+ leaf_breadth_end()
+ {
+ return LeafNodeBreadthFirstIterator(this, 0, nullptr);
+ };
+
+ /** \brief Empty constructor. */
+ OctreeBase();
+
+ /** \brief Empty deconstructor. */
+ virtual ~OctreeBase();
+
+ /** \brief Copy constructor. */
+ OctreeBase(const OctreeBase& source)
+ : leaf_count_(source.leaf_count_)
+ , branch_count_(source.branch_count_)
+ , root_node_(new (BranchNode)(*(source.root_node_)))
+ , depth_mask_(source.depth_mask_)
+ , octree_depth_(source.octree_depth_)
+ , dynamic_depth_enabled_(source.dynamic_depth_enabled_)
+ , max_key_(source.max_key_)
+ {}
+
+ /** \brief Copy operator. */
+ OctreeBase&
+ operator=(const OctreeBase& source)
+ {
+ leaf_count_ = source.leaf_count_;
+ branch_count_ = source.branch_count_;
+ root_node_ = new (BranchNode)(*(source.root_node_));
+ depth_mask_ = source.depth_mask_;
+ max_key_ = source.max_key_;
+ octree_depth_ = source.octree_depth_;
+ return (*this);
+ }
+
+ /** \brief Set the maximum amount of voxels per dimension.
+ * \param[in] max_voxel_index_arg maximum amount of voxels per dimension
+ */
+ void
+ setMaxVoxelIndex(unsigned int max_voxel_index_arg);
+
+ /** \brief Set the maximum depth of the octree.
+ * \param max_depth_arg: maximum depth of octree
+ */
+ void
+ setTreeDepth(unsigned int max_depth_arg);
+
+ /** \brief Get the maximum depth of the octree.
+ * \return depth_arg: maximum depth of octree
+ */
+ unsigned int
+ getTreeDepth() const
+ {
+ return this->octree_depth_;
+ }
+
+ /** \brief Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
+ * \note If leaf node already exist, this method returns the existing node
+ * \param idx_x_arg: index of leaf node in the X axis.
+ * \param idx_y_arg: index of leaf node in the Y axis.
+ * \param idx_z_arg: index of leaf node in the Z axis.
+ * \return pointer to new leaf node container.
+ */
+ LeafContainerT*
+ createLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
+
+ /** \brief Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
+ * \note If leaf node already exist, this method returns the existing node
+ * \param idx_x_arg: index of leaf node in the X axis.
+ * \param idx_y_arg: index of leaf node in the Y axis.
+ * \param idx_z_arg: index of leaf node in the Z axis.
+ * \return pointer to leaf node container if found, null pointer otherwise.
+ */
+ LeafContainerT*
+ findLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
+
+ /** \brief idx_x_arg for the existence of leaf node at (idx_x_arg, idx_y_arg,
+ * idx_z_arg).
+ * \param idx_x_arg: index of leaf node in the X axis.
+ * \param idx_y_arg: index of leaf node in the Y axis.
+ * \param idx_z_arg: index of leaf node in the Z axis.
+ * \return "true" if leaf node search is successful, otherwise it returns "false".
+ */
+ bool
+ existLeaf(unsigned int idx_x_arg,
+ unsigned int idx_y_arg,
+ unsigned int idx_z_arg) const;
+
+ /** \brief Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
+ * \param idx_x_arg: index of leaf node in the X axis.
+ * \param idx_y_arg: index of leaf node in the Y axis.
+ * \param idx_z_arg: index of leaf node in the Z axis.
+ */
+ void
+ removeLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
+
+ /** \brief Return the amount of existing leafs in the octree.
+ * \return amount of registered leaf nodes.
+ */
+ std::size_t
+ getLeafCount() const
+ {
+ return leaf_count_;
+ }
+
+ /** \brief Return the amount of existing branch nodes in the octree.
+ * \return amount of branch nodes.
+ */
+ std::size_t
+ getBranchCount() const
+ {
+ return branch_count_;
+ }
+
+ /** \brief Delete the octree structure and its leaf nodes.
+ */
+ void
+ deleteTree();
+
+ /** \brief Serialize octree into a binary output vector describing its branch node
+ * structure.
+ * \param binary_tree_out_arg: reference to output vector for writing binary tree
+ * structure.
+ */
+ void
+ serializeTree(std::vector<char>& binary_tree_out_arg);
+
+ /** \brief Serialize octree into a binary output vector describing its branch node
+ * structure and push all LeafContainerT elements stored in the octree to a vector.
+ * \param binary_tree_out_arg: reference to output vector for writing binary tree
+ * structure.
+ * \param leaf_container_vector_arg: pointer to all LeafContainerT objects in the
+ * octree
+ */
+ void
+ serializeTree(std::vector<char>& binary_tree_out_arg,
+ std::vector<LeafContainerT*>& leaf_container_vector_arg);
+
+ /** \brief Outputs a vector of all LeafContainerT elements that are stored within the
+ * octree leaf nodes.
+ * \param leaf_container_vector_arg: pointers to LeafContainerT vector that receives a
+ * copy of all LeafContainerT objects in the octree.
+ */
+ void
+ serializeLeafs(std::vector<LeafContainerT*>& leaf_container_vector_arg);
+
+ /** \brief Deserialize a binary octree description vector and create a corresponding
+ * octree structure. Leaf nodes are initialized with getDataTByKey(..).
+ * \param binary_tree_input_arg: reference to input vector for reading binary tree
+ * structure.
+ */
+ void
+ deserializeTree(std::vector<char>& binary_tree_input_arg);
+
+ /** \brief Deserialize a binary octree description and create a corresponding octree
+ * structure. Leaf nodes are initialized with LeafContainerT elements from the
+ * dataVector.
+ * \param binary_tree_input_arg: reference to input vector for reading binary tree
+ * structure. \param leaf_container_vector_arg: pointer to container vector.
+ */
+ void
+ deserializeTree(std::vector<char>& binary_tree_input_arg,
+ std::vector<LeafContainerT*>& leaf_container_vector_arg);
+
+protected:
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Protected octree methods based on octree keys
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /** \brief Create a leaf node
+ * \param key_arg: octree key addressing a leaf node.
+ * \return pointer to leaf node
+ */
+ LeafContainerT*
+ createLeaf(const OctreeKey& key_arg)
+ {
+
+ LeafNode* leaf_node;
+ BranchNode* leaf_node_parent;
+
+ createLeafRecursive(key_arg, depth_mask_, root_node_, leaf_node, leaf_node_parent);
+
+ LeafContainerT* ret = leaf_node->getContainerPtr();
+
+ return ret;
+ }
+
+ /** \brief Find leaf node
+ * \param key_arg: octree key addressing a leaf node.
+ * \return pointer to leaf node. If leaf node is not found, this pointer returns 0.
+ */
+ LeafContainerT*
+ findLeaf(const OctreeKey& key_arg) const
+ {
+ LeafContainerT* result = nullptr;
+ findLeafRecursive(key_arg, depth_mask_, root_node_, result);
+ return result;
+ }
+
+ /** \brief Check for existence of a leaf node in the octree
+ * \param key_arg: octree key addressing a leaf node.
+ * \return "true" if leaf node is found; "false" otherwise
+ */
+ bool
+ existLeaf(const OctreeKey& key_arg) const
+ {
+ return (findLeaf(key_arg) != nullptr);
+ }
+
+ /** \brief Remove leaf node from octree
+ * \param key_arg: octree key addressing a leaf node.
+ */
+ void
+ removeLeaf(const OctreeKey& key_arg)
+ {
+ if (key_arg <= max_key_)
+ deleteLeafRecursive(key_arg, depth_mask_, root_node_);
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Branch node access functions
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /** \brief Retrieve root node */
+ OctreeNode*
+ getRootNode() const
+ {
+ return this->root_node_;
+ }
+
+ /** \brief Check if branch is pointing to a particular child node
+ * \param branch_arg: reference to octree branch class
+ * \param child_idx_arg: index to child node
+ * \return "true" if pointer to child node exists; "false" otherwise
+ */
+ bool
+ branchHasChild(const BranchNode& branch_arg, unsigned char child_idx_arg) const
+ {
+ // test occupancyByte for child existence
+ return (branch_arg.getChildPtr(child_idx_arg) != nullptr);
+ }
+
+ /** \brief Retrieve a child node pointer for child node at child_idx.
+ * \param branch_arg: reference to octree branch class
+ * \param child_idx_arg: index to child node
+ * \return pointer to octree child node class
+ */
+ OctreeNode*
+ getBranchChildPtr(const BranchNode& branch_arg, unsigned char child_idx_arg) const
+ {
+ return branch_arg.getChildPtr(child_idx_arg);
+ }
+
+ /** \brief Assign new child node to branch
+ * \param branch_arg: reference to octree branch class
+ * \param child_idx_arg: index to child node
+ * \param new_child_arg: pointer to new child node
+ */
+ void
+ setBranchChildPtr(BranchNode& branch_arg,
+ unsigned char child_idx_arg,
+ OctreeNode* new_child_arg)
+ {
+ branch_arg[child_idx_arg] = new_child_arg;
+ }
+
+ /** \brief Generate bit pattern reflecting the existence of child node pointers
+ * \param branch_arg: reference to octree branch class
+ * \return a single byte with 8 bits of child node information
+ */
+ char
+ getBranchBitPattern(const BranchNode& branch_arg) const
+ {
+ char node_bits;
+
+ // create bit pattern
+ node_bits = 0;
+ for (unsigned char i = 0; i < 8; i++) {
+ const OctreeNode* child = branch_arg.getChildPtr(i);
+ node_bits |= static_cast<char>((!!child) << i);
+ }
+
+ return (node_bits);
+ }
+
+ /** \brief Delete child node and all its subchilds from octree
+ * \param branch_arg: reference to octree branch class
+ * \param child_idx_arg: index to child node
+ */
+ void
+ deleteBranchChild(BranchNode& branch_arg, unsigned char child_idx_arg)
+ {
+ if (branch_arg.hasChild(child_idx_arg)) {
+ OctreeNode* branch_child = branch_arg[child_idx_arg];
+
+ switch (branch_child->getNodeType()) {
+ case BRANCH_NODE: {
+ // free child branch recursively
+ deleteBranch(*static_cast<BranchNode*>(branch_child));
+ // delete branch node
+ delete branch_child;
+ } break;
+
+ case LEAF_NODE: {
+ // delete leaf node
+ delete branch_child;
+ break;
+ }
+ default:
+ break;
+ }
+
+ // set branch child pointer to 0
+ branch_arg[child_idx_arg] = nullptr;
+ }
+ }
+
+ /** \brief Delete branch and all its subchilds from octree
+ * \param branch_arg: reference to octree branch class
+ */
+ void
+ deleteBranch(BranchNode& branch_arg)
+ {
+ // delete all branch node children
+ for (char i = 0; i < 8; i++)
+ deleteBranchChild(branch_arg, i);
+ }
+
+ /** \brief Create and add a new branch child to a branch class
+ * \param branch_arg: reference to octree branch class
+ * \param child_idx_arg: index to child node
+ * \return pointer of new branch child to this reference
+ */
+ BranchNode*
+ createBranchChild(BranchNode& branch_arg, unsigned char child_idx_arg)
+ {
+ BranchNode* new_branch_child = new BranchNode();
+ branch_arg[child_idx_arg] = static_cast<OctreeNode*>(new_branch_child);
+
+ return new_branch_child;
+ }
+
+ /** \brief Create and add a new leaf child to a branch class
+ * \param branch_arg: reference to octree branch class
+ * \param child_idx_arg: index to child node
+ * \return pointer of new leaf child to this reference
+ */
+ LeafNode*
+ createLeafChild(BranchNode& branch_arg, unsigned char child_idx_arg)
+ {
+ LeafNode* new_leaf_child = new LeafNode();
+ branch_arg[child_idx_arg] = static_cast<OctreeNode*>(new_leaf_child);
+
+ return new_leaf_child;
+ }
-namespace pcl
-{
- namespace octree
- {
- /** \brief Octree class
- * \note The tree depth defines the maximum amount of octree voxels / leaf nodes (should be initially defined).
- * \note All leaf nodes are addressed by integer indices.
- * \note Note: The tree depth equates to the bit length of the voxel indices.
- * \ingroup octree
- * \author Julius Kammerl (julius@kammerl.de)
- */
- template<typename LeafContainerT = int,
- typename BranchContainerT = OctreeContainerEmpty >
- class OctreeBase
- {
- public:
-
- using OctreeT = OctreeBase<LeafContainerT, BranchContainerT>;
-
- using BranchNode = OctreeBranchNode<BranchContainerT>;
- using LeafNode = OctreeLeafNode<LeafContainerT>;
-
- using BranchContainer = BranchContainerT;
- using LeafContainer = LeafContainerT;
-
- protected:
-
- ///////////////////////////////////////////////////////////////////////
- // Members
- ///////////////////////////////////////////////////////////////////////
-
- /** \brief Amount of leaf nodes **/
- std::size_t leaf_count_;
-
- /** \brief Amount of branch nodes **/
- std::size_t branch_count_;
-
- /** \brief Pointer to root branch node of octree **/
- BranchNode* root_node_;
-
- /** \brief Depth mask based on octree depth **/
- unsigned int depth_mask_;
-
- /** \brief Octree depth */
- unsigned int octree_depth_;
-
- /** \brief Enable dynamic_depth **/
- bool dynamic_depth_enabled_;
-
- /** \brief key range */
- OctreeKey max_key_;
-
- public:
-
- // iterators are friends
- friend class OctreeIteratorBase<OctreeT> ;
- friend class OctreeDepthFirstIterator<OctreeT> ;
- friend class OctreeBreadthFirstIterator<OctreeT> ;
- friend class OctreeFixedDepthIterator<OctreeT> ;
- friend class OctreeLeafNodeDepthFirstIterator<OctreeT> ;
- friend class OctreeLeafNodeBreadthFirstIterator<OctreeT> ;
-
- // Octree default iterators
- using Iterator = OctreeDepthFirstIterator<OctreeT>;
- using ConstIterator = const OctreeDepthFirstIterator<OctreeT>;
-
- Iterator begin (unsigned int max_depth_arg = 0u)
- {
- return Iterator (this, max_depth_arg? max_depth_arg : this->octree_depth_);
- };
-
- const Iterator end ()
- {
- return Iterator (this, 0, nullptr);
- };
-
- // Octree leaf node iterators
- // The previous deprecated names
- // LeafNodeIterator and ConstLeafNodeIterator are deprecated.
- // Please use LeafNodeDepthFirstIterator and ConstLeafNodeDepthFirstIterator instead.
- using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator<OctreeT>;
- using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator<OctreeT>;
-
- [[deprecated("use leaf_depth_begin() instead")]]
- LeafNodeIterator leaf_begin (unsigned int max_depth_arg = 0u)
- {
- return LeafNodeIterator (this, max_depth_arg? max_depth_arg : this->octree_depth_);
- };
-
- [[deprecated("use leaf_depth_end() instead")]]
- const LeafNodeIterator leaf_end ()
- {
- return LeafNodeIterator (this, 0, nullptr);
- };
-
- // The currently valide names
- using LeafNodeDepthFirstIterator = OctreeLeafNodeDepthFirstIterator<OctreeT>;
- using ConstLeafNodeDepthFirstIterator = const OctreeLeafNodeDepthFirstIterator<OctreeT>;
-
- LeafNodeDepthFirstIterator leaf_depth_begin (unsigned int max_depth_arg = 0u)
- {
- return LeafNodeDepthFirstIterator (this, max_depth_arg? max_depth_arg : this->octree_depth_);
- };
-
- const LeafNodeDepthFirstIterator leaf_depth_end ()
- {
- return LeafNodeDepthFirstIterator (this, 0, nullptr);
- };
-
- // Octree depth-first iterators
- using DepthFirstIterator = OctreeDepthFirstIterator<OctreeT>;
- using ConstDepthFirstIterator = const OctreeDepthFirstIterator<OctreeT>;
-
- DepthFirstIterator depth_begin (unsigned int max_depth_arg = 0u)
- {
- return DepthFirstIterator (this, max_depth_arg? max_depth_arg : this->octree_depth_);
- };
-
- const DepthFirstIterator depth_end ()
- {
- return DepthFirstIterator (this, 0, nullptr);
- };
-
- // Octree breadth-first iterators
- using BreadthFirstIterator = OctreeBreadthFirstIterator<OctreeT>;
- using ConstBreadthFirstIterator = const OctreeBreadthFirstIterator<OctreeT>;
-
- BreadthFirstIterator breadth_begin (unsigned int max_depth_arg = 0u)
- {
- return BreadthFirstIterator (this, max_depth_arg? max_depth_arg : this->octree_depth_);
- };
-
- const BreadthFirstIterator breadth_end ()
- {
- return BreadthFirstIterator (this, 0, nullptr);
- };
-
- // Octree breadth iterators at a given depth
- using FixedDepthIterator = OctreeFixedDepthIterator<OctreeT>;
- using ConstFixedDepthIterator = const OctreeFixedDepthIterator<OctreeT>;
-
- FixedDepthIterator fixed_depth_begin (unsigned int fixed_depth_arg = 0u)
- {
- return FixedDepthIterator (this, fixed_depth_arg);
- };
-
- const FixedDepthIterator fixed_depth_end ()
- {
- return FixedDepthIterator (this, 0, nullptr);
- };
-
- // Octree leaf node iterators
- using LeafNodeBreadthFirstIterator = OctreeLeafNodeBreadthFirstIterator<OctreeT>;
- using ConstLeafNodeBreadthFirstIterator = const OctreeLeafNodeBreadthFirstIterator<OctreeT>;
-
- LeafNodeBreadthFirstIterator leaf_breadth_begin (unsigned int max_depth_arg = 0u)
- {
- return LeafNodeBreadthFirstIterator (this, max_depth_arg? max_depth_arg : this->octree_depth_);
- };
-
- const LeafNodeBreadthFirstIterator leaf_breadth_end ()
- {
- return LeafNodeBreadthFirstIterator (this, 0, nullptr);
- };
-
- /** \brief Empty constructor. */
- OctreeBase ();
-
- /** \brief Empty deconstructor. */
- virtual
- ~OctreeBase ();
-
- /** \brief Copy constructor. */
- OctreeBase (const OctreeBase& source) :
- leaf_count_ (source.leaf_count_),
- branch_count_ (source.branch_count_),
- root_node_ (new (BranchNode) (*(source.root_node_))),
- depth_mask_ (source.depth_mask_),
- octree_depth_ (source.octree_depth_),
- dynamic_depth_enabled_(source.dynamic_depth_enabled_),
- max_key_ (source.max_key_)
- {
- }
-
- /** \brief Copy operator. */
- OctreeBase&
- operator = (const OctreeBase &source)
- {
- leaf_count_ = source.leaf_count_;
- branch_count_ = source.branch_count_;
- root_node_ = new (BranchNode) (*(source.root_node_));
- depth_mask_ = source.depth_mask_;
- max_key_ = source.max_key_;
- octree_depth_ = source.octree_depth_;
- return (*this);
- }
-
- /** \brief Set the maximum amount of voxels per dimension.
- * \param[in] max_voxel_index_arg maximum amount of voxels per dimension
- */
- void
- setMaxVoxelIndex (unsigned int max_voxel_index_arg);
-
- /** \brief Set the maximum depth of the octree.
- * \param max_depth_arg: maximum depth of octree
- * */
- void
- setTreeDepth (unsigned int max_depth_arg);
-
- /** \brief Get the maximum depth of the octree.
- * \return depth_arg: maximum depth of octree
- * */
- unsigned int
- getTreeDepth () const
- {
- return this->octree_depth_;
- }
-
- /** \brief Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
- * \note If leaf node already exist, this method returns the existing node
- * \param idx_x_arg: index of leaf node in the X axis.
- * \param idx_y_arg: index of leaf node in the Y axis.
- * \param idx_z_arg: index of leaf node in the Z axis.
- * \return pointer to new leaf node container.
- * */
- LeafContainerT*
- createLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
-
- /** \brief Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
- * \note If leaf node already exist, this method returns the existing node
- * \param idx_x_arg: index of leaf node in the X axis.
- * \param idx_y_arg: index of leaf node in the Y axis.
- * \param idx_z_arg: index of leaf node in the Z axis.
- * \return pointer to leaf node container if found, null pointer otherwise.
- * */
- LeafContainerT*
- findLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
-
- /** \brief idx_x_arg for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
- * \param idx_x_arg: index of leaf node in the X axis.
- * \param idx_y_arg: index of leaf node in the Y axis.
- * \param idx_z_arg: index of leaf node in the Z axis.
- * \return "true" if leaf node search is successful, otherwise it returns "false".
- * */
- bool
- existLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const ;
-
- /** \brief Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
- * \param idx_x_arg: index of leaf node in the X axis.
- * \param idx_y_arg: index of leaf node in the Y axis.
- * \param idx_z_arg: index of leaf node in the Z axis.
- * */
- void
- removeLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
-
- /** \brief Return the amount of existing leafs in the octree.
- * \return amount of registered leaf nodes.
- * */
- std::size_t
- getLeafCount () const
- {
- return leaf_count_;
- }
-
- /** \brief Return the amount of existing branch nodes in the octree.
- * \return amount of branch nodes.
- * */
- std::size_t
- getBranchCount () const
- {
- return branch_count_;
- }
-
- /** \brief Delete the octree structure and its leaf nodes.
- * */
- void
- deleteTree ( );
-
- /** \brief Serialize octree into a binary output vector describing its branch node structure.
- * \param binary_tree_out_arg: reference to output vector for writing binary tree structure.
- * */
- void
- serializeTree (std::vector<char>& binary_tree_out_arg);
-
- /** \brief Serialize octree into a binary output vector describing its branch node structure and push all LeafContainerT elements stored in the octree to a vector.
- * \param binary_tree_out_arg: reference to output vector for writing binary tree structure.
- * \param leaf_container_vector_arg: pointer to all LeafContainerT objects in the octree
- * */
- void
- serializeTree (std::vector<char>& binary_tree_out_arg, std::vector<LeafContainerT*>& leaf_container_vector_arg);
-
- /** \brief Outputs a vector of all LeafContainerT elements that are stored within the octree leaf nodes.
- * \param leaf_container_vector_arg: pointers to LeafContainerT vector that receives a copy of all LeafContainerT objects in the octree.
- * */
- void
- serializeLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg);
-
- /** \brief Deserialize a binary octree description vector and create a corresponding octree structure. Leaf nodes are initialized with getDataTByKey(..).
- * \param binary_tree_input_arg: reference to input vector for reading binary tree structure.
- * */
- void
- deserializeTree (std::vector<char>& binary_tree_input_arg);
-
- /** \brief Deserialize a binary octree description and create a corresponding octree structure. Leaf nodes are initialized with LeafContainerT elements from the dataVector.
- * \param binary_tree_input_arg: reference to input vector for reading binary tree structure.
- * \param leaf_container_vector_arg: pointer to container vector.
- * */
- void
- deserializeTree (std::vector<char>& binary_tree_input_arg, std::vector<LeafContainerT*>& leaf_container_vector_arg);
-
- protected:
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // Protected octree methods based on octree keys
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- /** \brief Create a leaf node
- * \param key_arg: octree key addressing a leaf node.
- * \return pointer to leaf node
- * */
- LeafContainerT*
- createLeaf (const OctreeKey& key_arg)
- {
-
- LeafNode* leaf_node;
- BranchNode* leaf_node_parent;
-
- createLeafRecursive (key_arg, depth_mask_ ,root_node_, leaf_node, leaf_node_parent);
-
- LeafContainerT* ret = leaf_node->getContainerPtr();
-
- return ret;
- }
-
- /** \brief Find leaf node
- * \param key_arg: octree key addressing a leaf node.
- * \return pointer to leaf node. If leaf node is not found, this pointer returns 0.
- * */
- LeafContainerT*
- findLeaf (const OctreeKey& key_arg) const
- {
- LeafContainerT* result = nullptr;
- findLeafRecursive (key_arg, depth_mask_, root_node_, result);
- return result;
- }
-
- /** \brief Check for existence of a leaf node in the octree
- * \param key_arg: octree key addressing a leaf node.
- * \return "true" if leaf node is found; "false" otherwise
- * */
- bool
- existLeaf (const OctreeKey& key_arg) const
- {
- return (findLeaf(key_arg) != nullptr);
- }
-
- /** \brief Remove leaf node from octree
- * \param key_arg: octree key addressing a leaf node.
- * */
- void
- removeLeaf (const OctreeKey& key_arg)
- {
- if (key_arg <= max_key_)
- deleteLeafRecursive (key_arg, depth_mask_, root_node_);
- }
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // Branch node access functions
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- /** \brief Retrieve root node */
- OctreeNode*
- getRootNode () const
- {
- return this->root_node_;
- }
-
- /** \brief Check if branch is pointing to a particular child node
- * \param branch_arg: reference to octree branch class
- * \param child_idx_arg: index to child node
- * \return "true" if pointer to child node exists; "false" otherwise
- * */
- bool
- branchHasChild (const BranchNode& branch_arg,
- unsigned char child_idx_arg) const
- {
- // test occupancyByte for child existence
- return (branch_arg.getChildPtr(child_idx_arg) != nullptr);
- }
-
- /** \brief Retrieve a child node pointer for child node at child_idx.
- * \param branch_arg: reference to octree branch class
- * \param child_idx_arg: index to child node
- * \return pointer to octree child node class
- */
- OctreeNode*
- getBranchChildPtr (const BranchNode& branch_arg,
- unsigned char child_idx_arg) const
- {
- return branch_arg.getChildPtr(child_idx_arg);
- }
-
- /** \brief Assign new child node to branch
- * \param branch_arg: reference to octree branch class
- * \param child_idx_arg: index to child node
- * \param new_child_arg: pointer to new child node
- * */
- void setBranchChildPtr (BranchNode& branch_arg,
- unsigned char child_idx_arg,
- OctreeNode* new_child_arg)
- {
- branch_arg[child_idx_arg] = new_child_arg;
- }
-
- /** \brief Generate bit pattern reflecting the existence of child node pointers
- * \param branch_arg: reference to octree branch class
- * \return a single byte with 8 bits of child node information
- * */
- char
- getBranchBitPattern (const BranchNode& branch_arg) const
- {
- char node_bits;
-
- // create bit pattern
- node_bits = 0;
- for (unsigned char i = 0; i < 8; i++) {
- const OctreeNode* child = branch_arg.getChildPtr(i);
- node_bits |= static_cast<char> ((!!child) << i);
- }
-
- return (node_bits);
- }
-
- /** \brief Delete child node and all its subchilds from octree
- * \param branch_arg: reference to octree branch class
- * \param child_idx_arg: index to child node
- * */
- void
- deleteBranchChild (BranchNode& branch_arg, unsigned char child_idx_arg)
- {
- if (branch_arg.hasChild(child_idx_arg))
- {
- OctreeNode* branch_child = branch_arg[child_idx_arg];
-
- switch (branch_child->getNodeType ())
- {
- case BRANCH_NODE:
- {
- // free child branch recursively
- deleteBranch (*static_cast<BranchNode*> (branch_child));
- // delete branch node
- delete branch_child;
- }
- break;
-
- case LEAF_NODE:
- {
- // delete leaf node
- delete branch_child;
- break;
- }
- default:
- break;
- }
-
- // set branch child pointer to 0
- branch_arg[child_idx_arg] = nullptr;
- }
- }
-
- /** \brief Delete branch and all its subchilds from octree
- * \param branch_arg: reference to octree branch class
- * */
- void
- deleteBranch (BranchNode& branch_arg)
- {
- // delete all branch node children
- for (char i = 0; i < 8; i++)
- deleteBranchChild (branch_arg, i);
- }
-
- /** \brief Create and add a new branch child to a branch class
- * \param branch_arg: reference to octree branch class
- * \param child_idx_arg: index to child node
- * \return pointer of new branch child to this reference
- * */
- BranchNode* createBranchChild (BranchNode& branch_arg,
- unsigned char child_idx_arg)
- {
- BranchNode* new_branch_child = new BranchNode();
- branch_arg[child_idx_arg] = static_cast<OctreeNode*> (new_branch_child);
-
- return new_branch_child;
- }
-
- /** \brief Create and add a new leaf child to a branch class
- * \param branch_arg: reference to octree branch class
- * \param child_idx_arg: index to child node
- * \return pointer of new leaf child to this reference
- * */
- LeafNode*
- createLeafChild (BranchNode& branch_arg, unsigned char child_idx_arg)
- {
- LeafNode* new_leaf_child = new LeafNode();
- branch_arg[child_idx_arg] = static_cast<OctreeNode*> (new_leaf_child);
-
- return new_leaf_child;
- }
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // Recursive octree methods
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- /** \brief Create a leaf node at octree key. If leaf node does already exist, it is returned.
- * \param key_arg: reference to an octree key
- * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth indicator
- * \param branch_arg: current branch node
- * \param return_leaf_arg: return pointer to leaf node
- * \param parent_of_leaf_arg: return pointer to parent of leaf node
- * \return depth mask at which leaf node was created
- **/
- unsigned int
- createLeafRecursive (const OctreeKey& key_arg,
- unsigned int depth_mask_arg,
- BranchNode* branch_arg,
- LeafNode*& return_leaf_arg,
- BranchNode*& parent_of_leaf_arg);
-
- /** \brief Recursively search for a given leaf node and return a pointer.
- * \note If leaf node does not exist, a 0 pointer is returned.
- * \param key_arg: reference to an octree key
- * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth indicator
- * \param branch_arg: current branch node
- * \param result_arg: pointer to leaf node class
- **/
- void
- findLeafRecursive (const OctreeKey& key_arg,
- unsigned int depth_mask_arg,
- BranchNode* branch_arg,
- LeafContainerT*& result_arg) const;
-
- /** \brief Recursively search and delete leaf node
- * \param key_arg: reference to an octree key
- * \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator
- * \param branch_arg: current branch node
- * \return "true" if branch does not contain any childs; "false" otherwise. This indicates if current branch can be deleted, too.
- **/
- bool
- deleteLeafRecursive (const OctreeKey& key_arg,
- unsigned int depth_mask_arg,
- BranchNode* branch_arg);
-
- /** \brief Recursively explore the octree and output binary octree description together with a vector of leaf node LeafContainerTs.
- * \param branch_arg: current branch node
- * \param key_arg: reference to an octree key
- * \param binary_tree_out_arg: binary output vector
- * \param leaf_container_vector_arg: writes LeafContainerT pointers to this LeafContainerT* vector.
- **/
- void
- serializeTreeRecursive (const BranchNode* branch_arg,
- OctreeKey& key_arg,
- std::vector<char>* binary_tree_out_arg,
- typename std::vector<LeafContainerT*>* leaf_container_vector_arg) const;
-
- /** \brief Recursive method for deserializing octree structure
- * \param branch_arg: current branch node
- * \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator
- * \param key_arg: reference to an octree key
- * \param binary_tree_input_it_arg: iterator to binary input vector
- * \param binary_tree_input_it_end_arg: end iterator of binary input vector
- * \param leaf_container_vector_it_arg: iterator pointing to current LeafContainerT object to be added to a leaf node
- * \param leaf_container_vector_it_end_arg: iterator pointing to last object in LeafContainerT input vector.
- **/
- void
- deserializeTreeRecursive (BranchNode* branch_arg, unsigned int depth_mask_arg, OctreeKey& key_arg,
- typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
- typename std::vector<char>::const_iterator& binary_tree_input_it_end_arg,
- typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg,
- typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_end_arg);
-
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // Serialization callbacks
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- /** \brief Callback executed for every leaf node during serialization
- **/
- virtual void
- serializeTreeCallback (LeafContainerT&, const OctreeKey &) const
- {
-
- }
-
- /** \brief Callback executed for every leaf node during deserialization
- **/
- virtual void
- deserializeTreeCallback (LeafContainerT&, const OctreeKey&)
- {
-
- }
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // Helpers
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- /** \brief Helper function to calculate the binary logarithm
- * \param n_arg: some value
- * \return binary logarithm (log2) of argument n_arg
- */
- [[deprecated("use std::log2 instead")]]
- double
- Log2 (double n_arg)
- {
- return std::log2( n_arg );
- }
-
- /** \brief Test if octree is able to dynamically change its depth. This is required for adaptive bounding box adjustment.
- * \return "true"
- **/
- bool
- octreeCanResize ()
- {
- return (true);
- }
- };
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Recursive octree methods
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /** \brief Create a leaf node at octree key. If leaf node does already exist, it is
+ * returned.
+ * \param key_arg: reference to an octree key
+ * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth
+ * indicator
+ * \param branch_arg: current branch node
+ * \param return_leaf_arg: return pointer to leaf node
+ * \param parent_of_leaf_arg: return pointer to parent of leaf node
+ * \return depth mask at which leaf node was created
+ **/
+ unsigned int
+ createLeafRecursive(const OctreeKey& key_arg,
+ unsigned int depth_mask_arg,
+ BranchNode* branch_arg,
+ LeafNode*& return_leaf_arg,
+ BranchNode*& parent_of_leaf_arg);
+
+ /** \brief Recursively search for a given leaf node and return a pointer.
+ * \note If leaf node does not exist, a 0 pointer is returned.
+ * \param key_arg: reference to an octree key
+ * \param depth_mask_arg: depth mask used for octree key analysis and for branch
+ * depth indicator
+ * \param branch_arg: current branch node
+ * \param result_arg: pointer to leaf node class
+ **/
+ void
+ findLeafRecursive(const OctreeKey& key_arg,
+ unsigned int depth_mask_arg,
+ BranchNode* branch_arg,
+ LeafContainerT*& result_arg) const;
+
+ /** \brief Recursively search and delete leaf node
+ * \param key_arg: reference to an octree key
+ * \param depth_mask_arg: depth mask used for octree key analysis and branch depth
+ * indicator
+ * \param branch_arg: current branch node
+ * \return "true" if branch does not contain any childs; "false" otherwise. This
+ * indicates if current branch can be deleted, too.
+ **/
+ bool
+ deleteLeafRecursive(const OctreeKey& key_arg,
+ unsigned int depth_mask_arg,
+ BranchNode* branch_arg);
+
+ /** \brief Recursively explore the octree and output binary octree description
+ * together with a vector of leaf node LeafContainerTs.
+ * \param branch_arg: current branch node
+ * \param key_arg: reference to an octree key
+ * \param binary_tree_out_arg: binary output vector
+ * \param leaf_container_vector_arg: writes LeafContainerT pointers to this
+ *LeafContainerT* vector.
+ **/
+ void
+ serializeTreeRecursive(
+ const BranchNode* branch_arg,
+ OctreeKey& key_arg,
+ std::vector<char>* binary_tree_out_arg,
+ typename std::vector<LeafContainerT*>* leaf_container_vector_arg) const;
+
+ /** \brief Recursive method for deserializing octree structure
+ * \param branch_arg: current branch node
+ * \param depth_mask_arg: depth mask used for octree key analysis and branch depth
+ * indicator
+ * \param key_arg: reference to an octree key
+ * \param binary_tree_input_it_arg: iterator to binary input vector
+ * \param binary_tree_input_it_end_arg: end iterator of binary input vector
+ * \param leaf_container_vector_it_arg: iterator pointing to current LeafContainerT
+ * object to be added to a leaf node
+ * \param leaf_container_vector_it_end_arg: iterator pointing to last object in
+ * LeafContainerT input vector.
+ **/
+ void
+ deserializeTreeRecursive(
+ BranchNode* branch_arg,
+ unsigned int depth_mask_arg,
+ OctreeKey& key_arg,
+ typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
+ typename std::vector<char>::const_iterator& binary_tree_input_it_end_arg,
+ typename std::vector<LeafContainerT*>::const_iterator*
+ leaf_container_vector_it_arg,
+ typename std::vector<LeafContainerT*>::const_iterator*
+ leaf_container_vector_it_end_arg);
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Serialization callbacks
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /** \brief Callback executed for every leaf node during serialization
+ **/
+ virtual void
+ serializeTreeCallback(LeafContainerT&, const OctreeKey&) const
+ {}
+
+ /** \brief Callback executed for every leaf node during deserialization
+ **/
+ virtual void
+ deserializeTreeCallback(LeafContainerT&, const OctreeKey&)
+ {}
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Helpers
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /** \brief Helper function to calculate the binary logarithm
+ * \param n_arg: some value
+ * \return binary logarithm (log2) of argument n_arg
+ */
+ PCL_DEPRECATED("use std::log2 instead") double Log2(double n_arg)
+ {
+ return std::log2(n_arg);
+ }
+
+ /** \brief Test if octree is able to dynamically change its depth. This is required
+ *for adaptive bounding box adjustment.
+ * \return "true"
+ **/
+ bool
+ octreeCanResize()
+ {
+ return (true);
}
-}
+};
+} // namespace octree
+} // namespace pcl
#ifdef PCL_NO_PRECOMPILE
#include <pcl/octree/impl/octree_base.hpp>
#pragma once
-#include <vector>
#include <cstddef>
+#include <vector>
#include <pcl/pcl_macros.h>
-namespace pcl
-{
- namespace octree
+namespace pcl {
+namespace octree {
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Octree container class that can serve as a base to construct own leaf node
+ * container classes.
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+class OctreeContainerBase {
+public:
+ virtual ~OctreeContainerBase() = default;
+
+ /** \brief Equal comparison operator
+ */
+ virtual bool
+ operator==(const OctreeContainerBase&) const
+ {
+ return false;
+ }
+
+ /** \brief Inequal comparison operator
+ * \param[in] other OctreeContainerBase to compare with
+ */
+ bool
+ operator!=(const OctreeContainerBase& other) const
+ {
+ return (!operator==(other));
+ }
+
+ /** \brief Pure abstract method to get size of container (number of indices)
+ * \return number of points/indices stored in leaf node container.
+ */
+ virtual std::size_t
+ getSize() const
+ {
+ return 0u;
+ }
+
+ /** \brief Pure abstract reset leaf node implementation. */
+ virtual void
+ reset() = 0;
+
+ /** \brief Empty addPointIndex implementation. This leaf node does not store any point
+ * indices.
+ */
+ void
+ addPointIndex(const int&)
+ {}
+
+ /** \brief Empty getPointIndex implementation as this leaf node does not store any
+ * point indices.
+ */
+ void
+ getPointIndex(int&) const
+ {}
+
+ /** \brief Empty getPointIndices implementation as this leaf node does not store any
+ * data. \
+ */
+ void
+ getPointIndices(std::vector<int>&) const
+ {}
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Octree container class that does not store any information.
+ * \note Can be used for occupancy trees that are used for checking only the existence
+ * of leaf nodes in the tree
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+
+class OctreeContainerEmpty : public OctreeContainerBase {
+public:
+ /** \brief Octree deep copy method */
+ virtual OctreeContainerEmpty*
+ deepCopy() const
+ {
+ return (new OctreeContainerEmpty(*this));
+ }
+
+ /** \brief Abstract get size of container (number of DataT objects)
+ * \return number of DataT elements in leaf node container.
+ */
+ std::size_t
+ getSize() const override
+ {
+ return 0;
+ }
+
+ /** \brief Abstract reset leaf node implementation. */
+ void
+ reset() override
+ {}
+
+ /** \brief Empty addPointIndex implementation. This leaf node does not store any point
+ * indices.
+ */
+ void
+ addPointIndex(int)
+ {}
+
+ /** \brief Empty getPointIndex implementation as this leaf node does not store any
+ * point indices.
+ */
+ int
+ getPointIndex() const
+ {
+ assert("getPointIndex: undefined point index");
+ return -1;
+ }
+
+ /** \brief Empty getPointIndices implementation as this leaf node does not store any
+ * data.
+ */
+ void
+ getPointIndices(std::vector<int>&) const
+ {}
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Octree container class that does store a single point index.
+ * \note Enables the octree to store a single DataT element within its leaf nodes.
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+class OctreeContainerPointIndex : public OctreeContainerBase {
+public:
+ /** \brief Empty constructor. */
+ OctreeContainerPointIndex() { reset(); }
+
+ /** \brief Octree deep copy method */
+ virtual OctreeContainerPointIndex*
+ deepCopy() const
+ {
+ return (new OctreeContainerPointIndex(*this));
+ }
+
+ /** \brief Equal comparison operator
+ * \param[in] other OctreeContainerBase to compare with
+ */
+ bool
+ operator==(const OctreeContainerBase& other) const override
{
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Octree container class that can serve as a base to construct own leaf node container classes.
- * \author Julius Kammerl (julius@kammerl.de)
- */
- class OctreeContainerBase
- {
- public:
- virtual ~OctreeContainerBase () = default;
-
- /** \brief Equal comparison operator
- */
- virtual bool
- operator== (const OctreeContainerBase&) const
- {
- return false;
- }
-
- /** \brief Inequal comparison operator
- * \param[in] other OctreeContainerBase to compare with
- */
- bool
- operator!= (const OctreeContainerBase& other) const
- {
- return (!operator== (other));
- }
-
- /** \brief Pure abstract method to get size of container (number of indices)
- * \return number of points/indices stored in leaf node container.
- */
- virtual std::size_t
- getSize () const
- {
- return 0u;
- }
-
- /** \brief Pure abstract reset leaf node implementation. */
- virtual void
- reset () = 0;
-
- /** \brief Empty addPointIndex implementation. This leaf node does not store any point indices.
- */
- void
- addPointIndex (const int&)
- {
- }
-
- /** \brief Empty getPointIndex implementation as this leaf node does not store any point indices.
- */
- void
- getPointIndex (int&) const
- {
- }
-
- /** \brief Empty getPointIndices implementation as this leaf node does not store any data. \
- */
- void
- getPointIndices (std::vector<int>&) const
- {
- }
-
- };
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Octree container class that does not store any information.
- * \note Can be used for occupancy trees that are used for checking only the existence of leaf nodes in the tree
- * \author Julius Kammerl (julius@kammerl.de)
- */
-
- class OctreeContainerEmpty : public OctreeContainerBase
- {
- public:
- /** \brief Octree deep copy method */
- virtual OctreeContainerEmpty *
- deepCopy () const
- {
- return (new OctreeContainerEmpty (*this));
- }
-
- /** \brief Abstract get size of container (number of DataT objects)
- * \return number of DataT elements in leaf node container.
- */
- std::size_t
- getSize () const override
- {
- return 0;
- }
-
- /** \brief Abstract reset leaf node implementation. */
- void
- reset () override
- {
-
- }
-
- /** \brief Empty addPointIndex implementation. This leaf node does not store any point indices.
- */
- void
- addPointIndex (int)
- {
- }
-
- /** \brief Empty getPointIndex implementation as this leaf node does not store any point indices.
- */
- int
- getPointIndex () const
- {
- assert("getPointIndex: undefined point index");
- return -1;
- }
-
- /** \brief Empty getPointIndices implementation as this leaf node does not store any data. \
- */
- void
- getPointIndices (std::vector<int>&) const
- {
- }
-
- };
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Octree container class that does store a single point index.
- * \note Enables the octree to store a single DataT element within its leaf nodes.
- * \author Julius Kammerl (julius@kammerl.de)
- */
- class OctreeContainerPointIndex : public OctreeContainerBase
- {
- public:
- /** \brief Empty constructor. */
- OctreeContainerPointIndex ()
- {
- reset ();
- }
-
- /** \brief Octree deep copy method */
- virtual OctreeContainerPointIndex*
- deepCopy () const
- {
- return (new OctreeContainerPointIndex (*this));
- }
-
- /** \brief Equal comparison operator
- * \param[in] other OctreeContainerBase to compare with
- */
- bool
- operator== (const OctreeContainerBase& other) const override
- {
- const OctreeContainerPointIndex* otherConDataT = dynamic_cast<const OctreeContainerPointIndex*> (&other);
-
- return (this->data_ == otherConDataT->data_);
- }
-
- /** \brief Add point index to container memory. This container stores a only a single point index.
- * \param[in] data_arg index to be stored within leaf node.
- */
- void
- addPointIndex (int data_arg)
- {
- data_ = data_arg;
- }
-
- /** \brief Retrieve point index from container. This container stores a only a single point index
- * \return index stored within container.
- */
- int
- getPointIndex () const
- {
- return data_;
- }
-
- /** \brief Retrieve point indices from container. This container stores only a single point index
- * \param[out] data_vector_arg vector of point indices to be stored within data vector
- */
- void
- getPointIndices (std::vector<int>& data_vector_arg) const
- {
- if (data_>=0)
- data_vector_arg.push_back (data_);
- }
-
- /** \brief Get size of container (number of DataT objects)
- * \return number of DataT elements in leaf node container.
- */
- std::size_t
- getSize () const override
- {
- return data_<0 ? 0 : 1;
- }
-
- /** \brief Reset leaf node memory to zero. */
- void
- reset () override
- {
- data_ = -1;
- }
- protected:
- /** \brief Point index stored in octree. */
- int data_;
- };
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Octree container class that does store a vector of point indices.
- * \note Enables the octree to store multiple DataT elements within its leaf nodes.
- * \author Julius Kammerl (julius@kammerl.de)
- */
- class OctreeContainerPointIndices : public OctreeContainerBase
- {
- public:
- /** \brief Octree deep copy method */
- virtual OctreeContainerPointIndices *
- deepCopy () const
- {
- return (new OctreeContainerPointIndices (*this));
- }
-
- /** \brief Equal comparison operator
- * \param[in] other OctreeContainerDataTVector to compare with
- */
- bool
- operator== (const OctreeContainerBase& other) const override
- {
- const OctreeContainerPointIndices* otherConDataTVec = dynamic_cast<const OctreeContainerPointIndices*> (&other);
-
- return (this->leafDataTVector_ == otherConDataTVec->leafDataTVector_);
- }
-
- /** \brief Add point index to container memory. This container stores a vector of point indices.
- * \param[in] data_arg index to be stored within leaf node.
- */
- void
- addPointIndex (int data_arg)
- {
- leafDataTVector_.push_back (data_arg);
- }
-
- /** \brief Retrieve point index from container. This container stores a vector of point indices.
- * \return index stored within container.
- */
- int
- getPointIndex ( ) const
- {
- return leafDataTVector_.back ();
- }
-
- /** \brief Retrieve point indices from container. This container stores a vector of point indices.
- * \param[out] data_vector_arg vector of point indices to be stored within data vector
- */
- void
- getPointIndices (std::vector<int>& data_vector_arg) const
- {
- data_vector_arg.insert (data_vector_arg.end (), leafDataTVector_.begin (), leafDataTVector_.end ());
- }
-
- /** \brief Retrieve reference to point indices vector. This container stores a vector of point indices.
- * \return reference to vector of point indices to be stored within data vector
- */
- std::vector<int>&
- getPointIndicesVector ()
- {
- return leafDataTVector_;
- }
-
- /** \brief Get size of container (number of indices)
- * \return number of point indices in container.
- */
- std::size_t
- getSize () const override
- {
- return leafDataTVector_.size ();
- }
-
- /** \brief Reset leaf node. Clear DataT vector.*/
- void
- reset () override
- {
- leafDataTVector_.clear ();
- }
-
- protected:
- /** \brief Leaf node DataT vector. */
- std::vector<int> leafDataTVector_;
- };
+ const OctreeContainerPointIndex* otherConDataT =
+ dynamic_cast<const OctreeContainerPointIndex*>(&other);
+ return (this->data_ == otherConDataT->data_);
+ }
+
+ /** \brief Add point index to container memory. This container stores a only a single
+ * point index.
+ * \param[in] data_arg index to be stored within leaf node.
+ */
+ void
+ addPointIndex(int data_arg)
+ {
+ data_ = data_arg;
+ }
+
+ /** \brief Retrieve point index from container. This container stores a only a single
+ * point index
+ * \return index stored within container.
+ */
+ int
+ getPointIndex() const
+ {
+ return data_;
+ }
+
+ /** \brief Retrieve point indices from container. This container stores only a single
+ * point index
+ * \param[out] data_vector_arg vector of point indices to be stored within
+ * data vector
+ */
+ void
+ getPointIndices(std::vector<int>& data_vector_arg) const
+ {
+ if (data_ >= 0)
+ data_vector_arg.push_back(data_);
}
-}
+
+ /** \brief Get size of container (number of DataT objects)
+ * \return number of DataT elements in leaf node container.
+ */
+ std::size_t
+ getSize() const override
+ {
+ return data_ < 0 ? 0 : 1;
+ }
+
+ /** \brief Reset leaf node memory to zero. */
+ void
+ reset() override
+ {
+ data_ = -1;
+ }
+
+protected:
+ /** \brief Point index stored in octree. */
+ int data_;
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Octree container class that does store a vector of point indices.
+ * \note Enables the octree to store multiple DataT elements within its leaf nodes.
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+class OctreeContainerPointIndices : public OctreeContainerBase {
+public:
+ /** \brief Octree deep copy method */
+ virtual OctreeContainerPointIndices*
+ deepCopy() const
+ {
+ return (new OctreeContainerPointIndices(*this));
+ }
+
+ /** \brief Equal comparison operator
+ * \param[in] other OctreeContainerDataTVector to compare with
+ */
+ bool
+ operator==(const OctreeContainerBase& other) const override
+ {
+ const OctreeContainerPointIndices* otherConDataTVec =
+ dynamic_cast<const OctreeContainerPointIndices*>(&other);
+
+ return (this->leafDataTVector_ == otherConDataTVec->leafDataTVector_);
+ }
+
+ /** \brief Add point index to container memory. This container stores a vector of
+ * point indices.
+ * \param[in] data_arg index to be stored within leaf node.
+ */
+ void
+ addPointIndex(int data_arg)
+ {
+ leafDataTVector_.push_back(data_arg);
+ }
+
+ /** \brief Retrieve point index from container. This container stores a vector of
+ * point indices.
+ * \return index stored within container.
+ */
+ int
+ getPointIndex() const
+ {
+ return leafDataTVector_.back();
+ }
+
+ /** \brief Retrieve point indices from container. This container stores a vector of
+ * point indices.
+ * \param[out] data_vector_arg vector of point indices to be stored
+ * within data vector
+ */
+ void
+ getPointIndices(std::vector<int>& data_vector_arg) const
+ {
+ data_vector_arg.insert(
+ data_vector_arg.end(), leafDataTVector_.begin(), leafDataTVector_.end());
+ }
+
+ /** \brief Retrieve reference to point indices vector. This container stores a vector
+ * of point indices.
+ * \return reference to vector of point indices to be stored within data vector
+ */
+ std::vector<int>&
+ getPointIndicesVector()
+ {
+ return leafDataTVector_;
+ }
+
+ /** \brief Get size of container (number of indices)
+ * \return number of point indices in container.
+ */
+ std::size_t
+ getSize() const override
+ {
+ return leafDataTVector_.size();
+ }
+
+ /** \brief Reset leaf node. Clear DataT vector.*/
+ void
+ reset() override
+ {
+ leafDataTVector_.clear();
+ }
+
+protected:
+ /** \brief Leaf node DataT vector. */
+ std::vector<int> leafDataTVector_;
+};
+
+} // namespace octree
+} // namespace pcl
#include <pcl/octree/octree.h>
-#include <pcl/octree/impl/octree_base.hpp>
#include <pcl/octree/impl/octree2buf_base.hpp>
-#include <pcl/octree/impl/octree_pointcloud.hpp>
+#include <pcl/octree/impl/octree_base.hpp>
#include <pcl/octree/impl/octree_iterator.hpp>
+#include <pcl/octree/impl/octree_pointcloud.hpp>
#include <pcl/octree/impl/octree_search.hpp>
#pragma once
#include <cstddef>
-#include <vector>
#include <deque>
+#include <vector>
-#include <pcl/octree/octree_nodes.h>
#include <pcl/octree/octree_key.h>
+#include <pcl/octree/octree_nodes.h>
#include <iterator>
// Ignore warnings in the above headers
#ifdef __GNUC__
-#pragma GCC system_header
+#pragma GCC system_header
#endif
-namespace pcl
-{
- namespace octree
+namespace pcl {
+namespace octree {
+
+// Octree iterator state pushed on stack/list
+struct IteratorState {
+ OctreeNode* node_;
+ OctreeKey key_;
+ unsigned int depth_;
+};
+
+/** \brief @b Abstract octree iterator class
+ * \note Octree iterator base class
+ * \ingroup octree
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+template <typename OctreeT>
+class OctreeIteratorBase : public std::iterator<std::forward_iterator_tag,
+ const OctreeNode,
+ void,
+ const OctreeNode*,
+ const OctreeNode&> {
+public:
+ using LeafNode = typename OctreeT::LeafNode;
+ using BranchNode = typename OctreeT::BranchNode;
+
+ using LeafContainer = typename OctreeT::LeafContainer;
+ using BranchContainer = typename OctreeT::BranchContainer;
+
+ /** \brief Empty constructor.
+ */
+ explicit OctreeIteratorBase(unsigned int max_depth_arg = 0)
+ : octree_(0), current_state_(0), max_octree_depth_(max_depth_arg)
+ {
+ this->reset();
+ }
+
+ /** \brief Constructor.
+ * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
+ * root node.
+ * \param[in] max_depth_arg Depth limitation during traversal
+ */
+ explicit OctreeIteratorBase(OctreeT* octree_arg, unsigned int max_depth_arg = 0)
+ : octree_(octree_arg), current_state_(0), max_octree_depth_(max_depth_arg)
+ {
+ this->reset();
+ }
+
+ /** \brief Constructor.
+ * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
+ * root node.
+ * \param[in] max_depth_arg Depth limitation during traversal
+ * \param[in] current_state A pointer to the current iterator state
+ *
+ * \warning For advanced users only.
+ */
+ explicit OctreeIteratorBase(OctreeT* octree_arg,
+ unsigned int max_depth_arg,
+ IteratorState* current_state)
+ : octree_(octree_arg), current_state_(current_state), max_octree_depth_(max_depth_arg)
+ {}
+
+ /** \brief Empty deconstructor. */
+ virtual ~OctreeIteratorBase() {}
+
+ /** \brief Equal comparison operator
+ * \param[in] other OctreeIteratorBase to compare with
+ */
+ bool
+ operator==(const OctreeIteratorBase& other) const
+ {
+ if (this == &other) // same object
+ return true;
+ if (octree_ != other.octree_) // refer to different octrees
+ return false;
+ if (!current_state_ && !other.current_state_) // both are end iterators
+ return true;
+ if (max_octree_depth_ == other.max_octree_depth_ && current_state_ &&
+ other.current_state_ && // null dereference protection
+ current_state_->key_ == other.current_state_->key_)
+ return true;
+ return false;
+ }
+
+ /** \brief Inequal comparison operator
+ * \param[in] other OctreeIteratorBase to compare with
+ */
+ bool
+ operator!=(const OctreeIteratorBase& other) const
+ {
+ return !operator==(other);
+ }
+
+ /** \brief Reset iterator */
+ inline void
+ reset()
+ {
+ current_state_ = 0;
+ if (octree_ && (!max_octree_depth_)) {
+ max_octree_depth_ = octree_->getTreeDepth();
+ }
+ }
+
+ /** \brief Get octree key for the current iterator octree node
+ * \return octree key of current node
+ */
+ inline const OctreeKey&
+ getCurrentOctreeKey() const
+ {
+ assert(octree_ != 0);
+ assert(current_state_ != 0);
+
+ return (current_state_->key_);
+ }
+
+ /** \brief Get the current depth level of octree
+ * \return depth level
+ */
+ inline unsigned int
+ getCurrentOctreeDepth() const
+ {
+ assert(octree_ != 0);
+ assert(current_state_ != 0);
+
+ return (current_state_->depth_);
+ }
+
+ /** \brief Get the current octree node
+ * \return pointer to current octree node
+ */
+ inline OctreeNode*
+ getCurrentOctreeNode() const
+ {
+ assert(octree_ != 0);
+ assert(current_state_ != 0);
+
+ return (current_state_->node_);
+ }
+
+ /** \brief check if current node is a branch node
+ * \return true if current node is a branch node, false otherwise
+ */
+ inline bool
+ isBranchNode() const
+ {
+ assert(octree_ != 0);
+ assert(current_state_ != 0);
+
+ return (current_state_->node_->getNodeType() == BRANCH_NODE);
+ }
+
+ /** \brief check if current node is a branch node
+ * \return true if current node is a branch node, false otherwise
+ */
+ inline bool
+ isLeafNode() const
+ {
+ assert(octree_ != 0);
+ assert(current_state_ != 0);
+
+ return (current_state_->node_->getNodeType() == LEAF_NODE);
+ }
+
+ /** \brief *operator.
+ * \return pointer to the current octree node
+ */
+ inline OctreeNode* operator*() const
+ { // return designated object
+ if (octree_ && current_state_) {
+ return (current_state_->node_);
+ }
+ else {
+ return 0;
+ }
+ }
+
+ /** \brief Get bit pattern of children configuration of current node
+ * \return bit pattern (byte) describing the existence of 8 children of the current
+ * node
+ */
+ inline char
+ getNodeConfiguration() const
+ {
+ char ret = 0;
+
+ assert(octree_ != 0);
+ assert(current_state_ != 0);
+
+ if (isBranchNode()) {
+
+ // current node is a branch node
+ const BranchNode* current_branch =
+ static_cast<const BranchNode*>(current_state_->node_);
+
+ // get child configuration bit pattern
+ ret = octree_->getBranchBitPattern(*current_branch);
+ }
+
+ return (ret);
+ }
+
+ /** \brief Method for retrieving a single leaf container from the octree leaf node
+ * \return Reference to container class of leaf node.
+ */
+ const LeafContainer&
+ getLeafContainer() const
+ {
+ assert(octree_ != 0);
+ assert(current_state_ != 0);
+ assert(this->isLeafNode());
+
+ LeafNode* leaf_node = static_cast<LeafNode*>(current_state_->node_);
+
+ return leaf_node->getContainer();
+ }
+
+ /** \brief Method for retrieving a single leaf container from the octree leaf node
+ * \return Reference to container class of leaf node.
+ */
+ LeafContainer&
+ getLeafContainer()
+ {
+ assert(octree_ != 0);
+ assert(current_state_ != 0);
+ assert(this->isLeafNode());
+
+ LeafNode* leaf_node = static_cast<LeafNode*>(current_state_->node_);
+
+ return leaf_node->getContainer();
+ }
+
+ /** \brief Method for retrieving the container from an octree branch node
+ * \return BranchContainer.
+ */
+ const BranchContainer&
+ getBranchContainer() const
+ {
+ assert(octree_ != 0);
+ assert(current_state_ != 0);
+ assert(this->isBranchNode());
+
+ BranchNode* branch_node = static_cast<BranchNode*>(current_state_->node_);
+
+ return branch_node->getContainer();
+ }
+
+ /** \brief Method for retrieving the container from an octree branch node
+ * \return BranchContainer.
+ */
+ BranchContainer&
+ getBranchContainer()
+ {
+ assert(octree_ != 0);
+ assert(current_state_ != 0);
+ assert(this->isBranchNode());
+
+ BranchNode* branch_node = static_cast<BranchNode*>(current_state_->node_);
+
+ return branch_node->getContainer();
+ }
+
+ /** \brief get a integer identifier for current node (note: identifier depends on tree
+ * depth). \return node id.
+ */
+ virtual unsigned long
+ getNodeID() const
+ {
+ unsigned long id = 0;
+
+ assert(octree_ != 0);
+ assert(current_state_ != 0);
+
+ if (current_state_) {
+ const OctreeKey& key = getCurrentOctreeKey();
+ // calculate integer id with respect to octree key
+ unsigned int depth = octree_->getTreeDepth();
+ id = static_cast<unsigned long>(key.x) << (depth * 2) |
+ static_cast<unsigned long>(key.y) << (depth * 1) |
+ static_cast<unsigned long>(key.z) << (depth * 0);
+ }
+
+ return id;
+ }
+
+protected:
+ /** \brief Reference to octree class. */
+ OctreeT* octree_;
+
+ /** \brief Pointer to current iterator state. */
+ IteratorState* current_state_;
+
+ /** \brief Maximum octree depth */
+ unsigned int max_octree_depth_;
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Octree iterator class
+ * \note This class implements a forward iterator for traversing octrees in a
+ * depth-first manner.
+ * \ingroup octree
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+template <typename OctreeT>
+class OctreeDepthFirstIterator : public OctreeIteratorBase<OctreeT> {
+
+public:
+ using LeafNode = typename OctreeIteratorBase<OctreeT>::LeafNode;
+ using BranchNode = typename OctreeIteratorBase<OctreeT>::BranchNode;
+
+ /** \brief Empty constructor.
+ * \param[in] max_depth_arg Depth limitation during traversal
+ */
+ explicit OctreeDepthFirstIterator(unsigned int max_depth_arg = 0);
+
+ /** \brief Constructor.
+ * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
+ * root node.
+ * \param[in] max_depth_arg Depth limitation during traversal
+ */
+ explicit OctreeDepthFirstIterator(OctreeT* octree_arg,
+ unsigned int max_depth_arg = 0);
+
+ /** \brief Constructor.
+ * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
+ * root node.
+ * \param[in] max_depth_arg Depth limitation during traversal
+ * \param[in] current_state A pointer to the current iterator state
+ *
+ * \warning For advanced users only.
+ */
+ explicit OctreeDepthFirstIterator(
+ OctreeT* octree_arg,
+ unsigned int max_depth_arg,
+ IteratorState* current_state,
+ const std::vector<IteratorState>& stack = std::vector<IteratorState>())
+ : OctreeIteratorBase<OctreeT>(octree_arg, max_depth_arg, current_state), stack_(stack)
+ {}
+
+ /** \brief Copy Constructor.
+ * \param[in] other Another OctreeDepthFirstIterator to copy from
+ */
+ OctreeDepthFirstIterator(const OctreeDepthFirstIterator& other)
+ : OctreeIteratorBase<OctreeT>(other), stack_(other.stack_)
+ {
+ this->current_state_ = stack_.size() ? &stack_.back() : NULL;
+ }
+
+ /** \brief Copy assignment
+ * \param[in] src the iterator to copy into this
+ */
+ inline OctreeDepthFirstIterator&
+ operator=(const OctreeDepthFirstIterator& src)
+ {
+
+ OctreeIteratorBase<OctreeT>::operator=(src);
+
+ stack_ = src.stack_;
+
+ if (stack_.size()) {
+ this->current_state_ = &stack_.back();
+ }
+ else {
+ this->current_state_ = 0;
+ }
+
+ return (*this);
+ }
+
+ /** \brief Reset the iterator to the root node of the octree
+ */
+ virtual void
+ reset();
+
+ /** \brief Preincrement operator.
+ * \note recursively step to next octree node
+ */
+ OctreeDepthFirstIterator&
+ operator++();
+
+ /** \brief postincrement operator.
+ * \note recursively step to next octree node
+ */
+ inline OctreeDepthFirstIterator
+ operator++(int)
+ {
+ OctreeDepthFirstIterator _Tmp = *this;
+ ++*this;
+ return (_Tmp);
+ }
+
+ /** \brief Skip all child voxels of current node and return to parent node.
+ */
+ void
+ skipChildVoxels();
+
+protected:
+ /** Stack structure. */
+ std::vector<IteratorState> stack_;
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Octree iterator class
+ * \note This class implements a forward iterator for traversing octrees in a
+ * breadth-first manner.
+ * \ingroup octree
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+template <typename OctreeT>
+class OctreeBreadthFirstIterator : public OctreeIteratorBase<OctreeT> {
+public:
+ // public typedefs
+ using BranchNode = typename OctreeIteratorBase<OctreeT>::BranchNode;
+ using LeafNode = typename OctreeIteratorBase<OctreeT>::LeafNode;
+
+ /** \brief Empty constructor.
+ * \param[in] max_depth_arg Depth limitation during traversal
+ */
+ explicit OctreeBreadthFirstIterator(unsigned int max_depth_arg = 0);
+
+ /** \brief Constructor.
+ * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
+ * root node.
+ * \param[in] max_depth_arg Depth limitation during traversal
+ */
+ explicit OctreeBreadthFirstIterator(OctreeT* octree_arg,
+ unsigned int max_depth_arg = 0);
+
+ /** \brief Constructor.
+ * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
+ * root node.
+ * \param[in] max_depth_arg Depth limitation during traversal
+ * \param[in] current_state A pointer to the current iterator state
+ *
+ * \warning For advanced users only.
+ */
+ explicit OctreeBreadthFirstIterator(
+ OctreeT* octree_arg,
+ unsigned int max_depth_arg,
+ IteratorState* current_state,
+ const std::deque<IteratorState>& fifo = std::deque<IteratorState>())
+ : OctreeIteratorBase<OctreeT>(octree_arg, max_depth_arg, current_state), FIFO_(fifo)
+ {}
+
+ /** \brief Copy Constructor.
+ * \param[in] other Another OctreeBreadthFirstIterator to copy from
+ */
+ OctreeBreadthFirstIterator(const OctreeBreadthFirstIterator& other)
+ : OctreeIteratorBase<OctreeT>(other), FIFO_(other.FIFO_)
+ {
+ this->current_state_ = FIFO_.size() ? &FIFO_.front() : NULL;
+ }
+
+ /** \brief Copy operator.
+ * \param[in] src the iterator to copy into this
+ */
+ inline OctreeBreadthFirstIterator&
+ operator=(const OctreeBreadthFirstIterator& src)
{
- // Octree iterator state pushed on stack/list
- struct IteratorState{
- OctreeNode* node_;
- OctreeKey key_;
- unsigned int depth_;
- };
-
-
- /** \brief @b Abstract octree iterator class
- * \note Octree iterator base class
- * \ingroup octree
- * \author Julius Kammerl (julius@kammerl.de)
- */
- template<typename OctreeT>
- class OctreeIteratorBase : public std::iterator<std::forward_iterator_tag, const OctreeNode, void,
- const OctreeNode*, const OctreeNode&>
- {
- public:
-
- using LeafNode = typename OctreeT::LeafNode;
- using BranchNode = typename OctreeT::BranchNode;
-
- using LeafContainer = typename OctreeT::LeafContainer;
- using BranchContainer = typename OctreeT::BranchContainer;
-
- /** \brief Empty constructor.
- */
- explicit
- OctreeIteratorBase (unsigned int max_depth_arg = 0) :
- octree_ (0), current_state_(0), max_octree_depth_(max_depth_arg)
- {
- this->reset ();
- }
-
- /** \brief Constructor.
- * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
- * \param[in] max_depth_arg Depth limitation during traversal
- */
- explicit
- OctreeIteratorBase (OctreeT* octree_arg, unsigned int max_depth_arg = 0) :
- octree_ (octree_arg), current_state_(0), max_octree_depth_(max_depth_arg)
- {
- this->reset ();
- }
-
- /** \brief Constructor.
- * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
- * \param[in] max_depth_arg Depth limitation during traversal
- * \param[in] current_state A pointer to the current iterator state
- *
- * \warning For advanced users only.
- */
- explicit
- OctreeIteratorBase (OctreeT* octree_arg,
- unsigned int max_depth_arg,
- IteratorState* current_state)
- : octree_(octree_arg)
- , current_state_ (current_state)
- , max_octree_depth_ (max_depth_arg)
- {}
-
- /** \brief Empty deconstructor. */
- virtual
- ~OctreeIteratorBase ()
- {
- }
-
- /** \brief Equal comparison operator
- * \param[in] other OctreeIteratorBase to compare with
- */
- bool operator==(const OctreeIteratorBase& other) const
- {
- if (this == &other) // same object
- return true;
- if (octree_ != other.octree_) // refer to different octrees
- return false;
- if (!current_state_ && !other.current_state_) // both are end iterators
- return true;
- if (max_octree_depth_ == other.max_octree_depth_ &&
- current_state_ && other.current_state_ && // null dereference protection
- current_state_->key_ == other.current_state_->key_)
- return true;
- return false;
- }
-
- /** \brief Inequal comparison operator
- * \param[in] other OctreeIteratorBase to compare with
- */
- bool operator!=(const OctreeIteratorBase& other) const
- {
- return !operator== (other);
- }
-
- /** \brief Reset iterator */
- inline void reset ()
- {
- current_state_ = 0;
- if (octree_ && (!max_octree_depth_))
- {
- max_octree_depth_ = octree_->getTreeDepth();
- }
- }
-
- /** \brief Get octree key for the current iterator octree node
- * \return octree key of current node
- */
- inline const OctreeKey&
- getCurrentOctreeKey () const
- {
- assert(octree_!=0);
- assert(current_state_!=0);
-
- return (current_state_->key_);
- }
-
- /** \brief Get the current depth level of octree
- * \return depth level
- */
- inline unsigned int
- getCurrentOctreeDepth () const
- {
- assert(octree_!=0);
- assert(current_state_!=0);
-
- return (current_state_->depth_);
- }
-
- /** \brief Get the current octree node
- * \return pointer to current octree node
- */
- inline OctreeNode*
- getCurrentOctreeNode () const
- {
- assert(octree_!=0);
- assert(current_state_!=0);
-
- return (current_state_->node_);
- }
-
-
- /** \brief check if current node is a branch node
- * \return true if current node is a branch node, false otherwise
- */
- inline bool
- isBranchNode () const
- {
- assert(octree_!=0);
- assert(current_state_!=0);
-
- return (current_state_->node_->getNodeType () == BRANCH_NODE);
- }
-
- /** \brief check if current node is a branch node
- * \return true if current node is a branch node, false otherwise
- */
- inline bool
- isLeafNode () const
- {
- assert(octree_!=0);
- assert(current_state_!=0);
-
- return (current_state_->node_->getNodeType () == LEAF_NODE);
- }
-
- /** \brief *operator.
- * \return pointer to the current octree node
- */
- inline OctreeNode*
- operator* () const
- { // return designated object
- if (octree_ && current_state_)
- {
- return (current_state_->node_);
- } else
- {
- return 0;
- }
- }
-
- /** \brief Get bit pattern of children configuration of current node
- * \return bit pattern (byte) describing the existence of 8 children of the current node
- */
- inline char
- getNodeConfiguration () const
- {
- char ret = 0;
-
- assert(octree_!=0);
- assert(current_state_!=0);
-
- if (isBranchNode ())
- {
-
- // current node is a branch node
- const BranchNode* current_branch = static_cast<const BranchNode*> (current_state_->node_);
-
- // get child configuration bit pattern
- ret = octree_->getBranchBitPattern (*current_branch);
-
- }
-
- return (ret);
- }
-
- /** \brief Method for retrieving a single leaf container from the octree leaf node
- * \return Reference to container class of leaf node.
- */
- const LeafContainer&
- getLeafContainer () const
- {
- assert(octree_!=0);
- assert(current_state_!=0);
- assert(this->isLeafNode());
-
- LeafNode* leaf_node = static_cast<LeafNode*>(current_state_->node_);
-
- return leaf_node->getContainer();
- }
-
- /** \brief Method for retrieving a single leaf container from the octree leaf node
- * \return Reference to container class of leaf node.
- */
- LeafContainer&
- getLeafContainer ()
- {
- assert(octree_!=0);
- assert(current_state_!=0);
- assert(this->isLeafNode());
-
- LeafNode* leaf_node = static_cast<LeafNode*>(current_state_->node_);
-
- return leaf_node->getContainer();
- }
-
- /** \brief Method for retrieving the container from an octree branch node
- * \return BranchContainer.
- */
- const BranchContainer&
- getBranchContainer () const
- {
- assert(octree_!=0);
- assert(current_state_!=0);
- assert(this->isBranchNode());
-
- BranchNode* branch_node = static_cast<BranchNode*>(current_state_->node_);
-
- return branch_node->getContainer();
- }
-
- /** \brief Method for retrieving the container from an octree branch node
- * \return BranchContainer.
- */
- BranchContainer&
- getBranchContainer ()
- {
- assert(octree_!=0);
- assert(current_state_!=0);
- assert(this->isBranchNode());
-
- BranchNode* branch_node = static_cast<BranchNode*>(current_state_->node_);
-
- return branch_node->getContainer();
- }
-
- /** \brief get a integer identifier for current node (note: identifier depends on tree depth).
- * \return node id.
- */
- virtual unsigned long
- getNodeID () const
- {
- unsigned long id = 0;
-
- assert(octree_!=0);
- assert(current_state_!=0);
-
- if (current_state_)
- {
- const OctreeKey& key = getCurrentOctreeKey();
- // calculate integer id with respect to octree key
- unsigned int depth = octree_->getTreeDepth ();
- id = static_cast<unsigned long> (key.x) << (depth * 2)
- | static_cast<unsigned long> (key.y) << (depth * 1)
- | static_cast<unsigned long> (key.z) << (depth * 0);
- }
-
- return id;
- }
-
- protected:
- /** \brief Reference to octree class. */
- OctreeT* octree_;
-
- /** \brief Pointer to current iterator state. */
- IteratorState* current_state_;
-
- /** \brief Maximum octree depth */
- unsigned int max_octree_depth_;
- };
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Octree iterator class
- * \note This class implements a forward iterator for traversing octrees in a depth-first manner.
- * \ingroup octree
- * \author Julius Kammerl (julius@kammerl.de)
- */
- template<typename OctreeT>
- class OctreeDepthFirstIterator : public OctreeIteratorBase<OctreeT>
- {
-
- public:
-
- using LeafNode = typename OctreeIteratorBase<OctreeT>::LeafNode;
- using BranchNode = typename OctreeIteratorBase<OctreeT>::BranchNode;
-
- /** \brief Empty constructor.
- * \param[in] max_depth_arg Depth limitation during traversal
- */
- explicit
- OctreeDepthFirstIterator (unsigned int max_depth_arg = 0);
-
- /** \brief Constructor.
- * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
- * \param[in] max_depth_arg Depth limitation during traversal
- */
- explicit
- OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0);
-
- /** \brief Constructor.
- * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
- * \param[in] max_depth_arg Depth limitation during traversal
- * \param[in] current_state A pointer to the current iterator state
- *
- * \warning For advanced users only.
- */
- explicit
- OctreeDepthFirstIterator (OctreeT* octree_arg,
- unsigned int max_depth_arg,
- IteratorState* current_state,
- const std::vector<IteratorState>& stack = std::vector<IteratorState> ())
- : OctreeIteratorBase<OctreeT> (octree_arg, max_depth_arg, current_state)
- , stack_ (stack)
- {}
-
- /** \brief Copy Constructor.
- * \param[in] other Another OctreeDepthFirstIterator to copy from
- */
- OctreeDepthFirstIterator (const OctreeDepthFirstIterator& other)
- : OctreeIteratorBase<OctreeT> (other)
- , stack_ (other.stack_)
- {
- this->current_state_ = stack_.size ()? &stack_.back () : NULL;
- }
-
- /** \brief Copy assignment
- * \param[in] src the iterator to copy into this
- */
- inline OctreeDepthFirstIterator&
- operator = (const OctreeDepthFirstIterator& src)
- {
-
- OctreeIteratorBase<OctreeT>::operator=(src);
-
- stack_ = src.stack_;
-
- if (stack_.size())
- {
- this->current_state_ = &stack_.back ();
- } else
- {
- this->current_state_ = 0;
- }
-
- return (*this);
- }
-
- /** \brief Reset the iterator to the root node of the octree
- */
- virtual void
- reset ();
-
- /** \brief Preincrement operator.
- * \note recursively step to next octree node
- */
- OctreeDepthFirstIterator&
- operator++ ();
-
- /** \brief postincrement operator.
- * \note recursively step to next octree node
- */
- inline OctreeDepthFirstIterator
- operator++ (int)
- {
- OctreeDepthFirstIterator _Tmp = *this;
- ++*this;
- return (_Tmp);
- }
-
- /** \brief Skip all child voxels of current node and return to parent node.
- */
- void
- skipChildVoxels ();
-
- protected:
- /** Stack structure. */
- std::vector<IteratorState> stack_;
- };
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Octree iterator class
- * \note This class implements a forward iterator for traversing octrees in a breadth-first manner.
- * \ingroup octree
- * \author Julius Kammerl (julius@kammerl.de)
- */
- template<typename OctreeT>
- class OctreeBreadthFirstIterator : public OctreeIteratorBase<OctreeT>
- {
- public:
- // public typedefs
- using BranchNode = typename OctreeIteratorBase<OctreeT>::BranchNode;
- using LeafNode = typename OctreeIteratorBase<OctreeT>::LeafNode;
-
- /** \brief Empty constructor.
- * \param[in] max_depth_arg Depth limitation during traversal
- */
- explicit
- OctreeBreadthFirstIterator (unsigned int max_depth_arg = 0);
-
- /** \brief Constructor.
- * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
- * \param[in] max_depth_arg Depth limitation during traversal
- */
- explicit
- OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0);
-
- /** \brief Constructor.
- * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
- * \param[in] max_depth_arg Depth limitation during traversal
- * \param[in] current_state A pointer to the current iterator state
- *
- * \warning For advanced users only.
- */
- explicit
- OctreeBreadthFirstIterator (OctreeT* octree_arg,
- unsigned int max_depth_arg,
- IteratorState* current_state,
- const std::deque<IteratorState>& fifo = std::deque<IteratorState> ())
- : OctreeIteratorBase<OctreeT> (octree_arg, max_depth_arg, current_state)
- , FIFO_ (fifo)
- {}
-
- /** \brief Copy Constructor.
- * \param[in] other Another OctreeBreadthFirstIterator to copy from
- */
- OctreeBreadthFirstIterator (const OctreeBreadthFirstIterator& other)
- : OctreeIteratorBase<OctreeT> (other)
- , FIFO_ (other.FIFO_)
- {
- this->current_state_ = FIFO_.size ()? &FIFO_.front () : NULL;
- }
-
- /** \brief Copy operator.
- * \param[in] src the iterator to copy into this
- */
- inline OctreeBreadthFirstIterator&
- operator = (const OctreeBreadthFirstIterator& src)
- {
-
- OctreeIteratorBase<OctreeT>::operator=(src);
-
- FIFO_ = src.FIFO_;
-
- if (FIFO_.size())
- {
- this->current_state_ = &FIFO_.front();
- } else
- {
- this->current_state_ = 0;
- }
-
- return (*this);
- }
-
- /** \brief Reset the iterator to the root node of the octree
- */
- void
- reset ();
-
- /** \brief Preincrement operator.
- * \note step to next octree node
- */
- OctreeBreadthFirstIterator&
- operator++ ();
-
- /** \brief postincrement operator.
- * \note step to next octree node
- */
- inline OctreeBreadthFirstIterator
- operator++ (int)
- {
- OctreeBreadthFirstIterator _Tmp = *this;
- ++*this;
- return (_Tmp);
- }
-
- protected:
- /** FIFO list */
- std::deque<IteratorState> FIFO_;
- };
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Octree iterator class
- * \note Iterator over all existing nodes at a given depth. It walks across an octree
- * in a breadth-first manner.
- * \ingroup octree
- * \author Fabien Rozar (fabien.rozar@gmail.com)
- */
- template<typename OctreeT>
- class OctreeFixedDepthIterator : public OctreeBreadthFirstIterator<OctreeT>
- {
- public:
-
- // public typedefs
- using typename OctreeBreadthFirstIterator<OctreeT>::BranchNode;
- using typename OctreeBreadthFirstIterator<OctreeT>::LeafNode;
-
- /** \brief Empty constructor.
- */
- OctreeFixedDepthIterator ();
-
- /** \brief Constructor.
- * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
- * \param[in] fixed_depth_arg Depth level during traversal
- */
- explicit
- OctreeFixedDepthIterator (OctreeT* octree_arg, unsigned int fixed_depth_arg = 0);
-
- /** \brief Constructor.
- * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
- * \param[in] fixed_depth_arg Depth level during traversal
- * \param[in] current_state A pointer to the current iterator state
- * \param[in] fifo Internal container of octree node to go through
- *
- * \warning For advanced users only.
- */
- OctreeFixedDepthIterator (OctreeT* octree_arg,
- unsigned int fixed_depth_arg,
- IteratorState* current_state,
- const std::deque<IteratorState>& fifo = std::deque<IteratorState> ())
- : OctreeBreadthFirstIterator<OctreeT> (octree_arg, fixed_depth_arg, current_state, fifo)
- , fixed_depth_ (fixed_depth_arg)
- {}
-
- /** \brief Copy Constructor.
- * \param[in] other Another OctreeFixedDepthIterator to copy from
- */
- OctreeFixedDepthIterator (const OctreeFixedDepthIterator& other)
- : OctreeBreadthFirstIterator<OctreeT> (other)
- {
- this->fixed_depth_ = other.fixed_depth_;
- }
-
- /** \brief Copy assignment.
- * \param[in] src the iterator to copy into this
- * \return pointer to the current octree node
- */
- inline OctreeFixedDepthIterator&
- operator = (const OctreeFixedDepthIterator& src)
- {
- OctreeBreadthFirstIterator<OctreeT>::operator= (src);
- this->fixed_depth_ = src.fixed_depth_;
-
- return (*this);
- }
-
- /** \brief Reset the iterator to the first node at the depth given as parameter
- * \param[in] fixed_depth_arg Depth level during traversal
- */
- void
- reset (unsigned int fixed_depth_arg);
-
- /** \brief Reset the iterator to the first node at the current depth
- */
- void
- reset ()
- {
- this->reset (fixed_depth_);
- }
-
- protected:
- using OctreeBreadthFirstIterator<OctreeT>::FIFO_;
-
- /** \brief Given level of the node to be iterated */
- unsigned int fixed_depth_;
- };
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief Octree leaf node iterator class
- * \note This class implements a forward iterator for traversing the leaf nodes of an octree data structure.
- * \ingroup octree
- * \author Julius Kammerl (julius@kammerl.de)
- */
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- class OctreeLeafNodeDepthFirstIterator : public OctreeDepthFirstIterator<OctreeT>
- {
- using BranchNode = typename OctreeDepthFirstIterator<OctreeT>::BranchNode;
- using LeafNode = typename OctreeDepthFirstIterator<OctreeT>::LeafNode;
-
- public:
- /** \brief Empty constructor.
- * \param[in] max_depth_arg Depth limitation during traversal
- */
- explicit
- OctreeLeafNodeDepthFirstIterator (unsigned int max_depth_arg = 0) :
- OctreeDepthFirstIterator<OctreeT> (max_depth_arg)
- {
- reset ();
- }
-
- /** \brief Constructor.
- * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
- * \param[in] max_depth_arg Depth limitation during traversal
- */
- explicit
- OctreeLeafNodeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0) :
- OctreeDepthFirstIterator<OctreeT> (octree_arg, max_depth_arg)
- {
- reset ();
- }
-
- /** \brief Constructor.
- * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
- * \param[in] max_depth_arg Depth limitation during traversal
- * \param[in] current_state A pointer to the current iterator state
- *
- * \warning For advanced users only.
- */
- explicit
- OctreeLeafNodeDepthFirstIterator (OctreeT* octree_arg,
- unsigned int max_depth_arg,
- IteratorState* current_state,
- const std::vector<IteratorState>& stack = std::vector<IteratorState> ())
- : OctreeDepthFirstIterator<OctreeT> (octree_arg,
- max_depth_arg,
- current_state,
- stack)
- {}
-
- /** \brief Reset the iterator to the root node of the octree
- */
- inline void
- reset ()
- {
- OctreeDepthFirstIterator<OctreeT>::reset ();
- this->operator++ ();
- }
-
- /** \brief Preincrement operator.
- * \note recursively step to next octree leaf node
- */
- inline OctreeLeafNodeDepthFirstIterator&
- operator++ ()
- {
- do
- {
- OctreeDepthFirstIterator<OctreeT>::operator++ ();
- } while ((this->current_state_) && (this->current_state_->node_->getNodeType () != LEAF_NODE));
-
- return (*this);
- }
-
- /** \brief postincrement operator.
- * \note step to next octree node
- */
- inline OctreeLeafNodeDepthFirstIterator
- operator++ (int)
- {
- OctreeLeafNodeDepthFirstIterator _Tmp = *this;
- ++*this;
- return (_Tmp);
- }
-
- /** \brief *operator.
- * \return pointer to the current octree leaf node
- */
- OctreeNode*
- operator* () const
- {
- // return designated object
- OctreeNode* ret = 0;
-
- if (this->current_state_ && (this->current_state_->node_->getNodeType () == LEAF_NODE))
- ret = this->current_state_->node_;
- return (ret);
- }
- };
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief Octree leaf node iterator class
- * \note This class implements a forward iterator for traversing the leaf nodes of an octree data structure
- * in the breadth first way.
- * \ingroup octree
- * \author Fabien Rozar (fabien.rozar@gmail.com)
- */
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- template<typename OctreeT>
- class OctreeLeafNodeBreadthFirstIterator : public OctreeBreadthFirstIterator<OctreeT>
- {
- using BranchNode = typename OctreeBreadthFirstIterator<OctreeT>::BranchNode;
- using LeafNode = typename OctreeBreadthFirstIterator<OctreeT>::LeafNode;
-
- public:
- /** \brief Empty constructor.
- * \param[in] max_depth_arg Depth limitation during traversal
- */
- explicit
- OctreeLeafNodeBreadthFirstIterator (unsigned int max_depth_arg = 0);
-
- /** \brief Constructor.
- * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
- * \param[in] max_depth_arg Depth limitation during traversal
- */
- explicit
- OctreeLeafNodeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0);
-
- /** \brief Copy constructor.
- * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
- * \param[in] max_depth_arg Depth limitation during traversal
- * \param[in] current_state A pointer to the current iterator state
- * \param[in] fifo Internal container of octree node to go through
- *
- * \warning For advanced users only.
- */
- explicit
- OctreeLeafNodeBreadthFirstIterator (OctreeT* octree_arg,
- unsigned int max_depth_arg,
- IteratorState* current_state,
- const std::deque<IteratorState>& fifo = std::deque<IteratorState> ());
-
- /** \brief Reset the iterator to the first leaf in the breadth first way.
- */
- inline void
- reset ();
-
- /** \brief Preincrement operator.
- * \note recursively step to next octree leaf node
- */
- inline OctreeLeafNodeBreadthFirstIterator&
- operator++ ();
-
-
- /** \brief Postincrement operator.
- * \note step to next octree node
- */
- inline OctreeLeafNodeBreadthFirstIterator
- operator++ (int);
- };
-
- }
-}
+ OctreeIteratorBase<OctreeT>::operator=(src);
+
+ FIFO_ = src.FIFO_;
+
+ if (FIFO_.size()) {
+ this->current_state_ = &FIFO_.front();
+ }
+ else {
+ this->current_state_ = 0;
+ }
+
+ return (*this);
+ }
+
+ /** \brief Reset the iterator to the root node of the octree
+ */
+ void
+ reset();
+
+ /** \brief Preincrement operator.
+ * \note step to next octree node
+ */
+ OctreeBreadthFirstIterator&
+ operator++();
+
+ /** \brief postincrement operator.
+ * \note step to next octree node
+ */
+ inline OctreeBreadthFirstIterator
+ operator++(int)
+ {
+ OctreeBreadthFirstIterator _Tmp = *this;
+ ++*this;
+ return (_Tmp);
+ }
+
+protected:
+ /** FIFO list */
+ std::deque<IteratorState> FIFO_;
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Octree iterator class
+ * \note Iterator over all existing nodes at a given depth. It walks across an octree
+ * in a breadth-first manner.
+ * \ingroup octree
+ * \author Fabien Rozar (fabien.rozar@gmail.com)
+ */
+template <typename OctreeT>
+class OctreeFixedDepthIterator : public OctreeBreadthFirstIterator<OctreeT> {
+public:
+ // public typedefs
+ using typename OctreeBreadthFirstIterator<OctreeT>::BranchNode;
+ using typename OctreeBreadthFirstIterator<OctreeT>::LeafNode;
+
+ /** \brief Empty constructor.
+ */
+ OctreeFixedDepthIterator();
+
+ /** \brief Constructor.
+ * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
+ * root node.
+ * \param[in] fixed_depth_arg Depth level during traversal
+ */
+ explicit OctreeFixedDepthIterator(OctreeT* octree_arg,
+ unsigned int fixed_depth_arg = 0);
+
+ /** \brief Constructor.
+ * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
+ * root node.
+ * \param[in] fixed_depth_arg Depth level during traversal
+ * \param[in] current_state A pointer to the current iterator state
+ * \param[in] fifo Internal container of octree node to go through
+ *
+ * \warning For advanced users only.
+ */
+ OctreeFixedDepthIterator(
+ OctreeT* octree_arg,
+ unsigned int fixed_depth_arg,
+ IteratorState* current_state,
+ const std::deque<IteratorState>& fifo = std::deque<IteratorState>())
+ : OctreeBreadthFirstIterator<OctreeT>(
+ octree_arg, fixed_depth_arg, current_state, fifo)
+ , fixed_depth_(fixed_depth_arg)
+ {}
+
+ /** \brief Copy Constructor.
+ * \param[in] other Another OctreeFixedDepthIterator to copy from
+ */
+ OctreeFixedDepthIterator(const OctreeFixedDepthIterator& other)
+ : OctreeBreadthFirstIterator<OctreeT>(other)
+ {
+ this->fixed_depth_ = other.fixed_depth_;
+ }
+
+ /** \brief Copy assignment.
+ * \param[in] src the iterator to copy into this
+ * \return pointer to the current octree node
+ */
+ inline OctreeFixedDepthIterator&
+ operator=(const OctreeFixedDepthIterator& src)
+ {
+ OctreeBreadthFirstIterator<OctreeT>::operator=(src);
+ this->fixed_depth_ = src.fixed_depth_;
+
+ return (*this);
+ }
+
+ /** \brief Reset the iterator to the first node at the depth given as parameter
+ * \param[in] fixed_depth_arg Depth level during traversal
+ */
+ void
+ reset(unsigned int fixed_depth_arg);
+
+ /** \brief Reset the iterator to the first node at the current depth
+ */
+ void
+ reset()
+ {
+ this->reset(fixed_depth_);
+ }
+
+protected:
+ using OctreeBreadthFirstIterator<OctreeT>::FIFO_;
+
+ /** \brief Given level of the node to be iterated */
+ unsigned int fixed_depth_;
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief Octree leaf node iterator class
+ * \note This class implements a forward iterator for traversing the leaf nodes of an
+ * octree data structure.
+ * \ingroup octree
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+class OctreeLeafNodeDepthFirstIterator : public OctreeDepthFirstIterator<OctreeT> {
+ using BranchNode = typename OctreeDepthFirstIterator<OctreeT>::BranchNode;
+ using LeafNode = typename OctreeDepthFirstIterator<OctreeT>::LeafNode;
+
+public:
+ /** \brief Empty constructor.
+ * \param[in] max_depth_arg Depth limitation during traversal
+ */
+ explicit OctreeLeafNodeDepthFirstIterator(unsigned int max_depth_arg = 0)
+ : OctreeDepthFirstIterator<OctreeT>(max_depth_arg)
+ {
+ reset();
+ }
+
+ /** \brief Constructor.
+ * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
+ * root node.
+ * \param[in] max_depth_arg Depth limitation during traversal
+ */
+ explicit OctreeLeafNodeDepthFirstIterator(OctreeT* octree_arg,
+ unsigned int max_depth_arg = 0)
+ : OctreeDepthFirstIterator<OctreeT>(octree_arg, max_depth_arg)
+ {
+ reset();
+ }
+
+ /** \brief Constructor.
+ * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
+ * root node.
+ * \param[in] max_depth_arg Depth limitation during traversal
+ * \param[in] current_state A pointer to the current iterator state
+ *
+ * \warning For advanced users only.
+ */
+ explicit OctreeLeafNodeDepthFirstIterator(
+ OctreeT* octree_arg,
+ unsigned int max_depth_arg,
+ IteratorState* current_state,
+ const std::vector<IteratorState>& stack = std::vector<IteratorState>())
+ : OctreeDepthFirstIterator<OctreeT>(octree_arg, max_depth_arg, current_state, stack)
+ {}
+
+ /** \brief Reset the iterator to the root node of the octree
+ */
+ inline void
+ reset()
+ {
+ OctreeDepthFirstIterator<OctreeT>::reset();
+ this->operator++();
+ }
+
+ /** \brief Preincrement operator.
+ * \note recursively step to next octree leaf node
+ */
+ inline OctreeLeafNodeDepthFirstIterator&
+ operator++()
+ {
+ do {
+ OctreeDepthFirstIterator<OctreeT>::operator++();
+ } while ((this->current_state_) &&
+ (this->current_state_->node_->getNodeType() != LEAF_NODE));
+
+ return (*this);
+ }
+
+ /** \brief postincrement operator.
+ * \note step to next octree node
+ */
+ inline OctreeLeafNodeDepthFirstIterator
+ operator++(int)
+ {
+ OctreeLeafNodeDepthFirstIterator _Tmp = *this;
+ ++*this;
+ return (_Tmp);
+ }
+
+ /** \brief *operator.
+ * \return pointer to the current octree leaf node
+ */
+ OctreeNode* operator*() const
+ {
+ // return designated object
+ OctreeNode* ret = 0;
+
+ if (this->current_state_ &&
+ (this->current_state_->node_->getNodeType() == LEAF_NODE))
+ ret = this->current_state_->node_;
+ return (ret);
+ }
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief Octree leaf node iterator class
+ * \note This class implements a forward iterator for traversing the leaf nodes of an
+ * octree data structure in the breadth first way.
+ * \ingroup octree
+ * \author Fabien Rozar
+ * (fabien.rozar@gmail.com)
+ */
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename OctreeT>
+class OctreeLeafNodeBreadthFirstIterator : public OctreeBreadthFirstIterator<OctreeT> {
+ using BranchNode = typename OctreeBreadthFirstIterator<OctreeT>::BranchNode;
+ using LeafNode = typename OctreeBreadthFirstIterator<OctreeT>::LeafNode;
+
+public:
+ /** \brief Empty constructor.
+ * \param[in] max_depth_arg Depth limitation during traversal
+ */
+ explicit OctreeLeafNodeBreadthFirstIterator(unsigned int max_depth_arg = 0);
+
+ /** \brief Constructor.
+ * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
+ * root node.
+ * \param[in] max_depth_arg Depth limitation during traversal
+ */
+ explicit OctreeLeafNodeBreadthFirstIterator(OctreeT* octree_arg,
+ unsigned int max_depth_arg = 0);
+
+ /** \brief Copy constructor.
+ * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its
+ * root node.
+ * \param[in] max_depth_arg Depth limitation during traversal
+ * \param[in] current_state A pointer to the current iterator state
+ * \param[in] fifo Internal container of octree node to go through
+ *
+ * \warning For advanced users only.
+ */
+ explicit OctreeLeafNodeBreadthFirstIterator(
+ OctreeT* octree_arg,
+ unsigned int max_depth_arg,
+ IteratorState* current_state,
+ const std::deque<IteratorState>& fifo = std::deque<IteratorState>());
+
+ /** \brief Reset the iterator to the first leaf in the breadth first way.
+ */
+ inline void
+ reset();
+
+ /** \brief Preincrement operator.
+ * \note recursively step to next octree leaf node
+ */
+ inline OctreeLeafNodeBreadthFirstIterator&
+ operator++();
+
+ /** \brief Postincrement operator.
+ * \note step to next octree node
+ */
+ inline OctreeLeafNodeBreadthFirstIterator
+ operator++(int);
+};
+
+} // namespace octree
+} // namespace pcl
/*
* Note: Since octree iterators depend on octrees, don't precompile them.
#pragma once
-namespace pcl
-{
- namespace octree
+#include <cstdint>
+
+namespace pcl {
+namespace octree {
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Octree key class
+ * \note Octree keys contain integer indices for each coordinate axis in order to
+ * address an octree leaf node.
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+class OctreeKey {
+public:
+ /** \brief Empty constructor. */
+ OctreeKey() : x(0), y(0), z(0) {}
+
+ /** \brief Constructor for key initialization. */
+ OctreeKey(unsigned int keyX, unsigned int keyY, unsigned int keyZ)
+ : x(keyX), y(keyY), z(keyZ)
+ {}
+
+ /** \brief Copy constructor. */
+ OctreeKey(const OctreeKey& source) { memcpy(key_, source.key_, sizeof(key_)); }
+
+ OctreeKey&
+ operator=(const OctreeKey&) = default;
+
+ /** \brief Operator== for comparing octree keys with each other.
+ * \return "true" if leaf node indices are identical; "false" otherwise.
+ * */
+ bool
+ operator==(const OctreeKey& b) const
{
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Octree key class
- * \note Octree keys contain integer indices for each coordinate axis in order to address an octree leaf node.
- * \author Julius Kammerl (julius@kammerl.de)
- */
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- class OctreeKey
- {
- public:
-
- /** \brief Empty constructor. */
- OctreeKey () :
- x (0), y (0), z (0)
- {
- }
-
- /** \brief Constructor for key initialization. */
- OctreeKey (unsigned int keyX, unsigned int keyY, unsigned int keyZ) :
- x (keyX), y (keyY), z (keyZ)
- {
- }
-
- /** \brief Copy constructor. */
- OctreeKey (const OctreeKey& source)
- {
- memcpy(key_, source.key_, sizeof(key_));
- }
-
- OctreeKey& operator=(const OctreeKey&) = default;
-
- /** \brief Operator== for comparing octree keys with each other.
- * \return "true" if leaf node indices are identical; "false" otherwise.
- * */
- bool
- operator == (const OctreeKey& b) const
- {
- return ((b.x == this->x) && (b.y == this->y) && (b.z == this->z));
- }
-
- /** \brief Inequal comparison operator
- * \param[in] other OctreeIteratorBase to compare with
- * \return "true" if the current and other iterators are different ; "false" otherwise.
- */
- bool operator!= (const OctreeKey& other) const
- {
- return !operator== (other);
- }
-
- /** \brief Operator<= for comparing octree keys with each other.
- * \return "true" if key indices are not greater than the key indices of b ; "false" otherwise.
- * */
- bool
- operator <= (const OctreeKey& b) const
- {
- return ((b.x >= this->x) && (b.y >= this->y) && (b.z >= this->z));
- }
-
- /** \brief Operator>= for comparing octree keys with each other.
- * \return "true" if key indices are not smaller than the key indices of b ; "false" otherwise.
- * */
- bool
- operator >= (const OctreeKey& b) const
- {
- return ((b.x <= this->x) && (b.y <= this->y) && (b.z <= this->z));
- }
-
- /** \brief push a child node to the octree key
- * \param[in] childIndex index of child node to be added (0-7)
- * */
- inline void
- pushBranch (unsigned char childIndex)
- {
- this->x = (this->x << 1) | (!!(childIndex & (1 << 2)));
- this->y = (this->y << 1) | (!!(childIndex & (1 << 1)));
- this->z = (this->z << 1) | (!!(childIndex & (1 << 0)));
- }
-
- /** \brief pop child node from octree key
- * */
- inline void
- popBranch ()
- {
- this->x >>= 1;
- this->y >>= 1;
- this->z >>= 1;
- }
-
- /** \brief get child node index using depthMask
- * \param[in] depthMask bit mask with single bit set at query depth
- * \return child node index
- * */
- inline unsigned char
- getChildIdxWithDepthMask (unsigned int depthMask) const
- {
- return static_cast<unsigned char> (((!!(this->x & depthMask)) << 2)
- | ((!!(this->y & depthMask)) << 1)
- | (!!(this->z & depthMask)));
- }
-
- /* \brief maximum depth that can be addressed */
- static const unsigned char maxDepth = static_cast<unsigned char>(sizeof(std::uint32_t)*8);
-
- // Indices addressing a voxel at (X, Y, Z)
-
- union
- {
- struct
- {
- std::uint32_t x;
- std::uint32_t y;
- std::uint32_t z;
- };
- std::uint32_t key_[3];
- };
+ return ((b.x == this->x) && (b.y == this->y) && (b.z == this->z));
+ }
+ /** \brief Inequal comparison operator
+ * \param[in] other OctreeIteratorBase to compare with
+ * \return "true" if the current and other iterators are different ; "false"
+ * otherwise.
+ */
+ bool
+ operator!=(const OctreeKey& other) const
+ {
+ return !operator==(other);
+ }
- };
+ /** \brief Operator<= for comparing octree keys with each other.
+ * \return "true" if key indices are not greater than the key indices of b ; "false"
+ * otherwise.
+ * */
+ bool
+ operator<=(const OctreeKey& b) const
+ {
+ return ((b.x >= this->x) && (b.y >= this->y) && (b.z >= this->z));
+ }
+
+ /** \brief Operator>= for comparing octree keys with each other.
+ * \return "true" if key indices are not smaller than the key indices of b ; "false"
+ * otherwise.
+ * */
+ bool
+ operator>=(const OctreeKey& b) const
+ {
+ return ((b.x <= this->x) && (b.y <= this->y) && (b.z <= this->z));
+ }
+
+ /** \brief push a child node to the octree key
+ * \param[in] childIndex index of child node to be added (0-7)
+ * */
+ inline void
+ pushBranch(unsigned char childIndex)
+ {
+ this->x = (this->x << 1) | (!!(childIndex & (1 << 2)));
+ this->y = (this->y << 1) | (!!(childIndex & (1 << 1)));
+ this->z = (this->z << 1) | (!!(childIndex & (1 << 0)));
+ }
+
+ /** \brief pop child node from octree key
+ * */
+ inline void
+ popBranch()
+ {
+ this->x >>= 1;
+ this->y >>= 1;
+ this->z >>= 1;
}
-}
+
+ /** \brief get child node index using depthMask
+ * \param[in] depthMask bit mask with single bit set at query depth
+ * \return child node index
+ * */
+ inline unsigned char
+ getChildIdxWithDepthMask(unsigned int depthMask) const
+ {
+ return static_cast<unsigned char>(((!!(this->x & depthMask)) << 2) |
+ ((!!(this->y & depthMask)) << 1) |
+ (!!(this->z & depthMask)));
+ }
+
+ /* \brief maximum depth that can be addressed */
+ static const unsigned char maxDepth =
+ static_cast<unsigned char>(sizeof(std::uint32_t) * 8);
+
+ // Indices addressing a voxel at (X, Y, Z)
+
+ union {
+ struct {
+ std::uint32_t x;
+ std::uint32_t y;
+ std::uint32_t z;
+ };
+ std::uint32_t key_[3];
+ };
+};
+} // namespace octree
+} // namespace pcl
#include <pcl/pcl_macros.h>
-namespace pcl
-{
- namespace octree
- {
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Octree node pool
- * \note Used to reduce memory allocation and class instantiation events when generating octrees at high rate
- * \author Julius Kammerl (julius@kammerl.de)
- */
- template<typename NodeT>
- class OctreeNodePool
- {
- public:
- /** \brief Empty constructor. */
- OctreeNodePool () :
- nodePool_ ()
- {
- }
+namespace pcl {
+namespace octree {
- /** \brief Empty deconstructor. */
- virtual
- ~OctreeNodePool ()
- {
- deletePool ();
- }
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Octree node pool
+ * \note Used to reduce memory allocation and class instantiation events when generating
+ * octrees at high rate
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+template <typename NodeT>
+class OctreeNodePool {
+public:
+ /** \brief Empty constructor. */
+ OctreeNodePool() : nodePool_() {}
- /** \brief Push node to pool
- * \param node_arg: add this node to the pool
- * */
- inline
- void
- pushNode (NodeT* node_arg)
- {
- nodePool_.push_back (node_arg);
- }
+ /** \brief Empty deconstructor. */
+ virtual ~OctreeNodePool() { deletePool(); }
- /** \brief Pop node from pool - Allocates new nodes if pool is empty
- * \return Pointer to octree node
- * */
- inline NodeT*
- popNode ()
- {
+ /** \brief Push node to pool
+ * \param node_arg: add this node to the pool
+ * */
+ inline void
+ pushNode(NodeT* node_arg)
+ {
+ nodePool_.push_back(node_arg);
+ }
- NodeT* newLeafNode;
+ /** \brief Pop node from pool - Allocates new nodes if pool is empty
+ * \return Pointer to octree node
+ * */
+ inline NodeT*
+ popNode()
+ {
- if (!nodePool_.size ())
- {
- // leaf pool is empty
- // we need to create a new octree leaf class
- newLeafNode = new NodeT ();
- }
- else
- {
- // reuse leaf node from branch pool
- newLeafNode = nodePool_.back ();
- nodePool_.pop_back ();
- newLeafNode->reset ();
- }
+ NodeT* newLeafNode;
- return newLeafNode;
- }
+ if (!nodePool_.size()) {
+ // leaf pool is empty
+ // we need to create a new octree leaf class
+ newLeafNode = new NodeT();
+ }
+ else {
+ // reuse leaf node from branch pool
+ newLeafNode = nodePool_.back();
+ nodePool_.pop_back();
+ newLeafNode->reset();
+ }
+ return newLeafNode;
+ }
- /** \brief Delete all nodes in pool
- * */
- void
- deletePool ()
- {
- // delete all branch instances from branch pool
- while (!nodePool_.empty ())
- {
- delete (nodePool_.back ());
- nodePool_.pop_back ();
- }
- }
+ /** \brief Delete all nodes in pool
+ * */
+ void
+ deletePool()
+ {
+ // delete all branch instances from branch pool
+ while (!nodePool_.empty()) {
+ delete (nodePool_.back());
+ nodePool_.pop_back();
+ }
+ }
- protected:
- std::vector<NodeT*> nodePool_;
- };
+protected:
+ std::vector<NodeT*> nodePool_;
+};
- }
-}
+} // namespace octree
+} // namespace pcl
#include "octree_container.h"
-namespace pcl
-{
- namespace octree
- {
+namespace pcl {
+namespace octree {
- // enum of node types within the octree
- enum node_type_t
- {
- BRANCH_NODE, LEAF_NODE
- };
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Abstract octree node class
- * \note Every octree node should implement the getNodeType () method
- * \author Julius Kammerl (julius@kammerl.de)
- */
- class PCL_EXPORTS OctreeNode
- {
- public:
-
- OctreeNode ()
- {
- }
+// enum of node types within the octree
+enum node_type_t { BRANCH_NODE, LEAF_NODE };
- virtual
- ~OctreeNode ()
- {
- }
- /** \brief Pure virtual method for receiving the type of octree node (branch or leaf) */
- virtual node_type_t
- getNodeType () const = 0;
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Abstract octree node class
+ * \note Every octree node should implement the getNodeType () method
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+class PCL_EXPORTS OctreeNode {
+public:
+ OctreeNode() {}
+
+ virtual ~OctreeNode() {}
+ /** \brief Pure virtual method for receiving the type of octree node (branch or leaf)
+ */
+ virtual node_type_t
+ getNodeType() const = 0;
+
+ /** \brief Pure virtual method to perform a deep copy of the octree */
+ virtual OctreeNode*
+ deepCopy() const = 0;
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Abstract octree leaf class
+ * \note Octree leafs may collect data of type DataT
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
- /** \brief Pure virtual method to perform a deep copy of the octree */
- virtual OctreeNode*
- deepCopy () const = 0;
+template <typename ContainerT>
+class OctreeLeafNode : public OctreeNode {
+public:
+ /** \brief Empty constructor. */
+ OctreeLeafNode() : OctreeNode() {}
- };
+ /** \brief Copy constructor. */
+ OctreeLeafNode(const OctreeLeafNode& source) : OctreeNode()
+ {
+ container_ = source.container_;
+ }
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Abstract octree leaf class
- * \note Octree leafs may collect data of type DataT
- * \author Julius Kammerl (julius@kammerl.de)
- */
+ /** \brief Empty deconstructor. */
- template<typename ContainerT>
- class OctreeLeafNode : public OctreeNode
- {
- public:
+ ~OctreeLeafNode() {}
- /** \brief Empty constructor. */
- OctreeLeafNode () :
- OctreeNode ()
- {
- }
+ /** \brief Method to perform a deep copy of the octree */
+ OctreeLeafNode<ContainerT>*
+ deepCopy() const override
+ {
+ return new OctreeLeafNode<ContainerT>(*this);
+ }
- /** \brief Copy constructor. */
- OctreeLeafNode (const OctreeLeafNode& source) :
- OctreeNode ()
- {
- container_ = source.container_;
- }
+ /** \brief Get the type of octree node. Returns LEAVE_NODE type */
+ node_type_t
+ getNodeType() const override
+ {
+ return LEAF_NODE;
+ }
- /** \brief Empty deconstructor. */
-
- ~OctreeLeafNode ()
- {
- }
+ /** \brief Get const pointer to container */
+ const ContainerT* operator->() const { return &container_; }
- /** \brief Method to perform a deep copy of the octree */
- OctreeLeafNode<ContainerT>*
- deepCopy () const override
- {
- return new OctreeLeafNode<ContainerT> (*this);
- }
+ /** \brief Get pointer to container */
+ ContainerT* operator->() { return &container_; }
- /** \brief Get the type of octree node. Returns LEAVE_NODE type */
- node_type_t
- getNodeType () const override
- {
- return LEAF_NODE;
- }
+ /** \brief Get const reference to container */
+ const ContainerT& operator*() const { return container_; }
- /** \brief Get const pointer to container */
- const ContainerT*
- operator->() const
- {
- return &container_;
- }
+ /** \brief Get reference to container */
+ ContainerT& operator*() { return container_; }
- /** \brief Get pointer to container */
- ContainerT*
- operator-> ()
- {
- return &container_;
- }
+ /** \brief Get const reference to container */
+ const ContainerT&
+ getContainer() const
+ {
+ return container_;
+ }
- /** \brief Get const reference to container */
- const ContainerT&
- operator* () const
- {
- return container_;
- }
+ /** \brief Get reference to container */
+ ContainerT&
+ getContainer()
+ {
+ return container_;
+ }
- /** \brief Get reference to container */
- ContainerT&
- operator* ()
- {
- return container_;
- }
+ /** \brief Get const pointer to container */
+ const ContainerT*
+ getContainerPtr() const
+ {
+ return &container_;
+ }
- /** \brief Get const reference to container */
- const ContainerT&
- getContainer () const
- {
- return container_;
- }
+ /** \brief Get pointer to container */
+ ContainerT*
+ getContainerPtr()
+ {
+ return &container_;
+ }
- /** \brief Get reference to container */
- ContainerT&
- getContainer ()
- {
- return container_;
- }
+protected:
+ ContainerT container_;
- /** \brief Get const pointer to container */
- const ContainerT*
- getContainerPtr() const
- {
- return &container_;
- }
+public:
+ // Type ContainerT may have fixed-size Eigen objects inside
+ PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
- /** \brief Get pointer to container */
- ContainerT*
- getContainerPtr ()
- {
- return &container_;
- }
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Abstract octree branch class
+ * \note Octree branch classes may collect data of type DataT
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+template <typename ContainerT>
+class OctreeBranchNode : public OctreeNode {
+public:
+ /** \brief Empty constructor. */
+ OctreeBranchNode() : OctreeNode()
+ {
+ // reset pointer to child node vectors
+ memset(child_node_array_, 0, sizeof(child_node_array_));
+ }
- protected:
- ContainerT container_;
-
- public:
- //Type ContainerT may have fixed-size Eigen objects inside
- PCL_MAKE_ALIGNED_OPERATOR_NEW
- };
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Abstract octree branch class
- * \note Octree branch classes may collect data of type DataT
- * \author Julius Kammerl (julius@kammerl.de)
- */
- template<typename ContainerT>
- class OctreeBranchNode : public OctreeNode
- {
- public:
+ /** \brief Empty constructor. */
+ OctreeBranchNode(const OctreeBranchNode& source) : OctreeNode()
+ {
+ memset(child_node_array_, 0, sizeof(child_node_array_));
- /** \brief Empty constructor. */
- OctreeBranchNode () :
- OctreeNode()
- {
- // reset pointer to child node vectors
- memset (child_node_array_, 0, sizeof(child_node_array_));
- }
+ for (unsigned char i = 0; i < 8; ++i)
+ if (source.child_node_array_[i])
+ child_node_array_[i] = source.child_node_array_[i]->deepCopy();
+ }
- /** \brief Empty constructor. */
- OctreeBranchNode (const OctreeBranchNode& source) :
- OctreeNode()
- {
- memset (child_node_array_, 0, sizeof(child_node_array_));
+ /** \brief Copy operator. */
+ inline OctreeBranchNode&
+ operator=(const OctreeBranchNode& source)
+ {
+ memset(child_node_array_, 0, sizeof(child_node_array_));
- for (unsigned char i = 0; i < 8; ++i)
- if (source.child_node_array_[i])
- child_node_array_[i] = source.child_node_array_[i]->deepCopy ();
- }
+ for (unsigned char i = 0; i < 8; ++i)
+ if (source.child_node_array_[i])
+ child_node_array_[i] = source.child_node_array_[i]->deepCopy();
+ return (*this);
+ }
- /** \brief Copy operator. */
- inline OctreeBranchNode&
- operator = (const OctreeBranchNode &source)
- {
- memset (child_node_array_, 0, sizeof(child_node_array_));
+ /** \brief Octree deep copy method */
+ OctreeBranchNode*
+ deepCopy() const override
+ {
+ return (new OctreeBranchNode<ContainerT>(*this));
+ }
- for (unsigned char i = 0; i < 8; ++i)
- if (source.child_node_array_[i])
- child_node_array_[i] = source.child_node_array_[i]->deepCopy ();
- return (*this);
- }
+ /** \brief Empty deconstructor. */
- /** \brief Octree deep copy method */
- OctreeBranchNode*
- deepCopy () const override
- {
- return (new OctreeBranchNode<ContainerT> (*this));
- }
+ ~OctreeBranchNode() {}
- /** \brief Empty deconstructor. */
-
- ~OctreeBranchNode ()
- {
- }
+ /** \brief Access operator.
+ * \param child_idx_arg: index to child node
+ * \return OctreeNode pointer
+ * */
+ inline OctreeNode*& operator[](unsigned char child_idx_arg)
+ {
+ assert(child_idx_arg < 8);
+ return child_node_array_[child_idx_arg];
+ }
- /** \brief Access operator.
- * \param child_idx_arg: index to child node
- * \return OctreeNode pointer
- * */
- inline OctreeNode*&
- operator[] (unsigned char child_idx_arg)
- {
- assert(child_idx_arg < 8);
- return child_node_array_[child_idx_arg];
- }
+ /** \brief Get pointer to child
+ * \param child_idx_arg: index to child node
+ * \return OctreeNode pointer
+ * */
+ inline OctreeNode*
+ getChildPtr(unsigned char child_idx_arg) const
+ {
+ assert(child_idx_arg < 8);
+ return child_node_array_[child_idx_arg];
+ }
- /** \brief Get pointer to child
- * \param child_idx_arg: index to child node
- * \return OctreeNode pointer
- * */
- inline OctreeNode*
- getChildPtr (unsigned char child_idx_arg) const
- {
- assert(child_idx_arg < 8);
- return child_node_array_[child_idx_arg];
- }
+ /** \brief Get pointer to child
+ * \return OctreeNode pointer
+ * */
+ inline void
+ setChildPtr(OctreeNode* child, unsigned char index)
+ {
+ assert(index < 8);
+ child_node_array_[index] = child;
+ }
- /** \brief Get pointer to child
- * \return OctreeNode pointer
- * */
- inline void setChildPtr (OctreeNode* child, unsigned char index)
- {
- assert(index < 8);
- child_node_array_[index] = child;
- }
+ /** \brief Check if branch is pointing to a particular child node
+ * \param child_idx_arg: index to child node
+ * \return "true" if pointer to child node exists; "false" otherwise
+ * */
+ inline bool
+ hasChild(unsigned char child_idx_arg) const
+ {
+ return (child_node_array_[child_idx_arg] != nullptr);
+ }
+ /** \brief Check if branch can be pruned
+ * \note if all children are leaf nodes AND contain identical containers, branch can
+ * be pruned
+ * \return "true" if branch can be pruned; "false" otherwise
+ **/
+ /* inline bool isPrunable () const
+ {
+ const OctreeNode* firstChild = child_node_array_[0];
+ if (!firstChild || firstChild->getNodeType()==BRANCH_NODE)
+ return false;
- /** \brief Check if branch is pointing to a particular child node
- * \param child_idx_arg: index to child node
- * \return "true" if pointer to child node exists; "false" otherwise
- * */
- inline bool hasChild (unsigned char child_idx_arg) const
+ bool prunable = true;
+ for (unsigned char i = 1; i < 8 && prunable; ++i)
{
- return (child_node_array_[child_idx_arg] != nullptr);
+ const OctreeNode* child = child_node_array_[i];
+ if ( (!child) ||
+ (child->getNodeType()==BRANCH_NODE) ||
+ ((*static_cast<const OctreeContainerBase*>(child)) == (*static_cast<const
+ OctreeContainerBase*>(child)) ) ) prunable = false;
}
+ return prunable;
+ }*/
- /** \brief Check if branch can be pruned
- * \note if all children are leaf nodes AND contain identical containers, branch can be pruned
- * \return "true" if branch can be pruned; "false" otherwise
- * */
- /* inline bool isPrunable () const
- {
- const OctreeNode* firstChild = child_node_array_[0];
- if (!firstChild || firstChild->getNodeType()==BRANCH_NODE)
- return false;
-
- bool prunable = true;
- for (unsigned char i = 1; i < 8 && prunable; ++i)
- {
- const OctreeNode* child = child_node_array_[i];
- if ( (!child) ||
- (child->getNodeType()==BRANCH_NODE) ||
- ((*static_cast<const OctreeContainerBase*>(child)) == (*static_cast<const OctreeContainerBase*>(child)) ) )
- prunable = false;
- }
-
- return prunable;
- }*/
-
- /** \brief Get the type of octree node. Returns LEAVE_NODE type */
- node_type_t
- getNodeType () const override
- {
- return BRANCH_NODE;
- }
-
- // reset node
- void reset()
- {
- memset(child_node_array_, 0, sizeof(child_node_array_));
- container_.reset();
- }
+ /** \brief Get the type of octree node. Returns LEAVE_NODE type */
+ node_type_t
+ getNodeType() const override
+ {
+ return BRANCH_NODE;
+ }
+ // reset node
+ void
+ reset()
+ {
+ memset(child_node_array_, 0, sizeof(child_node_array_));
+ container_.reset();
+ }
- /** \brief Get const pointer to container */
- const ContainerT*
- operator->() const
- {
- return &container_;
- }
+ /** \brief Get const pointer to container */
+ const ContainerT* operator->() const { return &container_; }
- /** \brief Get pointer to container */
- ContainerT*
- operator-> ()
- {
- return &container_;
- }
+ /** \brief Get pointer to container */
+ ContainerT* operator->() { return &container_; }
- /** \brief Get const reference to container */
- const ContainerT&
- operator* () const
- {
- return container_;
- }
+ /** \brief Get const reference to container */
+ const ContainerT& operator*() const { return container_; }
- /** \brief Get reference to container */
- ContainerT&
- operator* ()
- {
- return container_;
- }
+ /** \brief Get reference to container */
+ ContainerT& operator*() { return container_; }
- /** \brief Get const reference to container */
- const ContainerT&
- getContainer () const
- {
- return container_;
- }
+ /** \brief Get const reference to container */
+ const ContainerT&
+ getContainer() const
+ {
+ return container_;
+ }
- /** \brief Get reference to container */
- ContainerT&
- getContainer ()
- {
- return container_;
- }
+ /** \brief Get reference to container */
+ ContainerT&
+ getContainer()
+ {
+ return container_;
+ }
- /** \brief Get const pointer to container */
- const ContainerT*
- getContainerPtr() const
- {
- return &container_;
- }
+ /** \brief Get const pointer to container */
+ const ContainerT*
+ getContainerPtr() const
+ {
+ return &container_;
+ }
- /** \brief Get pointer to container */
- ContainerT*
- getContainerPtr ()
- {
- return &container_;
- }
+ /** \brief Get pointer to container */
+ ContainerT*
+ getContainerPtr()
+ {
+ return &container_;
+ }
- protected:
- OctreeNode* child_node_array_[8];
+protected:
+ OctreeNode* child_node_array_[8];
- ContainerT container_;
- };
- }
-}
+ ContainerT container_;
+};
+} // namespace octree
+} // namespace pcl
#include <vector>
-namespace pcl
-{
- namespace octree
+namespace pcl {
+namespace octree {
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Octree pointcloud class
+ * \note Octree implementation for pointclouds. Only indices are stored by the octree
+ * leaf nodes (zero-copy).
+ * \note The octree pointcloud class needs to be initialized with its voxel resolution.
+ * Its bounding box is automatically adjusted
+ * \note according to the pointcloud dimension or it can be predefined.
+ * \note Note: The tree depth equates to the resolution and the bounding box dimensions
+ * of the octree.
+ * \tparam PointT: type of point used in pointcloud
+ * \tparam LeafContainerT: leaf node container
+ * \tparam BranchContainerT: branch node container
+ * \tparam OctreeT: octree implementation
+ * \ingroup octree
+ * \author Julius Kammerl * (julius@kammerl.de)
+ */
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT,
+ typename LeafContainerT = OctreeContainerPointIndices,
+ typename BranchContainerT = OctreeContainerEmpty,
+ typename OctreeT = OctreeBase<LeafContainerT, BranchContainerT>>
+
+class OctreePointCloud : public OctreeT {
+public:
+ using Base = OctreeT;
+
+ using LeafNode = typename OctreeT::LeafNode;
+ using BranchNode = typename OctreeT::BranchNode;
+
+ /** \brief Octree pointcloud constructor.
+ * \param[in] resolution_arg octree resolution at lowest octree level
+ */
+ OctreePointCloud(const double resolution_arg);
+
+ // public typedefs
+ using IndicesPtr = shared_ptr<std::vector<int>>;
+ using IndicesConstPtr = shared_ptr<const std::vector<int>>;
+
+ using PointCloud = pcl::PointCloud<PointT>;
+ using PointCloudPtr = typename PointCloud::Ptr;
+ using PointCloudConstPtr = typename PointCloud::ConstPtr;
+
+ // public typedefs for single/double buffering
+ using SingleBuffer = OctreePointCloud<PointT,
+ LeafContainerT,
+ BranchContainerT,
+ OctreeBase<LeafContainerT>>;
+ // using DoubleBuffer = OctreePointCloud<PointT, LeafContainerT, BranchContainerT,
+ // Octree2BufBase<LeafContainerT> >;
+
+ // Boost shared pointers
+ using Ptr =
+ shared_ptr<OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>>;
+ using ConstPtr = shared_ptr<
+ const OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>>;
+
+ // Eigen aligned allocator
+ using AlignedPointTVector = std::vector<PointT, Eigen::aligned_allocator<PointT>>;
+ using AlignedPointXYZVector =
+ std::vector<PointXYZ, Eigen::aligned_allocator<PointXYZ>>;
+
+ /** \brief Provide a pointer to the input data set.
+ * \param[in] cloud_arg the const boost shared pointer to a PointCloud message
+ * \param[in] indices_arg the point indices subset that is to be used from \a cloud -
+ * if 0 the whole point cloud is used
+ */
+ inline void
+ setInputCloud(const PointCloudConstPtr& cloud_arg,
+ const IndicesConstPtr& indices_arg = IndicesConstPtr())
+ {
+ input_ = cloud_arg;
+ indices_ = indices_arg;
+ }
+
+ /** \brief Get a pointer to the vector of indices used.
+ * \return pointer to vector of indices used.
+ */
+ inline IndicesConstPtr const
+ getIndices() const
+ {
+ return (indices_);
+ }
+
+ /** \brief Get a pointer to the input point cloud dataset.
+ * \return pointer to pointcloud input class.
+ */
+ inline PointCloudConstPtr
+ getInputCloud() const
+ {
+ return (input_);
+ }
+
+ /** \brief Set the search epsilon precision (error bound) for nearest neighbors
+ * searches.
+ * \param[in] eps precision (error bound) for nearest neighbors searches
+ */
+ inline void
+ setEpsilon(double eps)
+ {
+ epsilon_ = eps;
+ }
+
+ /** \brief Get the search epsilon precision (error bound) for nearest neighbors
+ * searches. */
+ inline double
+ getEpsilon() const
+ {
+ return (epsilon_);
+ }
+
+ /** \brief Set/change the octree voxel resolution
+ * \param[in] resolution_arg side length of voxels at lowest tree level
+ */
+ inline void
+ setResolution(double resolution_arg)
+ {
+ // octree needs to be empty to change its resolution
+ assert(this->leaf_count_ == 0);
+
+ resolution_ = resolution_arg;
+
+ getKeyBitSize();
+ }
+
+ /** \brief Get octree voxel resolution
+ * \return voxel resolution at lowest tree level
+ */
+ inline double
+ getResolution() const
+ {
+ return (resolution_);
+ }
+
+ /** \brief Get the maximum depth of the octree.
+ * \return depth_arg: maximum depth of octree
+ * */
+ inline unsigned int
+ getTreeDepth() const
{
+ return this->octree_depth_;
+ }
+
+ /** \brief Add points from input point cloud to octree. */
+ void
+ addPointsFromInputCloud();
+
+ /** \brief Add point at given index from input point cloud to octree. Index will be
+ * also added to indices vector.
+ * \param[in] point_idx_arg index of point to be added
+ * \param[in] indices_arg pointer to indices vector of the dataset (given by \a
+ * setInputCloud)
+ */
+ void
+ addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg);
+
+ /** \brief Add point simultaneously to octree and input point cloud.
+ * \param[in] point_arg point to be added
+ * \param[in] cloud_arg pointer to input point cloud dataset (given by \a
+ * setInputCloud)
+ */
+ void
+ addPointToCloud(const PointT& point_arg, PointCloudPtr cloud_arg);
+
+ /** \brief Add point simultaneously to octree and input point cloud. A corresponding
+ * index will be added to the indices vector.
+ * \param[in] point_arg point to be added
+ * \param[in] cloud_arg pointer to input point cloud dataset (given by \a
+ * setInputCloud)
+ * \param[in] indices_arg pointer to indices vector of the dataset (given by \a
+ * setInputCloud)
+ */
+ void
+ addPointToCloud(const PointT& point_arg,
+ PointCloudPtr cloud_arg,
+ IndicesPtr indices_arg);
+
+ /** \brief Check if voxel at given point exist.
+ * \param[in] point_arg point to be checked
+ * \return "true" if voxel exist; "false" otherwise
+ */
+ bool
+ isVoxelOccupiedAtPoint(const PointT& point_arg) const;
+
+ /** \brief Delete the octree structure and its leaf nodes.
+ * */
+ void
+ deleteTree()
+ {
+ // reset bounding box
+ min_x_ = min_y_ = max_y_ = min_z_ = max_z_ = 0;
+ this->bounding_box_defined_ = false;
+
+ OctreeT::deleteTree();
+ }
+
+ /** \brief Check if voxel at given point coordinates exist.
+ * \param[in] point_x_arg X coordinate of point to be checked
+ * \param[in] point_y_arg Y coordinate of point to be checked
+ * \param[in] point_z_arg Z coordinate of point to be checked
+ * \return "true" if voxel exist; "false" otherwise
+ */
+ bool
+ isVoxelOccupiedAtPoint(const double point_x_arg,
+ const double point_y_arg,
+ const double point_z_arg) const;
+
+ /** \brief Check if voxel at given point from input cloud exist.
+ * \param[in] point_idx_arg point to be checked
+ * \return "true" if voxel exist; "false" otherwise
+ */
+ bool
+ isVoxelOccupiedAtPoint(const int& point_idx_arg) const;
+
+ /** \brief Get a PointT vector of centers of all occupied voxels.
+ * \param[out] voxel_center_list_arg results are written to this vector of PointT
+ * elements
+ * \return number of occupied voxels
+ */
+ int
+ getOccupiedVoxelCenters(AlignedPointTVector& voxel_center_list_arg) const;
+
+ /** \brief Get a PointT vector of centers of voxels intersected by a line segment.
+ * This returns a approximation of the actual intersected voxels by walking
+ * along the line with small steps. Voxels are ordered, from closest to
+ * furthest w.r.t. the origin.
+ * \param[in] origin origin of the line segment
+ * \param[in] end end of the line segment
+ * \param[out] voxel_center_list results are written to this vector of PointT elements
+ * \param[in] precision determines the size of the steps: step_size =
+ * octree_resolution x precision
+ * \return number of intersected voxels
+ */
+ int
+ getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f& origin,
+ const Eigen::Vector3f& end,
+ AlignedPointTVector& voxel_center_list,
+ float precision = 0.2);
+
+ /** \brief Delete leaf node / voxel at given point
+ * \param[in] point_arg point addressing the voxel to be deleted.
+ */
+ void
+ deleteVoxelAtPoint(const PointT& point_arg);
+
+ /** \brief Delete leaf node / voxel at given point from input cloud
+ * \param[in] point_idx_arg index of point addressing the voxel to be deleted.
+ */
+ void
+ deleteVoxelAtPoint(const int& point_idx_arg);
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Bounding box methods
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /** \brief Investigate dimensions of pointcloud data set and define corresponding
+ * bounding box for octree. */
+ void
+ defineBoundingBox();
+
+ /** \brief Define bounding box for octree
+ * \note Bounding box cannot be changed once the octree contains elements.
+ * \param[in] min_x_arg X coordinate of lower bounding box corner
+ * \param[in] min_y_arg Y coordinate of lower bounding box corner
+ * \param[in] min_z_arg Z coordinate of lower bounding box corner
+ * \param[in] max_x_arg X coordinate of upper bounding box corner
+ * \param[in] max_y_arg Y coordinate of upper bounding box corner
+ * \param[in] max_z_arg Z coordinate of upper bounding box corner
+ */
+ void
+ defineBoundingBox(const double min_x_arg,
+ const double min_y_arg,
+ const double min_z_arg,
+ const double max_x_arg,
+ const double max_y_arg,
+ const double max_z_arg);
+
+ /** \brief Define bounding box for octree
+ * \note Lower bounding box point is set to (0, 0, 0)
+ * \note Bounding box cannot be changed once the octree contains elements.
+ * \param[in] max_x_arg X coordinate of upper bounding box corner
+ * \param[in] max_y_arg Y coordinate of upper bounding box corner
+ * \param[in] max_z_arg Z coordinate of upper bounding box corner
+ */
+ void
+ defineBoundingBox(const double max_x_arg,
+ const double max_y_arg,
+ const double max_z_arg);
+
+ /** \brief Define bounding box cube for octree
+ * \note Lower bounding box corner is set to (0, 0, 0)
+ * \note Bounding box cannot be changed once the octree contains elements.
+ * \param[in] cubeLen_arg side length of bounding box cube.
+ */
+ void
+ defineBoundingBox(const double cubeLen_arg);
+
+ /** \brief Get bounding box for octree
+ * \note Bounding box cannot be changed once the octree contains elements.
+ * \param[in] min_x_arg X coordinate of lower bounding box corner
+ * \param[in] min_y_arg Y coordinate of lower bounding box corner
+ * \param[in] min_z_arg Z coordinate of lower bounding box corner
+ * \param[in] max_x_arg X coordinate of upper bounding box corner
+ * \param[in] max_y_arg Y coordinate of upper bounding box corner
+ * \param[in] max_z_arg Z coordinate of upper bounding box corner
+ */
+ void
+ getBoundingBox(double& min_x_arg,
+ double& min_y_arg,
+ double& min_z_arg,
+ double& max_x_arg,
+ double& max_y_arg,
+ double& max_z_arg) const;
+
+ /** \brief Calculates the squared diameter of a voxel at given tree depth
+ * \param[in] tree_depth_arg depth/level in octree
+ * \return squared diameter
+ */
+ double
+ getVoxelSquaredDiameter(unsigned int tree_depth_arg) const;
+
+ /** \brief Calculates the squared diameter of a voxel at leaf depth
+ * \return squared diameter
+ */
+ inline double
+ getVoxelSquaredDiameter() const
+ {
+ return getVoxelSquaredDiameter(this->octree_depth_);
+ }
+
+ /** \brief Calculates the squared voxel cube side length at given tree depth
+ * \param[in] tree_depth_arg depth/level in octree
+ * \return squared voxel cube side length
+ */
+ double
+ getVoxelSquaredSideLen(unsigned int tree_depth_arg) const;
+
+ /** \brief Calculates the squared voxel cube side length at leaf level
+ * \return squared voxel cube side length
+ */
+ inline double
+ getVoxelSquaredSideLen() const
+ {
+ return getVoxelSquaredSideLen(this->octree_depth_);
+ }
+
+ /** \brief Generate bounds of the current voxel of an octree iterator
+ * \param[in] iterator: octree iterator
+ * \param[out] min_pt lower bound of voxel
+ * \param[out] max_pt upper bound of voxel
+ */
+ inline void
+ getVoxelBounds(const OctreeIteratorBase<OctreeT>& iterator,
+ Eigen::Vector3f& min_pt,
+ Eigen::Vector3f& max_pt) const
+ {
+ this->genVoxelBoundsFromOctreeKey(iterator.getCurrentOctreeKey(),
+ iterator.getCurrentOctreeDepth(),
+ min_pt,
+ max_pt);
+ }
+
+ /** \brief Enable dynamic octree structure
+ * \note Leaf nodes are kept as close to the root as possible and are only expanded
+ * if the number of DataT objects within a leaf node exceeds a fixed limit.
+ * \param maxObjsPerLeaf: maximum number of DataT objects per leaf
+ * */
+ inline void
+ enableDynamicDepth(std::size_t maxObjsPerLeaf)
+ {
+ assert(this->leaf_count_ == 0);
+ max_objs_per_leaf_ = maxObjsPerLeaf;
+
+ this->dynamic_depth_enabled_ = max_objs_per_leaf_ > 0;
+ }
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Octree pointcloud class
- * \note Octree implementation for pointclouds. Only indices are stored by the octree leaf nodes (zero-copy).
- * \note The octree pointcloud class needs to be initialized with its voxel resolution. Its bounding box is automatically adjusted
- * \note according to the pointcloud dimension or it can be predefined.
- * \note Note: The tree depth equates to the resolution and the bounding box dimensions of the octree.
- * \note
- * \note typename: PointT: type of point used in pointcloud
- * \note typename: LeafContainerT: leaf node container (
- * \note typename: BranchContainerT: branch node container
- * \note typename: OctreeT: octree implementation ()
- * \ingroup octree
- * \author Julius Kammerl (julius@kammerl.de)
- */
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- template<typename PointT, typename LeafContainerT = OctreeContainerPointIndices,
- typename BranchContainerT = OctreeContainerEmpty,
- typename OctreeT = OctreeBase<LeafContainerT, BranchContainerT> >
-
- class OctreePointCloud : public OctreeT
- {
- public:
- using Base = OctreeT;
-
- using LeafNode = typename OctreeT::LeafNode;
- using BranchNode = typename OctreeT::BranchNode;
-
- /** \brief Octree pointcloud constructor.
- * \param[in] resolution_arg octree resolution at lowest octree level
- */
- OctreePointCloud (const double resolution_arg);
-
- // public typedefs
- using IndicesPtr = shared_ptr<std::vector<int> >;
- using IndicesConstPtr = shared_ptr<const std::vector<int> >;
-
- using PointCloud = pcl::PointCloud<PointT>;
- using PointCloudPtr = typename PointCloud::Ptr;
- using PointCloudConstPtr = typename PointCloud::ConstPtr;
-
- // public typedefs for single/double buffering
- using SingleBuffer = OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeBase<LeafContainerT> >;
- // using DoubleBuffer = OctreePointCloud<PointT, LeafContainerT, BranchContainerT, Octree2BufBase<LeafContainerT> >;
-
- // Boost shared pointers
- using Ptr = shared_ptr<OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT> >;
- using ConstPtr = shared_ptr<const OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT> >;
-
- // Eigen aligned allocator
- using AlignedPointTVector = std::vector<PointT, Eigen::aligned_allocator<PointT> >;
- using AlignedPointXYZVector = std::vector<PointXYZ, Eigen::aligned_allocator<PointXYZ> >;
-
- /** \brief Provide a pointer to the input data set.
- * \param[in] cloud_arg the const boost shared pointer to a PointCloud message
- * \param[in] indices_arg the point indices subset that is to be used from \a cloud - if 0 the whole point cloud is used
- */
- inline void setInputCloud (const PointCloudConstPtr &cloud_arg,
- const IndicesConstPtr &indices_arg = IndicesConstPtr ())
- {
- input_ = cloud_arg;
- indices_ = indices_arg;
- }
-
- /** \brief Get a pointer to the vector of indices used.
- * \return pointer to vector of indices used.
- */
- inline IndicesConstPtr const getIndices () const
- {
- return (indices_);
- }
-
- /** \brief Get a pointer to the input point cloud dataset.
- * \return pointer to pointcloud input class.
- */
- inline PointCloudConstPtr getInputCloud () const
- {
- return (input_);
- }
-
- /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches.
- * \param[in] eps precision (error bound) for nearest neighbors searches
- */
- inline void setEpsilon (double eps)
- {
- epsilon_ = eps;
- }
-
- /** \brief Get the search epsilon precision (error bound) for nearest neighbors searches. */
- inline double getEpsilon () const
- {
- return (epsilon_);
- }
-
- /** \brief Set/change the octree voxel resolution
- * \param[in] resolution_arg side length of voxels at lowest tree level
- */
- inline void setResolution (double resolution_arg)
- {
- // octree needs to be empty to change its resolution
- assert( this->leaf_count_ == 0);
-
- resolution_ = resolution_arg;
-
- getKeyBitSize ();
- }
-
- /** \brief Get octree voxel resolution
- * \return voxel resolution at lowest tree level
- */
- inline double getResolution () const
- {
- return (resolution_);
- }
-
- /** \brief Get the maximum depth of the octree.
- * \return depth_arg: maximum depth of octree
- * */
- inline unsigned int getTreeDepth () const
- {
- return this->octree_depth_;
- }
-
- /** \brief Add points from input point cloud to octree. */
- void
- addPointsFromInputCloud ();
-
- /** \brief Add point at given index from input point cloud to octree. Index will be also added to indices vector.
- * \param[in] point_idx_arg index of point to be added
- * \param[in] indices_arg pointer to indices vector of the dataset (given by \a setInputCloud)
- */
- void
- addPointFromCloud (const int point_idx_arg, IndicesPtr indices_arg);
-
- /** \brief Add point simultaneously to octree and input point cloud.
- * \param[in] point_arg point to be added
- * \param[in] cloud_arg pointer to input point cloud dataset (given by \a setInputCloud)
- */
- void
- addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg);
-
- /** \brief Add point simultaneously to octree and input point cloud. A corresponding index will be added to the indices vector.
- * \param[in] point_arg point to be added
- * \param[in] cloud_arg pointer to input point cloud dataset (given by \a setInputCloud)
- * \param[in] indices_arg pointer to indices vector of the dataset (given by \a setInputCloud)
- */
- void
- addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg, IndicesPtr indices_arg);
-
- /** \brief Check if voxel at given point exist.
- * \param[in] point_arg point to be checked
- * \return "true" if voxel exist; "false" otherwise
- */
- bool
- isVoxelOccupiedAtPoint (const PointT& point_arg) const;
-
- /** \brief Delete the octree structure and its leaf nodes.
- * */
- void deleteTree ()
- {
- // reset bounding box
- min_x_ = min_y_ = max_y_ = min_z_ = max_z_ = 0;
- this->bounding_box_defined_ = false;
-
- OctreeT::deleteTree ();
- }
-
- /** \brief Check if voxel at given point coordinates exist.
- * \param[in] point_x_arg X coordinate of point to be checked
- * \param[in] point_y_arg Y coordinate of point to be checked
- * \param[in] point_z_arg Z coordinate of point to be checked
- * \return "true" if voxel exist; "false" otherwise
- */
- bool
- isVoxelOccupiedAtPoint (const double point_x_arg, const double point_y_arg, const double point_z_arg) const;
-
- /** \brief Check if voxel at given point from input cloud exist.
- * \param[in] point_idx_arg point to be checked
- * \return "true" if voxel exist; "false" otherwise
- */
- bool
- isVoxelOccupiedAtPoint (const int& point_idx_arg) const;
-
- /** \brief Get a PointT vector of centers of all occupied voxels.
- * \param[out] voxel_center_list_arg results are written to this vector of PointT elements
- * \return number of occupied voxels
- */
- int
- getOccupiedVoxelCenters (AlignedPointTVector &voxel_center_list_arg) const;
-
- /** \brief Get a PointT vector of centers of voxels intersected by a line segment.
- * This returns a approximation of the actual intersected voxels by walking
- * along the line with small steps. Voxels are ordered, from closest to
- * furthest w.r.t. the origin.
- * \param[in] origin origin of the line segment
- * \param[in] end end of the line segment
- * \param[out] voxel_center_list results are written to this vector of PointT elements
- * \param[in] precision determines the size of the steps: step_size = octree_resolution x precision
- * \return number of intersected voxels
- */
- int
- getApproxIntersectedVoxelCentersBySegment (
- const Eigen::Vector3f& origin, const Eigen::Vector3f& end,
- AlignedPointTVector &voxel_center_list, float precision = 0.2);
-
- /** \brief Delete leaf node / voxel at given point
- * \param[in] point_arg point addressing the voxel to be deleted.
- */
- void
- deleteVoxelAtPoint (const PointT& point_arg);
-
- /** \brief Delete leaf node / voxel at given point from input cloud
- * \param[in] point_idx_arg index of point addressing the voxel to be deleted.
- */
- void
- deleteVoxelAtPoint (const int& point_idx_arg);
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // Bounding box methods
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- /** \brief Investigate dimensions of pointcloud data set and define corresponding bounding box for octree. */
- void
- defineBoundingBox ();
-
- /** \brief Define bounding box for octree
- * \note Bounding box cannot be changed once the octree contains elements.
- * \param[in] min_x_arg X coordinate of lower bounding box corner
- * \param[in] min_y_arg Y coordinate of lower bounding box corner
- * \param[in] min_z_arg Z coordinate of lower bounding box corner
- * \param[in] max_x_arg X coordinate of upper bounding box corner
- * \param[in] max_y_arg Y coordinate of upper bounding box corner
- * \param[in] max_z_arg Z coordinate of upper bounding box corner
- */
- void
- defineBoundingBox (const double min_x_arg, const double min_y_arg, const double min_z_arg,
- const double max_x_arg, const double max_y_arg, const double max_z_arg);
-
- /** \brief Define bounding box for octree
- * \note Lower bounding box point is set to (0, 0, 0)
- * \note Bounding box cannot be changed once the octree contains elements.
- * \param[in] max_x_arg X coordinate of upper bounding box corner
- * \param[in] max_y_arg Y coordinate of upper bounding box corner
- * \param[in] max_z_arg Z coordinate of upper bounding box corner
- */
- void
- defineBoundingBox (const double max_x_arg, const double max_y_arg, const double max_z_arg);
-
- /** \brief Define bounding box cube for octree
- * \note Lower bounding box corner is set to (0, 0, 0)
- * \note Bounding box cannot be changed once the octree contains elements.
- * \param[in] cubeLen_arg side length of bounding box cube.
- */
- void
- defineBoundingBox (const double cubeLen_arg);
-
- /** \brief Get bounding box for octree
- * \note Bounding box cannot be changed once the octree contains elements.
- * \param[in] min_x_arg X coordinate of lower bounding box corner
- * \param[in] min_y_arg Y coordinate of lower bounding box corner
- * \param[in] min_z_arg Z coordinate of lower bounding box corner
- * \param[in] max_x_arg X coordinate of upper bounding box corner
- * \param[in] max_y_arg Y coordinate of upper bounding box corner
- * \param[in] max_z_arg Z coordinate of upper bounding box corner
- */
- void
- getBoundingBox (double& min_x_arg, double& min_y_arg, double& min_z_arg,
- double& max_x_arg, double& max_y_arg, double& max_z_arg) const;
-
- /** \brief Calculates the squared diameter of a voxel at given tree depth
- * \param[in] tree_depth_arg depth/level in octree
- * \return squared diameter
- */
- double
- getVoxelSquaredDiameter (unsigned int tree_depth_arg) const;
-
- /** \brief Calculates the squared diameter of a voxel at leaf depth
- * \return squared diameter
- */
- inline double
- getVoxelSquaredDiameter () const
- {
- return getVoxelSquaredDiameter (this->octree_depth_);
- }
-
- /** \brief Calculates the squared voxel cube side length at given tree depth
- * \param[in] tree_depth_arg depth/level in octree
- * \return squared voxel cube side length
- */
- double
- getVoxelSquaredSideLen (unsigned int tree_depth_arg) const;
-
- /** \brief Calculates the squared voxel cube side length at leaf level
- * \return squared voxel cube side length
- */
- inline double getVoxelSquaredSideLen () const
- {
- return getVoxelSquaredSideLen (this->octree_depth_);
- }
-
- /** \brief Generate bounds of the current voxel of an octree iterator
- * \param[in] iterator: octree iterator
- * \param[out] min_pt lower bound of voxel
- * \param[out] max_pt upper bound of voxel
- */
- inline void
- getVoxelBounds (const OctreeIteratorBase<OctreeT>& iterator, Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt) const
- {
- this->genVoxelBoundsFromOctreeKey (iterator.getCurrentOctreeKey (),
- iterator.getCurrentOctreeDepth (), min_pt, max_pt);
- }
-
- /** \brief Enable dynamic octree structure
- * \note Leaf nodes are kept as close to the root as possible and are only expanded if the number of DataT objects within a leaf node exceeds a fixed limit.
- * \param maxObjsPerLeaf: maximum number of DataT objects per leaf
- * */
- inline void
- enableDynamicDepth ( std::size_t maxObjsPerLeaf )
- {
- assert(this->leaf_count_==0);
- max_objs_per_leaf_ = maxObjsPerLeaf;
-
- this->dynamic_depth_enabled_ = max_objs_per_leaf_ > 0;
- }
-
-
- protected:
-
- /** \brief Add point at index from input pointcloud dataset to octree
- * \param[in] point_idx_arg the index representing the point in the dataset given by \a setInputCloud to be added
- */
- virtual void
- addPointIdx (const int point_idx_arg);
-
- /** \brief Add point at index from input pointcloud dataset to octree
- * \param[in] leaf_node to be expanded
- * \param[in] parent_branch parent of leaf node to be expanded
- * \param[in] child_idx child index of leaf node (in parent branch)
- * \param[in] depth_mask of leaf node to be expanded
- */
- void
- expandLeafNode (LeafNode* leaf_node, BranchNode* parent_branch, unsigned char child_idx, unsigned int depth_mask);
-
- /** \brief Get point at index from input pointcloud dataset
- * \param[in] index_arg index representing the point in the dataset given by \a setInputCloud
- * \return PointT from input pointcloud dataset
- */
- const PointT&
- getPointByIndex (const unsigned int index_arg) const;
-
- /** \brief Find octree leaf node at a given point
- * \param[in] point_arg query point
- * \return pointer to leaf node. If leaf node does not exist, pointer is 0.
- */
- LeafContainerT*
- findLeafAtPoint (const PointT& point_arg) const
- {
- OctreeKey key;
-
- // generate key for point
- this->genOctreeKeyforPoint (point_arg, key);
-
- return (this->findLeaf (key));
- }
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // Protected octree methods based on octree keys
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- /** \brief Define octree key setting and octree depth based on defined bounding box. */
- void
- getKeyBitSize ();
-
- /** \brief Grow the bounding box/octree until point fits
- * \param[in] point_idx_arg point that should be within bounding box;
- */
- void
- adoptBoundingBoxToPoint (const PointT& point_idx_arg);
-
- /** \brief Checks if given point is within the bounding box of the octree
- * \param[in] point_idx_arg point to be checked for bounding box violations
- * \return "true" - no bound violation
- */
- inline bool isPointWithinBoundingBox (const PointT& point_idx_arg) const
- {
- return (! ( (point_idx_arg.x < min_x_) || (point_idx_arg.y < min_y_)
- || (point_idx_arg.z < min_z_) || (point_idx_arg.x >= max_x_)
- || (point_idx_arg.y >= max_y_) || (point_idx_arg.z >= max_z_)));
- }
-
- /** \brief Generate octree key for voxel at a given point
- * \param[in] point_arg the point addressing a voxel
- * \param[out] key_arg write octree key to this reference
- */
- void
- genOctreeKeyforPoint (const PointT & point_arg,
- OctreeKey &key_arg) const;
-
- /** \brief Generate octree key for voxel at a given point
- * \param[in] point_x_arg X coordinate of point addressing a voxel
- * \param[in] point_y_arg Y coordinate of point addressing a voxel
- * \param[in] point_z_arg Z coordinate of point addressing a voxel
- * \param[out] key_arg write octree key to this reference
- */
- void
- genOctreeKeyforPoint (const double point_x_arg, const double point_y_arg, const double point_z_arg,
- OctreeKey & key_arg) const;
-
- /** \brief Virtual method for generating octree key for a given point index.
- * \note This method enables to assign indices to leaf nodes during octree deserialization.
- * \param[in] data_arg index value representing a point in the dataset given by \a setInputCloud
- * \param[out] key_arg write octree key to this reference
- * \return "true" - octree keys are assignable
- */
- virtual bool
- genOctreeKeyForDataT (const int& data_arg, OctreeKey & key_arg) const;
-
- /** \brief Generate a point at center of leaf node voxel
- * \param[in] key_arg octree key addressing a leaf node.
- * \param[out] point_arg write leaf node voxel center to this point reference
- */
- void
- genLeafNodeCenterFromOctreeKey (const OctreeKey & key_arg,
- PointT& point_arg) const;
-
- /** \brief Generate a point at center of octree voxel at given tree level
- * \param[in] key_arg octree key addressing an octree node.
- * \param[in] tree_depth_arg octree depth of query voxel
- * \param[out] point_arg write leaf node center point to this reference
- */
- void
- genVoxelCenterFromOctreeKey (const OctreeKey & key_arg,
- unsigned int tree_depth_arg, PointT& point_arg) const;
-
- /** \brief Generate bounds of an octree voxel using octree key and tree depth arguments
- * \param[in] key_arg octree key addressing an octree node.
- * \param[in] tree_depth_arg octree depth of query voxel
- * \param[out] min_pt lower bound of voxel
- * \param[out] max_pt upper bound of voxel
- */
- void
- genVoxelBoundsFromOctreeKey (const OctreeKey & key_arg,
- unsigned int tree_depth_arg, Eigen::Vector3f &min_pt,
- Eigen::Vector3f &max_pt) const;
-
- /** \brief Recursively search the tree for all leaf nodes and return a vector of voxel centers.
- * \param[in] node_arg current octree node to be explored
- * \param[in] key_arg octree key addressing a leaf node.
- * \param[out] voxel_center_list_arg results are written to this vector of PointT elements
- * \return number of voxels found
- */
- int
- getOccupiedVoxelCentersRecursive (const BranchNode* node_arg,
- const OctreeKey& key_arg,
- AlignedPointTVector &voxel_center_list_arg) const;
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // Globals
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief Pointer to input point cloud dataset. */
- PointCloudConstPtr input_;
-
- /** \brief A pointer to the vector of point indices to use. */
- IndicesConstPtr indices_;
-
- /** \brief Epsilon precision (error bound) for nearest neighbors searches. */
- double epsilon_;
-
- /** \brief Octree resolution. */
- double resolution_;
-
- // Octree bounding box coordinates
- double min_x_;
- double max_x_;
-
- double min_y_;
- double max_y_;
-
- double min_z_;
- double max_z_;
-
- /** \brief Flag indicating if octree has defined bounding box. */
- bool bounding_box_defined_;
-
- /** \brief Amount of DataT objects per leafNode before expanding branch
- * \note zero indicates a fixed/maximum depth octree structure
- * **/
- std::size_t max_objs_per_leaf_;
- };
+protected:
+ /** \brief Add point at index from input pointcloud dataset to octree
+ * \param[in] point_idx_arg the index representing the point in the dataset given by
+ * \a setInputCloud to be added
+ */
+ virtual void
+ addPointIdx(const int point_idx_arg);
+
+ /** \brief Add point at index from input pointcloud dataset to octree
+ * \param[in] leaf_node to be expanded
+ * \param[in] parent_branch parent of leaf node to be expanded
+ * \param[in] child_idx child index of leaf node (in parent branch)
+ * \param[in] depth_mask of leaf node to be expanded
+ */
+ void
+ expandLeafNode(LeafNode* leaf_node,
+ BranchNode* parent_branch,
+ unsigned char child_idx,
+ unsigned int depth_mask);
+
+ /** \brief Get point at index from input pointcloud dataset
+ * \param[in] index_arg index representing the point in the dataset given by \a
+ * setInputCloud
+ * \return PointT from input pointcloud dataset
+ */
+ const PointT&
+ getPointByIndex(const unsigned int index_arg) const;
+
+ /** \brief Find octree leaf node at a given point
+ * \param[in] point_arg query point
+ * \return pointer to leaf node. If leaf node does not exist, pointer is 0.
+ */
+ LeafContainerT*
+ findLeafAtPoint(const PointT& point_arg) const
+ {
+ OctreeKey key;
+
+ // generate key for point
+ this->genOctreeKeyforPoint(point_arg, key);
+
+ return (this->findLeaf(key));
+ }
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Protected octree methods based on octree keys
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /** \brief Define octree key setting and octree depth based on defined bounding box.
+ */
+ void
+ getKeyBitSize();
+
+ /** \brief Grow the bounding box/octree until point fits
+ * \param[in] point_idx_arg point that should be within bounding box;
+ */
+ void
+ adoptBoundingBoxToPoint(const PointT& point_idx_arg);
+
+ /** \brief Checks if given point is within the bounding box of the octree
+ * \param[in] point_idx_arg point to be checked for bounding box violations
+ * \return "true" - no bound violation
+ */
+ inline bool
+ isPointWithinBoundingBox(const PointT& point_idx_arg) const
+ {
+ return (!((point_idx_arg.x < min_x_) || (point_idx_arg.y < min_y_) ||
+ (point_idx_arg.z < min_z_) || (point_idx_arg.x >= max_x_) ||
+ (point_idx_arg.y >= max_y_) || (point_idx_arg.z >= max_z_)));
}
-}
+
+ /** \brief Generate octree key for voxel at a given point
+ * \param[in] point_arg the point addressing a voxel
+ * \param[out] key_arg write octree key to this reference
+ */
+ void
+ genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const;
+
+ /** \brief Generate octree key for voxel at a given point
+ * \param[in] point_x_arg X coordinate of point addressing a voxel
+ * \param[in] point_y_arg Y coordinate of point addressing a voxel
+ * \param[in] point_z_arg Z coordinate of point addressing a voxel
+ * \param[out] key_arg write octree key to this reference
+ */
+ void
+ genOctreeKeyforPoint(const double point_x_arg,
+ const double point_y_arg,
+ const double point_z_arg,
+ OctreeKey& key_arg) const;
+
+ /** \brief Virtual method for generating octree key for a given point index.
+ * \note This method enables to assign indices to leaf nodes during octree
+ * deserialization.
+ * \param[in] data_arg index value representing a point in the dataset given by \a
+ * setInputCloud
+ * \param[out] key_arg write octree key to this reference \return "true" - octree keys
+ * are assignable
+ */
+ virtual bool
+ genOctreeKeyForDataT(const int& data_arg, OctreeKey& key_arg) const;
+
+ /** \brief Generate a point at center of leaf node voxel
+ * \param[in] key_arg octree key addressing a leaf node.
+ * \param[out] point_arg write leaf node voxel center to this point reference
+ */
+ void
+ genLeafNodeCenterFromOctreeKey(const OctreeKey& key_arg, PointT& point_arg) const;
+
+ /** \brief Generate a point at center of octree voxel at given tree level
+ * \param[in] key_arg octree key addressing an octree node.
+ * \param[in] tree_depth_arg octree depth of query voxel
+ * \param[out] point_arg write leaf node center point to this reference
+ */
+ void
+ genVoxelCenterFromOctreeKey(const OctreeKey& key_arg,
+ unsigned int tree_depth_arg,
+ PointT& point_arg) const;
+
+ /** \brief Generate bounds of an octree voxel using octree key and tree depth
+ * arguments
+ * \param[in] key_arg octree key addressing an octree node.
+ * \param[in] tree_depth_arg octree depth of query voxel
+ * \param[out] min_pt lower bound of voxel
+ * \param[out] max_pt upper bound of voxel
+ */
+ void
+ genVoxelBoundsFromOctreeKey(const OctreeKey& key_arg,
+ unsigned int tree_depth_arg,
+ Eigen::Vector3f& min_pt,
+ Eigen::Vector3f& max_pt) const;
+
+ /** \brief Recursively search the tree for all leaf nodes and return a vector of voxel
+ * centers.
+ * \param[in] node_arg current octree node to be explored
+ * \param[in] key_arg octree key addressing a leaf node.
+ * \param[out] voxel_center_list_arg results are written to this vector of PointT
+ * elements
+ * \return number of voxels found
+ */
+ int
+ getOccupiedVoxelCentersRecursive(const BranchNode* node_arg,
+ const OctreeKey& key_arg,
+ AlignedPointTVector& voxel_center_list_arg) const;
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Globals
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Pointer to input point cloud dataset. */
+ PointCloudConstPtr input_;
+
+ /** \brief A pointer to the vector of point indices to use. */
+ IndicesConstPtr indices_;
+
+ /** \brief Epsilon precision (error bound) for nearest neighbors searches. */
+ double epsilon_;
+
+ /** \brief Octree resolution. */
+ double resolution_;
+
+ // Octree bounding box coordinates
+ double min_x_;
+ double max_x_;
+
+ double min_y_;
+ double max_y_;
+
+ double min_z_;
+ double max_z_;
+
+ /** \brief Flag indicating if octree has defined bounding box. */
+ bool bounding_box_defined_;
+
+ /** \brief Amount of DataT objects per leafNode before expanding branch
+ * \note zero indicates a fixed/maximum depth octree structure
+ * **/
+ std::size_t max_objs_per_leaf_;
+};
+
+} // namespace octree
+} // namespace pcl
#ifdef PCL_NO_PRECOMPILE
#include <pcl/octree/impl/octree_pointcloud.hpp>
#include <pcl/octree/octree_pointcloud.h>
#include <pcl/octree/octree_pointcloud_adjacency_container.h>
-#include <set>
#include <list>
+#include <set>
-namespace pcl
-{
+namespace pcl {
- namespace octree
+namespace octree {
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Octree pointcloud voxel class which maintains adjacency information for
+ * its voxels.
+ *
+ * This pointcloud octree class generates an octree from a point cloud (zero-copy). The
+ * octree pointcloud is initialized with its voxel resolution. Its bounding box is
+ * automatically adjusted or can be predefined.
+ *
+ * The OctreePointCloudAdjacencyContainer class can be used to store data in leaf nodes.
+ *
+ * An optional transform function can be provided which changes how the voxel grid is
+ * computed - this can be used to, for example, make voxel bins larger as they increase
+ * in distance from the origin (camera). \note See SupervoxelClustering for an example
+ * of how to provide a transform function.
+ *
+ * If used in academic work, please cite:
+ *
+ * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter
+ * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds
+ * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition
+ * (CVPR) 2013
+ *
+ * \ingroup octree
+ * \author Jeremie Papon (jpapon@gmail.com) */
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT,
+ typename LeafContainerT = OctreePointCloudAdjacencyContainer<PointT>,
+ typename BranchContainerT = OctreeContainerEmpty>
+class OctreePointCloudAdjacency
+: public OctreePointCloud<PointT, LeafContainerT, BranchContainerT> {
+
+public:
+ using OctreeBaseT = OctreeBase<LeafContainerT, BranchContainerT>;
+
+ using OctreeAdjacencyT =
+ OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>;
+ using Ptr = shared_ptr<OctreeAdjacencyT>;
+ using ConstPtr = shared_ptr<const OctreeAdjacencyT>;
+
+ using OctreePointCloudT =
+ OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeBaseT>;
+ using LeafNode = typename OctreePointCloudT::LeafNode;
+ using BranchNode = typename OctreePointCloudT::BranchNode;
+
+ using PointCloud = pcl::PointCloud<PointT>;
+ using PointCloudPtr = typename PointCloud::Ptr;
+ using PointCloudConstPtr = typename PointCloud::ConstPtr;
+
+ // BGL graph
+ using VoxelAdjacencyList = boost::
+ adjacency_list<boost::setS, boost::setS, boost::undirectedS, PointT, float>;
+ using VoxelID = typename VoxelAdjacencyList::vertex_descriptor;
+ using EdgeID = typename VoxelAdjacencyList::edge_descriptor;
+
+ // Leaf vector - pointers to all leaves
+ using LeafVectorT = std::vector<LeafContainerT*>;
+
+ // Fast leaf iterators that don't require traversing tree
+ using iterator = typename LeafVectorT::iterator;
+ using const_iterator = typename LeafVectorT::const_iterator;
+
+ inline iterator
+ begin()
{
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Octree pointcloud voxel class which maintains adjacency information for its voxels.
- *
- * This pointcloud octree class generates an octree from a point cloud (zero-copy). The octree pointcloud is
- * initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined.
- *
- * The OctreePointCloudAdjacencyContainer class can be used to store data in leaf nodes.
- *
- * An optional transform function can be provided which changes how the voxel grid is computed - this can be used to,
- * for example, make voxel bins larger as they increase in distance from the origin (camera).
- * \note See SupervoxelClustering for an example of how to provide a transform function.
- *
- * If used in academic work, please cite:
- *
- * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter
- * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds
- * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013
- *
- * \ingroup octree
- * \author Jeremie Papon (jpapon@gmail.com) */
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- template <typename PointT,
- typename LeafContainerT = OctreePointCloudAdjacencyContainer<PointT>,
- typename BranchContainerT = OctreeContainerEmpty>
- class OctreePointCloudAdjacency : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT>
- {
-
- public:
-
- using OctreeBaseT = OctreeBase<LeafContainerT, BranchContainerT>;
-
- using OctreeAdjacencyT = OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>;
- using Ptr = shared_ptr<OctreeAdjacencyT>;
- using ConstPtr = shared_ptr<const OctreeAdjacencyT>;
-
- using OctreePointCloudT = OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeBaseT>;
- using LeafNode = typename OctreePointCloudT::LeafNode;
- using BranchNode = typename OctreePointCloudT::BranchNode;
-
- using PointCloud = pcl::PointCloud<PointT>;
- using PointCloudPtr = typename PointCloud::Ptr;
- using PointCloudConstPtr = typename PointCloud::ConstPtr;
-
- // BGL graph
- using VoxelAdjacencyList = boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, PointT, float>;
- using VoxelID = typename VoxelAdjacencyList::vertex_descriptor;
- using EdgeID = typename VoxelAdjacencyList::edge_descriptor;
-
- // Leaf vector - pointers to all leaves
- using LeafVectorT = std::vector<LeafContainerT *>;
-
- // Fast leaf iterators that don't require traversing tree
- using iterator = typename LeafVectorT::iterator;
- using const_iterator = typename LeafVectorT::const_iterator;
-
- inline iterator begin () { return (leaf_vector_.begin ()); }
- inline iterator end () { return (leaf_vector_.end ()); }
- inline LeafContainerT* at (std::size_t idx) { return leaf_vector_.at (idx); }
-
- // Size of neighbors
- inline std::size_t size () const { return leaf_vector_.size (); }
-
- /** \brief Constructor.
- *
- * \param[in] resolution_arg Octree resolution at lowest octree level (voxel size) */
- OctreePointCloudAdjacency (const double resolution_arg);
-
- /** \brief Adds points from cloud to the octree.
- *
- * \note This overrides addPointsFromInputCloud() from the OctreePointCloud class. */
- void
- addPointsFromInputCloud ();
-
- /** \brief Gets the leaf container for a given point.
- *
- * \param[in] point_arg Point to search for
- *
- * \returns Pointer to the leaf container - null if no leaf container found. */
- LeafContainerT*
- getLeafContainerAtPoint (const PointT& point_arg) const;
-
- /** \brief Computes an adjacency graph of voxel relations.
- *
- * \warning This slows down rapidly as cloud size increases due to the number of edges.
- *
- * \param[out] voxel_adjacency_graph Boost Graph Library Adjacency graph of the voxel touching relationships.
- * Vertices are PointT, edges represent touching, and edge lengths are the distance between the points. */
- void
- computeVoxelAdjacencyGraph (VoxelAdjacencyList &voxel_adjacency_graph);
-
- /** \brief Sets a point transform (and inverse) used to transform the space of the input cloud.
- *
- * This is useful for changing how adjacency is calculated - such as relaxing the adjacency criterion for
- * points further from the camera.
- *
- * \param[in] transform_func A boost:function pointer to the transform to be used. The transform must have one
- * parameter (a point) which it modifies in place. */
- void
- setTransformFunction (std::function<void (PointT &p)> transform_func)
- {
- transform_func_ = transform_func;
- }
-
- /** \brief Tests whether input point is occluded from specified camera point by other voxels.
- *
- * \param[in] point_arg Point to test for
- * \param[in] camera_pos Position of camera, defaults to origin
- *
- * \returns True if path to camera is blocked by a voxel, false otherwise. */
- bool
- testForOcclusion (const PointT& point_arg, const PointXYZ &camera_pos = PointXYZ (0, 0, 0));
-
- protected:
-
- /** \brief Add point at index from input pointcloud dataset to octree.
- *
- * \param[in] point_idx_arg The index representing the point in the dataset given by setInputCloud() to be added
- *
- * \note This virtual implementation allows the use of a transform function to compute keys. */
- void
- addPointIdx (const int point_idx_arg) override;
-
- /** \brief Fills in the neighbors fields for new voxels.
- *
- * \param[in] key_arg Key of the voxel to check neighbors for
- * \param[in] leaf_container Pointer to container of the leaf to check neighbors for */
- void
- computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container);
-
- /** \brief Generates octree key for specified point (uses transform if provided).
- *
- * \param[in] point_arg Point to generate key for
- * \param[out] key_arg Resulting octree key */
- void
- genOctreeKeyforPoint (const PointT& point_arg, OctreeKey& key_arg) const;
-
- private:
-
- /** \brief Add point at given index from input point cloud to octree.
- *
- * Index will be also added to indices vector. This functionality is not enabled for adjacency octree. */
- using OctreePointCloudT::addPointFromCloud;
-
- /** \brief Add point simultaneously to octree and input point cloud.
- *
- * This functionality is not enabled for adjacency octree. */
- using OctreePointCloudT::addPointToCloud;
-
- using OctreePointCloudT::input_;
- using OctreePointCloudT::resolution_;
- using OctreePointCloudT::min_x_;
- using OctreePointCloudT::min_y_;
- using OctreePointCloudT::min_z_;
- using OctreePointCloudT::max_x_;
- using OctreePointCloudT::max_y_;
- using OctreePointCloudT::max_z_;
-
- /// Local leaf pointer vector used to make iterating through leaves fast.
- LeafVectorT leaf_vector_;
-
- std::function<void (PointT &p)> transform_func_;
-
- };
+ return (leaf_vector_.begin());
+ }
+ inline iterator
+ end()
+ {
+ return (leaf_vector_.end());
+ }
+ inline LeafContainerT*
+ at(std::size_t idx)
+ {
+ return leaf_vector_.at(idx);
+ }
+ // Size of neighbors
+ inline std::size_t
+ size() const
+ {
+ return leaf_vector_.size();
}
-}
+ /** \brief Constructor.
+ *
+ * \param[in] resolution_arg Octree resolution at lowest octree level (voxel size) */
+ OctreePointCloudAdjacency(const double resolution_arg);
+
+ /** \brief Adds points from cloud to the octree.
+ *
+ * \note This overrides addPointsFromInputCloud() from the OctreePointCloud class. */
+ void
+ addPointsFromInputCloud();
+
+ /** \brief Gets the leaf container for a given point.
+ *
+ * \param[in] point_arg Point to search for
+ *
+ * \returns Pointer to the leaf container - null if no leaf container found. */
+ LeafContainerT*
+ getLeafContainerAtPoint(const PointT& point_arg) const;
+
+ /** \brief Computes an adjacency graph of voxel relations.
+ *
+ * \warning This slows down rapidly as cloud size increases due to the number of
+ * edges.
+ *
+ * \param[out] voxel_adjacency_graph Boost Graph Library Adjacency graph of the voxel
+ * touching relationships. Vertices are PointT, edges represent touching, and edge
+ * lengths are the distance between the points. */
+ void
+ computeVoxelAdjacencyGraph(VoxelAdjacencyList& voxel_adjacency_graph);
+
+ /** \brief Sets a point transform (and inverse) used to transform the space of the
+ * input cloud.
+ *
+ * This is useful for changing how adjacency is calculated - such as relaxing the
+ * adjacency criterion for points further from the camera.
+ *
+ * \param[in] transform_func A boost:function pointer to the transform to be used. The
+ * transform must have one parameter (a point) which it modifies in place. */
+ void
+ setTransformFunction(std::function<void(PointT& p)> transform_func)
+ {
+ transform_func_ = transform_func;
+ }
-// Note: Do not precompile this octree type because it is typically used with custom leaf containers.
+ /** \brief Tests whether input point is occluded from specified camera point by other
+ * voxels.
+ *
+ * \param[in] point_arg Point to test for
+ * \param[in] camera_pos Position of camera, defaults to origin
+ *
+ * \returns True if path to camera is blocked by a voxel, false otherwise. */
+ bool
+ testForOcclusion(const PointT& point_arg,
+ const PointXYZ& camera_pos = PointXYZ(0, 0, 0));
+
+protected:
+ /** \brief Add point at index from input pointcloud dataset to octree.
+ *
+ * \param[in] point_idx_arg The index representing the point in the dataset given by
+ * setInputCloud() to be added
+ *
+ * \note This virtual implementation allows the use of a transform function to compute
+ * keys. */
+ void
+ addPointIdx(const int point_idx_arg) override;
+
+ /** \brief Fills in the neighbors fields for new voxels.
+ *
+ * \param[in] key_arg Key of the voxel to check neighbors for
+ * \param[in] leaf_container Pointer to container of the leaf to check neighbors for
+ */
+ void
+ computeNeighbors(OctreeKey& key_arg, LeafContainerT* leaf_container);
+
+ /** \brief Generates octree key for specified point (uses transform if provided).
+ *
+ * \param[in] point_arg Point to generate key for
+ * \param[out] key_arg Resulting octree key */
+ void
+ genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const;
+
+private:
+ /** \brief Add point at given index from input point cloud to octree.
+ *
+ * Index will be also added to indices vector. This functionality is not enabled for
+ * adjacency octree. */
+ using OctreePointCloudT::addPointFromCloud;
+
+ /** \brief Add point simultaneously to octree and input point cloud.
+ *
+ * This functionality is not enabled for adjacency octree. */
+ using OctreePointCloudT::addPointToCloud;
+
+ using OctreePointCloudT::input_;
+ using OctreePointCloudT::max_x_;
+ using OctreePointCloudT::max_y_;
+ using OctreePointCloudT::max_z_;
+ using OctreePointCloudT::min_x_;
+ using OctreePointCloudT::min_y_;
+ using OctreePointCloudT::min_z_;
+ using OctreePointCloudT::resolution_;
+
+ /// Local leaf pointer vector used to make iterating through leaves fast.
+ LeafVectorT leaf_vector_;
+
+ std::function<void(PointT& p)> transform_func_;
+};
+
+} // namespace octree
+
+} // namespace pcl
+
+// Note: Do not precompile this octree type because it is typically used with custom
+// leaf containers.
#include <pcl/octree/impl/octree_pointcloud_adjacency.hpp>
#pragma once
-namespace pcl
-{
-
- namespace octree
- {
- /** \brief @b Octree adjacency leaf container class- stores a list of pointers to neighbors, number of points added, and a DataT value
- * \note This class implements a leaf node that stores pointers to neighboring leaves
- * \note This class also has a virtual computeData function, which is called by octreePointCloudAdjacency::addPointsFromInputCloud.
- * \note You should make explicit instantiations of it for your pointtype/datatype combo (if needed) see supervoxel_clustering.hpp for an example of this
- */
- template<typename PointInT, typename DataT = PointInT>
- class OctreePointCloudAdjacencyContainer : public OctreeContainerBase
- {
- template<typename T, typename U, typename V>
- friend class OctreePointCloudAdjacency;
- public:
- using NeighborListT = std::list<OctreePointCloudAdjacencyContainer<PointInT, DataT> *>;
- using const_iterator = typename NeighborListT::const_iterator;
- //const iterators to neighbors
- inline const_iterator cbegin () const { return (neighbors_.begin ()); }
- inline const_iterator cend () const { return (neighbors_.end ()); }
- //size of neighbors
- inline std::size_t size () const { return neighbors_.size (); }
-
- /** \brief Class initialization. */
- OctreePointCloudAdjacencyContainer () :
- OctreeContainerBase ()
- {
- this->reset();
- }
-
- /** \brief Empty class deconstructor. */
- ~OctreePointCloudAdjacencyContainer ()
- {
- }
-
- /** \brief Returns the number of neighbors this leaf has
- * \returns number of neighbors
- */
- std::size_t
- getNumNeighbors () const
- {
- return neighbors_.size ();
- }
+namespace pcl {
- /** \brief Gets the number of points contributing to this leaf */
- int
- getPointCounter () const { return num_points_; }
-
- /** \brief Returns a reference to the data member to access it without copying */
- DataT&
- getData () { return data_; }
-
- /** \brief Sets the data member
- * \param[in] data_arg New value for data
- */
- void
- setData (const DataT& data_arg) { data_ = data_arg;}
-
- /** \brief virtual method to get size of container
- * \return number of points added to leaf node container.
- */
- std::size_t
- getSize () const override
- {
- return num_points_;
- }
- protected:
- //iterators to neighbors
- using iterator = typename NeighborListT::iterator;
- inline iterator begin () { return (neighbors_.begin ()); }
- inline iterator end () { return (neighbors_.end ()); }
-
- /** \brief deep copy function */
- virtual OctreePointCloudAdjacencyContainer *
- deepCopy () const
- {
- OctreePointCloudAdjacencyContainer *new_container = new OctreePointCloudAdjacencyContainer;
- new_container->setNeighbors (this->neighbors_);
- new_container->setPointCounter (this->num_points_);
- return new_container;
- }
-
- /** \brief Add new point to container- this just counts points
- * \note To actually store data in the leaves, need to specialize this
- * for your point and data type as in supervoxel_clustering.hpp
- */
- // param[in] new_point the new point to add
- void
- addPoint (const PointInT& /*new_point*/)
- {
- using namespace pcl::common;
- ++num_points_;
- }
-
- /** \brief Function for working on data added. Base implementation does nothing
- * */
- void
- computeData ()
- {
- }
-
- /** \brief Sets the number of points contributing to this leaf */
- void
- setPointCounter (int points_arg) { num_points_ = points_arg; }
-
- /** \brief Clear the voxel centroid */
- void
- reset () override
- {
- neighbors_.clear ();
- num_points_ = 0;
- data_ = DataT ();
- }
-
- /** \brief Add new neighbor to voxel.
- * \param[in] neighbor the new neighbor to add
- */
- void
- addNeighbor (OctreePointCloudAdjacencyContainer *neighbor)
- {
- neighbors_.push_back (neighbor);
- }
-
- /** \brief Remove neighbor from neighbor set.
- * \param[in] neighbor the neighbor to remove
- */
- void
- removeNeighbor (OctreePointCloudAdjacencyContainer *neighbor)
- {
- for (iterator neighb_it = neighbors_.begin(); neighb_it != neighbors_.end(); ++neighb_it)
- {
- if ( *neighb_it == neighbor)
- {
- neighbors_.erase (neighb_it);
- return;
- }
- }
- }
-
- /** \brief Sets the whole neighbor set
- * \param[in] neighbor_arg the new set
- */
- void
- setNeighbors (const NeighborListT &neighbor_arg)
- {
- neighbors_ = neighbor_arg;
+namespace octree {
+/** \brief @b Octree adjacency leaf container class- stores a list of pointers to
+ * neighbors, number of points added, and a DataT value \note This class implements a
+ * leaf node that stores pointers to neighboring leaves \note This class also has a
+ * virtual computeData function, which is called by
+ * octreePointCloudAdjacency::addPointsFromInputCloud. \note You should make explicit
+ * instantiations of it for your pointtype/datatype combo (if needed) see
+ * supervoxel_clustering.hpp for an example of this
+ */
+template <typename PointInT, typename DataT = PointInT>
+class OctreePointCloudAdjacencyContainer : public OctreeContainerBase {
+ template <typename T, typename U, typename V>
+ friend class OctreePointCloudAdjacency;
+
+public:
+ using NeighborListT = std::list<OctreePointCloudAdjacencyContainer<PointInT, DataT>*>;
+ using const_iterator = typename NeighborListT::const_iterator;
+ // const iterators to neighbors
+ inline const_iterator
+ cbegin() const
+ {
+ return (neighbors_.begin());
+ }
+ inline const_iterator
+ cend() const
+ {
+ return (neighbors_.end());
+ }
+ // size of neighbors
+ inline std::size_t
+ size() const
+ {
+ return neighbors_.size();
+ }
+
+ /** \brief Class initialization. */
+ OctreePointCloudAdjacencyContainer() : OctreeContainerBase() { this->reset(); }
+
+ /** \brief Empty class deconstructor. */
+ ~OctreePointCloudAdjacencyContainer() {}
+
+ /** \brief Returns the number of neighbors this leaf has
+ * \returns number of neighbors
+ */
+ std::size_t
+ getNumNeighbors() const
+ {
+ return neighbors_.size();
+ }
+
+ /** \brief Gets the number of points contributing to this leaf */
+ int
+ getPointCounter() const
+ {
+ return num_points_;
+ }
+
+ /** \brief Returns a reference to the data member to access it without copying */
+ DataT&
+ getData()
+ {
+ return data_;
+ }
+
+ /** \brief Sets the data member
+ * \param[in] data_arg New value for data
+ */
+ void
+ setData(const DataT& data_arg)
+ {
+ data_ = data_arg;
+ }
+
+ /** \brief virtual method to get size of container
+ * \return number of points added to leaf node container.
+ */
+ std::size_t
+ getSize() const override
+ {
+ return num_points_;
+ }
+
+protected:
+ // iterators to neighbors
+ using iterator = typename NeighborListT::iterator;
+ inline iterator
+ begin()
+ {
+ return (neighbors_.begin());
+ }
+ inline iterator
+ end()
+ {
+ return (neighbors_.end());
+ }
+
+ /** \brief deep copy function */
+ virtual OctreePointCloudAdjacencyContainer*
+ deepCopy() const
+ {
+ OctreePointCloudAdjacencyContainer* new_container =
+ new OctreePointCloudAdjacencyContainer;
+ new_container->setNeighbors(this->neighbors_);
+ new_container->setPointCounter(this->num_points_);
+ return new_container;
+ }
+
+ /** \brief Add new point to container- this just counts points
+ * \note To actually store data in the leaves, need to specialize this
+ * for your point and data type as in supervoxel_clustering.hpp
+ */
+ // param[in] new_point the new point to add
+ void
+ addPoint(const PointInT& /*new_point*/)
+ {
+ using namespace pcl::common;
+ ++num_points_;
+ }
+
+ /** \brief Function for working on data added. Base implementation does nothing
+ * */
+ void
+ computeData()
+ {}
+
+ /** \brief Sets the number of points contributing to this leaf */
+ void
+ setPointCounter(int points_arg)
+ {
+ num_points_ = points_arg;
+ }
+
+ /** \brief Clear the voxel centroid */
+ void
+ reset() override
+ {
+ neighbors_.clear();
+ num_points_ = 0;
+ data_ = DataT();
+ }
+
+ /** \brief Add new neighbor to voxel.
+ * \param[in] neighbor the new neighbor to add
+ */
+ void
+ addNeighbor(OctreePointCloudAdjacencyContainer* neighbor)
+ {
+ neighbors_.push_back(neighbor);
+ }
+
+ /** \brief Remove neighbor from neighbor set.
+ * \param[in] neighbor the neighbor to remove
+ */
+ void
+ removeNeighbor(OctreePointCloudAdjacencyContainer* neighbor)
+ {
+ for (iterator neighb_it = neighbors_.begin(); neighb_it != neighbors_.end();
+ ++neighb_it) {
+ if (*neighb_it == neighbor) {
+ neighbors_.erase(neighb_it);
+ return;
}
-
- private:
- int num_points_;
- NeighborListT neighbors_;
- DataT data_;
- };
- }
-}
+ }
+ }
+
+ /** \brief Sets the whole neighbor set
+ * \param[in] neighbor_arg the new set
+ */
+ void
+ setNeighbors(const NeighborListT& neighbor_arg)
+ {
+ neighbors_ = neighbor_arg;
+ }
+
+private:
+ int num_points_;
+ NeighborListT neighbors_;
+ DataT data_;
+};
+} // namespace octree
+} // namespace pcl
#include <boost/shared_ptr.hpp>
-#include <pcl/octree/octree_pointcloud.h>
#include <pcl/octree/octree2buf_base.h>
+#include <pcl/octree/octree_pointcloud.h>
-namespace pcl
-{
- namespace octree
- {
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Octree pointcloud change detector class
- * \note This pointcloud octree class generate an octrees from a point cloud (zero-copy). It allows to detect new leaf nodes and serialize their point indices
- * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined.
- * \note
- * \note typename: PointT: type of point used in pointcloud
- * \ingroup octree
- * \author Julius Kammerl (julius@kammerl.de)
- */
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- template<typename PointT,
- typename LeafContainerT = OctreeContainerPointIndices,
- typename BranchContainerT = OctreeContainerEmpty >
+namespace pcl {
+namespace octree {
- class OctreePointCloudChangeDetector : public OctreePointCloud<PointT,
- LeafContainerT, BranchContainerT, Octree2BufBase<LeafContainerT, BranchContainerT> >
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Octree pointcloud change detector class
+ * \note This pointcloud octree class generate an octrees from a point cloud
+ * (zero-copy). It allows to detect new leaf nodes and serialize their point indices
+ * \note The octree pointcloud is initialized with its voxel resolution. Its bounding
+ * box is automatically adjusted or can be predefined.
+ * \tparam PointT type of point used in pointcloud
+ * \ingroup octree
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT,
+ typename LeafContainerT = OctreeContainerPointIndices,
+ typename BranchContainerT = OctreeContainerEmpty>
- {
+class OctreePointCloudChangeDetector
+: public OctreePointCloud<PointT,
+ LeafContainerT,
+ BranchContainerT,
+ Octree2BufBase<LeafContainerT, BranchContainerT>>
- public:
+{
- using Ptr = shared_ptr<OctreePointCloudChangeDetector<PointT, LeafContainerT, BranchContainerT>>;
- using ConstPtr = shared_ptr<const OctreePointCloudChangeDetector<PointT, LeafContainerT, BranchContainerT>>;
+public:
+ using Ptr = shared_ptr<
+ OctreePointCloudChangeDetector<PointT, LeafContainerT, BranchContainerT>>;
+ using ConstPtr = shared_ptr<
+ const OctreePointCloudChangeDetector<PointT, LeafContainerT, BranchContainerT>>;
- /** \brief Constructor.
- * \param resolution_arg: octree resolution at lowest octree level
- * */
- OctreePointCloudChangeDetector (const double resolution_arg) :
- OctreePointCloud<PointT, LeafContainerT, BranchContainerT,
- Octree2BufBase<LeafContainerT, BranchContainerT> > (resolution_arg)
- {
- }
+ /** \brief Constructor.
+ * \param resolution_arg: octree resolution at lowest octree level
+ * */
+ OctreePointCloudChangeDetector(const double resolution_arg)
+ : OctreePointCloud<PointT,
+ LeafContainerT,
+ BranchContainerT,
+ Octree2BufBase<LeafContainerT, BranchContainerT>>(resolution_arg)
+ {}
- /** \brief Get a indices from all leaf nodes that did not exist in previous buffer.
- * \param indicesVector_arg: results are written to this vector of int indices
- * \param minPointsPerLeaf_arg: minimum amount of points required within leaf node to become serialized.
- * \return number of point indices
- */
- std::size_t getPointIndicesFromNewVoxels (std::vector<int> &indicesVector_arg,
- const int minPointsPerLeaf_arg = 0)
- {
+ /** \brief Get a indices from all leaf nodes that did not exist in previous buffer.
+ * \param indicesVector_arg: results are written to this vector of int indices
+ * \param minPointsPerLeaf_arg: minimum amount of points required within leaf node to
+ * become serialized.
+ * \return number of point indices
+ */
+ std::size_t
+ getPointIndicesFromNewVoxels(std::vector<int>& indicesVector_arg,
+ const int minPointsPerLeaf_arg = 0)
+ {
- std::vector<OctreeContainerPointIndices*> leaf_containers;
- this->serializeNewLeafs (leaf_containers);
+ std::vector<OctreeContainerPointIndices*> leaf_containers;
+ this->serializeNewLeafs(leaf_containers);
- for (const auto &leaf_container : leaf_containers)
- {
- if (static_cast<int> (leaf_container->getSize ()) >= minPointsPerLeaf_arg)
- leaf_container->getPointIndices(indicesVector_arg);
- }
+ for (const auto& leaf_container : leaf_containers) {
+ if (static_cast<int>(leaf_container->getSize()) >= minPointsPerLeaf_arg)
+ leaf_container->getPointIndices(indicesVector_arg);
+ }
- return (indicesVector_arg.size ());
- }
- };
+ return (indicesVector_arg.size());
}
-}
+};
+} // namespace octree
+} // namespace pcl
-#define PCL_INSTANTIATE_OctreePointCloudChangeDetector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudChangeDetector<T>;
+#define PCL_INSTANTIATE_OctreePointCloudChangeDetector(T) \
+ template class PCL_EXPORTS pcl::octree::OctreePointCloudChangeDetector<T>;
#include <pcl/octree/octree_pointcloud.h>
-namespace pcl
-{
- namespace octree
+namespace pcl {
+namespace octree {
+/** \brief @b Octree pointcloud density leaf node class
+ * \note This class implements a leaf node that counts the amount of points which fall
+ * into its voxel space.
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+class OctreePointCloudDensityContainer : public OctreeContainerBase {
+public:
+ /** \brief Class initialization. */
+ OctreePointCloudDensityContainer() : point_counter_(0) {}
+
+ /** \brief Empty class deconstructor. */
+ ~OctreePointCloudDensityContainer() {}
+
+ /** \brief deep copy function */
+ virtual OctreePointCloudDensityContainer*
+ deepCopy() const
+ {
+ return (new OctreePointCloudDensityContainer(*this));
+ }
+
+ /** \brief Equal comparison operator
+ * \param[in] other OctreePointCloudDensityContainer to compare with
+ */
+ bool
+ operator==(const OctreeContainerBase& other) const override
{
- /** \brief @b Octree pointcloud density leaf node class
- * \note This class implements a leaf node that counts the amount of points which fall into its voxel space.
- * \author Julius Kammerl (julius@kammerl.de)
- */
- class OctreePointCloudDensityContainer : public OctreeContainerBase
- {
- public:
- /** \brief Class initialization. */
- OctreePointCloudDensityContainer () : point_counter_ (0)
- {
- }
-
- /** \brief Empty class deconstructor. */
- ~OctreePointCloudDensityContainer ()
- {
- }
-
- /** \brief deep copy function */
- virtual OctreePointCloudDensityContainer *
- deepCopy () const
- {
- return (new OctreePointCloudDensityContainer (*this));
- }
-
- /** \brief Equal comparison operator
- * \param[in] other OctreePointCloudDensityContainer to compare with
- */
- bool operator==(const OctreeContainerBase& other) const override
- {
- const OctreePointCloudDensityContainer* otherContainer =
- dynamic_cast<const OctreePointCloudDensityContainer*>(&other);
-
- return (this->point_counter_==otherContainer->point_counter_);
- }
-
- /** \brief Read input data. Only an internal counter is increased.
- */
- void
- addPointIndex (int)
- {
- point_counter_++;
- }
-
- /** \brief Return point counter.
- * \return Amount of points
- */
- unsigned int
- getPointCounter ()
- {
- return (point_counter_);
- }
-
- /** \brief Reset leaf node. */
- void
- reset () override
- {
- point_counter_ = 0;
- }
-
- private:
- unsigned int point_counter_;
-
- };
-
- /** \brief @b Octree pointcloud density class
- * \note This class generate an octrees from a point cloud (zero-copy). Only the amount of points that fall into the leaf node voxel are stored.
- * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined.
- * \note
- * \note typename: PointT: type of point used in pointcloud
- * \ingroup octree
- * \author Julius Kammerl (julius@kammerl.de)
- */
- template<typename PointT, typename LeafContainerT = OctreePointCloudDensityContainer, typename BranchContainerT = OctreeContainerEmpty >
- class OctreePointCloudDensity : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT>
- {
- public:
-
- /** \brief OctreePointCloudDensity class constructor.
- * \param resolution_arg: octree resolution at lowest octree level
- * */
- OctreePointCloudDensity (const double resolution_arg) :
- OctreePointCloud<PointT, LeafContainerT, BranchContainerT> (resolution_arg)
- {
- }
-
- /** \brief Empty class deconstructor. */
-
- ~OctreePointCloudDensity ()
- {
- }
-
- /** \brief Get the amount of points within a leaf node voxel which is addressed by a point
- * \param[in] point_arg: a point addressing a voxel
- * \return amount of points that fall within leaf node voxel
- */
- unsigned int
- getVoxelDensityAtPoint (const PointT& point_arg) const
- {
- unsigned int point_count = 0;
-
- OctreePointCloudDensityContainer* leaf = this->findLeafAtPoint (point_arg);
-
- if (leaf)
- point_count = leaf->getPointCounter ();
-
- return (point_count);
- }
- };
+ const OctreePointCloudDensityContainer* otherContainer =
+ dynamic_cast<const OctreePointCloudDensityContainer*>(&other);
+
+ return (this->point_counter_ == otherContainer->point_counter_);
+ }
+
+ /** \brief Read input data. Only an internal counter is increased.
+ */
+ void
+ addPointIndex(int)
+ {
+ point_counter_++;
+ }
+
+ /** \brief Return point counter.
+ * \return Amount of points
+ */
+ unsigned int
+ getPointCounter()
+ {
+ return (point_counter_);
+ }
+
+ /** \brief Reset leaf node. */
+ void
+ reset() override
+ {
+ point_counter_ = 0;
+ }
+
+private:
+ unsigned int point_counter_;
+};
+
+/** \brief @b Octree pointcloud density class
+ * \note This class generate an octrees from a point cloud (zero-copy). Only the amount
+ * of points that fall into the leaf node voxel are stored.
+ * \note The octree pointcloud is initialized with its voxel resolution. Its bounding
+ * box is automatically adjusted or can be predefined.
+ * \tparam PointT type of point used in pointcloud
+ * \ingroup octree
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+template <typename PointT,
+ typename LeafContainerT = OctreePointCloudDensityContainer,
+ typename BranchContainerT = OctreeContainerEmpty>
+class OctreePointCloudDensity
+: public OctreePointCloud<PointT, LeafContainerT, BranchContainerT> {
+public:
+ /** \brief OctreePointCloudDensity class constructor.
+ * \param resolution_arg: octree resolution at lowest octree level
+ * */
+ OctreePointCloudDensity(const double resolution_arg)
+ : OctreePointCloud<PointT, LeafContainerT, BranchContainerT>(resolution_arg)
+ {}
+
+ /** \brief Empty class deconstructor. */
+
+ ~OctreePointCloudDensity() {}
+
+ /** \brief Get the amount of points within a leaf node voxel which is addressed by a
+ * point
+ * \param[in] point_arg: a point addressing a voxel \return amount of points
+ * that fall within leaf node voxel
+ */
+ unsigned int
+ getVoxelDensityAtPoint(const PointT& point_arg) const
+ {
+ unsigned int point_count = 0;
+
+ OctreePointCloudDensityContainer* leaf = this->findLeafAtPoint(point_arg);
+
+ if (leaf)
+ point_count = leaf->getPointCounter();
+
+ return (point_count);
}
-}
+};
+} // namespace octree
+} // namespace pcl
-#define PCL_INSTANTIATE_OctreePointCloudDensity(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudDensity<T>;
+#define PCL_INSTANTIATE_OctreePointCloudDensity(T) \
+ template class PCL_EXPORTS pcl::octree::OctreePointCloudDensity<T>;
#include <pcl/octree/octree_pointcloud.h>
-namespace pcl
+namespace pcl {
+namespace octree {
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Octree pointcloud occupancy class
+ * \tparam PointT type of point used in pointcloud
+ * This pointcloud octree class generate an octrees from a point cloud (zero-copy). No
+ * information is stored at the lead nodes. It can be used of occupancy checks.
+ * \note The octree pointcloud is initialized with its voxel resolution. Its bounding
+ * box is automatically adjusted or can be predefined.
+ * \ingroup octree
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT,
+ typename LeafContainerT = OctreeContainerEmpty,
+ typename BranchContainerT = OctreeContainerEmpty>
+class OctreePointCloudOccupancy
+: public OctreePointCloud<PointT,
+ LeafContainerT,
+ BranchContainerT,
+ OctreeBase<LeafContainerT, BranchContainerT>>
+
{
- namespace octree
+
+public:
+ // public typedefs for single/double buffering
+ using SingleBuffer =
+ OctreePointCloudOccupancy<PointT, LeafContainerT, BranchContainerT>;
+ using DoubleBuffer =
+ OctreePointCloudOccupancy<PointT, LeafContainerT, BranchContainerT>;
+
+ // public point cloud typedefs
+ using PointCloud =
+ typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::PointCloud;
+ using PointCloudPtr = typename OctreePointCloud<PointT,
+ LeafContainerT,
+ BranchContainerT>::PointCloudPtr;
+ using PointCloudConstPtr =
+ typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::
+ PointCloudConstPtr;
+
+ /** \brief Constructor.
+ * \param resolution_arg: octree resolution at lowest octree level
+ * */
+ OctreePointCloudOccupancy(const double resolution_arg)
+ : OctreePointCloud<PointT,
+ LeafContainerT,
+ BranchContainerT,
+ OctreeBase<LeafContainerT, BranchContainerT>>(resolution_arg)
+ {}
+
+ /** \brief Empty class constructor. */
+
+ ~OctreePointCloudOccupancy() {}
+
+ /** \brief Set occupied voxel at point.
+ * \param point_arg: input point
+ * */
+ void
+ setOccupiedVoxelAtPoint(const PointT& point_arg)
{
+ OctreeKey key;
+
+ // make sure bounding box is big enough
+ this->adoptBoundingBoxToPoint(point_arg);
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Octree pointcloud occupancy class
- * \note This pointcloud octree class generate an octrees from a point cloud (zero-copy). No information is stored at the lead nodes. It can be used of occupancy checks.
- * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined.
- * \note
- * \note typename: PointT: type of point used in pointcloud
- * \ingroup octree
- * \author Julius Kammerl (julius@kammerl.de)
- */
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- template<typename PointT,
- typename LeafContainerT = OctreeContainerEmpty,
- typename BranchContainerT = OctreeContainerEmpty >
- class OctreePointCloudOccupancy : public OctreePointCloud<PointT, LeafContainerT,
- BranchContainerT, OctreeBase<LeafContainerT, BranchContainerT> >
-
- {
-
- public:
- // public typedefs for single/double buffering
- using SingleBuffer = OctreePointCloudOccupancy<PointT, LeafContainerT, BranchContainerT>;
- using DoubleBuffer = OctreePointCloudOccupancy<PointT, LeafContainerT, BranchContainerT>;
-
- // public point cloud typedefs
- using PointCloud = typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::PointCloud;
- using PointCloudPtr = typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::PointCloudPtr;
- using PointCloudConstPtr = typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::PointCloudConstPtr;
-
- /** \brief Constructor.
- * \param resolution_arg: octree resolution at lowest octree level
- * */
- OctreePointCloudOccupancy (const double resolution_arg) :
- OctreePointCloud<PointT, LeafContainerT, BranchContainerT,
- OctreeBase<LeafContainerT, BranchContainerT> > (resolution_arg)
- {
- }
-
- /** \brief Empty class constructor. */
-
- ~OctreePointCloudOccupancy ()
- {
- }
-
- /** \brief Set occupied voxel at point.
- * \param point_arg: input point
- * */
- void setOccupiedVoxelAtPoint( const PointT& point_arg ) {
- OctreeKey key;
-
- // make sure bounding box is big enough
- this->adoptBoundingBoxToPoint (point_arg);
-
- // generate key
- this->genOctreeKeyforPoint (point_arg, key);
-
- // add point to octree at key
- this->createLeaf (key);
- }
-
- /** \brief Set occupied voxels at all points from point cloud.
- * \param cloud_arg: input point cloud
- * */
- void setOccupiedVoxelsAtPointsFromCloud( PointCloudPtr cloud_arg ) {
- for (std::size_t i = 0; i < cloud_arg->points.size (); i++)
- {
- // check for NaNs
- if (isFinite(cloud_arg->points[i])) {
- // set voxel at point
- this->setOccupiedVoxelAtPoint (cloud_arg->points[i]);
- }
- }
- }
-
- };
+ // generate key
+ this->genOctreeKeyforPoint(point_arg, key);
+
+ // add point to octree at key
+ this->createLeaf(key);
+ }
+
+ /** \brief Set occupied voxels at all points from point cloud.
+ * \param cloud_arg: input point cloud
+ * */
+ void
+ setOccupiedVoxelsAtPointsFromCloud(PointCloudPtr cloud_arg)
+ {
+ for (std::size_t i = 0; i < cloud_arg->points.size(); i++) {
+ // check for NaNs
+ if (isFinite(cloud_arg->points[i])) {
+ // set voxel at point
+ this->setOccupiedVoxelAtPoint(cloud_arg->points[i]);
+ }
+ }
}
+};
+} // namespace octree
-}
+} // namespace pcl
-#define PCL_INSTANTIATE_OctreePointCloudOccupancy(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudOccupancy<T>;
+#define PCL_INSTANTIATE_OctreePointCloudOccupancy(T) \
+ template class PCL_EXPORTS pcl::octree::OctreePointCloudOccupancy<T>;
#include <pcl/octree/octree_pointcloud.h>
-namespace pcl
-{
- namespace octree
- {
+namespace pcl {
+namespace octree {
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Octree pointcloud point vector class
- * \note This pointcloud octree class generate an octrees from a point cloud (zero-copy). Every leaf node contains a list of point indices of the dataset given by \a setInputCloud.
- * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined.
- * \note
- * \note typename: PointT: type of point used in pointcloud
- * \ingroup octree
- * \author Julius Kammerl (julius@kammerl.de)
- */
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- template<typename PointT,
- typename LeafContainerT = OctreeContainerPointIndices,
- typename BranchContainerT = OctreeContainerEmpty,
- typename OctreeT = OctreeBase<LeafContainerT, BranchContainerT> >
- class OctreePointCloudPointVector : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
- {
-
- public:
- // public typedefs for single/double buffering
- using SingleBuffer = OctreePointCloudPointVector<PointT, LeafContainerT, BranchContainerT,
- OctreeBase<LeafContainerT, BranchContainerT> >;
- // typedef OctreePointCloudPointVector<PointT, LeafContainerT, BranchContainerT,
- // Octree2BufBase<int, LeafContainerT, BranchContainerT> > DoubleBuffer;
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Octree pointcloud point vector class
+ * \note This pointcloud octree class generate an octrees from a point cloud
+ * (zero-copy). Every leaf node contains a list of point indices of the dataset given by
+ * \a setInputCloud.
+ * \note The octree pointcloud is initialized with its voxel resolution. Its bounding
+ * box is automatically adjusted or can be predefined.
+ * \tparam PointT type of point used in pointcloud
+ * \ingroup octree
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT,
+ typename LeafContainerT = OctreeContainerPointIndices,
+ typename BranchContainerT = OctreeContainerEmpty,
+ typename OctreeT = OctreeBase<LeafContainerT, BranchContainerT>>
+class OctreePointCloudPointVector
+: public OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT> {
- /** \brief Constructor.
- * \param resolution_arg: octree resolution at lowest octree level
- * */
- OctreePointCloudPointVector (const double resolution_arg) :
- OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT> (resolution_arg)
- {
- }
+public:
+ // public typedefs for single/double buffering
+ using SingleBuffer =
+ OctreePointCloudPointVector<PointT,
+ LeafContainerT,
+ BranchContainerT,
+ OctreeBase<LeafContainerT, BranchContainerT>>;
+ // typedef OctreePointCloudPointVector<PointT, LeafContainerT, BranchContainerT,
+ // Octree2BufBase<int, LeafContainerT, BranchContainerT> > DoubleBuffer;
- /** \brief Empty class constructor. */
- ~OctreePointCloudPointVector ()
- {
- }
+ /** \brief Constructor.
+ * \param resolution_arg: octree resolution at lowest octree level
+ * */
+ OctreePointCloudPointVector(const double resolution_arg)
+ : OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>(resolution_arg)
+ {}
- };
- }
-}
+ /** \brief Empty class constructor. */
+ ~OctreePointCloudPointVector() {}
+};
+} // namespace octree
+} // namespace pcl
-#define PCL_INSTANTIATE_OctreePointCloudPointVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudPointVector<T>;
+#define PCL_INSTANTIATE_OctreePointCloudPointVector(T) \
+ template class PCL_EXPORTS pcl::octree::OctreePointCloudPointVector<T>;
#include <pcl/octree/octree_pointcloud.h>
-namespace pcl
-{
- namespace octree
- {
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Octree pointcloud single point class
- * \note This pointcloud octree class generate an octrees from a point cloud (zero-copy). Every leaf node contains a single point index from the dataset given by \a setInputCloud.
- * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined.
- * \note
- * \note typename: PointT: type of point used in pointcloud
- * \ingroup octree
- * \author Julius Kammerl (julius@kammerl.de)
- */
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- template<typename PointT, typename LeafContainerT = OctreeContainerPointIndex,
- typename BranchContainerT = OctreeContainerEmpty,
- typename OctreeT = OctreeBase<LeafContainerT, BranchContainerT> >
+namespace pcl {
+namespace octree {
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Octree pointcloud single point class
+ * \note This pointcloud octree class generate an octrees from a point cloud
+ * (zero-copy). Every leaf node contains a single point index from the dataset given by
+ * \a setInputCloud.
+ * \note The octree pointcloud is initialized with its voxel resolution. Its bounding
+ * box is automatically adjusted or can be predefined.
+ * \tparam PointT type of point used in pointcloud
+ * \ingroup octree
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT,
+ typename LeafContainerT = OctreeContainerPointIndex,
+ typename BranchContainerT = OctreeContainerEmpty,
+ typename OctreeT = OctreeBase<LeafContainerT, BranchContainerT>>
- class OctreePointCloudSinglePoint : public OctreePointCloud<PointT, LeafContainerT,
- BranchContainerT, OctreeT>
- {
+class OctreePointCloudSinglePoint
+: public OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT> {
- public:
- // public typedefs for single/double buffering
- using SingleBuffer = OctreePointCloudSinglePoint<PointT, LeafContainerT, BranchContainerT,
- OctreeBase<LeafContainerT, BranchContainerT> >;
+public:
+ // public typedefs for single/double buffering
+ using SingleBuffer =
+ OctreePointCloudSinglePoint<PointT,
+ LeafContainerT,
+ BranchContainerT,
+ OctreeBase<LeafContainerT, BranchContainerT>>;
// typedef OctreePointCloudSinglePoint<PointT, LeafContainerT, BranchContainerT,
- // Octree2BufBase<int, LeafContainerT, BranchContainerT> > DoubleBuffer;
-
- /** \brief Constructor.
- * \param resolution_arg: octree resolution at lowest octree level
- * */
- OctreePointCloudSinglePoint (const double resolution_arg) :
- OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT> (resolution_arg)
- {
- }
+ // Octree2BufBase<int, LeafContainerT, BranchContainerT> > DoubleBuffer;
- /** \brief Empty class constructor. */
- ~OctreePointCloudSinglePoint ()
- {
- }
+ /** \brief Constructor.
+ * \param resolution_arg: octree resolution at lowest octree level
+ * */
+ OctreePointCloudSinglePoint(const double resolution_arg)
+ : OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>(resolution_arg)
+ {}
- };
+ /** \brief Empty class constructor. */
+ ~OctreePointCloudSinglePoint() {}
+};
- }
-}
+} // namespace octree
+} // namespace pcl
-#define PCL_INSTANTIATE_OctreePointCloudSinglePoint(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudSinglePoint<T>;
+#define PCL_INSTANTIATE_OctreePointCloudSinglePoint(T) \
+ template class PCL_EXPORTS pcl::octree::OctreePointCloudSinglePoint<T>;
#include <pcl/octree/octree_pointcloud.h>
-namespace pcl
-{
- namespace octree
+namespace pcl {
+namespace octree {
+/** \brief @b Octree pointcloud voxel centroid leaf node class
+ * \note This class implements a leaf node that calculates the mean centroid of all
+ * points added this octree container.
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+template <typename PointT>
+class OctreePointCloudVoxelCentroidContainer : public OctreeContainerBase {
+public:
+ /** \brief Class initialization. */
+ OctreePointCloudVoxelCentroidContainer() { this->reset(); }
+
+ /** \brief Empty class deconstructor. */
+ ~OctreePointCloudVoxelCentroidContainer() {}
+
+ /** \brief deep copy function */
+ virtual OctreePointCloudVoxelCentroidContainer*
+ deepCopy() const
+ {
+ return (new OctreePointCloudVoxelCentroidContainer(*this));
+ }
+
+ /** \brief Equal comparison operator - set to false
+ */
+ // param[in] OctreePointCloudVoxelCentroidContainer to compare with
+ bool
+ operator==(const OctreeContainerBase&) const override
+ {
+ return (false);
+ }
+
+ /** \brief Add new point to voxel.
+ * \param[in] new_point the new point to add
+ */
+ void
+ addPoint(const PointT& new_point)
+ {
+ using namespace pcl::common;
+
+ ++point_counter_;
+
+ point_sum_ += new_point;
+ }
+
+ /** \brief Calculate centroid of voxel.
+ * \param[out] centroid_arg the resultant centroid of the voxel
+ */
+ void
+ getCentroid(PointT& centroid_arg) const
+ {
+ using namespace pcl::common;
+
+ if (point_counter_) {
+ centroid_arg = point_sum_;
+ centroid_arg /= static_cast<float>(point_counter_);
+ }
+ else {
+ centroid_arg *= 0.0f;
+ }
+ }
+
+ /** \brief Reset leaf container. */
+ void
+ reset() override
+ {
+ using namespace pcl::common;
+
+ point_counter_ = 0;
+ point_sum_ *= 0.0f;
+ }
+
+private:
+ unsigned int point_counter_;
+ PointT point_sum_;
+};
+
+/** \brief @b Octree pointcloud voxel centroid class
+ * \note This class generate an octrees from a point cloud (zero-copy). It provides a
+ * vector of centroids for all occupied voxels.
+ * \note The octree pointcloud is initialized with its voxel resolution. Its bounding
+ * box is automatically adjusted or can be predefined.
+ * \tparam PointT type of point used in pointcloud
+ * \ingroup octree
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+template <typename PointT,
+ typename LeafContainerT = OctreePointCloudVoxelCentroidContainer<PointT>,
+ typename BranchContainerT = OctreeContainerEmpty>
+class OctreePointCloudVoxelCentroid
+: public OctreePointCloud<PointT, LeafContainerT, BranchContainerT> {
+public:
+ using Ptr = shared_ptr<OctreePointCloudVoxelCentroid<PointT, LeafContainerT>>;
+ using ConstPtr =
+ shared_ptr<const OctreePointCloudVoxelCentroid<PointT, LeafContainerT>>;
+
+ using OctreeT = OctreePointCloud<PointT, LeafContainerT, BranchContainerT>;
+ using LeafNode = typename OctreeT::LeafNode;
+ using BranchNode = typename OctreeT::BranchNode;
+
+ /** \brief OctreePointCloudVoxelCentroids class constructor.
+ * \param[in] resolution_arg octree resolution at lowest octree level
+ */
+ OctreePointCloudVoxelCentroid(const double resolution_arg)
+ : OctreePointCloud<PointT, LeafContainerT, BranchContainerT>(resolution_arg)
+ {}
+
+ /** \brief Empty class deconstructor. */
+
+ ~OctreePointCloudVoxelCentroid() {}
+
+ /** \brief Add DataT object to leaf node at octree key.
+ * \param pointIdx_arg
+ */
+ void
+ addPointIdx(const int pointIdx_arg) override
+ {
+ OctreeKey key;
+
+ assert(pointIdx_arg < static_cast<int>(this->input_->points.size()));
+
+ const PointT& point = this->input_->points[pointIdx_arg];
+
+ // make sure bounding box is big enough
+ this->adoptBoundingBoxToPoint(point);
+
+ // generate key
+ this->genOctreeKeyforPoint(point, key);
+
+ // add point to octree at key
+ LeafContainerT* container = this->createLeaf(key);
+ container->addPoint(point);
+ }
+
+ /** \brief Get centroid for a single voxel addressed by a PointT point.
+ * \param[in] point_arg point addressing a voxel in octree
+ * \param[out] voxel_centroid_arg centroid is written to this PointT reference
+ * \return "true" if voxel is found; "false" otherwise
+ */
+ bool
+ getVoxelCentroidAtPoint(const PointT& point_arg, PointT& voxel_centroid_arg) const;
+
+ /** \brief Get centroid for a single voxel addressed by a PointT point from input
+ * cloud.
+ * \param[in] point_idx_arg point index from input cloud addressing a voxel in octree
+ * \param[out] voxel_centroid_arg centroid is written to this PointT reference
+ * \return "true" if voxel is found; "false" otherwise
+ */
+ inline bool
+ getVoxelCentroidAtPoint(const int& point_idx_arg, PointT& voxel_centroid_arg) const
{
- /** \brief @b Octree pointcloud voxel centroid leaf node class
- * \note This class implements a leaf node that calculates the mean centroid of all points added this octree container.
- * \author Julius Kammerl (julius@kammerl.de)
- */
- template<typename PointT>
- class OctreePointCloudVoxelCentroidContainer : public OctreeContainerBase
- {
- public:
- /** \brief Class initialization. */
- OctreePointCloudVoxelCentroidContainer ()
- {
- this->reset();
- }
-
- /** \brief Empty class deconstructor. */
- ~OctreePointCloudVoxelCentroidContainer ()
- {
- }
-
- /** \brief deep copy function */
- virtual OctreePointCloudVoxelCentroidContainer *
- deepCopy () const
- {
- return (new OctreePointCloudVoxelCentroidContainer (*this));
- }
-
- /** \brief Equal comparison operator - set to false
- */
- // param[in] OctreePointCloudVoxelCentroidContainer to compare with
- bool operator==(const OctreeContainerBase&) const override
- {
- return ( false );
- }
-
- /** \brief Add new point to voxel.
- * \param[in] new_point the new point to add
- */
- void
- addPoint (const PointT& new_point)
- {
- using namespace pcl::common;
-
- ++point_counter_;
-
- point_sum_ += new_point;
- }
-
- /** \brief Calculate centroid of voxel.
- * \param[out] centroid_arg the resultant centroid of the voxel
- */
- void
- getCentroid (PointT& centroid_arg) const
- {
- using namespace pcl::common;
-
- if (point_counter_)
- {
- centroid_arg = point_sum_;
- centroid_arg /= static_cast<float> (point_counter_);
- }
- else
- {
- centroid_arg *= 0.0f;
- }
- }
-
- /** \brief Reset leaf container. */
- void
- reset () override
- {
- using namespace pcl::common;
-
- point_counter_ = 0;
- point_sum_ *= 0.0f;
- }
-
- private:
- unsigned int point_counter_;
- PointT point_sum_;
- };
-
- /** \brief @b Octree pointcloud voxel centroid class
- * \note This class generate an octrees from a point cloud (zero-copy). It provides a vector of centroids for all occupied voxels.
- * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined.
- * \note
- * \note typename: PointT: type of point used in pointcloud
- *
- * \ingroup octree
- * \author Julius Kammerl (julius@kammerl.de)
- */
- template<typename PointT,
- typename LeafContainerT = OctreePointCloudVoxelCentroidContainer<PointT> ,
- typename BranchContainerT = OctreeContainerEmpty >
- class OctreePointCloudVoxelCentroid : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT>
- {
- public:
- using Ptr = shared_ptr<OctreePointCloudVoxelCentroid<PointT, LeafContainerT> >;
- using ConstPtr = shared_ptr<const OctreePointCloudVoxelCentroid<PointT, LeafContainerT> >;
-
- using OctreeT = OctreePointCloud<PointT, LeafContainerT, BranchContainerT>;
- using LeafNode = typename OctreeT::LeafNode;
- using BranchNode = typename OctreeT::BranchNode;
-
- /** \brief OctreePointCloudVoxelCentroids class constructor.
- * \param[in] resolution_arg octree resolution at lowest octree level
- */
- OctreePointCloudVoxelCentroid (const double resolution_arg) :
- OctreePointCloud<PointT, LeafContainerT, BranchContainerT> (resolution_arg)
- {
- }
-
- /** \brief Empty class deconstructor. */
-
- ~OctreePointCloudVoxelCentroid ()
- {
- }
-
- /** \brief Add DataT object to leaf node at octree key.
- * \param pointIdx_arg
- */
- void
- addPointIdx (const int pointIdx_arg) override
- {
- OctreeKey key;
-
- assert (pointIdx_arg < static_cast<int> (this->input_->points.size ()));
-
- const PointT& point = this->input_->points[pointIdx_arg];
-
- // make sure bounding box is big enough
- this->adoptBoundingBoxToPoint (point);
-
- // generate key
- this->genOctreeKeyforPoint (point, key);
-
- // add point to octree at key
- LeafContainerT* container = this->createLeaf(key);
- container->addPoint (point);
-
- }
-
- /** \brief Get centroid for a single voxel addressed by a PointT point.
- * \param[in] point_arg point addressing a voxel in octree
- * \param[out] voxel_centroid_arg centroid is written to this PointT reference
- * \return "true" if voxel is found; "false" otherwise
- */
- bool
- getVoxelCentroidAtPoint (const PointT& point_arg, PointT& voxel_centroid_arg) const;
-
- /** \brief Get centroid for a single voxel addressed by a PointT point from input cloud.
- * \param[in] point_idx_arg point index from input cloud addressing a voxel in octree
- * \param[out] voxel_centroid_arg centroid is written to this PointT reference
- * \return "true" if voxel is found; "false" otherwise
- */
- inline bool
- getVoxelCentroidAtPoint (const int& point_idx_arg, PointT& voxel_centroid_arg) const
- {
- // get centroid at point
- return (this->getVoxelCentroidAtPoint (this->input_->points[point_idx_arg], voxel_centroid_arg));
- }
-
- /** \brief Get PointT vector of centroids for all occupied voxels.
- * \param[out] voxel_centroid_list_arg results are written to this vector of PointT elements
- * \return number of occupied voxels
- */
- std::size_t
- getVoxelCentroids (typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::AlignedPointTVector &voxel_centroid_list_arg) const;
-
- /** \brief Recursively explore the octree and output a PointT vector of centroids for all occupied voxels.
- * \param[in] branch_arg: current branch node
- * \param[in] key_arg: current key
- * \param[out] voxel_centroid_list_arg results are written to this vector of PointT elements
- */
- void
- getVoxelCentroidsRecursive (const BranchNode* branch_arg,
- OctreeKey& key_arg,
- typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::AlignedPointTVector &voxel_centroid_list_arg) const;
-
- };
+ // get centroid at point
+ return (this->getVoxelCentroidAtPoint(this->input_->points[point_idx_arg],
+ voxel_centroid_arg));
}
-}
-// Note: Don't precompile this octree type to speed up compilation. It's probably rarely used.
+ /** \brief Get PointT vector of centroids for all occupied voxels.
+ * \param[out] voxel_centroid_list_arg results are written to this vector of PointT
+ * elements
+ * \return number of occupied voxels
+ */
+ std::size_t
+ getVoxelCentroids(
+ typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::
+ AlignedPointTVector& voxel_centroid_list_arg) const;
+
+ /** \brief Recursively explore the octree and output a PointT vector of centroids for
+ * all occupied voxels.
+ * \param[in] branch_arg: current branch node
+ * \param[in] key_arg: current key
+ * \param[out] voxel_centroid_list_arg results are written to this vector of PointT
+ * elements
+ */
+ void
+ getVoxelCentroidsRecursive(
+ const BranchNode* branch_arg,
+ OctreeKey& key_arg,
+ typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::
+ AlignedPointTVector& voxel_centroid_list_arg) const;
+};
+} // namespace octree
+} // namespace pcl
+
+// Note: Don't precompile this octree type to speed up compilation. It's probably rarely
+// used.
#include <pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp>
#include <pcl/octree/octree_pointcloud.h>
-namespace pcl
-{
- namespace octree
+namespace pcl {
+namespace octree {
+/** \brief @b Octree pointcloud search class
+ * \note This class provides several methods for spatial neighbor search based on octree
+ * structure
+ * \tparam PointT type of point used in pointcloud
+ * \ingroup octree
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+template <typename PointT,
+ typename LeafContainerT = OctreeContainerPointIndices,
+ typename BranchContainerT = OctreeContainerEmpty>
+class OctreePointCloudSearch
+: public OctreePointCloud<PointT, LeafContainerT, BranchContainerT> {
+public:
+ // public typedefs
+ using IndicesPtr = shared_ptr<std::vector<int>>;
+ using IndicesConstPtr = shared_ptr<const std::vector<int>>;
+
+ using PointCloud = pcl::PointCloud<PointT>;
+ using PointCloudPtr = typename PointCloud::Ptr;
+ using PointCloudConstPtr = typename PointCloud::ConstPtr;
+
+ // Boost shared pointers
+ using Ptr =
+ shared_ptr<OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>>;
+ using ConstPtr = shared_ptr<
+ const OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>>;
+
+ // Eigen aligned allocator
+ using AlignedPointTVector = std::vector<PointT, Eigen::aligned_allocator<PointT>>;
+
+ using OctreeT = OctreePointCloud<PointT, LeafContainerT, BranchContainerT>;
+ using LeafNode = typename OctreeT::LeafNode;
+ using BranchNode = typename OctreeT::BranchNode;
+
+ /** \brief Constructor.
+ * \param[in] resolution octree resolution at lowest octree level
+ */
+ OctreePointCloudSearch(const double resolution)
+ : OctreePointCloud<PointT, LeafContainerT, BranchContainerT>(resolution)
+ {}
+
+ /** \brief Search for neighbors within a voxel at given point
+ * \param[in] point point addressing a leaf node voxel
+ * \param[out] point_idx_data the resultant indices of the neighboring voxel points
+ * \return "true" if leaf node exist; "false" otherwise
+ */
+ bool
+ voxelSearch(const PointT& point, std::vector<int>& point_idx_data);
+
+ /** \brief Search for neighbors within a voxel at given point referenced by a point
+ * index
+ * \param[in] index the index in input cloud defining the query point
+ * \param[out] point_idx_data the resultant indices of the neighboring voxel points
+ * \return "true" if leaf node exist; "false" otherwise
+ */
+ bool
+ voxelSearch(const int index, std::vector<int>& point_idx_data);
+
+ /** \brief Search for k-nearest neighbors at the query point.
+ * \param[in] cloud the point cloud data
+ * \param[in] index the index in \a cloud representing the query point
+ * \param[in] k the number of neighbors to search for
+ * \param[out] k_indices the resultant indices of the neighboring points (must be
+ * resized to \a k a priori!)
+ * \param[out] k_sqr_distances the resultant squared distances to the neighboring
+ * points (must be resized to \a k a priori!)
+ * \return number of neighbors found
+ */
+ inline int
+ nearestKSearch(const PointCloud& cloud,
+ int index,
+ int k,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances)
{
- /** \brief @b Octree pointcloud search class
- * \note This class provides several methods for spatial neighbor search based on octree structure
- * \note typename: PointT: type of point used in pointcloud
- * \ingroup octree
- * \author Julius Kammerl (julius@kammerl.de)
- */
- template<typename PointT, typename LeafContainerT = OctreeContainerPointIndices , typename BranchContainerT = OctreeContainerEmpty >
- class OctreePointCloudSearch : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT>
+ return (nearestKSearch(cloud[index], k, k_indices, k_sqr_distances));
+ }
+
+ /** \brief Search for k-nearest neighbors at given query point.
+ * \param[in] p_q the given query point
+ * \param[in] k the number of neighbors to search for
+ * \param[out] k_indices the resultant indices of the neighboring points (must be
+ * resized to k a priori!)
+ * \param[out] k_sqr_distances the resultant squared distances to the neighboring
+ * points (must be resized to k a priori!)
+ * \return number of neighbors found
+ */
+ int
+ nearestKSearch(const PointT& p_q,
+ int k,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances);
+
+ /** \brief Search for k-nearest neighbors at query point
+ * \param[in] index index representing the query point in the dataset given by \a
+ * setInputCloud. If indices were given in setInputCloud, index will be the position
+ * in the indices vector.
+ * \param[in] k the number of neighbors to search for
+ * \param[out] k_indices the resultant indices of the neighboring points (must be
+ * resized to \a k a priori!)
+ * \param[out] k_sqr_distances the resultant squared distances to the neighboring
+ * points (must be resized to \a k a priori!)
+ * \return number of neighbors found
+ */
+ int
+ nearestKSearch(int index,
+ int k,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances);
+
+ /** \brief Search for approx. nearest neighbor at the query point.
+ * \param[in] cloud the point cloud data
+ * \param[in] query_index the index in \a cloud representing the query point
+ * \param[out] result_index the resultant index of the neighbor point
+ * \param[out] sqr_distance the resultant squared distance to the neighboring point
+ * \return number of neighbors found
+ */
+ inline void
+ approxNearestSearch(const PointCloud& cloud,
+ int query_index,
+ int& result_index,
+ float& sqr_distance)
+ {
+ return (approxNearestSearch(cloud.points[query_index], result_index, sqr_distance));
+ }
+
+ /** \brief Search for approx. nearest neighbor at the query point.
+ * \param[in] p_q the given query point
+ * \param[out] result_index the resultant index of the neighbor point
+ * \param[out] sqr_distance the resultant squared distance to the neighboring point
+ */
+ void
+ approxNearestSearch(const PointT& p_q, int& result_index, float& sqr_distance);
+
+ /** \brief Search for approx. nearest neighbor at the query point.
+ * \param[in] query_index index representing the query point in the dataset given by
+ * \a setInputCloud. If indices were given in setInputCloud, index will be the
+ * position in the indices vector.
+ * \param[out] result_index the resultant index of the neighbor point
+ * \param[out] sqr_distance the resultant squared distance to the neighboring point
+ * \return number of neighbors found
+ */
+ void
+ approxNearestSearch(int query_index, int& result_index, float& sqr_distance);
+
+ /** \brief Search for all neighbors of query point that are within a given radius.
+ * \param[in] cloud the point cloud data
+ * \param[in] index the index in \a cloud representing the query point
+ * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
+ * \param[out] k_indices the resultant indices of the neighboring points
+ * \param[out] k_sqr_distances the resultant squared distances to the neighboring
+ * points
+ * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
+ * \return number of neighbors found in radius
+ */
+ int
+ radiusSearch(const PointCloud& cloud,
+ int index,
+ double radius,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances,
+ unsigned int max_nn = 0)
+ {
+ return (
+ radiusSearch(cloud.points[index], radius, k_indices, k_sqr_distances, max_nn));
+ }
+
+ /** \brief Search for all neighbors of query point that are within a given radius.
+ * \param[in] p_q the given query point
+ * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
+ * \param[out] k_indices the resultant indices of the neighboring points
+ * \param[out] k_sqr_distances the resultant squared distances to the neighboring
+ * points
+ * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
+ * \return number of neighbors found in radius
+ */
+ int
+ radiusSearch(const PointT& p_q,
+ const double radius,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances,
+ unsigned int max_nn = 0) const;
+
+ /** \brief Search for all neighbors of query point that are within a given radius.
+ * \param[in] index index representing the query point in the dataset given by \a
+ * setInputCloud. If indices were given in setInputCloud, index will be the position
+ * in the indices vector
+ * \param[in] radius radius of the sphere bounding all of p_q's neighbors
+ * \param[out] k_indices the resultant indices of the neighboring points
+ * \param[out] k_sqr_distances the resultant squared distances to the neighboring
+ * points
+ * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
+ * \return number of neighbors found in radius
+ */
+ int
+ radiusSearch(int index,
+ const double radius,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances,
+ unsigned int max_nn = 0) const;
+
+ /** \brief Get a PointT vector of centers of all voxels that intersected by a ray
+ * (origin, direction).
+ * \param[in] origin ray origin
+ * \param[in] direction ray direction vector
+ * \param[out] voxel_center_list results are written to this vector of PointT elements
+ * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
+ * disable)
+ * \return number of intersected voxels
+ */
+ int
+ getIntersectedVoxelCenters(Eigen::Vector3f origin,
+ Eigen::Vector3f direction,
+ AlignedPointTVector& voxel_center_list,
+ int max_voxel_count = 0) const;
+
+ /** \brief Get indices of all voxels that are intersected by a ray (origin,
+ * direction).
+ * \param[in] origin ray origin \param[in] direction ray direction vector
+ * \param[out] k_indices resulting point indices from intersected voxels
+ * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
+ * disable)
+ * \return number of intersected voxels
+ */
+ int
+ getIntersectedVoxelIndices(Eigen::Vector3f origin,
+ Eigen::Vector3f direction,
+ std::vector<int>& k_indices,
+ int max_voxel_count = 0) const;
+
+ /** \brief Search for points within rectangular search area
+ * Points exactly on the edges of the search rectangle are included.
+ * \param[in] min_pt lower corner of search area
+ * \param[in] max_pt upper corner of search area
+ * \param[out] k_indices the resultant point indices
+ * \return number of points found within search area
+ */
+ int
+ boxSearch(const Eigen::Vector3f& min_pt,
+ const Eigen::Vector3f& max_pt,
+ std::vector<int>& k_indices) const;
+
+protected:
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Octree-based search routines & helpers
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b Priority queue entry for branch nodes
+ * \note This class defines priority queue entries for the nearest neighbor search.
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+ class prioBranchQueueEntry {
+ public:
+ /** \brief Empty constructor */
+ prioBranchQueueEntry() : node(), point_distance(0) {}
+
+ /** \brief Constructor for initializing priority queue entry.
+ * \param _node pointer to octree node
+ * \param _key octree key addressing voxel in octree structure
+ * \param[in] _point_distance distance of query point to voxel center
+ */
+ prioBranchQueueEntry(OctreeNode* _node, OctreeKey& _key, float _point_distance)
+ : node(_node), point_distance(_point_distance), key(_key)
+ {}
+
+ /** \brief Operator< for comparing priority queue entries with each other.
+ * \param[in] rhs the priority queue to compare this against
+ */
+ bool
+ operator<(const prioBranchQueueEntry rhs) const
{
- public:
- // public typedefs
- using IndicesPtr = shared_ptr<std::vector<int> >;
- using IndicesConstPtr = shared_ptr<const std::vector<int> >;
-
- using PointCloud = pcl::PointCloud<PointT>;
- using PointCloudPtr = typename PointCloud::Ptr;
- using PointCloudConstPtr = typename PointCloud::ConstPtr;
-
- // Boost shared pointers
- using Ptr = shared_ptr<OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> >;
- using ConstPtr = shared_ptr<const OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> >;
-
- // Eigen aligned allocator
- using AlignedPointTVector = std::vector<PointT, Eigen::aligned_allocator<PointT> >;
-
- using OctreeT = OctreePointCloud<PointT, LeafContainerT, BranchContainerT>;
- using LeafNode = typename OctreeT::LeafNode;
- using BranchNode = typename OctreeT::BranchNode;
-
- /** \brief Constructor.
- * \param[in] resolution octree resolution at lowest octree level
- */
- OctreePointCloudSearch (const double resolution) :
- OctreePointCloud<PointT, LeafContainerT, BranchContainerT> (resolution)
- {
- }
-
- /** \brief Search for neighbors within a voxel at given point
- * \param[in] point point addressing a leaf node voxel
- * \param[out] point_idx_data the resultant indices of the neighboring voxel points
- * \return "true" if leaf node exist; "false" otherwise
- */
- bool
- voxelSearch (const PointT& point, std::vector<int>& point_idx_data);
-
- /** \brief Search for neighbors within a voxel at given point referenced by a point index
- * \param[in] index the index in input cloud defining the query point
- * \param[out] point_idx_data the resultant indices of the neighboring voxel points
- * \return "true" if leaf node exist; "false" otherwise
- */
- bool
- voxelSearch (const int index, std::vector<int>& point_idx_data);
-
- /** \brief Search for k-nearest neighbors at the query point.
- * \param[in] cloud the point cloud data
- * \param[in] index the index in \a cloud representing the query point
- * \param[in] k the number of neighbors to search for
- * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
- * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
- * a priori!)
- * \return number of neighbors found
- */
- inline int
- nearestKSearch (const PointCloud &cloud, int index, int k, std::vector<int> &k_indices,
- std::vector<float> &k_sqr_distances)
- {
- return (nearestKSearch (cloud[index], k, k_indices, k_sqr_distances));
- }
-
- /** \brief Search for k-nearest neighbors at given query point.
- * \param[in] p_q the given query point
- * \param[in] k the number of neighbors to search for
- * \param[out] k_indices the resultant indices of the neighboring points (must be resized to k a priori!)
- * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to k a priori!)
- * \return number of neighbors found
- */
- int
- nearestKSearch (const PointT &p_q, int k, std::vector<int> &k_indices,
- std::vector<float> &k_sqr_distances);
-
- /** \brief Search for k-nearest neighbors at query point
- * \param[in] index index representing the query point in the dataset given by \a setInputCloud.
- * If indices were given in setInputCloud, index will be the position in the indices vector.
- * \param[in] k the number of neighbors to search for
- * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
- * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
- * a priori!)
- * \return number of neighbors found
- */
- int
- nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances);
-
- /** \brief Search for approx. nearest neighbor at the query point.
- * \param[in] cloud the point cloud data
- * \param[in] query_index the index in \a cloud representing the query point
- * \param[out] result_index the resultant index of the neighbor point
- * \param[out] sqr_distance the resultant squared distance to the neighboring point
- * \return number of neighbors found
- */
- inline void
- approxNearestSearch (const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
- {
- return (approxNearestSearch (cloud.points[query_index], result_index, sqr_distance));
- }
-
- /** \brief Search for approx. nearest neighbor at the query point.
- * \param[in] p_q the given query point
- * \param[out] result_index the resultant index of the neighbor point
- * \param[out] sqr_distance the resultant squared distance to the neighboring point
- */
- void
- approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance);
-
- /** \brief Search for approx. nearest neighbor at the query point.
- * \param[in] query_index index representing the query point in the dataset given by \a setInputCloud.
- * If indices were given in setInputCloud, index will be the position in the indices vector.
- * \param[out] result_index the resultant index of the neighbor point
- * \param[out] sqr_distance the resultant squared distance to the neighboring point
- * \return number of neighbors found
- */
- void
- approxNearestSearch (int query_index, int &result_index, float &sqr_distance);
-
- /** \brief Search for all neighbors of query point that are within a given radius.
- * \param[in] cloud the point cloud data
- * \param[in] index the index in \a cloud representing the query point
- * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
- * \param[out] k_indices the resultant indices of the neighboring points
- * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
- * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
- * \return number of neighbors found in radius
- */
- int
- radiusSearch (const PointCloud &cloud, int index, double radius, std::vector<int> &k_indices,
- std::vector<float> &k_sqr_distances, unsigned int max_nn = 0)
- {
- return (radiusSearch (cloud.points[index], radius, k_indices, k_sqr_distances, max_nn));
- }
-
- /** \brief Search for all neighbors of query point that are within a given radius.
- * \param[in] p_q the given query point
- * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
- * \param[out] k_indices the resultant indices of the neighboring points
- * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
- * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
- * \return number of neighbors found in radius
- */
- int
- radiusSearch (const PointT &p_q, const double radius, std::vector<int> &k_indices,
- std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
-
- /** \brief Search for all neighbors of query point that are within a given radius.
- * \param[in] index index representing the query point in the dataset given by \a setInputCloud.
- * If indices were given in setInputCloud, index will be the position in the indices vector
- * \param[in] radius radius of the sphere bounding all of p_q's neighbors
- * \param[out] k_indices the resultant indices of the neighboring points
- * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
- * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
- * \return number of neighbors found in radius
- */
- int
- radiusSearch (int index, const double radius, std::vector<int> &k_indices,
- std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
-
- /** \brief Get a PointT vector of centers of all voxels that intersected by a ray (origin, direction).
- * \param[in] origin ray origin
- * \param[in] direction ray direction vector
- * \param[out] voxel_center_list results are written to this vector of PointT elements
- * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
- * \return number of intersected voxels
- */
- int
- getIntersectedVoxelCenters (Eigen::Vector3f origin, Eigen::Vector3f direction,
- AlignedPointTVector &voxel_center_list, int max_voxel_count = 0) const;
-
- /** \brief Get indices of all voxels that are intersected by a ray (origin, direction).
- * \param[in] origin ray origin
- * \param[in] direction ray direction vector
- * \param[out] k_indices resulting point indices from intersected voxels
- * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
- * \return number of intersected voxels
- */
- int
- getIntersectedVoxelIndices (Eigen::Vector3f origin, Eigen::Vector3f direction,
- std::vector<int> &k_indices,
- int max_voxel_count = 0) const;
-
-
- /** \brief Search for points within rectangular search area
- * Points exactly on the edges of the search rectangle are included.
- * \param[in] min_pt lower corner of search area
- * \param[in] max_pt upper corner of search area
- * \param[out] k_indices the resultant point indices
- * \return number of points found within search area
- */
- int
- boxSearch (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector<int> &k_indices) const;
-
- protected:
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // Octree-based search routines & helpers
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Priority queue entry for branch nodes
- * \note This class defines priority queue entries for the nearest neighbor search.
- * \author Julius Kammerl (julius@kammerl.de)
- */
- class prioBranchQueueEntry
- {
- public:
- /** \brief Empty constructor */
- prioBranchQueueEntry () :
- node (), point_distance (0)
- {
- }
-
- /** \brief Constructor for initializing priority queue entry.
- * \param _node pointer to octree node
- * \param _key octree key addressing voxel in octree structure
- * \param[in] _point_distance distance of query point to voxel center
- */
- prioBranchQueueEntry (OctreeNode* _node, OctreeKey& _key, float _point_distance) :
- node (_node), point_distance (_point_distance), key (_key)
- {
- }
-
- /** \brief Operator< for comparing priority queue entries with each other.
- * \param[in] rhs the priority queue to compare this against
- */
- bool
- operator < (const prioBranchQueueEntry rhs) const
- {
- return (this->point_distance > rhs.point_distance);
- }
-
- /** \brief Pointer to octree node. */
- const OctreeNode* node;
-
- /** \brief Distance to query point. */
- float point_distance;
-
- /** \brief Octree key. */
- OctreeKey key;
- };
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief @b Priority queue entry for point candidates
- * \note This class defines priority queue entries for the nearest neighbor point candidates.
- * \author Julius Kammerl (julius@kammerl.de)
- */
- class prioPointQueueEntry
- {
- public:
-
- /** \brief Empty constructor */
- prioPointQueueEntry () :
- point_idx_ (0), point_distance_ (0)
- {
- }
-
- /** \brief Constructor for initializing priority queue entry.
- * \param[in] point_idx an index representing a point in the dataset given by \a setInputCloud
- * \param[in] point_distance distance of query point to voxel center
- */
- prioPointQueueEntry (unsigned int& point_idx, float point_distance) :
- point_idx_ (point_idx), point_distance_ (point_distance)
- {
- }
-
- /** \brief Operator< for comparing priority queue entries with each other.
- * \param[in] rhs priority queue to compare this against
- */
- bool
- operator< (const prioPointQueueEntry& rhs) const
- {
- return (this->point_distance_ < rhs.point_distance_);
- }
-
- /** \brief Index representing a point in the dataset given by \a setInputCloud. */
- int point_idx_;
-
- /** \brief Distance to query point. */
- float point_distance_;
- };
-
- /** \brief Helper function to calculate the squared distance between two points
- * \param[in] point_a point A
- * \param[in] point_b point B
- * \return squared distance between point A and point B
- */
- float
- pointSquaredDist (const PointT& point_a, const PointT& point_b) const;
-
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // Recursive search routine methods
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- /** \brief Recursive search method that explores the octree and finds neighbors within a given radius
- * \param[in] point query point
- * \param[in] radiusSquared squared search radius
- * \param[in] node current octree node to be explored
- * \param[in] key octree key addressing a leaf node.
- * \param[in] tree_depth current depth/level in the octree
- * \param[out] k_indices vector of indices found to be neighbors of query point
- * \param[out] k_sqr_distances squared distances of neighbors to query point
- * \param[in] max_nn maximum of neighbors to be found
- */
- void
- getNeighborsWithinRadiusRecursive (const PointT& point, const double radiusSquared,
- const BranchNode* node, const OctreeKey& key,
- unsigned int tree_depth, std::vector<int>& k_indices,
- std::vector<float>& k_sqr_distances, unsigned int max_nn) const;
-
- /** \brief Recursive search method that explores the octree and finds the K nearest neighbors
- * \param[in] point query point
- * \param[in] K amount of nearest neighbors to be found
- * \param[in] node current octree node to be explored
- * \param[in] key octree key addressing a leaf node.
- * \param[in] tree_depth current depth/level in the octree
- * \param[in] squared_search_radius squared search radius distance
- * \param[out] point_candidates priority queue of nearest neigbor point candidates
- * \return squared search radius based on current point candidate set found
- */
- double
- getKNearestNeighborRecursive (const PointT& point, unsigned int K, const BranchNode* node,
- const OctreeKey& key, unsigned int tree_depth,
- const double squared_search_radius,
- std::vector<prioPointQueueEntry>& point_candidates) const;
-
- /** \brief Recursive search method that explores the octree and finds the approximate nearest neighbor
- * \param[in] point query point
- * \param[in] node current octree node to be explored
- * \param[in] key octree key addressing a leaf node.
- * \param[in] tree_depth current depth/level in the octree
- * \param[out] result_index result index is written to this reference
- * \param[out] sqr_distance squared distance to search
- */
- void
- approxNearestSearchRecursive (const PointT& point, const BranchNode* node, const OctreeKey& key,
- unsigned int tree_depth, int& result_index, float& sqr_distance);
-
- /** \brief Recursively search the tree for all intersected leaf nodes and return a vector of voxel centers.
- * This algorithm is based off the paper An Efficient Parametric Algorithm for Octree Traversal:
- * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
- * \param[in] min_x octree nodes X coordinate of lower bounding box corner
- * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
- * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
- * \param[in] max_x octree nodes X coordinate of upper bounding box corner
- * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
- * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
- * \param[in] a
- * \param[in] node current octree node to be explored
- * \param[in] key octree key addressing a leaf node.
- * \param[out] voxel_center_list results are written to this vector of PointT elements
- * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
- * \return number of voxels found
- */
- int
- getIntersectedVoxelCentersRecursive (double min_x, double min_y, double min_z, double max_x, double max_y,
- double max_z, unsigned char a, const OctreeNode* node,
- const OctreeKey& key, AlignedPointTVector &voxel_center_list,
- int max_voxel_count) const;
-
-
- /** \brief Recursive search method that explores the octree and finds points within a rectangular search area
- * \param[in] min_pt lower corner of search area
- * \param[in] max_pt upper corner of search area
- * \param[in] node current octree node to be explored
- * \param[in] key octree key addressing a leaf node.
- * \param[in] tree_depth current depth/level in the octree
- * \param[out] k_indices the resultant point indices
- */
- void
- boxSearchRecursive (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode* node,
- const OctreeKey& key, unsigned int tree_depth, std::vector<int>& k_indices) const;
-
- /** \brief Recursively search the tree for all intersected leaf nodes and return a vector of indices.
- * This algorithm is based off the paper An Efficient Parametric Algorithm for Octree Traversal:
- * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
- * \param[in] min_x octree nodes X coordinate of lower bounding box corner
- * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
- * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
- * \param[in] max_x octree nodes X coordinate of upper bounding box corner
- * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
- * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
- * \param[in] a
- * \param[in] node current octree node to be explored
- * \param[in] key octree key addressing a leaf node.
- * \param[out] k_indices resulting indices
- * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
- * \return number of voxels found
- */
- int
- getIntersectedVoxelIndicesRecursive (double min_x, double min_y, double min_z,
- double max_x, double max_y, double max_z,
- unsigned char a, const OctreeNode* node, const OctreeKey& key,
- std::vector<int> &k_indices,
- int max_voxel_count) const;
-
- /** \brief Initialize raytracing algorithm
- * \param origin
- * \param direction
- * \param[in] min_x octree nodes X coordinate of lower bounding box corner
- * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
- * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
- * \param[in] max_x octree nodes X coordinate of upper bounding box corner
- * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
- * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
- * \param a
- */
- inline void
- initIntersectedVoxel (Eigen::Vector3f &origin, Eigen::Vector3f &direction,
- double &min_x, double &min_y, double &min_z,
- double &max_x, double &max_y, double &max_z,
- unsigned char &a) const
- {
- // Account for division by zero when direction vector is 0.0
- const float epsilon = 1e-10f;
- if (direction.x () == 0.0)
- direction.x () = epsilon;
- if (direction.y () == 0.0)
- direction.y () = epsilon;
- if (direction.z () == 0.0)
- direction.z () = epsilon;
-
- // Voxel childIdx remapping
- a = 0;
-
- // Handle negative axis direction vector
- if (direction.x () < 0.0)
- {
- origin.x () = static_cast<float> (this->min_x_) + static_cast<float> (this->max_x_) - origin.x ();
- direction.x () = -direction.x ();
- a |= 4;
- }
- if (direction.y () < 0.0)
- {
- origin.y () = static_cast<float> (this->min_y_) + static_cast<float> (this->max_y_) - origin.y ();
- direction.y () = -direction.y ();
- a |= 2;
- }
- if (direction.z () < 0.0)
- {
- origin.z () = static_cast<float> (this->min_z_) + static_cast<float> (this->max_z_) - origin.z ();
- direction.z () = -direction.z ();
- a |= 1;
- }
- min_x = (this->min_x_ - origin.x ()) / direction.x ();
- max_x = (this->max_x_ - origin.x ()) / direction.x ();
- min_y = (this->min_y_ - origin.y ()) / direction.y ();
- max_y = (this->max_y_ - origin.y ()) / direction.y ();
- min_z = (this->min_z_ - origin.z ()) / direction.z ();
- max_z = (this->max_z_ - origin.z ()) / direction.z ();
- }
-
- /** \brief Find first child node ray will enter
- * \param[in] min_x octree nodes X coordinate of lower bounding box corner
- * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
- * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
- * \param[in] mid_x octree nodes X coordinate of bounding box mid line
- * \param[in] mid_y octree nodes Y coordinate of bounding box mid line
- * \param[in] mid_z octree nodes Z coordinate of bounding box mid line
- * \return the first child node ray will enter
- */
- inline int
- getFirstIntersectedNode (double min_x, double min_y, double min_z, double mid_x, double mid_y, double mid_z) const
- {
- int currNode = 0;
-
- if (min_x > min_y)
- {
- if (min_x > min_z)
- {
- // max(min_x, min_y, min_z) is min_x. Entry plane is YZ.
- if (mid_y < min_x)
- currNode |= 2;
- if (mid_z < min_x)
- currNode |= 1;
- }
- else
- {
- // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
- if (mid_x < min_z)
- currNode |= 4;
- if (mid_y < min_z)
- currNode |= 2;
- }
- }
- else
- {
- if (min_y > min_z)
- {
- // max(min_x, min_y, min_z) is min_y. Entry plane is XZ.
- if (mid_x < min_y)
- currNode |= 4;
- if (mid_z < min_y)
- currNode |= 1;
- }
- else
- {
- // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
- if (mid_x < min_z)
- currNode |= 4;
- if (mid_y < min_z)
- currNode |= 2;
- }
- }
-
- return currNode;
- }
-
- /** \brief Get the next visited node given the current node upper
- * bounding box corner. This function accepts three float values, and
- * three int values. The function returns the ith integer where the
- * ith float value is the minimum of the three float values.
- * \param[in] x current nodes X coordinate of upper bounding box corner
- * \param[in] y current nodes Y coordinate of upper bounding box corner
- * \param[in] z current nodes Z coordinate of upper bounding box corner
- * \param[in] a next node if exit Plane YZ
- * \param[in] b next node if exit Plane XZ
- * \param[in] c next node if exit Plane XY
- * \return the next child node ray will enter or 8 if exiting
- */
- inline int
- getNextIntersectedNode (double x, double y, double z, int a, int b, int c) const
- {
- if (x < y)
- {
- if (x < z)
- return a;
- return c;
- }
- if (y < z)
- return b;
- return c;
- }
-
- };
+ return (this->point_distance > rhs.point_distance);
+ }
+
+ /** \brief Pointer to octree node. */
+ const OctreeNode* node;
+
+ /** \brief Distance to query point. */
+ float point_distance;
+
+ /** \brief Octree key. */
+ OctreeKey key;
+ };
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b Priority queue entry for point candidates
+ * \note This class defines priority queue entries for the nearest neighbor point
+ * candidates.
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+ class prioPointQueueEntry {
+ public:
+ /** \brief Empty constructor */
+ prioPointQueueEntry() : point_idx_(0), point_distance_(0) {}
+
+ /** \brief Constructor for initializing priority queue entry.
+ * \param[in] point_idx an index representing a point in the dataset given by \a
+ * setInputCloud \param[in] point_distance distance of query point to voxel center
+ */
+ prioPointQueueEntry(unsigned int& point_idx, float point_distance)
+ : point_idx_(point_idx), point_distance_(point_distance)
+ {}
+
+ /** \brief Operator< for comparing priority queue entries with each other.
+ * \param[in] rhs priority queue to compare this against
+ */
+ bool
+ operator<(const prioPointQueueEntry& rhs) const
+ {
+ return (this->point_distance_ < rhs.point_distance_);
+ }
+
+ /** \brief Index representing a point in the dataset given by \a setInputCloud. */
+ int point_idx_;
+
+ /** \brief Distance to query point. */
+ float point_distance_;
+ };
+
+ /** \brief Helper function to calculate the squared distance between two points
+ * \param[in] point_a point A
+ * \param[in] point_b point B
+ * \return squared distance between point A and point B
+ */
+ float
+ pointSquaredDist(const PointT& point_a, const PointT& point_b) const;
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Recursive search routine methods
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /** \brief Recursive search method that explores the octree and finds neighbors within
+ * a given radius
+ * \param[in] point query point \param[in] radiusSquared squared search radius
+ * \param[in] node current octree node to be explored
+ * \param[in] key octree key addressing a leaf node.
+ * \param[in] tree_depth current depth/level in the octree
+ * \param[out] k_indices vector of indices found to be neighbors of query point
+ * \param[out] k_sqr_distances squared distances of neighbors to query point
+ * \param[in] max_nn maximum of neighbors to be found
+ */
+ void
+ getNeighborsWithinRadiusRecursive(const PointT& point,
+ const double radiusSquared,
+ const BranchNode* node,
+ const OctreeKey& key,
+ unsigned int tree_depth,
+ std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances,
+ unsigned int max_nn) const;
+
+ /** \brief Recursive search method that explores the octree and finds the K nearest
+ * neighbors
+ * \param[in] point query point
+ * \param[in] K amount of nearest neighbors to be found
+ * \param[in] node current octree node to be explored
+ * \param[in] key octree key addressing a leaf node.
+ * \param[in] tree_depth current depth/level in the octree
+ * \param[in] squared_search_radius squared search radius distance
+ * \param[out] point_candidates priority queue of nearest neigbor point candidates
+ * \return squared search radius based on current point candidate set found
+ */
+ double
+ getKNearestNeighborRecursive(
+ const PointT& point,
+ unsigned int K,
+ const BranchNode* node,
+ const OctreeKey& key,
+ unsigned int tree_depth,
+ const double squared_search_radius,
+ std::vector<prioPointQueueEntry>& point_candidates) const;
+
+ /** \brief Recursive search method that explores the octree and finds the approximate
+ * nearest neighbor
+ * \param[in] point query point
+ * \param[in] node current octree node to be explored
+ * \param[in] key octree key addressing a leaf node.
+ * \param[in] tree_depth current depth/level in the octree
+ * \param[out] result_index result index is written to this reference
+ * \param[out] sqr_distance squared distance to search
+ */
+ void
+ approxNearestSearchRecursive(const PointT& point,
+ const BranchNode* node,
+ const OctreeKey& key,
+ unsigned int tree_depth,
+ int& result_index,
+ float& sqr_distance);
+
+ /** \brief Recursively search the tree for all intersected leaf nodes and return a
+ * vector of voxel centers. This algorithm is based off the paper An Efficient
+ * Parametric Algorithm for Octree Traversal:
+ * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
+ * \param[in] min_x octree nodes X coordinate of lower bounding box corner
+ * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
+ * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
+ * \param[in] max_x octree nodes X coordinate of upper bounding box corner
+ * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
+ * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
+ * \param[in] a
+ * \param[in] node current octree node to be explored
+ * \param[in] key octree key addressing a leaf node.
+ * \param[out] voxel_center_list results are written to this vector of PointT elements
+ * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
+ * disable)
+ * \return number of voxels found
+ */
+ int
+ getIntersectedVoxelCentersRecursive(double min_x,
+ double min_y,
+ double min_z,
+ double max_x,
+ double max_y,
+ double max_z,
+ unsigned char a,
+ const OctreeNode* node,
+ const OctreeKey& key,
+ AlignedPointTVector& voxel_center_list,
+ int max_voxel_count) const;
+
+ /** \brief Recursive search method that explores the octree and finds points within a
+ * rectangular search area
+ * \param[in] min_pt lower corner of search area
+ * \param[in] max_pt upper corner of search area
+ * \param[in] node current octree node to be explored
+ * \param[in] key octree key addressing a leaf node.
+ * \param[in] tree_depth current depth/level in the octree
+ * \param[out] k_indices the resultant point indices
+ */
+ void
+ boxSearchRecursive(const Eigen::Vector3f& min_pt,
+ const Eigen::Vector3f& max_pt,
+ const BranchNode* node,
+ const OctreeKey& key,
+ unsigned int tree_depth,
+ std::vector<int>& k_indices) const;
+
+ /** \brief Recursively search the tree for all intersected leaf nodes and return a
+ * vector of indices. This algorithm is based off the paper An Efficient Parametric
+ * Algorithm for Octree Traversal: http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
+ * \param[in] min_x octree nodes X coordinate of lower bounding box corner
+ * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
+ * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
+ * \param[in] max_x octree nodes X coordinate of upper bounding box corner
+ * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
+ * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
+ * \param[in] a
+ * \param[in] node current octree node to be explored
+ * \param[in] key octree key addressing a leaf node.
+ * \param[out] k_indices resulting indices
+ * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0:
+ * disable)
+ * \return number of voxels found
+ */
+ int
+ getIntersectedVoxelIndicesRecursive(double min_x,
+ double min_y,
+ double min_z,
+ double max_x,
+ double max_y,
+ double max_z,
+ unsigned char a,
+ const OctreeNode* node,
+ const OctreeKey& key,
+ std::vector<int>& k_indices,
+ int max_voxel_count) const;
+
+ /** \brief Initialize raytracing algorithm
+ * \param origin
+ * \param direction
+ * \param[in] min_x octree nodes X coordinate of lower bounding box corner
+ * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
+ * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
+ * \param[in] max_x octree nodes X coordinate of upper bounding box corner
+ * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
+ * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
+ * \param a
+ */
+ inline void
+ initIntersectedVoxel(Eigen::Vector3f& origin,
+ Eigen::Vector3f& direction,
+ double& min_x,
+ double& min_y,
+ double& min_z,
+ double& max_x,
+ double& max_y,
+ double& max_z,
+ unsigned char& a) const
+ {
+ // Account for division by zero when direction vector is 0.0
+ const float epsilon = 1e-10f;
+ if (direction.x() == 0.0)
+ direction.x() = epsilon;
+ if (direction.y() == 0.0)
+ direction.y() = epsilon;
+ if (direction.z() == 0.0)
+ direction.z() = epsilon;
+
+ // Voxel childIdx remapping
+ a = 0;
+
+ // Handle negative axis direction vector
+ if (direction.x() < 0.0) {
+ origin.x() = static_cast<float>(this->min_x_) + static_cast<float>(this->max_x_) -
+ origin.x();
+ direction.x() = -direction.x();
+ a |= 4;
+ }
+ if (direction.y() < 0.0) {
+ origin.y() = static_cast<float>(this->min_y_) + static_cast<float>(this->max_y_) -
+ origin.y();
+ direction.y() = -direction.y();
+ a |= 2;
+ }
+ if (direction.z() < 0.0) {
+ origin.z() = static_cast<float>(this->min_z_) + static_cast<float>(this->max_z_) -
+ origin.z();
+ direction.z() = -direction.z();
+ a |= 1;
+ }
+ min_x = (this->min_x_ - origin.x()) / direction.x();
+ max_x = (this->max_x_ - origin.x()) / direction.x();
+ min_y = (this->min_y_ - origin.y()) / direction.y();
+ max_y = (this->max_y_ - origin.y()) / direction.y();
+ min_z = (this->min_z_ - origin.z()) / direction.z();
+ max_z = (this->max_z_ - origin.z()) / direction.z();
+ }
+
+ /** \brief Find first child node ray will enter
+ * \param[in] min_x octree nodes X coordinate of lower bounding box corner
+ * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
+ * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
+ * \param[in] mid_x octree nodes X coordinate of bounding box mid line
+ * \param[in] mid_y octree nodes Y coordinate of bounding box mid line
+ * \param[in] mid_z octree nodes Z coordinate of bounding box mid line
+ * \return the first child node ray will enter
+ */
+ inline int
+ getFirstIntersectedNode(double min_x,
+ double min_y,
+ double min_z,
+ double mid_x,
+ double mid_y,
+ double mid_z) const
+ {
+ int currNode = 0;
+
+ if (min_x > min_y) {
+ if (min_x > min_z) {
+ // max(min_x, min_y, min_z) is min_x. Entry plane is YZ.
+ if (mid_y < min_x)
+ currNode |= 2;
+ if (mid_z < min_x)
+ currNode |= 1;
+ }
+ else {
+ // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
+ if (mid_x < min_z)
+ currNode |= 4;
+ if (mid_y < min_z)
+ currNode |= 2;
+ }
+ }
+ else {
+ if (min_y > min_z) {
+ // max(min_x, min_y, min_z) is min_y. Entry plane is XZ.
+ if (mid_x < min_y)
+ currNode |= 4;
+ if (mid_z < min_y)
+ currNode |= 1;
+ }
+ else {
+ // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
+ if (mid_x < min_z)
+ currNode |= 4;
+ if (mid_y < min_z)
+ currNode |= 2;
+ }
+ }
+
+ return currNode;
+ }
+
+ /** \brief Get the next visited node given the current node upper
+ * bounding box corner. This function accepts three float values, and
+ * three int values. The function returns the ith integer where the
+ * ith float value is the minimum of the three float values.
+ * \param[in] x current nodes X coordinate of upper bounding box corner
+ * \param[in] y current nodes Y coordinate of upper bounding box corner
+ * \param[in] z current nodes Z coordinate of upper bounding box corner
+ * \param[in] a next node if exit Plane YZ
+ * \param[in] b next node if exit Plane XZ
+ * \param[in] c next node if exit Plane XY
+ * \return the next child node ray will enter or 8 if exiting
+ */
+ inline int
+ getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const
+ {
+ if (x < y) {
+ if (x < z)
+ return a;
+ return c;
+ }
+ if (y < z)
+ return b;
+ return c;
}
-}
+};
+} // namespace octree
+} // namespace pcl
#ifdef PCL_NO_PRECOMPILE
#include <pcl/octree/impl/octree_search.hpp>
template class PCL_EXPORTS pcl::octree::OctreeBase<int>;
template class PCL_EXPORTS pcl::octree::Octree2BufBase<int>;
+template class PCL_EXPORTS
+ pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndices,
+ pcl::octree::OctreeContainerEmpty>;
-template class PCL_EXPORTS pcl::octree::OctreeBase<
- pcl::octree::OctreeContainerPointIndices,
- pcl::octree::OctreeContainerEmpty >;
+template class PCL_EXPORTS
+ pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndices,
+ pcl::octree::OctreeContainerEmpty>;
-template class PCL_EXPORTS pcl::octree::Octree2BufBase<
- pcl::octree::OctreeContainerPointIndices,
- pcl::octree::OctreeContainerEmpty >;
-
-template class PCL_EXPORTS pcl::octree::OctreeBase<
- pcl::octree::OctreeContainerEmpty,
- pcl::octree::OctreeContainerEmpty >;
+template class PCL_EXPORTS pcl::octree::OctreeBase<pcl::octree::OctreeContainerEmpty,
+ pcl::octree::OctreeContainerEmpty>;
#ifndef PCL_NO_PRECOMPILE
#include <pcl/impl/instantiate.hpp>
PCL_INSTANTIATE(OctreePointCloudSearch, PCL_XYZ_POINT_TYPES)
-
// PCL_INSTANTIATE(OctreePointCloudSingleBufferWithLeafDataT, PCL_XYZ_POINT_TYPES)
PCL_INSTANTIATE(OctreePointCloudSingleBufferWithEmptyLeaf, PCL_XYZ_POINT_TYPES)
// PCL_INSTANTIATE(OctreePointCloudVoxelCentroid, PCL_XYZ_POINT_TYPES)
// PCL_INSTANTIATE(OctreePointCloudAdjacency, PCL_XYZ_POINT_TYPES)
-#endif // PCL_NO_PRECOMPILE
-
+#endif // PCL_NO_PRECOMPILE
#ifndef __PCL_OUTOFCORE_LRU_CACHE__
#define __PCL_OUTOFCORE_LRU_CACHE__
+#include <cstddef>
#include <cassert>
#include <list>
+#include <map>
+#include <utility>
template<typename T>
class LRUCacheItem
}
size -= tail_size;
- key_it++;
- evict_count++;
+ ++key_it;
+ ++evict_count;
}
// Evict enough items to make room for the new item
Eigen::Vector4f origin;
Eigen::Quaternionf orientation;
- int pcd_version;
-
+
if (boost::filesystem::exists (disk_storage_filename_))
{
// PCL_INFO ("[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] Reading points from disk from %s.\n", __FUNCTION__ , disk_storage_filename_->c_str ());
+ int pcd_version;
int res = reader.read (disk_storage_filename_, *dst, origin, orientation, pcd_version);
(void)res;
assert (res != -1);
void
OutofcoreCloud::pcdReaderThread ()
{
-
- std::string pcd_file;
std::size_t timestamp = 0;
while (true)
+set(SUBSYS_NAME tools)
+
# pcl_outofcore_process
PCL_ADD_EXECUTABLE(pcl_outofcore_process COMPONENT ${SUBSYS_NAME} SOURCES outofcore_process.cpp)
target_link_libraries(pcl_outofcore_process pcl_common pcl_filters pcl_io pcl_octree pcl_outofcore)
{
used_clusters[cluster] = true;
for(std::vector<int>::const_iterator points_iterator = input_clusters[cluster].getIndices().indices.begin();
- points_iterator != input_clusters[cluster].getIndices().indices.end(); points_iterator++)
+ points_iterator != input_clusters[cluster].getIndices().indices.end(); ++points_iterator)
{
point_indices.indices.push_back(*points_iterator);
}
}
// Associate cluster points to one of the maximum:
- for(std::vector<int>::const_iterator points_iterator = cluster.getIndices().indices.begin(); points_iterator != cluster.getIndices().indices.end(); points_iterator++)
+ for(std::vector<int>::const_iterator points_iterator = cluster.getIndices().indices.begin(); points_iterator != cluster.getIndices().indices.end(); ++points_iterator)
{
PointT* current_point = &cloud_->points[*points_iterator]; // current point cloud point
Eigen::Vector3f p_current_eigen(current_point->x, current_point->y, current_point->z); // conversion to eigen
buckets_.resize(std::size_t((cluster.getMax()(1) - cluster.getMin()(1)) / bin_size_) + 1, 0);
buckets_cloud_indices_.resize(buckets_.size(), 0);
- for(std::vector<int>::const_iterator pit = cluster.getIndices().indices.begin(); pit != cluster.getIndices().indices.end(); pit++)
+ for(std::vector<int>::const_iterator pit = cluster.getIndices().indices.begin(); pit != cluster.getIndices().indices.end(); ++pit)
{
PointT* p = &cloud_->points[*pit];
int index;
// Search for local maxima:
maxima_number_ = 0;
int left = buckets_[0]; // current left element
- int right = 0; // current right element
float offset = 0; // used to center the maximum to the right place
maxima_indices_.resize(std::size_t(buckets_.size()), 0);
maxima_cloud_indices_.resize(std::size_t(buckets_.size()), 0);
int i = 1;
while (i < (static_cast<int> (buckets_.size()) - 1))
{
- right = buckets_[i+1];
+ int right = buckets_[i+1]; // current right element
if ((buckets_[i] > left) && (buckets_[i] > right))
{
// Search where to insert the new element (in an ordered array):
for (int i = 0; i < maxima_number_; i++)
{
bool good_maximum = true;
- float distance = 0;
PointT* p_current = &cloud_->points[maxima_cloud_indices_[i]]; // pointcloud point referring to the current maximum
Eigen::Vector3f p_current_eigen(p_current->x, p_current->y, p_current->z); // conversion to eigen
p_previous_eigen -= ground_coeffs_.head(3) * t; // projection of the point on the groundplane
// distance of the projection of the points on the groundplane:
- distance = (p_current_eigen-p_previous_eigen).norm();
+ float distance = (p_current_eigen-p_previous_eigen).norm();
if (distance < min_dist_between_maxima_)
{
good_maximum = false;
getline (SVM_file,line); // read SVM_weights line
tok_pos = line.find_first_of('[', 0); // search for token "["
std::size_t tok_end_pos = line.find_first_of(']', 0); // search for token "]" , end of SVM weights
- std::size_t prev_tok_pos;
while (tok_pos < tok_end_pos) // while end of SVM_weights is not reached
{
- prev_tok_pos = tok_pos;
+ std::size_t prev_tok_pos = tok_pos;
tok_pos = line.find_first_of(',', prev_tok_pos+1); // search for token ","
SVM_weights_.push_back(std::atof(line.substr(prev_tok_pos+1, tok_pos-prev_tok_pos-1).c_str()));
}
float min_z = c_z_;
float max_x = c_x_;
float max_z = c_z_;
- for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++)
+ for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); ++pit)
{
PointT* p = &input_cloud->points[*pit];
float min_z = c_z_;
float max_y = c_y_;
float max_z = c_z_;
- for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++)
+ for (std::vector<int>::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); ++pit)
{
PointT* p = &input_cloud->points[*pit];
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, Inc.
*
- * All rights reserved.
+ * All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
#include <pcl/point_types.h>
#include <pcl/recognition/point_types.h>
+#include <algorithm>
+#include <cstddef>
+#include <list>
+#include <vector>
namespace pcl
{
std::vector<int> explained_indices;
std::vector<float> outliers_weight;
std::vector<float> explained_indices_distances;
- std::vector<float> unexplained_indices_weights;
std::vector<int> nn_indices;
std::vector<float> nn_distances;
#ifndef SIMPLE_OCTREE_HPP_
#define SIMPLE_OCTREE_HPP_
+#include <algorithm>
+#include <cmath>
+
//===============================================================================================================================
template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
}
Node* node = root_;
- const Scalar *c;
- int id;
// Go down to the right leaf
for ( int l = 0 ; l < tree_levels_ ; ++l )
{
node->createChildren ();
- c = node->getCenter ();
- id = 0;
+ const Scalar *c = node->getCenter ();
+ int id = 0;
if ( x >= c[0] ) id |= 4;
if ( y >= c[1] ) id |= 2;
}
Node* node = root_;
- const Scalar *c;
- int id;
// Go down to the right leaf
for ( int l = 0 ; l < tree_levels_ ; ++l )
if ( !node->hasChildren () )
return (nullptr);
- c = node->getCenter ();
- id = 0;
+ const Scalar *c = node->getCenter ();
+ int id = 0;
if ( x >= c[0] ) id |= 4;
if ( y >= c[1] ) id |= 2;
return (data_.data());
}
- [[deprecated("Use new version diff getDifferenceMask(mask0, mask1)")]]
+ PCL_DEPRECATED("Use new version diff getDifferenceMask(mask0, mask1)")
static void
getDifferenceMask(const MaskMap& mask0, const MaskMap& mask1, MaskMap& diff_mask);
#pragma once
+#include <algorithm>
+#include <cstddef>
+#include <list>
+#include <set>
#include <vector>
namespace pcl
#pragma once
#include <pcl/pcl_exports.h>
+
#include <set>
+#include <vector>
namespace pcl
{
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, Inc.
*
- * All rights reserved.
+ * All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
#include <pcl/point_types.h>
#include <pcl/features/linear_least_squares_normal.h>
+#include <algorithm>
+#include <cmath>
+#include <cstddef>
+// #include <iostream>
+#include <limits>
+#include <list>
+#include <vector>
+
namespace pcl
{
target_indices[i] = original_correspondences[i].index_match;
}
- // from pcl/registration/icp.hpp:
- std::vector<int> source_indices_good;
- std::vector<int> target_indices_good;
{
// From the set of correspondences found, attempt to remove outliers
// Create the registration model
target_indices[i] = original_correspondences[i].index_match;
}
- // from pcl/registration/icp.hpp:
- std::vector<int> source_indices_good;
- std::vector<int> target_indices_good;
-
// From the set of correspondences found, attempt to remove outliers
typename pcl::SampleConsensusModelRegistration2D<PointT>::Ptr model (new pcl::SampleConsensusModelRegistration2D<PointT> (input_, source_indices));
// Pass the target_indices
* $Id$
*
*/
-#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_TYPES_H_
-#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_TYPES_H_
-#include <limits>
+#pragma once
+
#include <pcl/registration/eigen.h>
-//////////////////////////////////////////////////////////////////////////////////////////
+#include <cstddef>
+#include <vector>
+
inline void
pcl::registration::getCorDistMeanStd (const pcl::Correspondences &correspondences, double &mean, double &stddev)
{
stddev = sqrt (variance);
}
-//////////////////////////////////////////////////////////////////////////////////////////
inline void
pcl::registration::getQueryIndices (const pcl::Correspondences& correspondences, std::vector<int>& indices)
{
indices[i] = correspondences[i].index_query;
}
-//////////////////////////////////////////////////////////////////////////////////////////
+
inline void
pcl::registration::getMatchIndices (const pcl::Correspondences& correspondences, std::vector<int>& indices)
{
for (std::size_t i = 0; i < correspondences.size (); ++i)
indices[i] = correspondences[i].index_match;
}
-
-#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_TYPES_H_ */
int *p_min = new int[num_vertices (g)];
double *d = new double[num_vertices (g)];
double *d_min = new double[num_vertices (g)];
- double dist;
bool do_swap = false;
std::list<int>::iterator start_min, end_min;
// process all junctions
while (!crossings.empty ())
{
- dist = -1;
+ double dist = -1;
// find shortest crossing for all vertices on the loop
for (auto crossings_it = crossings.begin (); crossings_it != crossings.end (); )
{
delete[] d_min;
boost::graph_traits<LOAGraph>::adjacency_iterator adjacent_it, adjacent_it_end;
- int s;
// error propagation
while (!branches.empty ())
{
- s = branches.front ();
+ int s = branches.front ();
branches.pop_front ();
for (std::tie (adjacent_it, adjacent_it_end) = adjacent_vertices (s, g); adjacent_it != adjacent_it_end; ++adjacent_it)
MatchingCandidates candidates (1);
std::vector <int> base_indices (4);
- float ratio[2];
all_candidates[i] = candidates;
if (!abort)
{
+ float ratio[2];
// select four coplanar point base
if (selectBase (base_indices, ratio) == 0)
{
std::vector <int> temp (base_indices.begin (), base_indices.end ());
// loop over all combinations of base points
- for (std::vector <int>::const_iterator i = copy.begin (), i_e = copy.end (); i != i_e; i++)
- for (std::vector <int>::const_iterator j = copy.begin (), j_e = copy.end (); j != j_e; j++)
+ for (std::vector <int>::const_iterator i = copy.begin (), i_e = copy.end (); i != i_e; ++i)
+ for (std::vector <int>::const_iterator j = copy.begin (), j_e = copy.end (); j != j_e; ++j)
{
if (i == j)
continue;
- for (std::vector <int>::const_iterator k = copy.begin (), k_e = copy.end (); k != k_e; k++)
+ for (std::vector <int>::const_iterator k = copy.begin (), k_e = copy.end (); k != k_e; ++k)
{
if (k == j || k == i)
continue;
std::vector <int>::const_iterator l = copy.begin ();
while (l == i || l == j || l == k)
- l++;
+ ++l;
temp[0] = *i;
temp[1] = *j;
candidates.clear ();
// loop over all candidates starting from the best one
- for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; it_candidate++)
+ for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; ++it_candidate)
{
// stop if current candidate has no valid score
if (it_candidate->fitness_score == FLT_MAX)
const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle ();
const float translation3d = diff.block <3, 1> (0, 3).norm ();
unique = angle3d > min_angle3d && translation3d > min_translation3d;
- it++;
+ ++it;
}
// add candidate to best candidates
candidates.clear ();
// loop over all candidates starting from the best one
- for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; it_candidate++)
+ for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; ++it_candidate)
{
// stop if current candidate has score below threshold
if (it_candidate->fitness_score > t)
const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle ();
const float translation3d = diff.block <3, 1> (0, 3).norm ();
unique = angle3d > min_angle3d && translation3d > min_translation3d;
- it++;
+ ++it;
}
// add candidate to best candidates
std::vector<int> sample_indices (nr_samples_);
std::vector<int> corresponding_indices (nr_samples_);
PointCloudSource input_transformed;
- float error, lowest_error (0);
+ float lowest_error (0);
final_transformation_ = guess;
int i_iter = 0;
// Transform the data and compute the error
transformPointCloud (*input_, input_transformed, transformation_);
- error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));
+ float error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));
// If the new error is lower, update the final transformation
if (i_iter == 0 || error < lowest_error)
{
// Attempt to use the forward edge, otherwise use backward edge, otherwise there was no edge
Edge e;
- bool present1, present2;
+ bool present1;
std::tie (e, present1) = edge (vi, vj, *slam_graph_);
if (!present1)
{
+ bool present2;
std::tie (e, present2) = edge (vj, vi, *slam_graph_);
if (!present2)
continue;
Eigen::Matrix<double, 6, 6> hessian;
double score = 0;
- double delta_p_norm;
// Calculate derivates of initial transform vector, subsequent derivative calculations are done in the step length determination.
score = computeDerivatives (score_gradient, hessian, output, p);
delta_p = sv.solve (-score_gradient);
//Calculate step length with guarnteed sufficient decrease [More, Thuente 1994]
- delta_p_norm = delta_p.norm ();
+ double delta_p_norm = delta_p.norm ();
if (delta_p_norm == 0 || std::isnan(delta_p_norm))
{
std::vector<float> distances;
target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances);
- for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); neighborhood_it++)
+ for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); ++neighborhood_it)
{
cell = *neighborhood_it;
x_pt = input_->points[idx];
std::vector<float> distances;
target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances);
- for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); neighborhood_it++)
+ for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); ++neighborhood_it)
{
cell = *neighborhood_it;
normal_distributions_ (cells_[0], cells_[1])
{
// sort through all points, assigning them to distributions:
- NormalDist* n;
std::size_t used_points = 0;
for (std::size_t i = 0; i < cloud->size (); i++)
- if ((n = normalDistForPoint (cloud->at (i))))
+ if (NormalDist* n = normalDistForPoint (cloud->at (i)))
{
n->addIdx (i);
used_points++;
PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of bins on level 0: %u vs %u\n", pyramid_a->hist_levels[0].hist.size (), pyramid_b->hist_levels[0].hist.size ());
return -1;
}
- float match_count_level = 0.0f, match_count_prev_level = 0.0f;
+ float match_count_level = 0.0f;
for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size (); ++bin_i)
{
if (pyramid_a->hist_levels[0].hist[bin_i] < pyramid_b->hist_levels[0].hist[bin_i])
return -1;
}
- match_count_prev_level = match_count_level;
+ float match_count_prev_level = match_count_level;
match_count_level = 0.0f;
for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size (); ++bin_i)
{
iterations_ = 0;
double d_best_penalty = std::numeric_limits<double>::max();
- std::vector<int> best_model;
std::vector<int> selection;
Eigen::VectorXf model_coefficients;
std::vector<double> distances;
double d_best_penalty = std::numeric_limits<double>::max();
double k = 1.0;
- std::vector<int> best_model;
std::vector<int> selection;
Eigen::VectorXf model_coefficients;
std::vector<double> distances;
double d_best_penalty = std::numeric_limits<double>::max();
double k = 1.0;
- std::vector<int> best_model;
std::vector<int> selection;
Eigen::VectorXf model_coefficients;
std::vector<double> distances;
std::size_t n_inliers_count;
unsigned skipped_count = 0;
+
// suppress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
const unsigned max_skip = max_iterations_ * 10;
double d_best_penalty = std::numeric_limits<double>::max();
double k = 1.0;
- std::vector<int> best_model;
std::vector<int> selection;
Eigen::VectorXf model_coefficients;
std::vector<double> distances;
#pragma once
+#include <pcl/pcl_macros.h>
#include <pcl/segmentation/boost.h>
#include <pcl/segmentation/comparator.h>
#include <pcl/point_types.h>
using experimental::EuclideanClusterComparator<PointT, PointLT>::setExcludeLabels;
/** \brief Default constructor for EuclideanClusterComparator. */
- [[deprecated("remove PointNT from template parameters")]]
+ PCL_DEPRECATED("remove PointNT from template parameters")
EuclideanClusterComparator ()
: normals_ ()
, angular_threshold_ (0.0f)
/** \brief Provide a pointer to the input normals.
* \param[in] normals the input normal cloud
*/
- [[deprecated("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")]]
+ PCL_DEPRECATED("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")
inline void
setInputNormals (const PointCloudNConstPtr& normals) { normals_ = normals; }
/** \brief Get the input normals. */
- [[deprecated("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")]]
+ PCL_DEPRECATED("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")
inline PointCloudNConstPtr
getInputNormals () const { return (normals_); }
/** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
* \param[in] angular_threshold the tolerance in radians
*/
- [[deprecated("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")]]
+ PCL_DEPRECATED("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")
inline void
setAngularThreshold (float angular_threshold)
{
}
/** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
- [[deprecated("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")]]
+ PCL_DEPRECATED("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")
inline float
getAngularThreshold () const { return (std::acos (angular_threshold_) ); }
/** \brief Set labels in the label cloud to exclude.
* \param[in] exclude_labels a vector of bools corresponding to whether or not a given label should be considered
*/
- [[deprecated("use setExcludeLabels(const ExcludeLabelSetConstPtr &) instead")]]
+ PCL_DEPRECATED("use setExcludeLabels(const ExcludeLabelSetConstPtr &) instead")
void
setExcludeLabels (const std::vector<bool>& exclude_labels)
{
if (k_arg == 0)
return;
- bool is_convex;
unsigned int kcount = 0;
EdgeIterator edge_itr, edge_itr_end, next_edge;
{
next_edge++; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge
- is_convex = sv_adjacency_list_[*edge_itr].is_convex;
+ bool is_convex = sv_adjacency_list_[*edge_itr].is_convex;
if (is_convex) // If edge is (0-)convex
{
template <typename PointT> void
pcl::LCCPSegmentation<PointT>::calculateConvexConnections (SupervoxelAdjacencyList& adjacency_list_arg)
{
- bool is_convex;
EdgeIterator edge_itr, edge_itr_end, next_edge;
boost::tie (edge_itr, edge_itr_end) = boost::edges (adjacency_list_arg);
std::uint32_t target_sv_label = adjacency_list_arg[boost::target (*edge_itr, adjacency_list_arg)];
float normal_difference;
- is_convex = connIsConvex (source_sv_label, target_sv_label, normal_difference);
+ bool is_convex = connIsConvex (source_sv_label, target_sv_label, normal_difference);
adjacency_list_arg[*edge_itr].is_convex = is_convex;
adjacency_list_arg[*edge_itr].is_valid = is_convex;
adjacency_list_arg[*edge_itr].normal_difference = normal_difference;
clusters.resize (clusters_.size ());
std::vector<pcl::PointIndices>::iterator cluster_iter_input = clusters.begin ();
- for (std::vector<pcl::PointIndices>::const_iterator cluster_iter = clusters_.begin (); cluster_iter != clusters_.end (); cluster_iter++)
+ for (std::vector<pcl::PointIndices>::const_iterator cluster_iter = clusters_.begin (); cluster_iter != clusters_.end (); ++cluster_iter)
{
if ((static_cast<int> (cluster_iter->indices.size ()) >= min_pts_per_cluster_) &&
(static_cast<int> (cluster_iter->indices.size ()) <= max_pts_per_cluster_))
{
*cluster_iter_input = *cluster_iter;
- cluster_iter_input++;
+ ++cluster_iter_input;
}
}
cluster_iter = clusters_.erase (cluster_iter);
}
else
- cluster_iter++;
+ ++cluster_iter;
}
clusters.reserve (clusters_.size ());
float dist_thresh = distance_threshold_;
int homogeneous_region_number = 0;
- int curr_homogeneous_region = 0;
for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
{
- curr_homogeneous_region = 0;
+ int curr_homogeneous_region = 0;
if (segment_labels_[i_seg] == -1)
{
segment_labels_[i_seg] = homogeneous_region_number;
while (itr1 < itr2)
{
while (!(itr1->indices.empty ()) && itr1 < itr2)
- itr1++;
+ ++itr1;
while ( itr2->indices.empty () && itr1 < itr2)
- itr2--;
+ --itr2;
if (itr1 != itr2)
itr1->indices.swap (itr2->indices);
pcl::copyPointCloud (*input_,*labeled_cloud);
typename pcl::PointCloud <PointT>::const_iterator i_input = input_->begin ();
- std::vector <int> indices;
- std::vector <float> sqr_distances;
for (auto i_labeled = labeled_cloud->begin (); i_labeled != labeled_cloud->end (); ++i_labeled,++i_input)
{
if ( !pcl::isFinite<PointT> (*i_input))
#include <pcl/segmentation/planar_region.h>
#include <pcl/pcl_base.h>
+#include <pcl/pcl_macros.h>
#include <pcl/common/angles.h>
#include <pcl/PointIndices.h>
#include <pcl/ModelCoefficients.h>
* \param [in] labels The labels produced by the initial segmentation
* \param [in] label_indices The list of indices corresponding to each label
*/
- [[deprecated("centroids and covariances parameters are not used; they are deprecated and will be removed in future releases")]]
+ PCL_DEPRECATED("centroids and covariances parameters are not used; they are deprecated and will be removed in future releases")
void
refine (std::vector<ModelCoefficients>& model_coefficients,
std::vector<PointIndices>& inlier_indices,
#pragma once
#include <pcl/pcl_base.h>
+#include <pcl/pcl_macros.h>
#include <pcl/search/pcl_search.h>
namespace pcl
pcl::PointCloud<PointT> &output);
template <typename PointT>
- [[deprecated("tgt parameter is not used; it is deprecated and will be removed in future releases")]]
+ PCL_DEPRECATED("tgt parameter is not used; it is deprecated and will be removed in future releases")
inline void getPointCloudDifference (
const pcl::PointCloud<PointT> &src,
const pcl::PointCloud<PointT> & /* tgt */,
*/
SupervoxelClustering (float voxel_resolution, float seed_resolution);
- [[deprecated("constructor with flag for using the single camera transform is deprecated. Default behavior is now to use the transform for organized clouds, and not use it for unorganized. Use setUseSingleCameraTransform() to override the defaults.")]]
+ PCL_DEPRECATED("constructor with flag for using the single camera transform is deprecated. Default behavior is now to use the transform for organized clouds, and not use it for unorganized. Use setUseSingleCameraTransform() to override the defaults.")
SupervoxelClustering (float voxel_resolution, float seed_resolution, bool) : SupervoxelClustering (voxel_resolution, seed_resolution) { }
/** \brief This destructor destroys the cloud, normals and search method used for
* color(it's random). Points that are unlabeled will be black
* \note This will expand the label_colors_ vector so that it can accommodate all labels
*/
- [[deprecated("use getLabeledCloud() instead. An example of how to display and save with colorized labels can be found in examples/segmentation/example_supervoxels.cpp")]]
+ PCL_DEPRECATED("use getLabeledCloud() instead. An example of how to display and save with colorized labels can be found in examples/segmentation/example_supervoxels.cpp")
typename pcl::PointCloud<PointXYZRGBA>::Ptr
getColoredCloud () const
{
# Install include files
PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs})
-add_subdirectory(tools)
+if(BUILD_tools)
+ add_subdirectory(tools)
+endif()
+set(SUBSYS_NAME tools)
+
find_package(GLEW)
find_package(GLUT)
}
/** \brief Virtual destructor. */
- ~StereoGrabberBase() throw();
+ ~StereoGrabberBase() noexcept;
/** \brief Starts playing the list of Stereo images if frames_per_second is > 0.
* Otherwise it works as a trigger: publishes only the next pair in the list. */
{}
///////////////////////////////////////////////////////////////////////////////////////////
-pcl::StereoGrabberBase::~StereoGrabberBase() throw()
+pcl::StereoGrabberBase::~StereoGrabberBase() noexcept
{
stop();
delete impl_;
template <class NodeData,class Real>
OctNode<NodeData,Real>* OctNode<NodeData,Real>::__faceNeighbor(int dir,int off,int forceChildren){
if(!parent){return NULL;}
- int pIndex=int(this-parent->children);
+ int pIndex=int(this-(parent->children));
pIndex^=(1<<dir);
if((pIndex & (1<<dir))==(off<<dir)){return &parent->children[pIndex];}
else{
template <class NodeData,class Real>
const OctNode<NodeData,Real>* OctNode<NodeData,Real>::__faceNeighbor(int dir,int off) const {
if(!parent){return NULL;}
- int pIndex=int(this-parent->children);
+ int pIndex=int(this-(parent->children));
pIndex^=(1<<dir);
if((pIndex & (1<<dir))==(off<<dir)){return &parent->children[pIndex];}
else{
template <class NodeData,class Real>
const OctNode<NodeData,Real>* OctNode<NodeData,Real>::__edgeNeighbor(int o,const int i[2],const int idx[2]) const{
if(!parent){return NULL;}
- int pIndex=int(this-parent->children);
+ int pIndex=int(this-(parent->children));
int aIndex,x[DIMENSION];
Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]);
template <class NodeData,class Real>
OctNode<NodeData,Real>* OctNode<NodeData,Real>::__edgeNeighbor(int o,const int i[2],const int idx[2],int forceChildren){
if(!parent){return NULL;}
- int pIndex=int(this-parent->children);
+ int pIndex=int(this-(parent->children));
int aIndex,x[DIMENSION];
Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]);
int pIndex,aIndex=0;
if(!parent){return NULL;}
- pIndex=int(this-parent->children);
+ pIndex=int(this-(parent->children));
aIndex=(cornerIndex ^ pIndex); // The disagreement bits
pIndex=(~pIndex)&7; // The antipodal point
if(aIndex==7){ // Agree on no bits
int pIndex,aIndex=0;
if(!parent){return NULL;}
- pIndex=int(this-parent->children);
+ pIndex=int(this-(parent->children));
aIndex=(cornerIndex ^ pIndex); // The disagreement bits
pIndex=(~pIndex)&7; // The antipodal point
if(aIndex==7){ // Agree on no bits
std::vector<int> qhid_to_pcidx (max_vertex_id);
int num_facets = qh num_facets;
- int dd = 0;
if (dim_ == 3)
{
vertexT *anyVertex = static_cast<vertexT*> (facet->vertices->e[0].p);
double *center = facet->center;
double r = qh_pointdist (anyVertex->point,center,dim_);
- facetT *neighb;
if (voronoi_centers_)
{
ridgeT *ridge, **ridgep;
FOREACHridge_ (facet->ridges)
{
- neighb = otherfacet_ (ridge, facet);
+ facetT *neighb = otherfacet_ (ridge, facet);
if ((neighb->visitid != qh visit_id))
qh_setappend (&triangles_set, ridge);
}
if (voronoi_centers_)
voronoi_centers_->points.resize (num_facets);
+ int dd = 0;
FORALLfacets
{
// Facets are the delaunay triangles (2d)
alpha_shape.points.resize (vertices);
- std::vector<std::vector<int> > connected;
PointCloud alpha_shape_sorted;
alpha_shape_sorted.points.resize (vertices);
// Initializing
int is_free=0, nr_parts=0, increase_nnn4fn=0, increase_nnn4s=0, increase_dist=0, nr_touched = 0;
- bool is_fringe;
angles_.resize(nnn_);
std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > uvn_nn (nnn_);
Eigen::Vector2f uvn_s;
}
}
- is_fringe = true;
+ bool is_fringe = true;
while (is_fringe)
{
is_fringe = false;
std::vector<int>::iterator first_gap_after = angleIdx.end ();
std::vector<int>::iterator last_gap_before = angleIdx.begin ();
int nr_gaps = 0;
- for (std::vector<int>::iterator it = angleIdx.begin (); it != angleIdx.end () - 1; it++)
+ for (std::vector<int>::iterator it = angleIdx.begin (); it != angleIdx.end () - 1; ++it)
{
if (gaps[*it])
{
Eigen::Vector2f S1;
Eigen::Vector2f S2;
std::vector<int> to_erase;
- for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; it++)
+ for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; ++it)
{
if (gaps[*(it-1)])
angle_so_far = 0;
{
std::vector<int>::iterator prev_it;
int erased_idx = static_cast<int> (to_erase.size ()) -1;
- for (prev_it = it-1; (erased_idx != -1) && (it != angleIdx.begin()); it--)
+ for (prev_it = it-1; (erased_idx != -1) && (it != angleIdx.begin()); --it)
if (*it == to_erase[erased_idx])
erased_idx--;
else
break;
bool can_delete = true;
- for (std::vector<int>::iterator curr_it = prev_it+1; curr_it != it+1; curr_it++)
+ for (std::vector<int>::iterator curr_it = prev_it+1; curr_it != it+1; ++curr_it)
{
tmp_ = coords_[angles_[*curr_it].index] - proj_qp_;
X[0] = tmp_.dot(u_);
}
for (const int &it : to_erase)
{
- for (std::vector<int>::iterator iter = angleIdx.begin(); iter != angleIdx.end(); iter++)
+ for (std::vector<int>::iterator iter = angleIdx.begin(); iter != angleIdx.end(); ++iter)
if (it == *iter)
{
angleIdx.erase(iter);
changed_1st_fn_ = false;
changed_2nd_fn_ = false;
new2boundary_ = NONE;
- for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; it++)
+ for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; ++it)
{
current_index_ = angles_[*it].index;
// processing for each face
for (std::size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i)
{
- std::size_t idx;
Eigen::Vector2f tmp_VT;
for (std::size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
{
- idx = tex_mesh.tex_polygons[m][i].vertices[j];
+ std::size_t idx = tex_mesh.tex_polygons[m][i].vertices[j];
memcpy (&x_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float));
memcpy (&y_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float));
memcpy (&z_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float));
// convert mesh's cloud to pcl format for ease
pcl::fromPCLPointCloud2 (tex_mesh.cloud, *originalCloud);
- // texture coordinates for each mesh
- std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texture_map;
-
for (std::size_t m = 0; m < cams.size (); ++m)
{
// get current camera parameters
std::vector<float> grid_;
/** \brief The grid resolution */
- int res_x_, res_y_, res_z_;
+ int res_x_ = 32, res_y_ = 32, res_z_ = 32;
/** \brief bounding box */
Eigen::Array3f upper_boundary_;
/** \brief Sets whether the surface and normal are approximated using a polynomial, or only via tangent estimation.
* \param[in] polynomial_fit set to true for polynomial fit
*/
- [[deprecated("use setPolynomialOrder() instead")]]
+ PCL_DEPRECATED("use setPolynomialOrder() instead")
inline void
setPolynomialFit (bool polynomial_fit)
{
}
/** \brief Get the polynomial_fit value (true if the surface and normal are approximated using a polynomial). */
- [[deprecated("use getPolynomialOrder() instead")]]
+ PCL_DEPRECATED("use getPolynomialOrder() instead")
inline bool
getPolynomialFit () const { return (order_ > 1); }
};
template <typename PointInT, typename PointOutT>
- using MovingLeastSquaresOMP
[[deprecated("use MovingLeastSquares instead, it supports OpenMP now")]] = MovingLeastSquares<PointInT, PointOutT>;
+ using MovingLeastSquaresOMP
PCL_DEPRECATED("use MovingLeastSquares instead, it supports OpenMP now") = MovingLeastSquares<PointInT, PointOutT>;
}
#ifdef PCL_NO_PRECOMPILE
#include "pcl/surface/3rdparty/opennurbs/opennurbs.h"
+#include <pcl/pcl_macros.h>
+
// This define is up here so anybody who want's to defeat the
// bozo vaccine has to be doing it on purpose.
#define BOZO_VACCINE_699FCC4262D4488c9109F1B7A37CE926
// SDKBREAK Oct 30, 07 - LW
// This function should not be used any longer
-[[deprecated("Use the version that takes a model transform argument")]]
+PCL_DEPRECATED("Use the version that takes a model transform argument")
bool ON_Annotation2::GetTextXform(
ON_RECT /*gdi_text_rect*/,
const ON_Font& /*font*/,
// New function added Oct 30, 07 - LW
// To use model xform to draw annotation in blocks correctly
#if 0
-[[deprecated("Use the version that takes a dimstyle pointer")]]
+PCL_DEPRECATED("Use the version that takes a dimstyle pointer")
bool ON_Annotation2::GetTextXform(
ON_RECT gdi_text_rect,
const ON_Font& font,
// SDKBREAK Oct 30, 07 - LW
// This function should not be used any longer
-[[deprecated("Use the version that takes a model transform argument")]]
+PCL_DEPRECATED("Use the version that takes a model transform argument")
bool ON_Annotation2::GetTextXform(
ON_RECT /*gdi_text_rect*/,
int /*gdi_height_of_I*/,
FittingSurface fit (&data_list[n1], nurbs_list[n1]);
FittingSurface::Parameter paramFP (1.0, param.smoothness, 0.0, 1.0, param.smoothness, 0.0);
- std::vector<double> wBnd, wInt;
for (std::size_t i = 0; i < data_list[n1].boundary.size (); i++)
data_list[n1].boundary_weight.push_back (param.boundary_weight);
for (std::size_t i = 0; i < data_list[n1].interior.size (); i++)
void
SparseMat::printLong ()
{
- std::map<int, std::map<int, double> >::iterator it_row;
- std::map<int, double>::iterator it_col;
-
int si, sj;
size (si, sj);
#include <pcl/2d/edge.h>
#include <pcl/2d/morphology.h>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <fstream>
#include <pcl/point_types.h>
add_library(pcl_gtest STATIC ${GTEST_SRC_DIR}/src/gtest-all.cc)
enable_testing()
-add_custom_target(tests "${CMAKE_CTEST_COMMAND}" "-V" VERBATIM)
+
+if(MSVC)
+ # VS needs -C config to run correct
+ add_custom_target(tests "${CMAKE_CTEST_COMMAND}" -C $<$<CONFIG:Debug>:Debug>$<$<CONFIG:Release>:Release> -V -T Test VERBATIM)
+else()
+ add_custom_target(tests "${CMAKE_CTEST_COMMAND}" -V -T Test VERBATIM)
+endif()
+
set_target_properties(tests PROPERTIES FOLDER "Tests")
+include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
+
add_subdirectory(2d)
add_subdirectory(common)
add_subdirectory(features)
PCL_ADD_TEST(common_test_macros test_macros FILES test_macros.cpp LINK_WITH pcl_gtest pcl_common)
PCL_ADD_TEST(common_vector_average test_vector_average FILES test_vector_average.cpp LINK_WITH pcl_gtest)
PCL_ADD_TEST(common_common test_common FILES test_common.cpp LINK_WITH pcl_gtest pcl_common)
+PCL_ADD_TEST(common_parse test_parse FILES test_parse.cpp LINK_WITH pcl_gtest pcl_common)
PCL_ADD_TEST(common_geometry test_geometry FILES test_geometry.cpp LINK_WITH pcl_gtest pcl_common)
PCL_ADD_TEST(common_copy_point test_copy_point FILES test_copy_point.cpp LINK_WITH pcl_gtest pcl_common)
PCL_ADD_TEST(common_transforms test_transforms FILES test_transforms.cpp LINK_WITH pcl_gtest pcl_common)
* \author: Qinghua Li (qinghua__li@163.com)
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <iostream>
#include <pcl/range_image/bearing_angle_image.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/common/common.h>
#include <pcl/common/distances.h>
#include <pcl/common/intersections.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_tests.h>
#include <pcl/common/colors.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_tests.h>
#include <pcl/common/common.h>
#include <pcl/common/distances.h>
template <typename T> class XYZPointTypesTest : public ::testing::Test { };
using XYZPointTypes = ::testing::Types<BOOST_PP_SEQ_ENUM(PCL_XYZ_POINT_TYPES)>;
-TYPED_TEST_CASE(XYZPointTypesTest, XYZPointTypes);
+TYPED_TEST_SUITE(XYZPointTypesTest, XYZPointTypes);
TYPED_TEST(XYZPointTypesTest, GetVectorXfMap)
{
TypeParam pt;
template <typename T> class NormalPointTypesTest : public ::testing::Test { };
using NormalPointTypes = ::testing::Types<BOOST_PP_SEQ_ENUM(PCL_NORMAL_POINT_TYPES)>;
-TYPED_TEST_CASE(NormalPointTypesTest, NormalPointTypes);
+TYPED_TEST_SUITE(NormalPointTypesTest, NormalPointTypes);
TYPED_TEST(NormalPointTypesTest, GetNormalVectorXfMap)
{
TypeParam pt;
template <typename T> class RGBPointTypesTest : public ::testing::Test { };
using RGBPointTypes = ::testing::Types<BOOST_PP_SEQ_ENUM(PCL_RGB_POINT_TYPES)>;
-TYPED_TEST_CASE(RGBPointTypesTest, RGBPointTypes);
+TYPED_TEST_SUITE(RGBPointTypesTest, RGBPointTypes);
TYPED_TEST(RGBPointTypesTest, GetRGBVectorXi)
{
TypeParam pt; pt.r = 1; pt.g = 2; pt.b = 3; pt.a = 4;
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_config.h>
#include <pcl/pcl_tests.h>
#include <pcl/point_cloud.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/common/copy_point.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <random>
*/
/** \author Nizar Sallem */
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/common/gaussian.h>
TEST(PCL, GaussianKernel)
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_tests.h>
#include <pcl/point_types.h>
#include <pcl/common/random.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/common/geometry.h>
#include <pcl/point_types.h>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename T> class XYZPointTypesTest : public ::testing::Test { };
using XYZPointTypes = ::testing::Types<BOOST_PP_SEQ_ENUM(PCL_XYZ_POINT_TYPES)>;
-TYPED_TEST_CASE(XYZPointTypesTest, XYZPointTypes);
+TYPED_TEST_SUITE(XYZPointTypesTest, XYZPointTypes);
TYPED_TEST(XYZPointTypesTest, Distance)
{
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_tests.h>
#include <pcl/point_types.h>
#include <pcl/common/point_operators.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_tests.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
* $Id$
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_config.h>
#include <pcl/pcl_tests.h>
#include <pcl/common/eigen.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_tests.h>
#include <pcl/point_types.h>
#include <pcl/common/point_operators.h>
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2020-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/test/gtest.h>
+#include <pcl/pcl_tests.h>
+#include <pcl/console/parse.h>
+
+using namespace std;
+
+///////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, parse_double)
+{
+ const char arg0[] = {"test_name"};
+ const char arg1[] = {"double"};
+ const char arg2_1[] = {"-3.14e+0"};
+ const char arg2_2[] = {"-3.14f+0"};
+ const char arg2_3[] = {"3.14e+309"};
+
+ const char* argv_1[] = { &arg0[0], &arg1[0], &arg2_1[0], nullptr};
+ const char* argv_2[] = { &arg0[0], &arg1[0], &arg2_2[0], nullptr};
+ const char* argv_3[] = { &arg0[0], &arg1[0], &arg2_3[0], nullptr};
+ const int argc = static_cast<int> (sizeof (argv_1)/sizeof (argv_1[0])) - 1;
+
+ int index = -1;
+ double value = 0;
+
+ index = pcl::console::parse_argument (argc, argv_1, "double", value);
+ EXPECT_DOUBLE_EQ(-3.14, value);
+ EXPECT_EQ(1, index);
+
+ index = pcl::console::parse_argument (argc, argv_2, "double", value);
+ EXPECT_EQ(-1, index);
+
+ index = pcl::console::parse_argument (argc, argv_3, "double", value);
+ EXPECT_EQ(-1, index);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, parse_float)
+{
+ const char arg0[] = {"test_name"};
+ const char arg1[] = {"float"};
+ const char arg2_1[] = {"-3.14e+0"};
+ const char arg2_2[] = {"-3.14f+0"};
+ const char arg2_3[] = {"3.14e+39"};
+
+ const char* argv_1[] = { &arg0[0], &arg1[0], &arg2_1[0], nullptr};
+ const char* argv_2[] = { &arg0[0], &arg1[0], &arg2_2[0], nullptr};
+ const char* argv_3[] = { &arg0[0], &arg1[0], &arg2_3[0], nullptr};
+ const int argc = static_cast<int> (sizeof (argv_1)/sizeof (argv_1[0])) - 1;
+
+ int index = -1;
+ float value = 0;
+
+ index = pcl::console::parse_argument (argc, argv_1, "float", value);
+ EXPECT_FLOAT_EQ(-3.14, value);
+ EXPECT_EQ(1, index);
+
+ index = pcl::console::parse_argument (argc, argv_2, "float", value);
+ EXPECT_EQ(-1, index);
+
+ index = pcl::console::parse_argument (argc, argv_3, "float", value);
+ EXPECT_EQ(-1, index);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, parse_longint)
+{
+ const char arg0[] = {"test_name"};
+ const char arg1[] = {"long_int"};
+ const char arg2_1[] = {"-314"};
+ const char arg2_2[] = {"3.14"};
+ const char arg2_3[] = {"18446744073709551615"};
+
+ const char* argv_1[] = { &arg0[0], &arg1[0], &arg2_1[0], nullptr};
+ const char* argv_2[] = { &arg0[0], &arg1[0], &arg2_2[0], nullptr};
+ const char* argv_3[] = { &arg0[0], &arg1[0], &arg2_3[0], nullptr};
+ const int argc = static_cast<int> (sizeof (argv_1)/sizeof (argv_1[0])) - 1;
+
+ int index = -1;
+ long int value = 0;
+
+ index = pcl::console::parse_argument (argc, argv_1, "long_int", value);
+ EXPECT_EQ(-314, value);
+ EXPECT_EQ(1, index);
+
+ index = pcl::console::parse_argument (argc, argv_2, "long_int", value);
+ EXPECT_EQ(-1, index);
+
+ index = pcl::console::parse_argument (argc, argv_3, "long_int", value);
+ EXPECT_EQ(-1, index);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, parse_unsignedint)
+{
+ const char arg0[] = {"test_name"};
+ const char arg1[] = {"unsigned_int"};
+ const char arg2_1[] = {"314"};
+ const char arg2_2[] = {"-314"};
+ const char arg2_3[] = {"18446744073709551615"};
+
+ const char* argv_1[] = { &arg0[0], &arg1[0], &arg2_1[0], nullptr};
+ const char* argv_2[] = { &arg0[0], &arg1[0], &arg2_2[0], nullptr};
+ const char* argv_3[] = { &arg0[0], &arg1[0], &arg2_3[0], nullptr};
+ const int argc = static_cast<int> (sizeof (argv_1)/sizeof (argv_1[0])) - 1;
+
+ int index = -1;
+ unsigned int value = 53;
+
+ index = pcl::console::parse_argument (argc, argv_1, "unsigned_int", value);
+ EXPECT_EQ(314, value);
+ EXPECT_EQ(1, index);
+
+ index = pcl::console::parse_argument (argc, argv_2, "unsigned_int", value);
+ EXPECT_EQ(-1, index);
+
+ index = pcl::console::parse_argument (argc, argv_3, "unsigned_int", value);
+ EXPECT_EQ(-1, index);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, parse_int)
+{
+ const char arg0[] = {"test_name"};
+ const char arg1[] = {"int"};
+ const char arg2_1[] = {"-314"};
+ const char arg2_2[] = {"3.14"};
+ const char arg2_3[] = {"18446744073709551615"};
+
+ const char* argv_1[] = { &arg0[0], &arg1[0], &arg2_1[0], nullptr};
+ const char* argv_2[] = { &arg0[0], &arg1[0], &arg2_2[0], nullptr};
+ const char* argv_3[] = { &arg0[0], &arg1[0], &arg2_3[0], nullptr};
+ const int argc = static_cast<int> (sizeof (argv_1)/sizeof (argv_1[0])) - 1;
+
+ int index = -1;
+ int value = 0;
+
+ index = pcl::console::parse_argument (argc, argv_1, "int", value);
+ EXPECT_EQ(-314, value);
+ EXPECT_EQ(1, index);
+
+ index = pcl::console::parse_argument (argc, argv_2, "int", value);
+ EXPECT_EQ(-1, index);
+
+ index = pcl::console::parse_argument (argc, argv_3, "int", value);
+ EXPECT_EQ(-1, index);
+}
+
+/* ---[ */
+int
+main (int argc, char** argv)
+{
+ testing::InitGoogleTest (&argc, argv);
+ return (RUN_ALL_TESTS ());
+}
+/* ]--- */
/** \author Nizar Sallem */
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/common/pca.h>
#include <pcl/point_types.h>
#include <pcl/pcl_tests.h>
* $Id: test_plane_intersection.cpp 5686 2012-05-11 20:59:00Z gioia $
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/common/common.h>
#include <pcl/common/intersections.h>
#include <pcl/pcl_tests.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_tests.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_tests.h>
#include <pcl/PolygonMesh.h>
{
testing::InitGoogleTest(&argc, argv);
return (RUN_ALL_TESTS());
-}
\ No newline at end of file
+}
* $Id$
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_config.h>
#include <pcl/pcl_tests.h>
#include <pcl/point_types.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
PCL_MAKE_ALIGNED_OPERATOR_NEW;
};
-TYPED_TEST_CASE (Transforms, TransformTypes);
+TYPED_TEST_SUITE (Transforms, TransformTypes);
TYPED_TEST (Transforms, PointCloudXYZDense)
{
*/
-#include <pcl/pcl_macros.h>
#include <pcl/point_traits.h>
#include <pcl/point_types.h>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
TEST (TypeTraits, HasCustomAllocatorTrait)
{
#include <pcl/pcl_macros.h>
#include <iostream>
#include <sstream>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/common/vector_average.h>
using namespace pcl;
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/pcl_tests.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/feature.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/pcl_tests.h>
#include <pcl/features/normal_3d_omp.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/boundary.h>
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/keypoints/brisk_2d.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/cppf.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/principal_curvatures.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/cvfh.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/pcl_tests.h>
#include <pcl/features/normal_3d.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/gasd.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/intensity_gradient.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/grsd.h>
#include <pcl/features/normal_3d.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/moment_invariants.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <sstream>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/features/narf.h>
#include <pcl/common/eigen.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
+#include <pcl/features/integral_image_normal.h>
#include <pcl/io/pcd_io.h>
using namespace pcl;
EXPECT_EQ (normals->points.size (), indices.size ());
}
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// There was an issue where the vectors for the indices and the
+// distances of the neighbor search weren't initialized correctly
+// due to a wrong usage of OpenMP.
+// See PR #3614 for details.
+// This tests if the vectors are initialized correctly.
+template<typename PointT>
+class DummySearch : public pcl::search::Search<PointT>
+{
+ public:
+ virtual int nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices,
+ std::vector<float> &k_sqr_distances ) const
+ {
+ (void)point;
+
+ EXPECT_GE (k_indices.size(), k);
+ EXPECT_GE (k_sqr_distances.size(), k);
+
+ return k;
+ }
+
+ virtual int radiusSearch (const PointT& point, double radius, std::vector<int>& k_indices,
+ std::vector<float>& k_sqr_distances, unsigned int max_nn = 0 ) const
+ {
+ (void)point;
+ (void)radius;
+ (void)k_indices;
+ (void)k_sqr_distances;
+
+ return max_nn;
+ }
+};
+
+
+TEST (PCL, NormalEstimationOpenMPInitVectors)
+{
+ NormalEstimationOMP<PointXYZ, Normal> n (4); // instantiate 4 threads
+
+ DummySearch<PointXYZ>::Ptr dummy (new DummySearch<PointXYZ>);
+
+ // Object
+ PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
+
+ // set parameters
+ PointCloud<PointXYZ>::Ptr cloudptr = cloud.makeShared ();
+ n.setInputCloud (cloudptr);
+ EXPECT_EQ (n.getInputCloud (), cloudptr);
+ pcl::IndicesPtr indicesptr (new pcl::Indices (indices));
+ n.setIndices (indicesptr);
+ EXPECT_EQ (n.getIndices (), indicesptr);
+ n.setSearchMethod (dummy);
+ EXPECT_EQ (n.getSearchMethod (), dummy);
+ n.setKSearch (static_cast<int> (indices.size ()));
+
+ // This will cause a call to DummySearch::nearestKSearch
+ // which checks if the vectors for the indices and the
+ // distances are correctly set up. See PR #3614.
+ n.compute (*normals);
+}
+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST (PCL, NormalEstimationOpenMP)
{
}
}
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// This tests the indexing issue from #3573
+// In certain cases when you used a subset of the indices
+// and set the depth dependent smoothing to false the
+// IntegralImageNormalEstimation could crash or produce
+// incorrect normals.
+// This test reproduces the issue.
+TEST (PCL, IntegralImageNormalEstimationIndexingIssue)
+{
+ PointCloud<PointXYZ>::Ptr cloudptr(new PointCloud<PointXYZ>());
+
+ cloudptr->width = 100;
+ cloudptr->height = 100;
+ cloudptr->points.clear();
+ cloudptr->points.resize(cloudptr->width * cloudptr->height);
+
+ int centerX = cloudptr->width >> 1;
+ int centerY = cloudptr->height >> 1;
+
+ int idx = 0;
+ for (int ypos = -centerY; ypos < centerY; ypos++)
+ {
+ for (int xpos = -centerX; xpos < centerX; xpos++)
+ {
+ double z = xpos < 0.0 ? 1.0 : 0.0;
+ double y = ypos;
+ double x = xpos;
+
+ cloudptr->points[idx++] = PointXYZ(float(x), float(y), float(z));
+ }
+ }
+
+ pcl::IndicesPtr indicesptr (new pcl::Indices ());
+ indicesptr->resize(cloudptr->size() / 2);
+ for (int i = 0; i < cloudptr->size() / 2; ++i)
+ {
+ (*indicesptr)[i] = i + cloudptr->size() / 2;
+ }
+
+ IntegralImageNormalEstimation<PointXYZ, Normal> n;
+
+ // Object
+ PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
+
+ // set parameters
+ n.setInputCloud (cloudptr);
+ n.setIndices (indicesptr);
+ n.setDepthDependentSmoothing (false);
+
+ // estimate
+ n.compute (*normals);
+
+ std::vector<PointXYZ> normalsVec;
+ normalsVec.resize(normals->size());
+ for( int i = 0; i < normals->size(); ++i )
+ {
+ normalsVec[i].x = normals->points[i].normal_x;
+ normalsVec[i].y = normals->points[i].normal_y;
+ normalsVec[i].z = normals->points[i].normal_z;
+ }
+
+ for (const auto &point : normals->points)
+ {
+ if (std::isnan( point.normal_x ) ||
+ std::isnan( point.normal_y ) ||
+ std::isnan( point.normal_z ))
+ {
+ continue;
+ }
+
+ EXPECT_NEAR (point.normal[0], 0.0, 1e-4);
+ EXPECT_NEAR (point.normal[1], 0.0, 1e-4);
+ EXPECT_NEAR (point.normal[2], -1.0, 1e-4);
+ EXPECT_TRUE (std::isnan(point.curvature));
+ }
+}
+
/* ---[ */
int
main (int argc, char** argv)
#define PCL_NO_PRECOMPILE
#endif
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/pfh.h>
#include <pcl/features/fpfh.h>
using FPFHEstimatorTypes = ::testing::Types
<FPFHEstimation<PointT, PointT, FPFHSignature33>,
FPFHEstimationOMP<PointT, PointT, FPFHSignature33> >;
-TYPED_TEST_CASE (FPFHTest, FPFHEstimatorTypes);
+TYPED_TEST_SUITE (FPFHTest, FPFHEstimatorTypes);
// This is a copy of the old FPFHEstimation test which will now
// be applied to both FPFHEstimation and FPFHEstimationOMP
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/ppf.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/vfh.h>
#include <pcl/features/usc.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/rift.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/rops_estimation.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/rsd.h>
#include <pcl/features/normal_3d.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/io/pcd_io.h>
using SHOTEstimatorTypes = ::testing::Types
<SHOTEstimation<PointXYZ, Normal, SHOT352>,
SHOTEstimationOMP<PointXYZ, Normal, SHOT352> >;
-TYPED_TEST_CASE (SHOTShapeTest, SHOTEstimatorTypes);
+TYPED_TEST_SUITE (SHOTShapeTest, SHOTEstimatorTypes);
// This is a copy of the old SHOTShapeEstimation test which will now
// be applied to both SHOTEstimation and SHOTEstimationOMP
using SHOTColorEstimatorTypes= ::testing::Types
<SHOTColorEstimation<PointXYZRGBA, Normal, SHOT1344>,
SHOTColorEstimationOMP<PointXYZRGBA, Normal, SHOT1344> >;
-TYPED_TEST_CASE (SHOTShapeAndColorTest, SHOTColorEstimatorTypes);
+TYPED_TEST_SUITE (SHOTShapeAndColorTest, SHOTColorEstimatorTypes);
// This is a copy of the old SHOTShapeAndColorEstimation test which will now
// be applied to both SHOTColorEstimation and SHOTColorEstimationOMP
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/pcl_tests.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/fast_bilateral.h>
#include <pcl/filters/fast_bilateral_omp.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_tests.h>
#include <pcl/point_types.h>
#include <pcl/filters/box_clipper3D.h>
testing::InitGoogleTest (&argc, argv);
return (RUN_ALL_TESTS ());
}
-/* ]--- */
\ No newline at end of file
+/* ]--- */
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_tests.h>
#include <pcl/filters/convolution.h>
#include <pcl/point_types.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/filters/grid_minimum.h>
#include <pcl/point_types.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/filters/local_maximum.h>
#include <pcl/point_types.h>
* POSSIBILITY OF SUCH DAMAGE.
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/model_outlier_removal.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/filters/morphological_filter.h>
#include <pcl/point_types.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
* POSSIBILITY OF SUCH DAMAGE.
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/common/generate.h>
#include <pcl/common/random.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/common/common.h>
#include <pcl/geometry/line_iterator.h>
#include <pcl/point_types.h>
#include <vector>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
// Needed for one test. Must be defined before including the mesh.
#define PCL_GEOMETRY_MESH_BASE_TEST_DELETE_FACE_MANIFOLD_2
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/geometry/triangle_mesh.h>
#include <pcl/pcl_macros.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/geometry/triangle_mesh.h>
#include <pcl/geometry/quad_mesh.h>
using MeshTraitsTypes = testing::Types <ManifoldMeshTraits, NonManifoldMeshTraits>;
-TYPED_TEST_CASE (TestMeshConversion, MeshTraitsTypes);
+TYPED_TEST_SUITE (TestMeshConversion, MeshTraitsTypes);
////////////////////////////////////////////////////////////////////////////////
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include "test_mesh_common_functions.h"
#include <pcl/geometry/polygon_mesh.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/geometry/polygon_mesh.h>
#include <pcl/geometry/get_boundary.h>
using Mesh = MeshT;
};
-TYPED_TEST_CASE (TestGetBoundary, MeshTypes);
+TYPED_TEST_SUITE (TestGetBoundary, MeshTypes);
////////////////////////////////////////////////////////////////////////////////
#include <string>
#include <sstream>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/geometry/mesh_indices.h>
using MeshIndexTypes = testing::Types <VertexIndex, HalfEdgeIndex, EdgeIndex, FaceIndex>;
-TYPED_TEST_CASE (TestMeshIndicesTyped, MeshIndexTypes);
+TYPED_TEST_SUITE (TestMeshIndicesTyped, MeshIndexTypes);
////////////////////////////////////////////////////////////////////////////////
#include <fstream>
#include <string>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/geometry/triangle_mesh.h>
#include <pcl/geometry/mesh_io.h>
#include <vector>
#include <typeinfo>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/geometry/polygon_mesh.h>
using Mesh = MeshT;
};
-TYPED_TEST_CASE (TestPolygonMesh, PolygonMeshTypes);
+TYPED_TEST_SUITE (TestPolygonMesh, PolygonMeshTypes);
////////////////////////////////////////////////////////////////////////////////
#include <vector>
#include <typeinfo>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/geometry/quad_mesh.h>
using Mesh = MeshT;
};
-TYPED_TEST_CASE (TestQuadMesh, QuadMeshTypes);
+TYPED_TEST_SUITE (TestQuadMesh, QuadMeshTypes);
////////////////////////////////////////////////////////////////////////////////
#include <vector>
#include <typeinfo>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/geometry/triangle_mesh.h>
using Mesh = MeshT;
};
-TYPED_TEST_CASE (TestTriangleMesh, TriangleMeshTypes);
+TYPED_TEST_SUITE (TestTriangleMesh, TriangleMeshTypes);
////////////////////////////////////////////////////////////////////////////////
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2019-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#pragma once
+
+#include <gtest/gtest.h>
+
+/**
+ * \file pcl/test/gtest.h
+ *
+ * \brief Defines all the PCL test macros used
+ * \ingroup test
+ */
+
+/**
+ * \brief Macro choose between TYPED_TEST_CASE and TYPED_TEST_SUITE depending on the GTest version
+ *
+ * \ingroup test
+ */
+#if !defined(TYPED_TEST_SUITE)
+ #define TYPED_TEST_SUITE TYPED_TEST_CASE
+#endif
+
+/**
+ * \brief Macro choose between INSTANTIATE_TEST_CASE_P and INSTANTIATE_TEST_SUITE_P depending on the GTest version
+ *
+ * \ingroup test
+ */
+#if !defined(INSTANTIATE_TEST_SUITE_P)
+ #define INSTANTIATE_TEST_SUITE_P INSTANTIATE_TEST_CASE_P
+#endif
+
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <cmath>
#include <cstdint>
};
using DataTypes = ::testing::Types<std::int8_t, std::int32_t, float>;
-TYPED_TEST_CASE (BuffersTest, DataTypes);
+TYPED_TEST_SUITE (BuffersTest, DataTypes);
TYPED_TEST (BuffersTest, SingleBuffer)
{
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/point_traits.h>
#include <pcl/point_types.h>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename T> class AutoIOTest : public testing::Test { };
using PCLXyzNormalPointTypes = ::testing::Types<BOOST_PP_SEQ_ENUM (PCL_XYZ_POINT_TYPES PCL_NORMAL_POINT_TYPES)>;
-TYPED_TEST_CASE (AutoIOTest, PCLXyzNormalPointTypes);
+TYPED_TEST_SUITE (AutoIOTest, PCLXyzNormalPointTypes);
TYPED_TEST (AutoIOTest, AutoLoadCloudFiles)
{
PointCloud<TypeParam> cloud;
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <iostream>
* POSSIBILITY OF SUCH DAMAGE.
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/octree/octree.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST (PCL, PLYReaderWriter)
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename T> class PLYPointCloudTest : public PLYColorTest { };
using RGBPointTypes = ::testing::Types<BOOST_PP_SEQ_ENUM (PCL_RGB_POINT_TYPES)>;
-TYPED_TEST_CASE (PLYPointCloudTest, RGBPointTypes);
+TYPED_TEST_SUITE (PLYPointCloudTest, RGBPointTypes);
TYPED_TEST (PLYPointCloudTest, LoadPLYFileColoredASCIIIntoPointCloud)
{
int res;
struct PLYCoordinatesIsDenseTest : public PLYTest {};
using XYZPointTypes = ::testing::Types<BOOST_PP_SEQ_ENUM (PCL_XYZ_POINT_TYPES)>;
-TYPED_TEST_CASE (PLYCoordinatesIsDenseTest, XYZPointTypes);
+TYPED_TEST_SUITE (PLYCoordinatesIsDenseTest, XYZPointTypes);
TYPED_TEST (PLYCoordinatesIsDenseTest, NaNInCoordinates)
{
struct PLYNormalsIsDenseTest : public PLYTest {};
using NormalPointTypes = ::testing::Types<BOOST_PP_SEQ_ENUM (PCL_NORMAL_POINT_TYPES)>;
-TYPED_TEST_CASE (PLYNormalsIsDenseTest, NormalPointTypes);
+TYPED_TEST_SUITE (PLYNormalsIsDenseTest, NormalPointTypes);
TYPED_TEST (PLYNormalsIsDenseTest, NaNInNormals)
{
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/point_traits.h>
#include <pcl/point_types.h>
#include <iostream>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/point_cloud_image_extractors.h>
#include <pcl/compression/entropy_range_coder.h>
#include <pcl/compression/impl/entropy_range_coder.hpp>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <vector>
#include <sstream>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST (PCL, Static_Range_Coder_Test)
{
- std::stringstream sstream;
- std::vector<char> inputCharData;
- std::vector<char> outputCharData;
+ // Run test for different vector sizes
+ for (unsigned int vectorSize: { 253, 10000 })
+ {
+ std::stringstream sstream;
+ std::vector<char> inputCharData;
+ std::vector<char> outputCharData;
- std::vector<unsigned int> inputIntData;
- std::vector<unsigned int> outputIntData;
+ std::vector<unsigned int> inputIntData;
+ std::vector<unsigned int> outputIntData;
- unsigned long writeByteLen;
- unsigned long readByteLen;
+ unsigned long writeByteLen;
+ unsigned long readByteLen;
- // vector size
- const unsigned int vectorSize = 10000;
+ inputCharData.resize(vectorSize);
+ outputCharData.resize(vectorSize);
- inputCharData.resize(vectorSize);
- outputCharData.resize(vectorSize);
+ inputIntData.resize(vectorSize);
+ outputIntData.resize(vectorSize);
- inputIntData.resize(vectorSize);
- outputIntData.resize(vectorSize);
+ // fill vectors with random data
+ for (std::size_t i=0; i<vectorSize; i++)
+ {
+ inputCharData[i] = static_cast<char> (rand () & 0xFF);
+ inputIntData[i] = static_cast<unsigned int> (rand () & 0xFFFF);
+ }
- // fill vectors with random data
- for (std::size_t i=0; i<vectorSize; i++)
- {
- inputCharData[i] = static_cast<char> (rand () & 0xFF);
- inputIntData[i] = static_cast<unsigned int> (rand () & 0xFFFF);
- }
+ // initialize static range coder
+ pcl::StaticRangeCoder rangeCoder;
- // initialize static range coder
- pcl::StaticRangeCoder rangeCoder;
+ // encode char vector to stringstream
+ writeByteLen = rangeCoder.encodeCharVectorToStream(inputCharData, sstream);
- // encode char vector to stringstream
- writeByteLen = rangeCoder.encodeCharVectorToStream(inputCharData, sstream);
+ // decode stringstream to char vector
+ readByteLen = rangeCoder.decodeStreamToCharVector(sstream, outputCharData);
- // decode stringstream to char vector
- readByteLen = rangeCoder.decodeStreamToCharVector(sstream, outputCharData);
+ // compare amount of bytes that are read and written to/from stream
+ EXPECT_EQ (writeByteLen, readByteLen);
+ EXPECT_EQ (writeByteLen, sstream.str().length());
- // compare amount of bytes that are read and written to/from stream
- EXPECT_EQ (writeByteLen, readByteLen);
- EXPECT_EQ (writeByteLen, sstream.str().length());
-
- // compare input and output vector - should be identical
- EXPECT_EQ (inputCharData.size(), outputCharData.size());
- EXPECT_EQ (inputCharData.size(), vectorSize);
+ // compare input and output vector - should be identical
+ EXPECT_EQ (inputCharData.size(), outputCharData.size());
+ EXPECT_EQ (inputCharData.size(), vectorSize);
- for (std::size_t i=0; i<vectorSize; i++)
- {
- EXPECT_EQ (inputCharData[i], outputCharData[i]);
- }
+ for (std::size_t i=0; i<vectorSize; i++)
+ {
+ EXPECT_EQ (inputCharData[i], outputCharData[i]);
+ }
- sstream.clear();
+ sstream.clear();
- // encode integer vector to stringstream
- writeByteLen = rangeCoder.encodeIntVectorToStream(inputIntData, sstream);
+ // encode integer vector to stringstream
+ writeByteLen = rangeCoder.encodeIntVectorToStream(inputIntData, sstream);
- // decode stringstream to integer vector
- readByteLen = rangeCoder.decodeStreamToIntVector(sstream, outputIntData);
+ // decode stringstream to integer vector
+ readByteLen = rangeCoder.decodeStreamToIntVector(sstream, outputIntData);
- // compare amount of bytes that are read and written to/from stream
- EXPECT_EQ (writeByteLen, readByteLen);
+ // compare amount of bytes that are read and written to/from stream
+ EXPECT_EQ (writeByteLen, readByteLen);
- // compare input and output vector - should be identical
- EXPECT_EQ (inputIntData.size(), outputIntData.size());
- EXPECT_EQ (inputIntData.size(), vectorSize);
+ // compare input and output vector - should be identical
+ EXPECT_EQ (inputIntData.size(), outputIntData.size());
+ EXPECT_EQ (inputIntData.size(), vectorSize);
- for (std::size_t i=0; i<vectorSize; i++)
- {
- EXPECT_EQ (inputIntData[i], outputIntData[i]);
+ for (std::size_t i=0; i<vectorSize; i++)
+ {
+ EXPECT_EQ (inputIntData[i], outputIntData[i]);
+ }
}
-
}
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <iostream> // For debug
#include <map>
#include <pcl/common/time.h>
/** \author Gioia Ballin */
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_tests.h>
#include <pcl/io/pcd_io.h>
/** \author Michael Dixon */
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
*
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <vector>
#include <pcl/octree/octree_iterator.h>
#include <pcl/common/projection_matrix.h>
#include <pcl/point_types.h>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
using pcl::octree::OctreeBase;
using pcl::octree::OctreeIteratorBase;
OctreeLeafNodeDepthFirstIterator<OctreeBase<int> >,
OctreeFixedDepthIterator<OctreeBase<int> >,
OctreeLeafNodeBreadthFirstIterator<OctreeBase<int> > >;
-TYPED_TEST_CASE (OctreeIteratorTest, OctreeIteratorTypes);
+TYPED_TEST_SUITE (OctreeIteratorTest, OctreeIteratorTypes);
TYPED_TEST (OctreeIteratorTest, CopyConstructor)
{
* Stephen Fox (foxstephend@gmail.com)
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <vector>
#include <cstdio>
* In Proceedings of the International Conference on Intelligent Robots and Systems (IROS) 2012, Vilamoura (Portugal), 2012.
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
* $Id: $
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/correspondence_estimation_normal_shooting.h>
#include <pcl/features/normal_3d.h>
#include <random>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/eigen.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <limits>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/registration/warp_point_rigid_3d.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/sample_consensus/msac.h>
#include <pcl/sample_consensus/lmeds.h>
RandomizedMEstimatorSampleConsensus<PointXYZ>,
MaximumLikelihoodSampleConsensus<PointXYZ>
>;
-TYPED_TEST_CASE(SacTest, sacTypes);
+TYPED_TEST_SUITE(SacTest, sacTypes);
TYPED_TEST(SacTest, InfiniteLoop)
{
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_tests.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/pcl_tests.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
*
*/
#include <iostream>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/common/time.h>
#include <pcl/search/pcl_search.h>
#include <pcl/point_cloud.h>
*/
#include <iostream>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/common/distances.h>
#include <pcl/common/time.h>
#include <pcl/search/pcl_search.h>
*
*/
#include <iostream>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/common/time.h>
#include <pcl/search/pcl_search.h>
#include <pcl/point_cloud.h>
*
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <vector>
#include <cstdio>
#include <pcl/common/time.h>
*/
/** \author Julius Kammerl (julius@kammerl.de)*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <vector>
search::OrganizedNeighbor<PointXYZ> organizedNeighborSearch;
- std::vector<int> k_indices;
- std::vector<float> k_sqr_distances;
-
- std::vector<int> k_indices_bruteforce;
- std::vector<float> k_sqr_distances_bruteforce;
-
// typical focal length from kinect
constexpr double oneOverFocalLength = 0.0018;
*
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <vector>
#include <stdio.h>
#include <pcl/common/time.h>
pcl::search::Search<PointXYZ>* organizedNeighborSearch = new pcl::search::OrganizedNeighbor<PointXYZ>();
- std::vector<int> k_indices;
- std::vector<float> k_sqr_distances;
-
- std::vector<int> k_indices_bruteforce;
- std::vector<float> k_sqr_distances_bruteforce;
-
// typical focal length from kinect
unsigned int randomIdx;
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <random>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <boost/property_tree/info_parser.hpp>
#include <boost/foreach.hpp>
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/segmentation/random_walker.h>
}
}
-INSTANTIATE_TEST_CASE_P (VariousGraphs,
+INSTANTIATE_TEST_SUITE_P (VariousGraphs,
RandomWalkerTest,
::testing::Values ("graph0.info",
"graph1.info",
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <random>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
*
*/
-#include <gtest/gtest.h>
+#include <pcl/test/gtest.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
{
print_error ("Syntax is: %s input1.pcd input2.pcd input3.pcd (etc.)\n", argv[0]);
print_info (" where options are:\n");
- print_info (" -min_depth z_min = the depth of the near clipping plane\n");
- print_info (" -max_depth z_max = the depth of the far clipping plane\n");
+ print_info (" -min_depth z_min = the depth of the near clipping plane\n");
+ print_info (" -max_depth z_max = the depth of the far clipping plane\n");
print_info (" -max_height y_max = the height of the vertical clipping plane\n");
print_info ("Two new template files will be created for each input file. They will append ");
print_info ("the following suffixes to the original filename:\n");
void printElapsedTimeAndNumberOfPoints (double t, int w, int h=1)
{
- print_info ("[done, "); print_value ("%g", t); print_info (" ms : ");
+ print_info ("[done, "); print_value ("%g", t); print_info (" ms : ");
print_value ("%d", w*h); print_info (" points]\n");
}
float min_depth, float max_depth, float max_height)
{
std::vector<bool> foreground_mask (input->size (), false);
-
+
// Mask off points outside the specified near and far depth thresholds
pcl::IndicesPtr indices (new std::vector<int>);
for (std::size_t i = 0; i < input->size (); ++i)
seg.setIndices (indices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
- seg.segment (*inliers, *coefficients);
-
+ seg.segment (*inliers, *coefficients);
+
// Mask off the plane inliers
for (const int &index : inliers->indices)
foreground_mask[index] = false;
}
void
-trainTemplate (const PointCloudXYZRGBA::ConstPtr & input, const std::vector<bool> &foreground_mask,
+trainTemplate (const PointCloudXYZRGBA::ConstPtr & input, const std::vector<bool> &foreground_mask,
pcl::LINEMOD & linemod)
{
pcl::ColorGradientModality<pcl::PointXYZRGBA> color_grad_mod;
color_grad_mod.setInputCloud (input);
color_grad_mod.processInputData ();
-
+
pcl::SurfaceNormalModality<pcl::PointXYZRGBA> surface_norm_mod;
surface_norm_mod.setInputCloud (input);
surface_norm_mod.processInputData ();
trainTemplate (input, foreground_mask, linemod);
// Save the LINEMOD template
- std::ofstream file_stream;
- file_stream.open (template_sqmmt_filename.c_str (), std::ofstream::out | std::ofstream::binary);
- linemod.getTemplate (0).serialize (file_stream);
- file_stream.close ();
+ linemod.saveTemplates (template_sqmmt_filename.c_str());
}
/* ---[ */
float max_height = std::numeric_limits<float>::max ();
parse_argument (argc, argv, "-max_height", max_height);
+ int error_code = 0;
+ bool processed_at_least_one_pcd = false;
+
// Segment and create templates for each input file
for (const int &p_file_index : p_file_indices)
{
// Load input file
const std::string input_filename = argv[p_file_index];
PointCloudXYZRGBA::Ptr cloud (new PointCloudXYZRGBA);
- if (!loadCloud (input_filename, *cloud))
- return (-1);
+
+ if (!loadCloud (input_filename, *cloud))
+ {
+ error_code = -1;
+ std::string warn_msg = "Could not load point cloud from file: " + input_filename + "\n";
+ print_warn (warn_msg.c_str ());
+ continue;
+ }
+
+ if (!cloud->isOrganized())
+ {
+ std::string warn_msg = "Unorganized point cloud detected. Skipping file " + input_filename + "\n";
+ print_warn(warn_msg.c_str());
+ continue;
+ }
+ else
+ {
+ processed_at_least_one_pcd = true;
+ }
// Construct output filenames
std::string sqmmt_filename = input_filename;
compute (cloud, min_depth, max_depth, max_height, pcd_filename, sqmmt_filename);
}
-}
+ if (!processed_at_least_one_pcd)
+ {
+ print_error("All input pcd files are unorganized.\n");
+ }
+ return error_code;
+}
int subdiv_level = 1;
double scan_dist = 3;
- std::string fname, base;
+ std::string fname;
char seq[256];
// Compute start/stop for vertical and horizontal
int k = *(H - 1);
a[j] = k;
q[k] += q[j] - 1;
- L++;
+ ++L;
if ( q[k] < 1.0 )
{
*L-- = k;
pcl::tracking::ParticleFilterTracker<PointInT, StateT>::initParticles (bool reset)
{
particles_.reset (new PointCloudState ());
- std::vector<double> initial_noise_mean;
if (reset)
{
representative_state_.zero ();
#include <pcl/pcl_macros.h>
+#include <vector>
+
namespace pcl
{
namespace visualization
scalars->SetNumberOfTuples (nr_points);
unsigned char* colors = scalars->GetPointer (0);
- int j = 0;
// If XYZ present, check if the points are invalid
int x_idx = -1;
for (std::size_t d = 0; d < fields_.size (); ++d)
if (x_idx != -1)
{
+ int j = 0;
// Color every point
for (vtkIdType cp = 0; cp < nr_points; ++cp)
{
#endif
// PCL includes
+#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/common/io.h>
#include <pcl/visualization/common/common.h>
* This virtual method should not be overriden or used. The default implementation
* is provided only for backwards compatibility with handlers that were written
* before PCL 1.10.0 and will be removed in future. */
- [[deprecated("use getColor() without parameters instead")]]
+ PCL_DEPRECATED("use getColor() without parameters instead")
virtual bool
getColor (vtkSmartPointer<vtkDataArray> &scalars) const {
scalars = getColor ();
* This virtual method should not be overriden or used. The default implementation
* is provided only for backwards compatibility with handlers that were written
* before PCL 1.10.0 and will be removed in future. */
- [[deprecated("use getColor() without parameters instead")]]
+ PCL_DEPRECATED("use getColor() without parameters instead")
virtual bool
getColor (vtkSmartPointer<vtkDataArray> &scalars) const {
scalars = getColor ();
style_->setUseVbos (use_vbos_);
#else
PCL_WARN ("[PCLVisualizer::setUseVbos] Has no effect when OpenGL version is ≥ 2\n");
+ (void) use_vbos;
#endif
}
projects / pcl.git / commitdiff