Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
boundary.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40
41#pragma once
42
43#include <pcl/features/boundary.h>
44#include <pcl/common/point_tests.h> // for pcl::isFinite
45
46#include <cfloat>
47
48
49//////////////////////////////////////////////////////////////////////////////////////////////
50template <typename PointInT, typename PointNT, typename PointOutT> bool
52 const pcl::PointCloud<PointInT> &cloud, int q_idx,
53 const pcl::Indices &indices,
54 const Eigen::Vector4f &u, const Eigen::Vector4f &v,
55 const float angle_threshold)
56{
57 return (isBoundaryPoint (cloud, cloud[q_idx], indices, u, v, angle_threshold));
58}
59
60//////////////////////////////////////////////////////////////////////////////////////////////
61template <typename PointInT, typename PointNT, typename PointOutT> bool
63 const pcl::PointCloud<PointInT> &cloud, const PointInT &q_point,
64 const pcl::Indices &indices,
65 const Eigen::Vector4f &u, const Eigen::Vector4f &v,
66 const float angle_threshold)
67{
68 if (indices.size () < 3)
69 return (false);
70
71 if (!std::isfinite (q_point.x) || !std::isfinite (q_point.y) || !std::isfinite (q_point.z))
72 return (false);
73
74 // Compute the angles between each neighboring point and the query point itself
75 std::vector<float> angles (indices.size ());
76 float max_dif = 0, dif;
77 int cp = 0;
78
79 for (const auto &index : indices)
80 {
81 if (!std::isfinite (cloud[index].x) ||
82 !std::isfinite (cloud[index].y) ||
83 !std::isfinite (cloud[index].z))
84 continue;
85
86 Eigen::Vector4f delta = cloud[index].getVector4fMap () - q_point.getVector4fMap ();
87 if (delta == Eigen::Vector4f::Zero())
88 continue;
89
90 angles[cp++] = std::atan2 (v.dot (delta), u.dot (delta)); // the angles are fine between -PI and PI too
91 }
92 if (cp == 0)
93 return (false);
94
95 angles.resize (cp);
96 std::sort (angles.begin (), angles.end ());
97
98 // Compute the maximal angle difference between two consecutive angles
99 for (std::size_t i = 0; i < angles.size () - 1; ++i)
100 {
101 dif = angles[i + 1] - angles[i];
102 if (max_dif < dif)
103 max_dif = dif;
104 }
105 // Get the angle difference between the last and the first
106 dif = 2 * static_cast<float> (M_PI) - angles[angles.size () - 1] + angles[0];
107 if (max_dif < dif)
108 max_dif = dif;
109
110 // Check results
111 return (max_dif > angle_threshold);
112}
113
114//////////////////////////////////////////////////////////////////////////////////////////////
115template <typename PointInT, typename PointNT, typename PointOutT> void
117{
118 // Allocate enough space to hold the results
119 // \note This resize is irrelevant for a radiusSearch ().
120 pcl::Indices nn_indices (k_);
121 std::vector<float> nn_dists (k_);
122
123 Eigen::Vector4f u = Eigen::Vector4f::Zero (), v = Eigen::Vector4f::Zero ();
124
125 output.is_dense = true;
126 // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense
127 if (input_->is_dense)
128 {
129 // Iterating over the entire index vector
130 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
131 {
132 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
133 {
134 output[idx].boundary_point = std::numeric_limits<std::uint8_t>::quiet_NaN ();
135 output.is_dense = false;
136 continue;
137 }
138
139 // Obtain a coordinate system on the least-squares plane
140 //v = (*normals_)[(*indices_)[idx]].getNormalVector4fMap ().unitOrthogonal ();
141 //u = (*normals_)[(*indices_)[idx]].getNormalVector4fMap ().cross3 (v);
142 getCoordinateSystemOnPlane ((*normals_)[(*indices_)[idx]], u, v);
143
144 // Estimate whether the point is lying on a boundary surface or not
145 output[idx].boundary_point = isBoundaryPoint (*surface_, (*input_)[(*indices_)[idx]], nn_indices, u, v, angle_threshold_);
146 }
147 }
148 else
149 {
150 // Iterating over the entire index vector
151 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
152 {
153 if (!isFinite ((*input_)[(*indices_)[idx]]) ||
154 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
155 {
156 output[idx].boundary_point = std::numeric_limits<std::uint8_t>::quiet_NaN ();
157 output.is_dense = false;
158 continue;
159 }
160
161 // Obtain a coordinate system on the least-squares plane
162 //v = (*normals_)[(*indices_)[idx]].getNormalVector4fMap ().unitOrthogonal ();
163 //u = (*normals_)[(*indices_)[idx]].getNormalVector4fMap ().cross3 (v);
164 getCoordinateSystemOnPlane ((*normals_)[(*indices_)[idx]], u, v);
165
166 // Estimate whether the point is lying on a boundary surface or not
167 output[idx].boundary_point = isBoundaryPoint (*surface_, (*input_)[(*indices_)[idx]], nn_indices, u, v, angle_threshold_);
168 }
169 }
170}
171
172#define PCL_INSTANTIATE_BoundaryEstimation(PointInT,PointNT,PointOutT) template class PCL_EXPORTS pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>;
173
void computeFeature(PointCloudOut &output) override
Estimate whether a set of points is lying on surface boundaries using an angle criterion for all poin...
Definition boundary.hpp:116
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition boundary.h:96
bool isBoundaryPoint(const pcl::PointCloud< PointInT > &cloud, int q_idx, const pcl::Indices &indices, const Eigen::Vector4f &u, const Eigen::Vector4f &v, const float angle_threshold)
Check whether a point is a boundary point in a planar patch of projected points given by indices.
Definition boundary.hpp:51
PointCloud represents the base class in PCL for storing collections of 3D points.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition point_tests.h:55
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
#define M_PI
Definition pcl_macros.h:201