correspondence_estimation.hpp
Go to the documentation of this file.
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 Willow Garage, Inc. 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  *
00037  */
00038 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
00039 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
00040 
00041 #include <pcl/common/concatenate.h>
00042 //#include <pcl/registration/correspondence_estimation.h>
00043 
00045 template <typename PointSource, typename PointTarget> inline void
00046 pcl::registration::CorrespondenceEstimation<PointSource, PointTarget>::setInputTarget (
00047     const PointCloudTargetConstPtr &cloud)
00048 {
00049   if (cloud->points.empty ())
00050   {
00051     PCL_ERROR ("[pcl::%s::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
00052     return;
00053   }
00054   target_ = cloud;
00055   tree_->setInputCloud (target_);
00056 }
00057 
00059 template <typename PointSource, typename PointTarget> void
00060 pcl::registration::CorrespondenceEstimation<PointSource, PointTarget>::determineCorrespondences (
00061     pcl::Correspondences &correspondences, float max_distance)
00062 {
00063   typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget;
00064 
00065   if (!initCompute ())
00066     return;
00067 
00068   if (!target_)
00069   {
00070     PCL_WARN ("[pcl::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
00071     return;
00072   }
00073 
00074   float max_dist_sqr = max_distance * max_distance;
00075 
00076   correspondences.resize (indices_->size ());
00077   std::vector<int> index (1);
00078   std::vector<float> distance (1);
00079   pcl::Correspondence corr;
00080   for (size_t i = 0; i < indices_->size (); ++i)
00081   {
00082     // Copy the source data to a target PointTarget format so we can search in the tree
00083     PointTarget pt;
00084     pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> (
00085           input_->points[(*indices_)[i]], 
00086           pt));
00087 
00088     //if (tree_->nearestKSearch (input_->points[(*indices_)[i]], 1, index, distance))
00089     if (tree_->nearestKSearch (pt, 1, index, distance))
00090     {
00091       if (distance[0] <= max_dist_sqr)
00092       {
00093         corr.index_query = static_cast<int> (i);
00094         corr.index_match = index[0];
00095         corr.distance = distance[0];
00096         correspondences[i] = corr;
00097         continue;
00098       }
00099     }
00100 //    correspondences[i] = pcl::Correspondence(i, -1, std::numeric_limits<float>::max());
00101   }
00102   deinitCompute ();
00103 }
00104 
00106 template <typename PointSource, typename PointTarget> void
00107 pcl::registration::CorrespondenceEstimation<PointSource, PointTarget>::determineReciprocalCorrespondences (
00108     pcl::Correspondences &correspondences)
00109 {
00110   typedef typename pcl::traits::fieldList<PointSource>::type FieldListSource;
00111   typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget;
00112   typedef typename pcl::intersect<FieldListSource, FieldListTarget>::type FieldList;
00113   
00114   if (!initCompute ())
00115     return;
00116 
00117   if (!target_)
00118   {
00119     PCL_WARN ("[pcl::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
00120     return;
00121   }
00122 
00123   // setup tree for reciprocal search
00124   pcl::KdTreeFLANN<PointSource> tree_reciprocal;
00125   tree_reciprocal.setInputCloud (input_, indices_);
00126 
00127   correspondences.resize (indices_->size());
00128   std::vector<int> index (1);
00129   std::vector<float> distance (1);
00130   std::vector<int> index_reciprocal (1);
00131   std::vector<float> distance_reciprocal (1);
00132   pcl::Correspondence corr;
00133   unsigned int nr_valid_correspondences = 0;
00134 
00135   for (size_t i = 0; i < indices_->size (); ++i)
00136   {
00137     // Copy the source data to a target PointTarget format so we can search in the tree
00138     PointTarget pt_src;
00139     pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointSource, PointTarget> (
00140           input_->points[(*indices_)[i]], 
00141           pt_src));
00142 
00143     //tree_->nearestKSearch (input_->points[(*indices_)[i]], 1, index, distance);
00144     tree_->nearestKSearch (pt_src, 1, index, distance);
00145 
00146     // Copy the target data to a target PointSource format so we can search in the tree_reciprocal
00147     PointSource pt_tgt;
00148     pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTarget, PointSource> (
00149           target_->points[index[0]],
00150           pt_tgt));
00151     //tree_reciprocal.nearestKSearch (target_->points[index[0]], 1, index_reciprocal, distance_reciprocal);
00152     tree_reciprocal.nearestKSearch (pt_tgt, 1, index_reciprocal, distance_reciprocal);
00153 
00154     if ((*indices_)[i] == index_reciprocal[0])
00155     {
00156       corr.index_query = (*indices_)[i];
00157       corr.index_match = index[0];
00158       corr.distance = distance[0];
00159       correspondences[nr_valid_correspondences] = corr;
00160       ++nr_valid_correspondences;
00161     }
00162   }
00163   correspondences.resize (nr_valid_correspondences);
00164 
00165   deinitCompute ();
00166 }
00167 
00168 //#define PCL_INSTANTIATE_CorrespondenceEstimation(T,U) template class PCL_EXPORTS pcl::registration::CorrespondenceEstimation<T,U>;
00169 
00170 #endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ */


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:14:43