Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id$ 00037 * 00038 */ 00039 00040 #ifndef PCL_SEARCH_ORGANIZED_NEIGHBOR_SEARCH_H_ 00041 #define PCL_SEARCH_ORGANIZED_NEIGHBOR_SEARCH_H_ 00042 00043 #include <pcl/point_cloud.h> 00044 #include <pcl/point_types.h> 00045 #include <pcl/search/search.h> 00046 #include <pcl/common/eigen.h> 00047 00048 #include <algorithm> 00049 #include <queue> 00050 #include <vector> 00051 #include <pcl/common/projection_matrix.h> 00052 00053 namespace pcl 00054 { 00055 namespace search 00056 { 00057 /** \brief OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds. 00058 * \author Radu B. Rusu, Julius Kammerl, Suat Gedikli, Koen Buys 00059 * \ingroup search 00060 */ 00061 template<typename PointT> 00062 class OrganizedNeighbor : public pcl::search::Search<PointT> 00063 { 00064 00065 public: 00066 // public typedefs 00067 typedef pcl::PointCloud<PointT> PointCloud; 00068 typedef boost::shared_ptr<PointCloud> PointCloudPtr; 00069 00070 typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr; 00071 typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr; 00072 00073 typedef boost::shared_ptr<pcl::search::OrganizedNeighbor<PointT> > Ptr; 00074 typedef boost::shared_ptr<const pcl::search::OrganizedNeighbor<PointT> > ConstPtr; 00075 00076 using pcl::search::Search<PointT>::indices_; 00077 using pcl::search::Search<PointT>::sorted_results_; 00078 using pcl::search::Search<PointT>::input_; 00079 00080 /** \brief Constructor 00081 * \param[in] sorted_results whether the results should be return sorted in ascending order on the distances or not. 00082 * This applies only for radius search, since knn always returns sorted resutls 00083 * \param[in] eps the threshold for the mean-squared-error of the estimation of the projection matrix. 00084 * if the MSE is above this value, the point cloud is considered as not from a projective device, 00085 * thus organized neighbor search can not be applied on that cloud. 00086 * \param[in] pyramid_level the level of the down sampled point cloud to be used for projection matrix estimation 00087 */ 00088 OrganizedNeighbor (bool sorted_results = false, float eps = 1e-4f, unsigned pyramid_level = 5) 00089 : Search<PointT> ("OrganizedNeighbor", sorted_results) 00090 , projection_matrix_ (Eigen::Matrix<float, 3, 4, Eigen::RowMajor>::Zero ()) 00091 , KR_ (Eigen::Matrix<float, 3, 3, Eigen::RowMajor>::Zero ()) 00092 , KR_KRT_ (Eigen::Matrix<float, 3, 3, Eigen::RowMajor>::Zero ()) 00093 , eps_ (eps) 00094 , pyramid_level_ (pyramid_level) 00095 , mask_ () 00096 { 00097 } 00098 00099 /** \brief Empty deconstructor. */ 00100 virtual ~OrganizedNeighbor () {} 00101 00102 /** \brief Test whether this search-object is valid (input is organized AND from projective device) 00103 * User should use this method after setting the input cloud, since setInput just prints an error 00104 * if input is not organized or a projection matrix could not be determined. 00105 * \return true if the input data is organized and from a projective device, false otherwise 00106 */ 00107 bool 00108 isValid () const 00109 { 00110 // determinant (KR) = determinant (K) * determinant (R) = determinant (K) = f_x * f_y. 00111 // If we expect at max an opening angle of 170degree in x-direction -> f_x = 2.0 * width / tan (85 degree); 00112 // 2 * tan (85 degree) ~ 22.86 00113 float min_f = 0.043744332f * static_cast<float>(input_->width); 00114 //std::cout << "isValid: " << determinant3x3Matrix<Eigen::Matrix3f> (KR_ / sqrt (KR_KRT_.coeff (8))) << " >= " << (min_f * min_f) << std::endl; 00115 return (determinant3x3Matrix<Eigen::Matrix3f> (KR_ / sqrtf (KR_KRT_.coeff (8))) >= (min_f * min_f)); 00116 } 00117 00118 /** \brief Compute the camera matrix 00119 * \param[out] camera_matrix the resultant computed camera matrix 00120 */ 00121 void 00122 computeCameraMatrix (Eigen::Matrix3f& camera_matrix) const; 00123 00124 /** \brief Provide a pointer to the input data set, if user has focal length he must set it before calling this 00125 * \param[in] cloud the const boost shared pointer to a PointCloud message 00126 * \param[in] indices the const boost shared pointer to PointIndices 00127 */ 00128 virtual void 00129 setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr &indices = IndicesConstPtr ()) 00130 { 00131 input_ = cloud; 00132 00133 mask_.resize (input_->size ()); 00134 input_ = cloud; 00135 indices_ = indices; 00136 00137 if (indices_.get () != NULL && indices_->size () != 0) 00138 { 00139 mask_.assign (input_->size (), 0); 00140 for (std::vector<int>::const_iterator iIt = indices_->begin (); iIt != indices_->end (); ++iIt) 00141 mask_[*iIt] = 1; 00142 } 00143 else 00144 mask_.assign (input_->size (), 1); 00145 00146 estimateProjectionMatrix (); 00147 } 00148 00149 /** \brief Search for all neighbors of query point that are within a given radius. 00150 * \param[in] p_q the given query point 00151 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors 00152 * \param[out] k_indices the resultant indices of the neighboring points 00153 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 00154 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to 00155 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be 00156 * returned. 00157 * \return number of neighbors found in radius 00158 */ 00159 int 00160 radiusSearch (const PointT &p_q, 00161 double radius, 00162 std::vector<int> &k_indices, 00163 std::vector<float> &k_sqr_distances, 00164 unsigned int max_nn = 0) const; 00165 00166 /** \brief estimated the projection matrix from the input cloud. */ 00167 void 00168 estimateProjectionMatrix (); 00169 00170 /** \brief Search for the k-nearest neighbors for a given query point. 00171 * \note limiting the maximum search radius (with setMaxDistance) can lead to a significant improvement in search speed 00172 * \param[in] p_q the given query point (\ref setInputCloud must be given a-priori!) 00173 * \param[in] k the number of neighbors to search for (used only if horizontal and vertical window not given already!) 00174 * \param[out] k_indices the resultant point indices (must be resized to \a k beforehand!) 00175 * \param[out] k_sqr_distances \note this function does not return distances 00176 * \return number of neighbors found 00177 * @todo still need to implements this functionality 00178 */ 00179 int 00180 nearestKSearch (const PointT &p_q, 00181 int k, 00182 std::vector<int> &k_indices, 00183 std::vector<float> &k_sqr_distances) const; 00184 00185 /** \brief projects a point into the image 00186 * \param[in] p point in 3D World Coordinate Frame to be projected onto the image plane 00187 * \param[out] q the 2D projected point in pixel coordinates (u,v) 00188 * @return true if projection is valid, false otherwise 00189 */ 00190 bool projectPoint (const PointT& p, pcl::PointXY& q) const; 00191 00192 protected: 00193 00194 struct Entry 00195 { 00196 Entry (int idx, float dist) : index (idx), distance (dist) {} 00197 Entry () : index (0), distance (0) {} 00198 unsigned index; 00199 float distance; 00200 00201 inline bool 00202 operator < (const Entry& other) const 00203 { 00204 return (distance < other.distance); 00205 } 00206 }; 00207 00208 /** \brief test if point given by index is among the k NN in results to the query point. 00209 * \param[in] query query point 00210 * \param[in] k number of maximum nn interested in 00211 * \param[in] queue priority queue with k NN 00212 * \param[in] index index on point to be tested 00213 * \return wheter the top element changed or not. 00214 */ 00215 inline bool 00216 testPoint (const PointT& query, unsigned k, std::priority_queue<Entry>& queue, unsigned index) const 00217 { 00218 const PointT& point = input_->points [index]; 00219 if (mask_ [index] && pcl_isfinite (point.x)) 00220 { 00221 //float squared_distance = (point.getVector3fMap () - query.getVector3fMap ()).squaredNorm (); 00222 float dist_x = point.x - query.x; 00223 float dist_y = point.y - query.y; 00224 float dist_z = point.z - query.z; 00225 float squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z; 00226 if (queue.size () < k) 00227 queue.push (Entry (index, squared_distance)); 00228 else if (queue.top ().distance > squared_distance) 00229 { 00230 queue.pop (); 00231 queue.push (Entry (index, squared_distance)); 00232 return true; // top element has changed! 00233 } 00234 } 00235 return false; 00236 } 00237 00238 inline void 00239 clipRange (int& begin, int &end, int min, int max) const 00240 { 00241 begin = std::max (std::min (begin, max), min); 00242 end = std::min (std::max (end, min), max); 00243 } 00244 00245 /** \brief Obtain a search box in 2D from a sphere with a radius in 3D 00246 * \param[in] point the query point (sphere center) 00247 * \param[in] squared_radius the squared sphere radius 00248 * \param[out] minX the min X box coordinate 00249 * \param[out] minY the min Y box coordinate 00250 * \param[out] maxX the max X box coordinate 00251 * \param[out] maxY the max Y box coordinate 00252 */ 00253 void 00254 getProjectedRadiusSearchBox (const PointT& point, float squared_radius, unsigned& minX, unsigned& minY, 00255 unsigned& maxX, unsigned& maxY) const; 00256 00257 00258 /** \brief the projection matrix. Either set by user or calculated by the first / each input cloud */ 00259 Eigen::Matrix<float, 3, 4, Eigen::RowMajor> projection_matrix_; 00260 00261 /** \brief inveser of the left 3x3 projection matrix which is K * R (with K being the camera matrix and R the rotation matrix)*/ 00262 Eigen::Matrix<float, 3, 3, Eigen::RowMajor> KR_; 00263 00264 /** \brief inveser of the left 3x3 projection matrix which is K * R (with K being the camera matrix and R the rotation matrix)*/ 00265 Eigen::Matrix<float, 3, 3, Eigen::RowMajor> KR_KRT_; 00266 00267 /** \brief epsilon value for the MSE of the projection matrix estimation*/ 00268 const float eps_; 00269 00270 /** \brief using only a subsample of points to calculate the projection matrix. pyramid_level_ = use down sampled cloud given by pyramid_level_*/ 00271 const unsigned pyramid_level_; 00272 00273 /** \brief mask, indicating whether the point was in the indices list or not.*/ 00274 std::vector<unsigned char> mask_; 00275 public: 00276 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00277 }; 00278 } 00279 } 00280 00281 #ifdef PCL_NO_PRECOMPILE 00282 #include <pcl/search/impl/organized.hpp> 00283 #endif 00284 00285 #endif 00286