Point Cloud Library (PCL) 1.13.0
organized.h
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 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 * $Id$
37 *
38 */
39
40#pragma once
41
42#include <pcl/memory.h>
43#include <pcl/pcl_macros.h>
44#include <pcl/point_cloud.h>
45#include <pcl/search/search.h>
46#include <pcl/common/eigen.h>
47
48#include <algorithm>
49#include <vector>
50
51namespace pcl
52{
53 namespace search
54 {
55 /** \brief OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
56 * \author Radu B. Rusu, Julius Kammerl, Suat Gedikli, Koen Buys
57 * \ingroup search
58 */
59 template<typename PointT>
61 {
62
63 public:
64 // public typedefs
67
69
70 using Ptr = shared_ptr<pcl::search::OrganizedNeighbor<PointT> >;
71 using ConstPtr = shared_ptr<const pcl::search::OrganizedNeighbor<PointT> >;
72
76
77 /** \brief Constructor
78 * \param[in] sorted_results whether the results should be return sorted in ascending order on the distances or not.
79 * This applies only for radius search, since knn always returns sorted resutls
80 * \param[in] eps the threshold for the mean-squared-error of the estimation of the projection matrix.
81 * if the MSE is above this value, the point cloud is considered as not from a projective device,
82 * thus organized neighbor search can not be applied on that cloud.
83 * \param[in] pyramid_level the level of the down sampled point cloud to be used for projection matrix estimation
84 */
85 OrganizedNeighbor (bool sorted_results = false, float eps = 1e-4f, unsigned pyramid_level = 5)
86 : Search<PointT> ("OrganizedNeighbor", sorted_results)
87 , projection_matrix_ (Eigen::Matrix<float, 3, 4, Eigen::RowMajor>::Zero ())
88 , KR_ (Eigen::Matrix<float, 3, 3, Eigen::RowMajor>::Zero ())
89 , KR_KRT_ (Eigen::Matrix<float, 3, 3, Eigen::RowMajor>::Zero ())
90 , eps_ (eps)
91 , pyramid_level_ (pyramid_level)
92 {
93 }
94
95 /** \brief Empty deconstructor. */
96 ~OrganizedNeighbor () override = default;
97
98 /** \brief Test whether this search-object is valid (input is organized AND from projective device)
99 * User should use this method after setting the input cloud, since setInput just prints an error
100 * if input is not organized or a projection matrix could not be determined.
101 * \return true if the input data is organized and from a projective device, false otherwise
102 */
103 bool
104 isValid () const
105 {
106 // determinant (KR) = determinant (K) * determinant (R) = determinant (K) = f_x * f_y.
107 // If we expect at max an opening angle of 170degree in x-direction -> f_x = 2.0 * width / tan (85 degree);
108 // 2 * tan (85 degree) ~ 22.86
109 float min_f = 0.043744332f * static_cast<float>(input_->width);
110 //std::cout << "isValid: " << determinant3x3Matrix<Eigen::Matrix3f> (KR_ / sqrt (KR_KRT_.coeff (8))) << " >= " << (min_f * min_f) << std::endl;
111 return (determinant3x3Matrix<Eigen::Matrix3f> (KR_ / std::sqrt (KR_KRT_.coeff (8))) >= (min_f * min_f));
112 }
113
114 /** \brief Compute the camera matrix
115 * \param[out] camera_matrix the resultant computed camera matrix
116 */
117 void
118 computeCameraMatrix (Eigen::Matrix3f& camera_matrix) const;
119
120 /** \brief Provide a pointer to the input data set, if user has focal length he must set it before calling this
121 * \param[in] cloud the const boost shared pointer to a PointCloud message
122 * \param[in] indices the const boost shared pointer to PointIndices
123 */
124 void
125 setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr &indices = IndicesConstPtr ()) override
126 {
127 input_ = cloud;
128
129 mask_.resize (input_->size ());
130 input_ = cloud;
131 indices_ = indices;
132
133 if (indices_ && !indices_->empty())
134 {
135 mask_.assign (input_->size (), 0);
136 for (const auto& idx : *indices_)
137 mask_[idx] = 1;
138 }
139 else
140 mask_.assign (input_->size (), 1);
141
143 }
144
145 /** \brief Search for all neighbors of query point that are within a given radius.
146 * \param[in] p_q the given query point
147 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
148 * \param[out] k_indices the resultant indices of the neighboring points
149 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
150 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
151 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
152 * returned.
153 * \return number of neighbors found in radius
154 */
155 int
156 radiusSearch (const PointT &p_q,
157 double radius,
158 Indices &k_indices,
159 std::vector<float> &k_sqr_distances,
160 unsigned int max_nn = 0) const override;
161
162 /** \brief estimated the projection matrix from the input cloud. */
163 void
165
166 /** \brief Search for the k-nearest neighbors for a given query point.
167 * \note limiting the maximum search radius (with setMaxDistance) can lead to a significant improvement in search speed
168 * \param[in] p_q the given query point (\ref setInputCloud must be given a-priori!)
169 * \param[in] k the number of neighbors to search for (used only if horizontal and vertical window not given already!)
170 * \param[out] k_indices the resultant point indices (must be resized to \a k beforehand!)
171 * \param[out] k_sqr_distances \note this function does not return distances
172 * \return number of neighbors found
173 * @todo still need to implements this functionality
174 */
175 int
176 nearestKSearch (const PointT &p_q,
177 int k,
178 Indices &k_indices,
179 std::vector<float> &k_sqr_distances) const override;
180
181 /** \brief projects a point into the image
182 * \param[in] p point in 3D World Coordinate Frame to be projected onto the image plane
183 * \param[out] q the 2D projected point in pixel coordinates (u,v)
184 * @return true if projection is valid, false otherwise
185 */
186 bool projectPoint (const PointT& p, pcl::PointXY& q) const;
187
188 protected:
189
190 struct Entry
191 {
192 Entry (index_t idx, float dist) : index (idx), distance (dist) {}
193 Entry () : index (0), distance (0) {}
195 float distance;
196
197 inline bool
198 operator < (const Entry& other) const
199 {
200 return (distance < other.distance);
201 }
202 };
203
204 /** \brief test if point given by index is among the k NN in results to the query point.
205 * \param[in] query query point
206 * \param[in] k number of maximum nn interested in
207 * \param[in,out] queue priority queue with k NN
208 * \param[in] index index on point to be tested
209 * \return whether the top element changed or not.
210 */
211 inline bool
212 testPoint (const PointT& query, unsigned k, std::vector<Entry>& queue, index_t index) const
213 {
214 const PointT& point = input_->points [index];
215 if (mask_ [index] && std::isfinite (point.x))
216 {
217 //float squared_distance = (point.getVector3fMap () - query.getVector3fMap ()).squaredNorm ();
218 float dist_x = point.x - query.x;
219 float dist_y = point.y - query.y;
220 float dist_z = point.z - query.z;
221 float squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z;
222 const auto queue_size = queue.size ();
223 const auto insert_into_queue = [&]{ queue.emplace (
224 std::upper_bound (queue.begin(), queue.end(), squared_distance,
225 [](float dist, const Entry& ent){ return dist<ent.distance; }),
226 index, squared_distance); };
227 if (queue_size < k)
228 {
229 insert_into_queue ();
230 return (queue_size + 1) == k;
231 }
232 if (queue.back ().distance > squared_distance)
233 {
234 queue.pop_back ();
235 insert_into_queue ();
236 return true; // top element has changed!
237 }
238 }
239 return false;
240 }
241
242 inline void
243 clipRange (int& begin, int &end, int min, int max) const
244 {
245 begin = std::max (std::min (begin, max), min);
246 end = std::min (std::max (end, min), max);
247 }
248
249 /** \brief Obtain a search box in 2D from a sphere with a radius in 3D
250 * \param[in] point the query point (sphere center)
251 * \param[in] squared_radius the squared sphere radius
252 * \param[out] minX the min X box coordinate
253 * \param[out] minY the min Y box coordinate
254 * \param[out] maxX the max X box coordinate
255 * \param[out] maxY the max Y box coordinate
256 */
257 void
258 getProjectedRadiusSearchBox (const PointT& point, float squared_radius, unsigned& minX, unsigned& minY,
259 unsigned& maxX, unsigned& maxY) const;
260
261
262 /** \brief the projection matrix. Either set by user or calculated by the first / each input cloud */
263 Eigen::Matrix<float, 3, 4, Eigen::RowMajor> projection_matrix_;
264
265 /** \brief inveser of the left 3x3 projection matrix which is K * R (with K being the camera matrix and R the rotation matrix)*/
266 Eigen::Matrix<float, 3, 3, Eigen::RowMajor> KR_;
267
268 /** \brief inveser of the left 3x3 projection matrix which is K * R (with K being the camera matrix and R the rotation matrix)*/
269 Eigen::Matrix<float, 3, 3, Eigen::RowMajor> KR_KRT_;
270
271 /** \brief epsilon value for the MSE of the projection matrix estimation*/
272 const float eps_;
273
274 /** \brief using only a subsample of points to calculate the projection matrix. pyramid_level_ = use down sampled cloud given by pyramid_level_*/
275 const unsigned pyramid_level_;
276
277 /** \brief mask, indicating whether the point was in the indices list or not.*/
278 std::vector<unsigned char> mask_;
279 public:
281 };
282 }
283}
284
285#ifdef PCL_NO_PRECOMPILE
286#include <pcl/search/impl/organized.hpp>
287#endif
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
Definition: organized.h:61
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input data set, if user has focal length he must set it before calling this.
Definition: organized.h:125
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: organized.h:68
int radiusSearch(const PointT &p_q, double radius, Indices &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const override
Search for all neighbors of query point that are within a given radius.
Definition: organized.hpp:49
int nearestKSearch(const PointT &p_q, int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for a given query point.
Definition: organized.hpp:114
bool isValid() const
Test whether this search-object is valid (input is organized AND from projective device) User should ...
Definition: organized.h:104
Eigen::Matrix< float, 3, 3, Eigen::RowMajor > KR_
inveser of the left 3x3 projection matrix which is K * R (with K being the camera matrix and R the ro...
Definition: organized.h:266
shared_ptr< const pcl::search::OrganizedNeighbor< PointT > > ConstPtr
Definition: organized.h:71
void computeCameraMatrix(Eigen::Matrix3f &camera_matrix) const
Compute the camera matrix.
Definition: organized.hpp:326
bool testPoint(const PointT &query, unsigned k, std::vector< Entry > &queue, index_t index) const
test if point given by index is among the k NN in results to the query point.
Definition: organized.h:212
std::vector< unsigned char > mask_
mask, indicating whether the point was in the indices list or not.
Definition: organized.h:278
void clipRange(int &begin, int &end, int min, int max) const
Definition: organized.h:243
Eigen::Matrix< float, 3, 4, Eigen::RowMajor > projection_matrix_
the projection matrix.
Definition: organized.h:263
void estimateProjectionMatrix()
estimated the projection matrix from the input cloud.
Definition: organized.hpp:333
typename PointCloud::Ptr PointCloudPtr
Definition: organized.h:66
const unsigned pyramid_level_
using only a subsample of points to calculate the projection matrix.
Definition: organized.h:275
Eigen::Matrix< float, 3, 3, Eigen::RowMajor > KR_KRT_
inveser of the left 3x3 projection matrix which is K * R (with K being the camera matrix and R the ro...
Definition: organized.h:269
shared_ptr< pcl::search::OrganizedNeighbor< PointT > > Ptr
Definition: organized.h:70
OrganizedNeighbor(bool sorted_results=false, float eps=1e-4f, unsigned pyramid_level=5)
Constructor.
Definition: organized.h:85
bool projectPoint(const PointT &p, pcl::PointXY &q) const
projects a point into the image
Definition: organized.hpp:378
void getProjectedRadiusSearchBox(const PointT &point, float squared_radius, unsigned &minX, unsigned &minY, unsigned &maxX, unsigned &maxY) const
Obtain a search box in 2D from a sphere with a radius in 3D.
Definition: organized.hpp:269
~OrganizedNeighbor() override=default
Empty deconstructor.
const float eps_
epsilon value for the MSE of the projection matrix estimation
Definition: organized.h:272
Generic search class.
Definition: search.h:75
PointCloudConstPtr input_
Definition: search.h:401
IndicesConstPtr indices_
Definition: search.h:402
pcl::IndicesConstPtr IndicesConstPtr
Definition: search.h:85
bool sorted_results_
Definition: search.h:403
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
Defines functions, macros and traits for allocating and using memory.
Definition: bfgs.h:10
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
Defines all the PCL and non-PCL macros used.
A 2D point structure representing Euclidean xy coordinates.
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: organized.h:191
bool operator<(const Entry &other) const
Definition: organized.h:198
float distance
Definition: organized.h:195
Entry()
Definition: organized.h:193
index_t index
Definition: organized.h:194
Entry(index_t idx, float dist)
Definition: organized.h:192