Point Cloud Library (PCL)  1.14.1-dev
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 //////////////////////////////////////////////////////////////////////////
44 template <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 //////////////////////////////////////////////////////////////////////////
66 template <typename PointT> void
67 pcl::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 //////////////////////////////////////////////////////////////////////////
120 template <typename PointT> void
121 pcl::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 //////////////////////////////////////////////////////////////////////////
180 template <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.
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133