correspondence_estimation_normal_shooting.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-2012, Willow Garage, Inc.
00006  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of the copyright holder(s) nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id$
00038  *
00039  */
00040 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
00041 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
00042 
00044 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
00045 pcl::registration::CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::initCompute ()
00046 {
00047   if (!source_normals_)
00048   {
00049     PCL_WARN ("[pcl::registration::%s::initCompute] Datasets containing normals for source have not been given!\n", getClassName ().c_str ());
00050     return (false);
00051   }
00052 
00053   return (CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute ());
00054 }
00055 
00057 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
00058 pcl::registration::CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::determineCorrespondences (
00059     pcl::Correspondences &correspondences, double max_distance)
00060 {
00061   if (!initCompute ())
00062     return;
00063 
00064   typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget;
00065   correspondences.resize (indices_->size ());
00066 
00067   std::vector<int> nn_indices (k_);
00068   std::vector<float> nn_dists (k_);
00069 
00070   double min_dist = std::numeric_limits<double>::max ();
00071   int min_index = 0;
00072   
00073   pcl::Correspondence corr;
00074   unsigned int nr_valid_correspondences = 0;
00075 
00076   // Check if the template types are the same. If true, avoid a copy.
00077   // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
00078   if (isSamePointType<PointSource, PointTarget> ())
00079   {
00080     PointTarget pt;
00081     // Iterate over the input set of source indices
00082     for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
00083     {
00084       tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
00085 
00086       // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
00087       min_dist = std::numeric_limits<double>::max ();
00088       
00089       // Find the best correspondence
00090       for (size_t j = 0; j < nn_indices.size (); j++)
00091       {
00092         // computing the distance between a point and a line in 3d. 
00093         // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
00094         pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x;
00095         pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y;
00096         pt.z = target_->points[nn_indices[j]].z - input_->points[*idx_i].z;
00097 
00098         const NormalT &normal = source_normals_->points[*idx_i];
00099         Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
00100         Eigen::Vector3d V (pt.x, pt.y, pt.z);
00101         Eigen::Vector3d C = N.cross (V);
00102         
00103         // Check if we have a better correspondence
00104         double dist = C.dot (C);
00105         if (dist < min_dist)
00106         {
00107           min_dist = dist;
00108           min_index = static_cast<int> (j);
00109         }
00110       }
00111       if (min_dist > max_distance)
00112         continue;
00113 
00114       corr.index_query = *idx_i;
00115       corr.index_match = nn_indices[min_index];
00116       corr.distance = nn_dists[min_index];//min_dist;
00117       correspondences[nr_valid_correspondences++] = corr;
00118     }
00119   }
00120   else
00121   {
00122     PointTarget pt;
00123     
00124     // Iterate over the input set of source indices
00125     for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
00126     {
00127       tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
00128  
00129       // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
00130       min_dist = std::numeric_limits<double>::max ();
00131       
00132       // Find the best correspondence
00133       for (size_t j = 0; j < nn_indices.size (); j++)
00134       {
00135         PointSource pt_src;
00136         // Copy the source data to a target PointTarget format so we can search in the tree
00137         pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> (
00138             input_->points[*idx_i], 
00139             pt_src));
00140 
00141         // computing the distance between a point and a line in 3d. 
00142         // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
00143         pt.x = target_->points[nn_indices[j]].x - pt_src.x;
00144         pt.y = target_->points[nn_indices[j]].y - pt_src.y;
00145         pt.z = target_->points[nn_indices[j]].z - pt_src.z;
00146         
00147         const NormalT &normal = source_normals_->points[*idx_i];
00148         Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
00149         Eigen::Vector3d V (pt.x, pt.y, pt.z);
00150         Eigen::Vector3d C = N.cross (V);
00151         
00152         // Check if we have a better correspondence
00153         double dist = C.dot (C);
00154         if (dist < min_dist)
00155         {
00156           min_dist = dist;
00157           min_index = static_cast<int> (j);
00158         }
00159       }
00160       if (min_dist > max_distance)
00161         continue;
00162       
00163       corr.index_query = *idx_i;
00164       corr.index_match = nn_indices[min_index];
00165       corr.distance = nn_dists[min_index];//min_dist;
00166       correspondences[nr_valid_correspondences++] = corr;
00167     }
00168   }
00169   correspondences.resize (nr_valid_correspondences);
00170   deinitCompute ();
00171 }
00172 
00174 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
00175 pcl::registration::CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::determineReciprocalCorrespondences (
00176     pcl::Correspondences &correspondences, double max_distance)
00177 {
00178   if (!initCompute ())
00179     return;
00180 
00181   typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget;
00182   
00183   // setup tree for reciprocal search
00184   // Set the internal point representation of choice
00185   if (!initComputeReciprocal ())
00186     return;
00187 
00188   correspondences.resize (indices_->size ());
00189 
00190   std::vector<int> nn_indices (k_);
00191   std::vector<float> nn_dists (k_);
00192   std::vector<int> index_reciprocal (1);
00193   std::vector<float> distance_reciprocal (1);
00194 
00195   double min_dist = std::numeric_limits<double>::max ();
00196   int min_index = 0;
00197   
00198   pcl::Correspondence corr;
00199   unsigned int nr_valid_correspondences = 0;
00200   int target_idx = 0;
00201 
00202   // Check if the template types are the same. If true, avoid a copy.
00203   // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
00204   if (isSamePointType<PointSource, PointTarget> ())
00205   {
00206     PointTarget pt;
00207     // Iterate over the input set of source indices
00208     for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
00209     {
00210       tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
00211 
00212       // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
00213       min_dist = std::numeric_limits<double>::max ();
00214       
00215       // Find the best correspondence
00216       for (size_t j = 0; j < nn_indices.size (); j++)
00217       {
00218         // computing the distance between a point and a line in 3d. 
00219         // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
00220         pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x;
00221         pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y;
00222         pt.z = target_->points[nn_indices[j]].z - input_->points[*idx_i].z;
00223 
00224         const NormalT &normal = source_normals_->points[*idx_i];
00225         Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
00226         Eigen::Vector3d V (pt.x, pt.y, pt.z);
00227         Eigen::Vector3d C = N.cross (V);
00228         
00229         // Check if we have a better correspondence
00230         double dist = C.dot (C);
00231         if (dist < min_dist)
00232         {
00233           min_dist = dist;
00234           min_index = static_cast<int> (j);
00235         }
00236       }
00237       if (min_dist > max_distance)
00238         continue;
00239 
00240       // Check if the correspondence is reciprocal
00241       target_idx = nn_indices[min_index];
00242       tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
00243 
00244       if (*idx_i != index_reciprocal[0])
00245         continue;
00246 
00247       // Correspondence IS reciprocal, save it and continue
00248       corr.index_query = *idx_i;
00249       corr.index_match = nn_indices[min_index];
00250       corr.distance = nn_dists[min_index];//min_dist;
00251       correspondences[nr_valid_correspondences++] = corr;
00252     }
00253   }
00254   else
00255   {
00256     PointTarget pt;
00257     
00258     // Iterate over the input set of source indices
00259     for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
00260     {
00261       tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
00262 
00263       // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
00264       min_dist = std::numeric_limits<double>::max ();
00265       
00266       // Find the best correspondence
00267       for (size_t j = 0; j < nn_indices.size (); j++)
00268       {
00269         PointSource pt_src;
00270         // Copy the source data to a target PointTarget format so we can search in the tree
00271         pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> (
00272             input_->points[*idx_i], 
00273             pt_src));
00274 
00275         // computing the distance between a point and a line in 3d. 
00276         // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
00277         pt.x = target_->points[nn_indices[j]].x - pt_src.x;
00278         pt.y = target_->points[nn_indices[j]].y - pt_src.y;
00279         pt.z = target_->points[nn_indices[j]].z - pt_src.z;
00280         
00281         const NormalT &normal = source_normals_->points[*idx_i];
00282         Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
00283         Eigen::Vector3d V (pt.x, pt.y, pt.z);
00284         Eigen::Vector3d C = N.cross (V);
00285         
00286         // Check if we have a better correspondence
00287         double dist = C.dot (C);
00288         if (dist < min_dist)
00289         {
00290           min_dist = dist;
00291           min_index = static_cast<int> (j);
00292         }
00293       }
00294       if (min_dist > max_distance)
00295         continue;
00296 
00297       // Check if the correspondence is reciprocal
00298       target_idx = nn_indices[min_index];
00299       tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
00300 
00301       if (*idx_i != index_reciprocal[0])
00302         continue;
00303 
00304       // Correspondence IS reciprocal, save it and continue
00305       corr.index_query = *idx_i;
00306       corr.index_match = nn_indices[min_index];
00307       corr.distance = nn_dists[min_index];//min_dist;
00308       correspondences[nr_valid_correspondences++] = corr;
00309     }
00310   }
00311   correspondences.resize (nr_valid_correspondences);
00312   deinitCompute ();
00313 }
00314 
00315 #endif    // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:23:00