models.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 
30 #include <swri_opencv_util/models.h>
31 
32 #include <opencv2/imgproc/imgproc.hpp>
33 
34 namespace swri_opencv_util
35 {
36 
37  int32_t Correspondence2d::GetInlierCount(const M& model, double max_error)
38  {
39  CalculateNorms(model, norms__);
40 
41  cv::compare(norms__, cv::Scalar(max_error * max_error), thresholded__, cv::CMP_LT);
42 
43  return cv::countNonZero(thresholded__);
44  }
45 
46  void Correspondence2d::GetInliers(const M& model, double max_error, std::vector<uint32_t>& indices)
47  {
48  CalculateNorms(model, norms__);
49 
50  indices.clear();
51  indices.reserve(norms__.rows);
52  double threshold = max_error * max_error;
53  for (int i = 0; i < norms__.rows; i++)
54  {
55  if (norms__.at<float>(i) < threshold)
56  {
57  indices.push_back(i);
58  }
59  }
60  }
61 
62  void Correspondence2d::CalculateNorms(const M& model, cv::Mat& norms)
63  {
64  cv::Mat src = data_(cv::Rect(0, 0, 2, data_.rows)).reshape(2);
65  cv::transform(src, predicted__, model);
66  cv::Mat measured = data_(cv::Rect(2, 0, 2, data_.rows));
67  cv::subtract(predicted__.reshape(1), measured, delta__);
68  cv::multiply(delta__, delta__, delta_squared__);
69  cv::add(
70  delta_squared__(cv::Rect(0, 0, 1, delta__.rows)),
71  delta_squared__(cv::Rect(1, 0, 1, delta__.rows)),
72  norms);
73  }
74 
75  bool Homography::GetModel(const std::vector<int32_t>& indices, M& model, double max_error) const
76  {
77  if (indices.size() != MIN_SIZE)
78  {
79  return false;
80  }
81 
82  cv::Mat src(MIN_SIZE, 1, CV_32FC2);
83  cv::Mat dst(MIN_SIZE, 1, CV_32FC2);
84 
85  for (int32_t i = 0; i < MIN_SIZE; i++)
86  {
87  const float* sample = data_.ptr<float>(indices[i]);
88  src.at<cv::Vec2f>(i, 0) = cv::Vec2f(sample[0], sample[1]);
89  dst.at<cv::Vec2f>(i, 0) = cv::Vec2f(sample[2], sample[3]);
90  }
91 
92  model = cv::getPerspectiveTransform(src, dst);
93 
94  // Test input points for if they all match the generated model.
95  cv::Mat predicted;
96  cv::perspectiveTransform(src, predicted, model);
97  cv::Mat delta, delta_squared, norms;
98  cv::subtract(predicted.reshape(1), dst.reshape(1), delta);
99  cv::multiply(delta, delta, delta_squared);
100  cv::add(
101  delta_squared(cv::Rect(0, 0, 1, delta.rows)),
102  delta_squared(cv::Rect(1, 0, 1, delta.rows)),
103  norms);
104 
105  double min, max;
106  cv::minMaxLoc(norms, &min, &max);
107 
108  return max < max_error * max_error;
109  }
110 
111  void Homography::CalculateNorms(const M& model, cv::Mat& norms)
112  {
113  cv::Mat src = data_(cv::Rect(0, 0, 2, data_.rows)).reshape(2);
114  cv::perspectiveTransform(src, predicted__, model);
115  cv::Mat measured = data_(cv::Rect(2, 0, 2, data_.rows));
116  cv::subtract(predicted__.reshape(1), measured, delta__);
117  cv::multiply(delta__, delta__, delta_squared__);
118  cv::add(
119  delta_squared__(cv::Rect(0, 0, 1, delta__.rows)),
120  delta_squared__(cv::Rect(1, 0, 1, delta__.rows)),
121  norms);
122  }
123 
124  bool AffineTransform2d::GetModel(const std::vector<int32_t>& indices, M& model, double max_error) const
125  {
126  if (indices.size() != MIN_SIZE)
127  {
128  return false;
129  }
130 
131  cv::Mat src(MIN_SIZE, 1, CV_32FC2);
132  cv::Mat dst(MIN_SIZE, 1, CV_32FC2);
133 
134  for (int32_t i = 0; i < MIN_SIZE; i++)
135  {
136  const float* sample = data_.ptr<float>(indices[i]);
137  src.at<cv::Vec2f>(i, 0) = cv::Vec2f(sample[0], sample[1]);
138  dst.at<cv::Vec2f>(i, 0) = cv::Vec2f(sample[2], sample[3]);
139  }
140 
141  model = cv::getAffineTransform(src, dst);
142 
143  // Test input points for if they all match the generated model.
144  cv::Mat predicted;
145  cv::transform(src, predicted, model);
146  cv::Mat delta, delta_squared, norms;
147  cv::subtract(predicted.reshape(1), dst.reshape(1), delta);
148  cv::multiply(delta, delta, delta_squared);
149  cv::add(
150  delta_squared(cv::Rect(0, 0, 1, delta.rows)),
151  delta_squared(cv::Rect(1, 0, 1, delta.rows)),
152  norms);
153 
154  double min, max;
155  cv::minMaxLoc(norms, &min, &max);
156 
157  return max < max_error * max_error;
158  }
159 
160  bool RigidTransform2d::GetModel(const std::vector<int32_t>& indices, M& model, double max_error) const
161  {
162  if (indices.size() != MIN_SIZE)
163  {
164  return false;
165  }
166 
167  cv::Point2f src[MIN_SIZE];
168  cv::Point2f dst[MIN_SIZE];
169 
170  for (int32_t i = 0; i < MIN_SIZE; i++)
171  {
172  const float* sample = data_.ptr<float>(indices[i]);
173  src[i].x = sample[0];
174  src[i].y = sample[1];
175  dst[i].x = sample[2];
176  dst[i].y = sample[3];
177  }
178 
179  double len_src = cv::norm(src[1] - src[0]);
180  double len_dst = cv::norm(dst[1] - dst[0]);
181  if (std::fabs(len_src - len_dst) >= max_error)
182  {
183  return false;
184  }
185 
186  // Construct a x-axis for both sets.
187  cv::Point2f src_x = (src[1] - src[0]) * (1.0 / len_src);
188  cv::Point2f dst_x = (dst[1] - dst[0]) * (1.0 / len_dst);
189 
190  // Construct a y-axis for both sets.
191  cv::Point2f src_y(src_x.y, -src_x.x);
192  src_y *= 1.0 / cv::norm(src_y);
193 
194  cv::Point2f dst_y(dst_x.y, -dst_x.x);
195  dst_y *= 1.0 / cv::norm(dst_y);
196 
197  // Build rotation matrices for both sets.
198  cv::Mat src_r(2, 2, CV_32F);
199  src_r.at<float>(0, 0) = src_x.x;
200  src_r.at<float>(1, 0) = src_x.y;
201  src_r.at<float>(0, 1) = src_y.x;
202  src_r.at<float>(1, 1) = src_y.y;
203 
204  cv::Mat dst_r(2, 2, CV_32F);
205  dst_r.at<float>(0, 0) = dst_x.x;
206  dst_r.at<float>(1, 0) = dst_x.y;
207  dst_r.at<float>(0, 1) = dst_y.x;
208  dst_r.at<float>(1, 1) = dst_y.y;
209 
210  // Solve for the rotation between src and dst
211  // R R_src = R_dst
212  // R = R_dst R_src^T
213  cv::Mat rotation = dst_r * src_r.t();
214 
215  // Calculate the translation between src (rotated) and dst.
216  cv::Mat src0_rotated(2, 1, CV_32F);
217  src0_rotated.at<float>(0, 0) = src[0].x;
218  src0_rotated.at<float>(1, 0) = src[0].y;
219  src0_rotated = rotation * src0_rotated;
220  float t_x = dst[0].x - src0_rotated.at<float>(0, 0);
221  float t_y = dst[0].y - src0_rotated.at<float>(1, 0);
222 
223  model.create(2, 3, CV_32F);
224  model.at<float>(0, 0) = rotation.at<float>(0, 0);
225  model.at<float>(0, 1) = rotation.at<float>(0, 1);
226  model.at<float>(1, 0) = rotation.at<float>(1, 0);
227  model.at<float>(1, 1) = rotation.at<float>(1, 1);
228  model.at<float>(0, 2) = t_x;
229  model.at<float>(1, 2) = t_y;
230 
231  return true;
232  }
233 
234  bool Translation2d::GetModel(const std::vector<int32_t>& indices, M& model, double max_error) const
235  {
236  if (indices.size() != MIN_SIZE)
237  {
238  return false;
239  }
240 
241  cv::Point2f src;
242  cv::Point2f dst;
243 
244  const float* sample = data_.ptr<float>(indices[0]);
245  src.x = sample[0];
246  src.y = sample[1];
247  dst.x = sample[2];
248  dst.y = sample[3];
249 
250  // Calculate the translation between src (rotated) and dst.
251  float t_x = dst.x - src.x;
252  float t_y = dst.y - src.y;
253 
254  model.create(2, 3, CV_32F);
255  model.at<float>(0, 0) = 1.0f;
256  model.at<float>(0, 1) = 0.0f;
257  model.at<float>(1, 0) = 0.0f;
258  model.at<float>(1, 1) = 1.0f;
259  model.at<float>(0, 2) = t_x;
260  model.at<float>(1, 2) = t_y;
261 
262  return true;
263  }
264 
265  bool Valid2dPointCorrespondences(
266  const cv::Mat& points1,
267  const cv::Mat& points2)
268  {
269  if (points1.type() != points2.type())
270  {
271  return false;
272  }
273 
274  if (points1.type() != CV_32FC2)
275  {
276  return false;
277  }
278 
279  if (points1.cols != points2.cols || points1.rows != points2.rows)
280  {
281  return false;
282  }
283 
284  if (points1.cols != 1 && points1.rows != 1)
285  {
286  return false;
287  }
288 
289  return true;
290  }
291 
292  bool Valid3dPointCorrespondences(
293  const cv::Mat& points1,
294  const cv::Mat& points2)
295  {
296  if (points1.type() != points2.type())
297  {
298  return false;
299  }
300 
301  if (points1.type() != CV_32FC3)
302  {
303  return false;
304  }
305 
306  if (points1.cols != points2.cols || points1.rows != points2.rows)
307  {
308  return false;
309  }
310 
311  if (points1.cols != 1)
312  {
313  return false;
314  }
315 
316  return true;
317  }
318 
319  bool ZipCorrespondences(
320  const cv::Mat& points1,
321  const cv::Mat& points2,
322  cv::Mat& correspondeces)
323  {
324  if (!Valid2dPointCorrespondences(points1, points2))
325  {
326  return false;
327  }
328 
329  size_t num_points = points1.cols;
330  bool row_order = false;
331  if (points1.rows > 1)
332  {
333  row_order = true;
334  num_points = points1.rows;
335  }
336 
337  // Put data into the correct format.
338  if (row_order)
339  {
340  cv::hconcat(points1.reshape(1), points2.reshape(1), correspondeces);
341  }
342  else
343  {
344  cv::hconcat(
345  points1.reshape(0, num_points).reshape(1),
346  points2.reshape(0, num_points).reshape(1),
347  correspondeces);
348  }
349 
350  return true;
351  }
352 
353 
354  bool PerpendicularPlaneWithPointFit::GetModel(const std::vector<int32_t>& indices, M& model,
355  double max_error) const
356  {
357  if (indices.size() != MIN_SIZE)
358  {
359  return false;
360  }
361 
362  // Check if points are collinear.
363 
364  cv::Mat points = data_.reshape(3);
365 
366  cv::Vec3f p1 = points.at<cv::Vec3f>(indices[0], 0);
367  cv::Vec3f p2 = points.at<cv::Vec3f>(indices[1], 0);
368  cv::Vec3f p3 = point_;
369 
370  cv::Point3f v12 = p2 - p1;
371  cv::Point3f v13 = p3 - p1;
372  float d12 = cv::norm(v12);
373  float d13 = cv::norm(v13);
374  float d = std::fabs(d12 * d13);
375  if (d == 0)
376  {
377  return false;
378  }
379 
380  float angle = std::acos(v12.dot(v13) / d);
381  if (angle < min_angle_ || angle + min_angle_ > M_PI)
382  {
383  return false;
384  }
385 
386  // Calculate model.
387 
388  cv::Vec3f normal = v12.cross(v13);
389  normal = normal / cv::norm(normal);
390 
391  if (std::acos(normal.dot(perp_axis_)) > max_axis_angle_)
392  {
393  return false;
394  }
395 
396  model.x = p1[0];
397  model.y = p1[1];
398  model.z = p1[2];
399  model.i = normal[0];
400  model.j = normal[1];
401  model.k = normal[2];
402 
403  return true;
404  }
405 
406  bool PlaneFit::GetModel(const std::vector<int32_t>& indices, M& model, double max_error) const
407  {
408  if (indices.size() != MIN_SIZE)
409  {
410  return false;
411  }
412 
413  // Check if points are collinear.
414 
415  cv::Mat points = data_.reshape(3);
416 
417  cv::Vec3f p1 = points.at<cv::Vec3f>(indices[0], 0);
418  cv::Vec3f p2 = points.at<cv::Vec3f>(indices[1], 0);
419  cv::Vec3f p3 = points.at<cv::Vec3f>(indices[2], 0);
420 
421  cv::Point3f v12 = p2 - p1;
422  cv::Point3f v13 = p3 - p1;
423  float d12 = cv::norm(v12);
424  float d13 = cv::norm(v13);
425  float d = std::fabs(d12 * d13);
426  if (d == 0)
427  {
428  return false;
429  }
430 
431  float angle = std::acos(v12.dot(v13) / d);
432  if (angle < min_angle_ || angle + min_angle_ > M_PI)
433  {
434  return false;
435  }
436 
437  // Calculate model.
438 
439  cv::Vec3f normal = v12.cross(v13);
440  normal = normal / cv::norm(normal);
441 
442  model.x = p1[0];
443  model.y = p1[1];
444  model.z = p1[2];
445  model.i = normal[0];
446  model.j = normal[1];
447  model.k = normal[2];
448 
449  return true;
450  }
451 
452  void PlaneFit::CalculateNorms(const M& model, cv::Mat& norms)
453  {
454  // Subtract the origin point of the plane from each data point
455  cv::Mat single_column = data_.reshape(3);
456  cv::subtract(single_column, cv::Scalar(model.x, model.y, model.z), delta__);
457 
458  // Calculate the dot product of each adjusted data point with the normal
459  // of the plane.
460  cv::Mat split_columns = delta__.reshape(1);
461  cv::Mat x = split_columns(cv::Rect(0, 0, 1, split_columns.rows));
462  cv::Mat y = split_columns(cv::Rect(1, 0, 1, split_columns.rows));
463  cv::Mat z = split_columns(cv::Rect(2, 0, 1, split_columns.rows));
464  x = x * model.i;
465  y = y * model.j;
466  z = z * model.k;
467  cv::add(x, y, norms);
468  cv::add(norms, z, norms);
469  norms = cv::abs(norms);
470  }
471 
472  bool LineFit3d::GetModel(const std::vector<int32_t>& indices, M& model, double max_error) const
473  {
474  if (indices.size() != MIN_SIZE)
475  {
476  return false;
477  }
478 
479  cv::Mat points = data_.reshape(3);
480 
481  cv::Point3f p1(points.at<cv::Vec3f>(indices[0], 0));
482  cv::Point3f p2(points.at<cv::Vec3f>(indices[1], 0));
483 
484  cv::Point3f v12 = p2 - p1;
485  v12 = cv::Vec3f(v12) / cv::norm(v12);
486 
487  model.x = p1.x;
488  model.y = p1.y;
489  model.z = p1.z;
490  model.i = v12.x;
491  model.j = v12.y;
492  model.k = v12.z;
493 
494  return true;
495  }
496 
497  void LineFit3d::CalculateNorms(const M& model, cv::Mat& norms)
498  {
499  // d = ||(x0 - p) - ((x0 - p) . n)n||
500 
501  cv::Scalar x0(model.x, model.y, model.z);
502  cv::Scalar n(model.i, model.j, model.k);
503  cv::Mat p = data_.reshape(3);
504 
505  if (temp1__.size != p.size)
506  {
507  temp1__ = cv::Mat(p.rows, p.cols, p.type());
508  }
509  temp1__.setTo(n);
510  cv::Mat n_columns = temp1__.reshape(1);
511  cv::Mat n_x = n_columns(cv::Rect(0, 0, 1, n_columns.rows));
512  cv::Mat n_y = n_columns(cv::Rect(1, 0, 1, n_columns.rows));
513  cv::Mat n_z = n_columns(cv::Rect(2, 0, 1, n_columns.rows));
514 
515  // (x0 - p)
516  cv::subtract(p, x0, x0_p__);
517 
518  // (x0 - p) . n
519  cv::multiply(x0_p__, temp1__, temp2__);
520  cv::reduce(temp2__.reshape(1), x0_p_dot_n__, 1, cv::REDUCE_SUM);
521 
522  // ((x0 - p) . n)n
523  cv::multiply(n_x, x0_p_dot_n__, n_x);
524  cv::multiply(n_y, x0_p_dot_n__, n_y);
525  cv::multiply(n_z, x0_p_dot_n__, n_z);
526 
527  // (x0 - p) - ((x0 - p) . n)n
528  cv::subtract(x0_p__, temp1__, temp1__);
529 
530  // d = ||(x0 - p) - ((x0 - p) . n)n||
531  cv::multiply(n_x, n_x, n_x);
532  cv::multiply(n_y, n_y, n_y);
533  cv::multiply(n_z, n_z, n_z);
534  cv::reduce(temp1__.reshape(1), norms, 1, cv::REDUCE_SUM);
535  cv::sqrt(norms, norms);
536  }
537 
538  bool OrthoLineFit3d::GetModel(const std::vector<int32_t>& indices, M& model, double max_error) const
539  {
540  if (indices.size() != MIN_SIZE)
541  {
542  return false;
543  }
544 
545  cv::Mat points = data_.reshape(3);
546 
547  cv::Point3f p1(points.at<cv::Vec3f>(indices[0], 0));
548  cv::Point3f p2(points.at<cv::Vec3f>(indices[1], 0));
549 
550  cv::Point3f v12 = p2 - p1;
551  v12 = cv::Vec3f(v12) / cv::norm(v12);
552 
553  // Check if orthogonal
554  cv::Point3f ortho(ortho_.i, ortho_.j, ortho_.k);
555  ortho = cv::Vec3f(ortho) / cv::norm(ortho);
556 
557  float angle = std::acos(v12.dot(ortho));
558  float error = std::fabs(M_PI * 0.5 - angle);
559  if (error > angle_tolerance_)
560  {
561  return false;
562  }
563 
564  model.x = p1.x;
565  model.y = p1.y;
566  model.z = p1.z;
567  model.i = v12.x;
568  model.j = v12.y;
569  model.k = v12.z;
570 
571  return true;
572  }
573 
574  bool CrossFit3d::GetModel(const std::vector<int32_t>& indices, M& model, double max_error) const
575  {
576  if (indices.size() != MIN_SIZE)
577  {
578  return false;
579  }
580 
581  // Check if points are collinear.
582 
583  cv::Mat points = data_.reshape(3);
584 
585  cv::Point3f p1(points.at<cv::Vec3f>(indices[0], 0));
586  cv::Point3f p2(points.at<cv::Vec3f>(indices[1], 0));
587  cv::Point3f p3(points.at<cv::Vec3f>(indices[2], 0));
588 
589  cv::Point3f v12 = p2 - p1;
590  cv::Point3f v13 = p3 - p1;
591  float d12 = cv::norm(v12);
592  float d13 = cv::norm(v13);
593  float d = std::fabs(d12 * d13);
594  if (d == 0)
595  {
596  return false;
597  }
598 
599  float angle = std::acos(v12.dot(v13) / d);
600  if (angle < min_angle_ || angle + min_angle_ > M_PI)
601  {
602  return false;
603  }
604 
605  // Calculate model.
606 
607  // Orthogonally project 3rd point onto first line to
608  // get line through 3rd point orthogonal to the first
609  // line.
610  cv::Point3f p4 = p1 + (v13.dot(v12) / v12.dot(v12)) * v12;
611  cv::Point3f v34 = p4 - p3;
612 
613  // Get unit vector of the lines.
614  v12 = cv::Vec3f(v12) / cv::norm(v12);
615  v34 = cv::Vec3f(v34) / cv::norm(v34);
616 
617  model.x = p4.x;
618  model.y = p4.y;
619  model.z = p4.z;
620  model.i1 = v12.x;
621  model.j1 = v12.y;
622  model.k1 = v12.z;
623  model.i2 = v34.x;
624  model.j2 = v34.y;
625  model.k2 = v34.z;
626 
627  return true;
628  }
629 
630  void CrossFit3d::CalculateNorms(const M& model, cv::Mat& norms)
631  {
633  // Line 1
635 
636  // d = ||(x0 - p) - ((x0 - p) . n)n||
637 
638  cv::Scalar x0(model.x, model.y, model.z);
639  cv::Scalar n(model.i1, model.j1, model.k1);
640  cv::Mat p = data_.reshape(3);
641 
642  if (temp1__.size != p.size)
643  {
644  temp1__ = cv::Mat(p.rows, p.cols, p.type());
645  }
646  temp1__.setTo(n);
647  cv::Mat n_columns = temp1__.reshape(1);
648  cv::Mat n_x = n_columns(cv::Rect(0, 0, 1, n_columns.rows));
649  cv::Mat n_y = n_columns(cv::Rect(1, 0, 1, n_columns.rows));
650  cv::Mat n_z = n_columns(cv::Rect(2, 0, 1, n_columns.rows));
651 
652  // (x0 - p)
653  cv::subtract(p, x0, x0_p__);
654 
655  // (x0 - p) . n
656  cv::multiply(x0_p__, temp1__, temp2__);
657  cv::reduce(temp2__.reshape(1), x0_p_dot_n__, 1, cv::REDUCE_SUM);
658 
659  // ((x0 - p) . n)n
660  cv::multiply(n_x, x0_p_dot_n__, n_x);
661  cv::multiply(n_y, x0_p_dot_n__, n_y);
662  cv::multiply(n_z, x0_p_dot_n__, n_z);
663 
664  // (x0 - p) - ((x0 - p) . n)n
665  cv::subtract(x0_p__, temp1__, temp1__);
666 
667  // d = ||(x0 - p) - ((x0 - p) . n)n||
668  cv::multiply(n_x, n_x, n_x);
669  cv::multiply(n_y, n_y, n_y);
670  cv::multiply(n_z, n_z, n_z);
671  cv::reduce(temp1__.reshape(1), temp3__, 1, cv::REDUCE_SUM);
672  cv::sqrt(temp3__, temp3__);
673 
674 
676  // Line 2
678 
679  // d = ||(x0 - p) - ((x0 - p) . n)n||
680 
681  n = cv::Scalar(model.i2, model.j2, model.k2);
682  temp1__.setTo(n);
683 
684  // (x0 - p)
685  cv::subtract(p, x0, x0_p__);
686 
687  // (x0 - p) . n
688  cv::multiply(x0_p__, temp1__, temp2__);
689  cv::reduce(temp2__.reshape(1), x0_p_dot_n__, 1, cv::REDUCE_SUM);
690 
691  // ((x0 - p) . n)n
692  cv::multiply(n_x, x0_p_dot_n__, n_x);
693  cv::multiply(n_y, x0_p_dot_n__, n_y);
694  cv::multiply(n_z, x0_p_dot_n__, n_z);
695 
696  // (x0 - p) - ((x0 - p) . n)n
697  cv::subtract(x0_p__, temp1__, temp1__);
698 
699  // d = ||(x0 - p) - ((x0 - p) . n)n||
700  cv::multiply(n_x, n_x, n_x);
701  cv::multiply(n_y, n_y, n_y);
702  cv::multiply(n_z, n_z, n_z);
703  cv::reduce(temp1__.reshape(1), norms, 1, cv::REDUCE_SUM);
704  cv::sqrt(norms, norms);
705 
706  // Use the minimum distance to either line.
707  norms = cv::min(norms, temp3__);
708  }
709 }
d
d
swri_opencv_util::Valid2dPointCorrespondences
bool Valid2dPointCorrespondences(const cv::Mat &points1, const cv::Mat &points2)
Definition: models.cpp:265
swri_opencv_util::Correspondence2d::GetInliers
void GetInliers(const M &model, double max_error, std::vector< uint32_t > &indices)
Definition: models.cpp:46
swri_opencv_util::Correspondence2d::predicted__
cv::Mat predicted__
Definition: models.h:66
swri_opencv_util::PerpendicularPlaneWithPointFit::GetModel
virtual bool GetModel(const std::vector< int32_t > &indices, M &model, double max_error) const
Definition: models.cpp:354
swri_opencv_util::CrossFit3d::GetModel
virtual bool GetModel(const std::vector< int32_t > &indices, M &model, double max_error) const
Definition: models.cpp:574
swri_opencv_util::Valid3dPointCorrespondences
bool Valid3dPointCorrespondences(const cv::Mat &points1, const cv::Mat &points2)
Definition: models.cpp:292
f
f
swri_opencv_util::Correspondence2d::thresholded__
cv::Mat thresholded__
Definition: models.h:69
swri_opencv_util::PlaneFit::GetModel
virtual bool GetModel(const std::vector< int32_t > &indices, M &model, double max_error) const
Definition: models.cpp:406
swri_opencv_util::Correspondence2d::delta__
cv::Mat delta__
Definition: models.h:67
swri_opencv_util::OrthoLineFit3d::GetModel
virtual bool GetModel(const std::vector< int32_t > &indices, M &model, double max_error) const
Definition: models.cpp:538
swri_opencv_util::Correspondence2d::norms__
cv::Mat norms__
Definition: models.h:65
swri_opencv_util::AffineTransform2d::GetModel
virtual bool GetModel(const std::vector< int32_t > &indices, M &model, double max_error) const
Definition: models.cpp:124
swri_opencv_util::Homography::GetModel
virtual bool GetModel(const std::vector< int32_t > &indices, M &model, double max_error) const
Definition: models.cpp:75
swri_opencv_util::Correspondence2d::data_
const T & data_
Definition: models.h:62
swri_opencv_util::Correspondence2d::M
cv::Mat M
Definition: models.h:44
swri_opencv_util
Definition: blend.h:35
swri_opencv_util::Homography::CalculateNorms
virtual void CalculateNorms(const M &model, cv::Mat &norms)
Definition: models.cpp:111
swri_opencv_util::Correspondence2d::GetInlierCount
int32_t GetInlierCount(const M &model, double max_error)
Definition: models.cpp:37
swri_opencv_util::ZipCorrespondences
bool ZipCorrespondences(const cv::Mat &points1, const cv::Mat &points2, cv::Mat &correspondences)
Definition: models.cpp:319
swri_opencv_util::Correspondence2d::delta_squared__
cv::Mat delta_squared__
Definition: models.h:68
swri_opencv_util::PlaneFit::CalculateNorms
virtual void CalculateNorms(const M &model, cv::Mat &norms)
Definition: models.cpp:452
swri_opencv_util::Correspondence2d::CalculateNorms
virtual void CalculateNorms(const M &model, cv::Mat &norms)
Definition: models.cpp:62
swri_opencv_util::Translation2d::GetModel
virtual bool GetModel(const std::vector< int32_t > &indices, M &model, double max_error) const
Definition: models.cpp:234
swri_opencv_util::CrossFit3d::CalculateNorms
virtual void CalculateNorms(const M &model, cv::Mat &norms)
Definition: models.cpp:630
swri_opencv_util::RigidTransform2d::GetModel
virtual bool GetModel(const std::vector< int32_t > &indices, M &model, double max_error) const
Definition: models.cpp:160
swri_opencv_util::LineFit3d::CalculateNorms
virtual void CalculateNorms(const M &model, cv::Mat &norms)
Definition: models.cpp:497
swri_opencv_util::LineFit3d::GetModel
virtual bool GetModel(const std::vector< int32_t > &indices, M &model, double max_error) const
Definition: models.cpp:472

swri_opencv_util
Author(s): Marc Alban
autogenerated on Sat Jan 21 2023 03:13:14