Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
sac_model_registration_2d.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2012-, Open Perception, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 */
37
38#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_
39#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_
40
41#include <pcl/sample_consensus/sac_model_registration_2d.h>
42
43//////////////////////////////////////////////////////////////////////////
44template <typename PointT> bool
46{
47 if (samples.size () != sample_size_)
48 {
49 PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
50 return (false);
51 }
52 return (true);
53 //using namespace pcl::common;
54 //using namespace pcl::traits;
55
56 //PointT p10 = (*input_)[samples[1]] - (*input_)[samples[0]];
57 //PointT p20 = (*input_)[samples[2]] - (*input_)[samples[0]];
58 //PointT p21 = (*input_)[samples[2]] - (*input_)[samples[1]];
59
60 //return ((p10.x * p10.x + p10.y * p10.y + p10.z * p10.z) > sample_dist_thresh_ &&
61 // (p20.x * p20.x + p20.y * p20.y + p20.z * p20.z) > sample_dist_thresh_ &&
62 // (p21.x * p21.x + p21.y * p21.y + p21.z * p21.z) > sample_dist_thresh_);
63}
64
65//////////////////////////////////////////////////////////////////////////
66template <typename PointT> void
67pcl::SampleConsensusModelRegistration2D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
68{
69 PCL_INFO ("[pcl::SampleConsensusModelRegistration2D<PointT>::getDistancesToModel]\n");
70 if (indices_->size () != indices_tgt_->size ())
71 {
72 PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::getDistancesToModel] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
73 distances.clear ();
74 return;
75 }
76 if (!target_)
77 {
78 PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::getDistanceToModel] No target dataset given!\n");
79 return;
80 }
81
82 distances.resize (indices_->size ());
83
84 // Get the 4x4 transformation
85 Eigen::Matrix4f transform;
86 transform.row (0).matrix () = model_coefficients.segment<4>(0);
87 transform.row (1).matrix () = model_coefficients.segment<4>(4);
88 transform.row (2).matrix () = model_coefficients.segment<4>(8);
89 transform.row (3).matrix () = model_coefficients.segment<4>(12);
90
91 for (std::size_t i = 0; i < indices_->size (); ++i)
92 {
93 Eigen::Vector4f pt_src ((*input_)[(*indices_)[i]].x,
94 (*input_)[(*indices_)[i]].y,
95 (*input_)[(*indices_)[i]].z, 1.0f);
96
97 Eigen::Vector4f p_tr (transform * pt_src);
98
99 // Project the point on the image plane
100 Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
101 Eigen::Vector3f uv (projection_matrix_ * p_tr3);
102
103 if (uv[2] < 0.0f)
104 {
105 continue;
106 }
107
108 uv /= uv[2];
109
110 // Calculate the distance from the transformed point to its correspondence
111 // need to compute the real norm here to keep MSAC and friends general
112 distances[i] = std::sqrt ((uv[0] - (*target_)[(*indices_tgt_)[i]].u) *
113 (uv[0] - (*target_)[(*indices_tgt_)[i]].u) +
114 (uv[1] - (*target_)[(*indices_tgt_)[i]].v) *
115 (uv[1] - (*target_)[(*indices_tgt_)[i]].v));
116 }
117}
118
119//////////////////////////////////////////////////////////////////////////
120template <typename PointT> void
121pcl::SampleConsensusModelRegistration2D<PointT>::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
122{
123 if (indices_->size () != indices_tgt_->size ())
124 {
125 PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
126 inliers.clear ();
127 return;
128 }
129 if (!target_)
130 {
131 PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] No target dataset given!\n");
132 return;
133 }
134
135 double thresh = threshold * threshold;
136
137 inliers.clear ();
138 error_sqr_dists_.clear ();
139 inliers.reserve (indices_->size ());
140 error_sqr_dists_.reserve (indices_->size ());
141
142 Eigen::Matrix4f transform;
143 transform.row (0).matrix () = model_coefficients.segment<4>(0);
144 transform.row (1).matrix () = model_coefficients.segment<4>(4);
145 transform.row (2).matrix () = model_coefficients.segment<4>(8);
146 transform.row (3).matrix () = model_coefficients.segment<4>(12);
147
148 for (std::size_t i = 0; i < indices_->size (); ++i)
149 {
150 Eigen::Vector4f pt_src ((*input_)[(*indices_)[i]].x,
151 (*input_)[(*indices_)[i]].y,
152 (*input_)[(*indices_)[i]].z, 1.0f);
153
154 Eigen::Vector4f p_tr (transform * pt_src);
155
156 // Project the point on the image plane
157 Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
158 Eigen::Vector3f uv (projection_matrix_ * p_tr3);
159
160 if (uv[2] < 0.0f)
161 continue;
162
163 uv /= uv[2];
164
165 double distance = ((uv[0] - (*target_)[(*indices_tgt_)[i]].u) *
166 (uv[0] - (*target_)[(*indices_tgt_)[i]].u) +
167 (uv[1] - (*target_)[(*indices_tgt_)[i]].v) *
168 (uv[1] - (*target_)[(*indices_tgt_)[i]].v));
169
170 // Calculate the distance from the transformed point to its correspondence
171 if (distance < thresh)
172 {
173 inliers.push_back ((*indices_)[i]);
174 error_sqr_dists_.push_back (distance);
175 }
176 }
177}
178
179//////////////////////////////////////////////////////////////////////////
180template <typename PointT> std::size_t
182 const Eigen::VectorXf &model_coefficients, const double threshold) const
183{
184 if (indices_->size () != indices_tgt_->size ())
185 {
186 PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::countWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
187 return (0);
188 }
189 if (!target_)
190 {
191 PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::countWithinDistance] No target dataset given!\n");
192 return (0);
193 }
194
195 double thresh = threshold * threshold;
196
197 Eigen::Matrix4f transform;
198 transform.row (0).matrix () = model_coefficients.segment<4>(0);
199 transform.row (1).matrix () = model_coefficients.segment<4>(4);
200 transform.row (2).matrix () = model_coefficients.segment<4>(8);
201 transform.row (3).matrix () = model_coefficients.segment<4>(12);
202
203 std::size_t nr_p = 0;
204
205 for (std::size_t i = 0; i < indices_->size (); ++i)
206 {
207 Eigen::Vector4f pt_src ((*input_)[(*indices_)[i]].x,
208 (*input_)[(*indices_)[i]].y,
209 (*input_)[(*indices_)[i]].z, 1.0f);
210
211 Eigen::Vector4f p_tr (transform * pt_src);
212
213 // Project the point on the image plane
214 Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
215 Eigen::Vector3f uv (projection_matrix_ * p_tr3);
216
217 if (uv[2] < 0.0f)
218 {
219 continue;
220 }
221
222 uv /= uv[2];
223
224 // Calculate the distance from the transformed point to its correspondence
225 if (((uv[0] - (*target_)[(*indices_tgt_)[i]].u) *
226 (uv[0] - (*target_)[(*indices_tgt_)[i]].u) +
227 (uv[1] - (*target_)[(*indices_tgt_)[i]].v) *
228 (uv[1] - (*target_)[(*indices_tgt_)[i]].v)) < thresh)
229 {
230 ++nr_p;
231 }
232 }
233 return (nr_p);
234}
235
236#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_
237
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
Select all the points which respect the given model coefficients as inliers.
virtual std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const
Count all the points which respect the given model coefficients as inliers.
bool isSampleGood(const Indices &samples) const
Check if a sample of indices results in a good sample of points indices.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const
Compute all distances from the transformed points to their correspondences.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133