organized.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, Willow Garage, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: organized.h 3548 2011-12-14 23:42:10Z rusu $
 *
 */

#ifndef PCL_SEARCH_ORGANIZED_NEIGHBOR_SEARCH_H_
#define PCL_SEARCH_ORGANIZED_NEIGHBOR_SEARCH_H_

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/search.h>

#include <algorithm>
#include <queue>
#include <vector>

namespace pcl
{
  namespace search
  {
    template<typename PointT>
    class OrganizedNeighbor : public pcl::search::Search<PointT>
    {

      public:
        // public typedefs
        typedef pcl::PointCloud<PointT> PointCloud;
        typedef boost::shared_ptr<PointCloud> PointCloudPtr;

        typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
        typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;

        typedef boost::shared_ptr<pcl::search::OrganizedNeighbor<PointT> > Ptr;
        typedef boost::shared_ptr<const pcl::search::OrganizedNeighbor<PointT> > ConstPtr;

        OrganizedNeighbor ()
        {
          max_distance_ = std::numeric_limits<double>::max ();
          oneOverFocalLength_ = 0.0f; //Indicate if it's not initialised
          horizontal_window_ = 0;
          vertical_window_ = 0;
          radiusLookupTableWidth_ =-1; 
          radiusLookupTableHeight_ =-1; 
          exactFocalLength_ = 0;      // we haven't estimated it yet or we haven't set it 
          precision_ = 0;
        }

        ~OrganizedNeighbor () {}

        inline void
        setInputCloud (const PointCloudConstPtr &cloud)
        {
          if (input_ != cloud)
            input_ = cloud;

          if (precision_ == 1)
          {
            if(!exactFocalLength_)
            {
              estimateFocalLengthFromInputCloud (*cloud);
              generateRadiusLookupTable (cloud->width, cloud->height);
              exactFocalLength_ = 1;
            }
          }
          else
          {
            oneOverFocalLength_ = 1.0f;
          }
        }

        inline void
        setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices)
        {
          if (input_ != cloud)
            input_ = cloud;

          indices_ = indices;

          if (precision_ == 1)
          {
            if (!exactFocalLength_)
            {
              estimateFocalLengthFromInputCloud (*cloud);
              generateRadiusLookupTable (cloud->width, cloud->height);
              exactFocalLength_ = 1;
            }
          }
          else
            oneOverFocalLength_ = 1.0f;
        }

        PointCloudConstPtr 
        getInputCloud () { return input_; }

        inline IndicesConstPtr const 
        getIndices () { return (indices_); }

        inline void 
        setPrecision (int precision) { precision_ = precision; }

        inline int 
        getPrecision () { return (precision_); }

        inline void 
        setOneOverFocalLength (double oneOverFocalLength) 
        {
          oneOverFocalLength_ = oneOverFocalLength;
          exactFocalLength_ = 1;
        }

        inline double 
        getOneOverFocalLength () { return (oneOverFocalLength_); }

        inline double 
        getMaxDistance () const { return (max_distance_); }

        inline void 
        setMaxDistance (double max_dist) { max_distance_ = max_dist; }

        inline void
        setSearchWindow (int horizontal, int vertical)
        {
          horizontal_window_ = horizontal;
          vertical_window_ = vertical;
        }

        void
        setSearchWindowAsK (int k);

        int 
        getHorizontalSearchWindow () const { return (horizontal_window_); }

        int 
        getVerticalSearchWindow () const { return (vertical_window_); }

        inline int
        radiusSearch (const PointCloud& cloud, 
                      int index, 
                      double radius, 
                      std::vector<int>& k_indices,
                      std::vector<float>& k_sqr_distances, 
                      int max_nn = std::numeric_limits<int>::max ())
        {
          return (radiusSearch (cloud.points[index], radius, k_indices, k_sqr_distances, max_nn));
        }

        int
        radiusSearch (const PointCloudConstPtr &cloud, 
                      int index, 
                      double radius,
                      std::vector<int> &k_indices, 
                      std::vector<float> &k_sqr_distances,
                      int max_nn = std::numeric_limits<int>::max ());

        int
        radiusSearch (int index, 
                      const double radius, 
                      std::vector<int> &k_indices,
                      std::vector<float> &k_sqr_distances, 
                      int max_nn = std::numeric_limits<int>::max ()) const;

        int
        radiusSearch (const PointT &p_q, 
                      const double radius, 
                      std::vector<int> &k_indices,
                      std::vector<float> &k_sqr_distances, 
                      int max_nn = std::numeric_limits<int>::max ()) const;

        double
        estimateFocalLengthFromInputCloud (const pcl::PointCloud<PointT> &cloud);

        int
        exactNearestKSearch (const PointT &p_q,
                             int k, 
                             std::vector<int> &k_indices,
                             std::vector<float> &k_sqr_distances);

        inline int
        exactNearestKSearch (int index, 
                             int k, 
                             std::vector<int> &k_indices, 
                             std::vector<float> &k_sqr_distances);

        inline int
        exactNearestKSearch (const pcl::PointCloud<PointT> &cloud, 
                             int index, 
                             int k, 
                             std::vector<int> &k_indices, 
                             std::vector<float> &k_sqr_distances);

        int
        nearestKSearch (const PointT &p_q,
                        int k, 
                        std::vector<int> &k_indices,
                        std::vector<float> &k_sqr_distances)
        {
          PCL_ERROR ("[pcl::search::OrganizedNeighbor::approxNearestKSearch] Method not implemented!\n");
          return (0);
        }

        int
        nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances);

        int
        nearestKSearch (const pcl::PointCloud<PointT> &cloud, int index, int k, 
                        std::vector<int> &k_indices, std::vector<float> &k_sqr_distances);

      protected:
        class RadiusSearchLoopkupEntry
        {
          public:
            RadiusSearchLoopkupEntry () {}

            virtual ~RadiusSearchLoopkupEntry () {}

            void
            defineShiftedSearchPoint (int x_shift, int y_shift)
            {
              x_diff_ = x_shift;
              y_diff_ = y_shift;

              squared_distance_ = x_diff_ * x_diff_ + y_diff_ * y_diff_;
            }

            bool
            operator< (const RadiusSearchLoopkupEntry& rhs) const
            {
              return (this->squared_distance_ < rhs.squared_distance_);
            }

            // Public globals
            int x_diff_;int y_diff_;int squared_distance_;
        };

        class NearestNeighborCandidate
        {
          public:
            NearestNeighborCandidate () {}

            virtual ~NearestNeighborCandidate () {}

            bool
            operator< (const NearestNeighborCandidate& rhs) const
            {
              return (this->squared_distance_ < rhs.squared_distance_);
            }

            // Public globals
            int index_;
            double squared_distance_;
        };

        const PointT&
        getPointByIndex (const unsigned int index) const;

        void
        generateRadiusLookupTable (unsigned int width, unsigned int height);

        inline void
        pointPlaneProjection (const PointT& point, int& xpos, int& ypos) const
        {
          xpos = (int)pcl_round (point.x / (point.z * oneOverFocalLength_));
          ypos = (int)pcl_round (point.y / (point.z * oneOverFocalLength_));
        }

        void
        getProjectedRadiusSearchBox (const PointT& point, double squared_radius, int& minX, int& minY,
                                     int& maxX, int& maxY) const;


        virtual std::string 
        getName () const { return ("Organized_Neighbor_Search"); }

        int horizontal_window_;
        int vertical_window_;
        int min_pts_;

        PointCloudConstPtr input_;
        IndicesConstPtr indices_;

        double max_distance_;

        double oneOverFocalLength_;

        std::vector<RadiusSearchLoopkupEntry> radiusSearchLookup_;

        int radiusLookupTableWidth_;
        int radiusLookupTableHeight_;

        int precision_;

        bool exactFocalLength_;
    };
  }
}

#endif