Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
correspondence_estimation_organized_projection.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40
41#ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
42#define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
43
44namespace pcl {
45
46namespace registration {
47
48template <typename PointSource, typename PointTarget, typename Scalar>
49bool
52{
53 // Set the target_cloud_updated_ variable to true, so that the kd-tree is not built -
54 // it is not needed for this class
55 target_cloud_updated_ = false;
57 return (false);
58
59 /// Check if the target cloud is organized
60 if (!target_->isOrganized()) {
61 PCL_WARN("[pcl::registration::%s::initCompute] Target cloud is not organized.\n",
62 getClassName().c_str());
63 return (false);
64 }
65
66 /// Put the projection matrix together
67 projection_matrix_(0, 0) = fx_;
68 projection_matrix_(1, 1) = fy_;
69 projection_matrix_(0, 2) = cx_;
70 projection_matrix_(1, 2) = cy_;
71
72 return (true);
73}
74
75template <typename PointSource, typename PointTarget, typename Scalar>
76void
78 determineCorrespondences(pcl::Correspondences& correspondences, double max_distance)
79{
80 if (!initCompute())
81 return;
82
83 correspondences.resize(indices_->size());
84 std::size_t c_index = 0;
85
86 for (const auto& src_idx : (*indices_)) {
87 if (isFinite((*input_)[src_idx])) {
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);
92
93 /// Check if the point was behind the camera
94 if (uv[2] <= 0)
95 continue;
96
97 int u = static_cast<int>(uv[0] / uv[2]);
98 int v = static_cast<int>(uv[1] / uv[2]);
99
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);
103 if (!isFinite(pt_tgt))
104 continue;
105 /// Check if the depth difference is larger than the threshold
106 if (std::abs(uv[2] - pt_tgt.z) > depth_threshold_)
107 continue;
108
109 double dist = (p_src3 - pt_tgt.getVector3fMap()).norm();
110 if (dist < max_distance)
111 correspondences[c_index++] = pcl::Correspondence(
112 src_idx, v * target_->width + u, static_cast<float>(dist));
113 }
114 }
115 }
116
117 correspondences.resize(c_index);
118}
119
120template <typename PointSource, typename PointTarget, typename Scalar>
121void
124 double max_distance)
125{
126 // Call the normal determineCorrespondences (...), as doing it both ways will not
127 // improve the results
128 determineCorrespondences(correspondences, max_distance);
129}
130
131} // namespace registration
132} // namespace pcl
133
134#endif // PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
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...
Definition point_tests.h:55
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Correspondence represents a match between two entities (e.g., points, descriptors,...