39 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ 40 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ 42 #include <pcl/common/copy_point.h> 45 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
bool 48 if (!source_normals_ || !target_normals_)
50 PCL_WARN (
"[pcl::registration::%s::initCompute] Datasets containing normals for source/target have not been given!\n", getClassName ().c_str ());
58 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void 65 correspondences.resize (indices_->size ());
67 std::vector<int> nn_indices (k_);
68 std::vector<float> nn_dists (k_);
70 float min_dist = std::numeric_limits<float>::max ();
74 unsigned int nr_valid_correspondences = 0;
78 if (isSamePointType<PointSource, PointTarget> ())
82 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
84 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
87 min_dist = std::numeric_limits<float>::max ();
90 for (
size_t j = 0; j < nn_indices.size (); j++)
92 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
93 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
94 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
95 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
100 min_index = static_cast<int> (j);
103 if (min_dist > max_distance)
108 corr.
distance = nn_dists[min_index];
109 correspondences[nr_valid_correspondences++] = corr;
117 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
119 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
122 min_dist = std::numeric_limits<float>::max ();
125 for (
size_t j = 0; j < nn_indices.size (); j++)
129 copyPoint (input_->points[*idx_i], pt_src);
131 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
132 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
133 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
134 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
139 min_index = static_cast<int> (j);
142 if (min_dist > max_distance)
147 corr.
distance = nn_dists[min_index];
148 correspondences[nr_valid_correspondences++] = corr;
151 correspondences.resize (nr_valid_correspondences);
156 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void 164 if(!initComputeReciprocal())
167 correspondences.resize (indices_->size ());
169 std::vector<int> nn_indices (k_);
170 std::vector<float> nn_dists (k_);
171 std::vector<int> index_reciprocal (1);
172 std::vector<float> distance_reciprocal (1);
174 float min_dist = std::numeric_limits<float>::max ();
178 unsigned int nr_valid_correspondences = 0;
183 if (isSamePointType<PointSource, PointTarget> ())
187 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
189 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
192 min_dist = std::numeric_limits<float>::max ();
195 for (
size_t j = 0; j < nn_indices.size (); j++)
197 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
198 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
199 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
200 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
205 min_index = static_cast<int> (j);
208 if (min_dist > max_distance)
212 target_idx = nn_indices[min_index];
213 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
215 if (*idx_i != index_reciprocal[0])
220 corr.
distance = nn_dists[min_index];
221 correspondences[nr_valid_correspondences++] = corr;
229 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
231 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
234 min_dist = std::numeric_limits<float>::max ();
237 for (
size_t j = 0; j < nn_indices.size (); j++)
241 copyPoint (input_->points[*idx_i], pt_src);
243 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
244 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
245 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
246 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
251 min_index = static_cast<int> (j);
254 if (min_dist > max_distance)
258 target_idx = nn_indices[min_index];
259 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
261 if (*idx_i != index_reciprocal[0])
266 corr.
distance = nn_dists[min_index];
267 correspondences[nr_valid_correspondences++] = corr;
270 correspondences.resize (nr_valid_correspondences);
274 #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ int index_match
Index of the matching (target) point.
Correspondence represents a match between two entities (e.g., points, descriptors,...
int index_query
Index of the query (source) point.
void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())
Determine the correspondences between input and target cloud.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
virtual void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())
Determine the reciprocal correspondences between input and target cloud.
bool initCompute()
Internal computation initalization.
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
Abstract CorrespondenceEstimationBase class.