nearest_pair_point_cloud_coherence.hpp
Go to the documentation of this file.
00001 #ifndef PCL_TRACKING_IMPL_NEAREST_PAIR_POINT_CLOUD_COHERENCE_H_
00002 #define PCL_TRACKING_IMPL_NEAREST_PAIR_POINT_CLOUD_COHERENCE_H_
00003 
00004 #include <pcl/search/kdtree.h>
00005 #include <pcl/search/organized.h>
00006 
00007 namespace pcl
00008 {
00009   namespace tracking
00010   {
00011     template <typename PointInT> void 
00012     NearestPairPointCloudCoherence<PointInT>::computeCoherence (
00013         const PointCloudInConstPtr &cloud, const IndicesConstPtr &, float &w)
00014     {
00015       double val = 0.0;
00016       //for (size_t i = 0; i < indices->size (); i++)
00017       for (size_t i = 0; i < cloud->points.size (); i++)
00018       {
00019         PointInT input_point = cloud->points[i];
00020         std::vector<int> k_indices(1);
00021         std::vector<float> k_distances(1);
00022         search_->nearestKSearch (input_point, 1, k_indices, k_distances);
00023         int k_index = k_indices[0];
00024         float k_distance = k_distances[0];
00025         if (k_distance < maximum_distance_ * maximum_distance_)
00026         {
00027           // nearest_targets.push_back (k_index);
00028           // nearest_inputs.push_back (i);
00029           PointInT target_point = target_input_->points[k_index];
00030           double coherence_val = 1.0;
00031           for (size_t i = 0; i < point_coherences_.size (); i++)
00032           {
00033             PointCoherencePtr coherence = point_coherences_[i];  
00034             double w = coherence->compute (input_point, target_point);
00035             coherence_val *= w;
00036           }
00037           val += coherence_val;
00038         }
00039       }
00040       w = - static_cast<float> (val);
00041     }
00042     
00043     template <typename PointInT> bool
00044     NearestPairPointCloudCoherence<PointInT>::initCompute ()
00045     {
00046       if (!PointCloudCoherence<PointInT>::initCompute ())
00047       {
00048         PCL_ERROR ("[pcl::%s::initCompute] PointCloudCoherence::Init failed.\n", getClassName ().c_str ());
00049         //deinitCompute ();
00050         return (false);
00051       }
00052       
00053       // initialize tree
00054       if (!search_)
00055         search_.reset (new pcl::search::KdTree<PointInT> (false));
00056       
00057       if (new_target_ && target_input_)
00058       {
00059         search_->setInputCloud (target_input_);
00060         new_target_ = false;
00061       }
00062       
00063       return true;
00064     }
00065   }
00066 }
00067 
00068 #define PCL_INSTANTIATE_NearestPairPointCloudCoherence(T) template class PCL_EXPORTS pcl::tracking::NearestPairPointCloudCoherence<T>;
00069 
00070 #endif


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:47