Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/search/include/pcl/search/organized.h
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