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 51 namespace 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> 60 class OrganizedNeighbor : public pcl::search::Search<PointT> 61 { 62 63 public: 64 // public typedefs 65 using PointCloud = pcl::PointCloud<PointT>; 66 using PointCloudPtr = typename PointCloud::Ptr; 67 68 using PointCloudConstPtr = typename PointCloud::ConstPtr; 69 70 using Ptr = shared_ptr<pcl::search::OrganizedNeighbor<PointT> >; 71 using ConstPtr = shared_ptr<const pcl::search::OrganizedNeighbor<PointT> >; 72 73 using pcl::search::Search<PointT>::indices_; 74 using pcl::search::Search<PointT>::sorted_results_; 75 using pcl::search::Search<PointT>::input_; 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. */ ~OrganizedNeighbor()96 ~OrganizedNeighbor () {} 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 isValid()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_ / ::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 142 estimateProjectionMatrix (); 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 164 estimateProjectionMatrix (); 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 { EntryEntry192 Entry (index_t idx, float dist) : index (idx), distance (dist) {} EntryEntry193 Entry () : index (0), distance (0) {} 194 index_t index; 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 testPoint(const PointT & query,unsigned k,std::vector<Entry> & queue,index_t index)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 clipRange(int & begin,int & end,int min,int max)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: 280 PCL_MAKE_ALIGNED_OPERATOR_NEW 281 }; 282 } 283 } 284 285 #ifdef PCL_NO_PRECOMPILE 286 #include <pcl/search/impl/organized.hpp> 287 #endif 288