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
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
00077
00078 if (isSamePointType<PointSource, PointTarget> ())
00079 {
00080 PointTarget pt;
00081
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
00087 min_dist = std::numeric_limits<double>::max ();
00088
00089
00090 for (size_t j = 0; j < nn_indices.size (); j++)
00091 {
00092
00093
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
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];
00117 correspondences[nr_valid_correspondences++] = corr;
00118 }
00119 }
00120 else
00121 {
00122 PointTarget pt;
00123
00124
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
00130 min_dist = std::numeric_limits<double>::max ();
00131
00132
00133 for (size_t j = 0; j < nn_indices.size (); j++)
00134 {
00135 PointSource pt_src;
00136
00137 pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> (
00138 input_->points[*idx_i],
00139 pt_src));
00140
00141
00142
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
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];
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
00184
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
00203
00204 if (isSamePointType<PointSource, PointTarget> ())
00205 {
00206 PointTarget pt;
00207
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
00213 min_dist = std::numeric_limits<double>::max ();
00214
00215
00216 for (size_t j = 0; j < nn_indices.size (); j++)
00217 {
00218
00219
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
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
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
00248 corr.index_query = *idx_i;
00249 corr.index_match = nn_indices[min_index];
00250 corr.distance = nn_dists[min_index];
00251 correspondences[nr_valid_correspondences++] = corr;
00252 }
00253 }
00254 else
00255 {
00256 PointTarget pt;
00257
00258
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
00264 min_dist = std::numeric_limits<double>::max ();
00265
00266
00267 for (size_t j = 0; j < nn_indices.size (); j++)
00268 {
00269 PointSource pt_src;
00270
00271 pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> (
00272 input_->points[*idx_i],
00273 pt_src));
00274
00275
00276
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
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
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
00305 corr.index_query = *idx_i;
00306 corr.index_match = nn_indices[min_index];
00307 corr.distance = nn_dists[min_index];
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_