42 #ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
43 #define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
49 namespace registration
52 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
bool
56 target_cloud_updated_ =
false;
61 if (!target_->isOrganized ())
63 PCL_WARN (
"[pcl::registration::%s::initCompute] Target cloud is not organized.\n", getClassName ().c_str ());
68 projection_matrix_ (0, 0) = fx_;
69 projection_matrix_ (1, 1) = fy_;
70 projection_matrix_ (0, 2) = cx_;
71 projection_matrix_ (1, 2) = cy_;
77 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
85 correspondences.resize (indices_->size ());
86 std::size_t c_index = 0;
88 for (std::vector<int>::const_iterator src_it = indices_->begin (); src_it != indices_->end (); ++src_it)
92 Eigen::Vector4f p_src (src_to_tgt_transformation_ * (*input_)[*src_it].getVector4fMap ());
93 Eigen::Vector3f p_src3 (p_src[0], p_src[1], p_src[2]);
94 Eigen::Vector3f uv (projection_matrix_ * p_src3);
100 int u =
static_cast<int> (uv[0] / uv[2]);
101 int v =
static_cast<int> (uv[1] / uv[2]);
103 if (u >= 0 && u <
static_cast<int> (target_->width) &&
104 v >= 0 && v < static_cast<int> (target_->height))
106 const PointTarget &pt_tgt = target_->at (u, v);
110 if (std::abs (uv[2] - pt_tgt.z) > depth_threshold_)
113 double dist = (p_src3 - pt_tgt.getVector3fMap ()).norm ();
114 if (dist < max_distance)
115 correspondences[c_index++] =
pcl::Correspondence (*src_it, v * target_->width + u,
static_cast<float> (dist));
120 correspondences.resize (c_index);
124 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
130 determineCorrespondences (correspondences, max_distance);
136 #endif // PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_