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  *
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_NORMAL_SHOOTING_H_
00039 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
00040 
00041 #include <pcl/registration/correspondence_estimation_normal_shooting.h>
00042 
00044 template <typename PointSource, typename PointTarget, typename NormalT> void
00045 pcl::registration::CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT>::determineCorrespondences (
00046     pcl::Correspondences &correspondences, float max_distance)
00047 {
00048   if (!initCompute ())
00049     return;
00050 
00051   if (!target_)
00052   {
00053     PCL_WARN ("[pcl::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
00054     return;
00055   }
00056   correspondences.resize (indices_->size ());
00057 
00058   float min_dist = std::numeric_limits<float>::max ();
00059   float dist = 0.0;
00060   int min_index = 0;
00061   tree_->setInputCloud (target_);
00062 
00063   std::vector<int> nn_indices (k_);
00064   std::vector<float> nn_dists (k_);
00065 
00066   pcl::Correspondence corr;
00067 
00068   for (size_t i = 0; i < source_normals_->points.size (); i++)
00069   {
00070     float *normal = source_normals_->points[i].normal;
00071 
00072     tree_->nearestKSearch (input_->points[i], k_, nn_indices, nn_dists);
00073 
00074     // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
00075     min_dist = std::numeric_limits<float>::max ();
00076     for (size_t j = 0; j < nn_indices.size (); j++)
00077     {
00078       int q = nn_indices[j];
00079       // computing the distance between a point and a line in 3d. 
00080       // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
00081       PointTarget pt;
00082       pt.x = target_->points[q].x - input_->points[i].x;
00083       pt.y = target_->points[q].y - input_->points[i].y;
00084       pt.z = target_->points[q].z - input_->points[i].z;
00085       Eigen::Vector3d N (normal[0], normal[1], normal[2]);
00086       Eigen::Vector3d V (pt.x, pt.y, pt.z);
00087       Eigen::Vector3d C = N.cross (V);
00088       dist = C.dot (C);
00089       if (dist < min_dist)
00090       {
00091         min_dist = dist;
00092         min_index = q;
00093       }
00094     }
00095     if (min_dist > max_distance)
00096       continue;
00097     corr.index_query = i;
00098     corr.index_match = min_index;
00099     corr.distance = min_dist;
00100     correspondences[i] = corr;
00101   }
00102   deinitCompute ();
00103 }
00104 
00105 #endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_ */


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