41#ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
42#define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
46namespace registration {
48template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
55 target_cloud_updated_ =
false;
60 if (!target_->isOrganized()) {
61 PCL_WARN(
"[pcl::registration::%s::initCompute] Target cloud is not organized.\n",
62 getClassName().
c_str());
67 projection_matrix_(0, 0) = fx_;
68 projection_matrix_(1, 1) = fy_;
69 projection_matrix_(0, 2) = cx_;
70 projection_matrix_(1, 2) = cy_;
75template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
83 correspondences.resize(indices_->size());
86 for (
const auto&
src_idx : (*indices_)) {
88 Eigen::Vector4f
p_src(src_to_tgt_transformation_ *
91 Eigen::Vector3f
uv(projection_matrix_ *
p_src3);
97 int u =
static_cast<int>(
uv[0] /
uv[2]);
98 int v =
static_cast<int>(
uv[1] /
uv[2]);
100 if (u >= 0 && u <
static_cast<int>(target_->width) && v >= 0 &&
102 const PointTarget&
pt_tgt = target_->at(u, v);
106 if (std::abs(
uv[2] -
pt_tgt.z) > depth_threshold_)
112 src_idx, v * target_->width + u,
static_cast<float>(
dist));
117 correspondences.resize(
c_index);
120template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
128 determineCorrespondences(correspondences,
max_distance);
Iterator class for point clouds with or without given indices.
Abstract CorrespondenceEstimationBase class.
void determineCorrespondences(Correspondences &correspondences, double max_distance)
Computes the correspondences, applying a maximum Euclidean distance threshold.
void determineReciprocalCorrespondences(Correspondences &correspondences, double max_distance)
Computes the correspondences, applying a maximum Euclidean distance threshold.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Correspondence represents a match between two entities (e.g., points, descriptors,...