83 correspondences.resize(indices_->size());
84 std::size_t c_index = 0;
86 for (
const auto& src_idx : (*indices_)) {
88 Eigen::Vector4f p_src(src_to_tgt_transformation_ *
89 (*input_)[src_idx].getVector4fMap());
90 Eigen::Vector3f p_src3(p_src[0], p_src[1], p_src[2]);
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 &&
101 v <
static_cast<int>(target_->height)) {
102 const PointTarget& pt_tgt = target_->at(u, v);
106 if (std::abs(uv[2] - pt_tgt.z) > depth_threshold_)
109 double dist = (p_src3 - pt_tgt.getVector3fMap()).norm();
110 if (dist < max_distance)
112 src_idx, v * target_->width + u,
static_cast<float>(dist));
117 correspondences.resize(c_index);