Point Cloud Library (PCL)  1.15.1-dev
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/common/point_tests.h> // for pcl::isFinite
46 #include <pcl/search/search.h>
47 #include <pcl/common/eigen.h>
48 
49 #include <algorithm>
50 #include <vector>
51 
52 namespace pcl
53 {
54  namespace search
55  {
56  /** \brief OrganizedNeighbor is a class for optimized nearest neighbor search in
57  * organized projectable point clouds, for instance from Time-Of-Flight cameras or
58  * stereo cameras. Note that rotating LIDARs may output organized clouds, but are
59  * not projectable via a pinhole camera model into two dimensions and thus will
60  * generally not work with this class.
61  * \author Radu B. Rusu, Julius Kammerl, Suat Gedikli, Koen Buys
62  * \ingroup search
63  */
64  template<typename PointT>
65  class OrganizedNeighbor : public pcl::search::Search<PointT>
66  {
67 
68  public:
69  // public typedefs
71  using PointCloudPtr = typename PointCloud::Ptr;
72 
74 
75  using Ptr = shared_ptr<pcl::search::OrganizedNeighbor<PointT> >;
76  using ConstPtr = shared_ptr<const pcl::search::OrganizedNeighbor<PointT> >;
77 
81 
82  /** \brief Constructor
83  * \param[in] sorted_results whether the results should be return sorted in ascending order on the distances or not.
84  * This applies only for radius search, since knn always returns sorted results
85  * \param[in] eps the threshold for the mean-squared-error of the estimation of the projection matrix.
86  * if the MSE is above this value, the point cloud is considered as not from a projective device,
87  * thus organized neighbor search can not be applied on that cloud.
88  * \param[in] pyramid_level the level of the down sampled point cloud to be used for projection matrix estimation
89  */
90  OrganizedNeighbor (bool sorted_results = false, float eps = 1e-4f, unsigned pyramid_level = 5)
91  : Search<PointT> ("OrganizedNeighbor", sorted_results)
92  , projection_matrix_ (Eigen::Matrix<float, 3, 4, Eigen::RowMajor>::Zero ())
93  , KR_ (Eigen::Matrix<float, 3, 3, Eigen::RowMajor>::Zero ())
94  , KR_KRT_ (Eigen::Matrix<float, 3, 3, Eigen::RowMajor>::Zero ())
95  , eps_ (eps)
96  , pyramid_level_ (pyramid_level)
97  {
98  }
99 
100  /** \brief Empty deconstructor. */
101  ~OrganizedNeighbor () override = default;
102 
103  /** \brief Test whether this search-object is valid (input is organized AND from projective device)
104  * User should use this method after setting the input cloud, since setInput just prints an error
105  * if input is not organized or a projection matrix could not be determined.
106  * \return true if the input data is organized and from a projective device, false otherwise
107  */
108  bool
109  isValid () const
110  {
111  // determinant (KR) = determinant (K) * determinant (R) = determinant (K) = f_x * f_y.
112  // If we expect at max an opening angle of 170degree in x-direction -> f_x = 2.0 * width / tan (85 degree);
113  // 2 * tan (85 degree) ~ 22.86
114  float min_f = 0.043744332f * static_cast<float>(input_->width);
115  //std::cout << "isValid: " << determinant3x3Matrix<Eigen::Matrix3f> (KR_ / sqrt (KR_KRT_.coeff (8))) << " >= " << (min_f * min_f) << std::endl;
116  return (determinant3x3Matrix<Eigen::Matrix3f> (KR_ / std::sqrt (KR_KRT_.coeff (8))) >= (min_f * min_f));
117  }
118 
119  /** \brief Compute the camera matrix
120  * \param[out] camera_matrix the resultant computed camera matrix
121  */
122  void
123  computeCameraMatrix (Eigen::Matrix3f& camera_matrix) const;
124 
125  /** \brief Provide a pointer to the input data set, if user has focal length he must set it before calling this
126  * \param[in] cloud the const boost shared pointer to a PointCloud message
127  * \param[in] indices the const boost shared pointer to PointIndices
128  */
129  bool
130  setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr &indices = IndicesConstPtr ()) override
131  {
132  input_ = cloud;
133 
134  mask_.resize (input_->size ());
135  input_ = cloud;
136  indices_ = indices;
137 
138  if (indices_ && !indices_->empty())
139  {
140  mask_.assign (input_->size (), 0);
141  for (const auto& idx : *indices_)
142  if (pcl::isFinite((*input_)[idx]))
143  mask_[idx] = 1;
144  }
145  else
146  {
147  mask_.assign (input_->size (), 0);
148  for (std::size_t idx=0; idx<input_->size(); ++idx)
149  if (pcl::isFinite((*input_)[idx]))
150  mask_[idx] = 1;
151  }
152 
153  return (eps_ < 0 || estimateProjectionMatrix ()) && testProjectionMatrix() && isValid ();
154  }
155 
156  /** \brief Search for all neighbors of query point that are within a given radius.
157  * \param[in] p_q the given query point
158  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
159  * \param[out] k_indices the resultant indices of the neighboring points
160  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
161  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
162  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
163  * returned.
164  * \return number of neighbors found in radius
165  */
166  int
167  radiusSearch (const PointT &p_q,
168  double radius,
169  Indices &k_indices,
170  std::vector<float> &k_sqr_distances,
171  unsigned int max_nn = 0) const override;
172 
173  /** \brief Estimate the projection matrix from the input cloud.
174  * \return True if it was possible to estimate the matrix, false otherwise
175  */
176  bool
178 
179  /** \brief Quick test if projection matrix and input cloud work together.
180  */
181  bool
182  testProjectionMatrix () const;
183 
184  /** \brief Set projection matrix manually. Projection matrix will _not_ be estimated automatically any more. If you want to use this, call it _before_ setInputCloud.
185  */
186  void
187  setProjectionMatrix (const Eigen::Matrix<float, 3, 4>& projection_matrix)
188  {
189  eps_ = -1.0f; // signal to not call estimateProjectionMatrix
190  projection_matrix_ = projection_matrix;
191  // rest is the same as in estimateProjectionMatrix():
192  // get left 3x3 sub matrix, which contains K * R, with K = camera matrix = [[fx s cx] [0 fy cy] [0 0 1]]
193  // and R being the rotation matrix
194  KR_ = projection_matrix_.topLeftCorner <3, 3> ();
195 
196  // precalculate KR * KR^T needed by calculations during nn-search
197  KR_KRT_ = KR_ * KR_.transpose ();
198  }
199 
200  /** \brief Search for the k-nearest neighbors for a given query point.
201  * \note limiting the maximum search radius (with setMaxDistance) can lead to a significant improvement in search speed
202  * \param[in] p_q the given query point (\ref setInputCloud must be given a-priori!)
203  * \param[in] k the number of neighbors to search for (used only if horizontal and vertical window not given already!)
204  * \param[out] k_indices the resultant point indices (must be resized to \a k beforehand!)
205  * \param[out] k_sqr_distances \note this function does not return distances
206  * \return number of neighbors found
207  * @todo still need to implements this functionality
208  */
209  int
210  nearestKSearch (const PointT &p_q,
211  int k,
212  Indices &k_indices,
213  std::vector<float> &k_sqr_distances) const override;
214 
215  /** \brief projects a point into the image
216  * \param[in] p point in 3D World Coordinate Frame to be projected onto the image plane
217  * \param[out] q the 2D projected point in pixel coordinates (u,v)
218  * @return true if projection is valid, false otherwise
219  */
220  bool projectPoint (const PointT& p, pcl::PointXY& q) const;
221 
222  protected:
223 
224  struct Entry
225  {
226  Entry (index_t idx, float dist) : index (idx), distance (dist) {}
227  Entry () : index (0), distance (0) {}
229  float distance;
230 
231  inline bool
232  operator < (const Entry& other) const
233  {
234  return (distance < other.distance);
235  }
236  };
237 
238  /** \brief test if point given by index is among the k NN in results to the query point.
239  * \param[in] query query point
240  * \param[in] k number of maximum nn interested in
241  * \param[in,out] queue priority queue with k NN
242  * \param[in] index index on point to be tested
243  * \return whether the top element changed or not.
244  */
245  inline bool
246  testPoint (const PointT& query, unsigned k, std::vector<Entry>& queue, index_t index) const
247  {
248  const PointT& point = input_->points [index];
249  if (mask_ [index])
250  {
251  //float squared_distance = (point.getVector3fMap () - query.getVector3fMap ()).squaredNorm ();
252  float dist_x = point.x - query.x;
253  float dist_y = point.y - query.y;
254  float dist_z = point.z - query.z;
255  float squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z;
256  const auto queue_size = queue.size ();
257  const auto insert_into_queue = [&]{ queue.emplace (
258  std::upper_bound (queue.begin(), queue.end(), squared_distance,
259  [](float dist, const Entry& ent){ return dist<ent.distance; }),
260  index, squared_distance); };
261  if (queue_size < k)
262  {
263  insert_into_queue ();
264  return (queue_size + 1) == k;
265  }
266  if (queue.back ().distance > squared_distance)
267  {
268  queue.pop_back ();
269  insert_into_queue ();
270  return true; // top element has changed!
271  }
272  }
273  return false;
274  }
275 
276  inline void
277  clipRange (int& begin, int &end, int min, int max) const
278  {
279  begin = std::max (std::min (begin, max), min);
280  end = std::min (std::max (end, min), max);
281  }
282 
283  /** \brief Obtain a search box in 2D from a sphere with a radius in 3D
284  * \param[in] point the query point (sphere center)
285  * \param[in] squared_radius the squared sphere radius
286  * \param[out] minX the min X box coordinate
287  * \param[out] minY the min Y box coordinate
288  * \param[out] maxX the max X box coordinate
289  * \param[out] maxY the max Y box coordinate
290  */
291  void
292  getProjectedRadiusSearchBox (const PointT& point, float squared_radius, unsigned& minX, unsigned& minY,
293  unsigned& maxX, unsigned& maxY) const;
294 
295 
296  /** \brief the projection matrix. Either set by user or calculated by the first / each input cloud */
297  Eigen::Matrix<float, 3, 4, Eigen::RowMajor> projection_matrix_;
298 
299  /** \brief inveser of the left 3x3 projection matrix which is K * R (with K being the camera matrix and R the rotation matrix)*/
300  Eigen::Matrix<float, 3, 3, Eigen::RowMajor> KR_;
301 
302  /** \brief inveser of the left 3x3 projection matrix which is K * R (with K being the camera matrix and R the rotation matrix)*/
303  Eigen::Matrix<float, 3, 3, Eigen::RowMajor> KR_KRT_;
304 
305  /** \brief epsilon value for the MSE of the projection matrix estimation*/
306  float eps_;
307 
308  /** \brief using only a subsample of points to calculate the projection matrix. pyramid_level_ = use down sampled cloud given by pyramid_level_*/
309  const unsigned pyramid_level_;
310 
311  /** \brief mask, indicating whether the point was in the indices list or not, and whether it is finite.*/
312  std::vector<unsigned char> mask_;
313  public:
315  };
316  }
317 }
318 
319 #ifdef PCL_NO_PRECOMPILE
320 #include <pcl/search/impl/organized.hpp>
321 #endif
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:174
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:414
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:415
OrganizedNeighbor is a class for optimized nearest neighbor search in organized projectable point clo...
Definition: organized.h:66
bool estimateProjectionMatrix()
Estimate the projection matrix from the input cloud.
Definition: organized.hpp:333
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: organized.h:73
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:109
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:300
shared_ptr< const pcl::search::OrganizedNeighbor< PointT > > ConstPtr
Definition: organized.h:76
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:246
std::vector< unsigned char > mask_
mask, indicating whether the point was in the indices list or not, and whether it is finite.
Definition: organized.h:312
float eps_
epsilon value for the MSE of the projection matrix estimation
Definition: organized.h:306
void setProjectionMatrix(const Eigen::Matrix< float, 3, 4 > &projection_matrix)
Set projection matrix manually.
Definition: organized.h:187
void clipRange(int &begin, int &end, int min, int max) const
Definition: organized.h:277
Eigen::Matrix< float, 3, 4, Eigen::RowMajor > projection_matrix_
the projection matrix.
Definition: organized.h:297
bool testProjectionMatrix() const
Quick test if projection matrix and input cloud work together.
Definition: organized.hpp:379
typename PointCloud::Ptr PointCloudPtr
Definition: organized.h:71
const unsigned pyramid_level_
using only a subsample of points to calculate the projection matrix.
Definition: organized.h:309
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:303
shared_ptr< pcl::search::OrganizedNeighbor< PointT > > Ptr
Definition: organized.h:75
OrganizedNeighbor(bool sorted_results=false, float eps=1e-4f, unsigned pyramid_level=5)
Constructor.
Definition: organized.h:90
bool 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:130
bool projectPoint(const PointT &p, pcl::PointXY &q) const
projects a point into the image
Definition: organized.hpp:399
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.
Generic search class.
Definition: search.h:75
PointCloudConstPtr input_
Definition: search.h:415
IndicesConstPtr indices_
Definition: search.h:416
pcl::IndicesConstPtr IndicesConstPtr
Definition: search.h:85
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:86
Defines functions, macros and traits for allocating and using memory.
Definition: bfgs.h:10
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
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:225
bool operator<(const Entry &other) const
Definition: organized.h:232
float distance
Definition: organized.h:229
Entry()
Definition: organized.h:227
index_t index
Definition: organized.h:228
Entry(index_t idx, float dist)
Definition: organized.h:226