41 #ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
42 #define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
45 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
bool
49 target_cloud_updated_ =
false;
54 if (!target_->isOrganized ())
56 PCL_WARN (
"[pcl::registration::%s::initCompute] Target cloud is not organized.\n", getClassName ().c_str ());
61 projection_matrix_ (0, 0) = fx_;
62 projection_matrix_ (1, 1) = fy_;
63 projection_matrix_ (0, 2) = cx_;
64 projection_matrix_ (1, 2) = cy_;
70 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
78 correspondences.resize (indices_->size ());
81 for (std::vector<int>::const_iterator src_it = indices_->begin (); src_it != indices_->end (); ++src_it)
83 if (
isFinite (input_->points[*src_it]))
85 Eigen::Vector4f p_src (src_to_tgt_transformation_ * input_->points[*src_it].getVector4fMap ());
86 Eigen::Vector3f p_src3 (p_src[0], p_src[1], p_src[2]);
87 Eigen::Vector3f uv (projection_matrix_ * p_src3);
93 int u =
static_cast<int> (uv[0] / uv[2]);
94 int v =
static_cast<int> (uv[1] / uv[2]);
96 if (u >= 0 && u < static_cast<int> (target_->width) &&
97 v >= 0 && v < static_cast<int> (target_->height))
99 const PointTarget &pt_tgt = target_->at (u, v);
103 if (fabs (uv[2] - pt_tgt.z) > depth_threshold_)
106 double dist = (p_src3 - pt_tgt.getVector3fMap ()).norm ();
107 if (dist < max_distance)
108 correspondences[c_index++] =
pcl::Correspondence (*src_it, v * target_->width + u, static_cast<float> (dist));
113 correspondences.resize (c_index);
117 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
123 determineCorrespondences (correspondences, max_distance);
126 #endif // PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
void determineCorrespondences(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...
Correspondence represents a match between two entities (e.g., points, descriptors, etc).
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
void determineReciprocalCorrespondences(Correspondences &correspondences, double max_distance)
Computes the correspondences, applying a maximum Euclidean distance threshold.
Abstract CorrespondenceEstimationBase class.