Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
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
00080
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