model_fit.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 
31 
32 #include <ros/ros.h>
33 
34 #include <opencv2/imgproc/imgproc.hpp>
35 
36 namespace swri_opencv_util
37 {
39  const cv::Mat& points1,
40  const cv::Mat& points2,
41  cv::Mat& inliers1,
42  cv::Mat& inliers2,
43  std::vector<uint32_t> &good_points,
44  int32_t& iterations,
45  double max_error,
46  double confidence,
47  int32_t max_iterations,
49  {
50  return FindModel2d<Translation2d>(
51  points1, points2, inliers1, inliers2, good_points, iterations, max_error,
52  confidence, max_iterations, rng);
53  }
54 
56  const cv::Mat& points1,
57  const cv::Mat& points2,
58  cv::Mat& inliers1,
59  cv::Mat& inliers2,
60  std::vector<uint32_t> &good_points,
61  int32_t& iterations,
62  double max_error,
63  double confidence,
64  int32_t max_iterations,
66  {
67  return FindModel2d<RigidTransform2d>(
68  points1, points2, inliers1, inliers2, good_points, iterations, max_error,
69  confidence, max_iterations, rng);
70  }
71 
72  cv::Mat FitRigidTransform2d(const cv::Mat& points1, const cv::Mat& points2)
73  {
74  cv::Mat transform;
75 
76  if (!Valid2dPointCorrespondences(points1, points2))
77  {
78  return transform;
79  }
80 
81  // The Kabsch algorithm, for calculating the optimal rotation matrix that
82  // minimizes the RMSD (root mean squared deviation) between two paired sets
83  // of points. http://en.wikipedia.org/wiki/Kabsch_algorithm
84 
85  // Get the centroids of the points.
86  cv::Scalar centroid1 = cv::mean(points1);
87  cv::Scalar centroid2 = cv::mean(points2);
88 
89  // Center the points around the origin.
90  cv::Mat points1_centered = (points1 - centroid1);
91  cv::Mat points2_centered = (points2 - centroid2);
92 
93  // Reshape the points into 2xN matrices.
94  points1_centered = points1_centered.reshape(1, points1.rows);
95  points2_centered = points2_centered.reshape(1, points1.rows);
96 
97  // Compute the covariance matrix.
98  cv::Mat cov = points1_centered.t() * points2_centered;
99 
100  // Compute the optimal rotation matrix.
101  cv::Mat W, U, Vt;
102  cv::SVD::compute(cov, W, U, Vt);
103  double d = cv::determinant(Vt.t() * U.t()) > 0 ? 1.0 : -1.0;
104  cv::Mat I = cv::Mat::eye(2, 2, CV_32F);
105  I.at<float>(1, 1) = d;
106  cv::Mat rotation = Vt.t() * I * U.t();
107 
108  // Compute the optimal translation.
109  cv::Mat c1_r(2, 1, CV_32F);
110  c1_r.at<float>(0, 0) = centroid1[0];
111  c1_r.at<float>(1, 0) = centroid1[1];
112  c1_r = rotation * c1_r;
113  float t_x = centroid2[0] - c1_r.at<float>(0, 0);
114  float t_y = centroid2[1] - c1_r.at<float>(1, 0);
115 
116  transform.create(2, 3, CV_32F);
117  transform.at<float>(0, 0) = rotation.at<float>(0, 0);
118  transform.at<float>(0, 1) = rotation.at<float>(0, 1);
119  transform.at<float>(1, 0) = rotation.at<float>(1, 0);
120  transform.at<float>(1, 1) = rotation.at<float>(1, 1);
121  transform.at<float>(0, 2) = t_x;
122  transform.at<float>(1, 2) = t_y;
123 
124  return transform;
125  }
126 
127  cv::Mat FitRotation3d(const cv::Mat& points1, const cv::Mat& points2)
128  {
129  cv::Mat matrix;
130 
131  if (!Valid3dPointCorrespondences(points1, points2))
132  {
133  return matrix;
134  }
135 
136  // The Kabsch algorithm, for calculating the optimal rotation matrix that
137  // minimizes the RMSD (root mean squared deviation) between two paired sets
138  // of points. http://en.wikipedia.org/wiki/Kabsch_algorithm
139 
140  // Compute the covariance matrix.
141  cv::Mat src = points1.reshape(1, points1.rows);
142  cv::Mat dst = points2.reshape(1, points1.rows);
143 
144  cv::Mat cov = src.t() * dst;
145 
146  // Compute the optimal rotation matrix.
147  cv::Mat W, U, Vt;
148  cv::SVD::compute(cov, W, U, Vt);
149  cv::Mat V = Vt.t();
150  double d = cv::determinant(V * U.t()) > 0 ? 1.0 : -1.0;
151  cv::Mat I = cv::Mat::eye(3, 3, CV_32F);
152  I.at<float>(2, 2) = d;
153  matrix = V * I * U.t();
154 
155  return matrix;
156  }
157 
158 
160  const cv::Mat& points1,
161  const cv::Mat& points2,
162  cv::Mat& inliers1,
163  cv::Mat& inliers2,
164  std::vector<uint32_t> &good_points,
165  int32_t& iterations,
166  double max_error,
167  double confidence,
168  int32_t max_iterations,
170  {
171  return FindModel2d<AffineTransform2d>(
172  points1, points2, inliers1, inliers2, good_points, iterations, max_error,
173  confidence, max_iterations, rng);
174  }
175 
176  cv::Mat FindHomography(
177  const cv::Mat& points1,
178  const cv::Mat& points2,
179  cv::Mat& inliers1,
180  cv::Mat& inliers2,
181  std::vector<uint32_t> &good_points,
182  int32_t& iterations,
183  double max_error,
184  double confidence,
185  int32_t max_iterations,
187  {
188  return FindModel2d<Homography>(
189  points1, points2, inliers1, inliers2, good_points, iterations, max_error,
190  confidence, max_iterations, rng);
191  }
192 
193  cv::Mat FitAffineTransform2d(const cv::Mat& points1, const cv::Mat& points2)
194  {
195  cv::Mat transform;
196 
197  if (!Valid2dPointCorrespondences(points1, points2))
198  {
199  return transform;
200  }
201 
202  bool row_order = points1.rows > 1;
203  int32_t size = row_order ? points1.rows : points1.cols;
204 
205  // Perform least squares fit on inliers to refine model.
206  // For least squares there are several decomposition methods:
207  // DECOMP_LU
208  // DECOMP_CHOLESKY ([A] must be symmetrical)
209  // DECOMP_EIG ([A] must be symmetrical)
210  // DECOMP_SVD
211  // DECOMP_QR
212  cv::Mat A(size, 3, CV_32F);
213  cv::Mat B = points2.reshape(1, 2);
214  if (row_order)
215  {
216  for (int32_t i = 0; i < size; ++i)
217  {
218  const cv::Vec2f& point = points1.at<cv::Vec2f>(i, 0);
219  cv::Vec3f& A_i = A.at<cv::Vec3f>(i, 0);
220  A_i[0] = point[0];
221  A_i[1] = point[1];
222  A_i[2] = 1.0;
223  }
224  }
225  else
226  {
227  B = points2.t();
228  B = B.reshape(1, 2);
229 
230  for (int32_t i = 0; i < size; ++i)
231  {
232  const cv::Vec2f& point = points1.at<cv::Vec2f>(0, i);
233  cv::Vec3f& A_i = A.at<cv::Vec3f>(i, 0);
234  A_i[0] = point[0];
235  A_i[1] = point[1];
236  A_i[2] = 1.0;
237  }
238  }
239 
240  cv::Mat x;
241  if (cv::solve(A, B, x))
242  {
243  transform = x;
244  }
245 
246  return transform;
247  }
248 
250  const cv::Vec3f& point_on_plane,
251  const cv::Vec3f& perp_axis,
252  double max_angle_from_perp,
253  const cv::Mat& points,
254  cv::Mat& inliers,
255  std::vector<uint32_t> &good_points,
256  int32_t& iterations,
257  double max_error,
258  double confidence,
259  int32_t min_iterations,
260  int32_t max_iterations,
262  {
264 
265  cv::Mat reshaped = points.reshape(3);
266  PerpendicularPlaneWithPointFit fit_model(reshaped, point_on_plane, perp_axis, max_angle_from_perp);
267  PlaneModel model = ransac.FitModel(
268  fit_model, max_error, confidence, min_iterations, max_iterations, good_points, iterations);
269 
270  if (good_points.empty())
271  {
272  return model;
273  }
274 
275  inliers = cv::Mat(good_points.size(), reshaped.cols, reshaped.type());
276  for (size_t i = 0; i < good_points.size(); ++i)
277  {
278  inliers.at<cv::Vec3f>(i, 0) = reshaped.at<cv::Vec3f>(good_points[i], 0);
279  }
280  inliers.reshape(points.channels());
281  return model;
282  }
283 
284 
286  const cv::Mat& points,
287  cv::Mat& inliers,
288  std::vector<uint32_t> &good_points,
289  int32_t& iterations,
290  double max_error,
291  double confidence,
292  int32_t min_iterations,
293  int32_t max_iterations,
295  {
297 
298  cv::Mat reshaped = points.reshape(3);
299  PlaneFit fit_model(reshaped);
300  PlaneModel model = ransac.FitModel(
301  fit_model, max_error, confidence, min_iterations, max_iterations, good_points, iterations);
302 
303  if (good_points.empty())
304  {
305  return model;
306  }
307 
308  inliers = cv::Mat(good_points.size(), reshaped.cols, reshaped.type());
309  for (size_t i = 0; i < good_points.size(); ++i)
310  {
311  inliers.at<cv::Vec3f>(i, 0) = reshaped.at<cv::Vec3f>(good_points[i], 0);
312  }
313  inliers.reshape(points.channels());
314  return model;
315  }
316 
317  PlaneModel FitPlane(const cv::Mat& points)
318  {
319  PlaneModel model;
320  if (points.rows < 3)
321  {
322  return model;
323  }
324  cv::Mat centroid;
325  cv::reduce(points.reshape(3), centroid, 0, cv::REDUCE_AVG);
326 
327  cv::Scalar c(centroid.at<float>(0, 0), centroid.at<float>(0, 1), centroid.at<float>(0, 2));
328 
329  cv::Mat A;
330  cv::subtract(points, c, A);
331 
332  cv::SVD svd;
333  cv::Mat w, u, vt;
334  cv::Mat At;
335  cv::transpose(A.reshape(1), At);
336  svd.compute(At, w, u, vt);
337 
338  cv::Point min_w_loc;
339  cv::minMaxLoc(w, NULL, NULL, &min_w_loc);
340 
341  model.x = centroid.at<float>(0, 0);
342  model.y = centroid.at<float>(0, 1);
343  model.z = centroid.at<float>(0, 2);
344  model.i = u.at<float>(0, min_w_loc.y);
345  model.j = u.at<float>(1, min_w_loc.y);
346  model.k = u.at<float>(2, min_w_loc.y);
347 
348  return model;
349  }
350 
352  const cv::Mat& points,
353  cv::Mat& inliers,
354  std::vector<uint32_t> &good_points,
355  int32_t& iterations,
356  double max_error,
357  double confidence,
358  int32_t min_iterations,
359  int32_t max_iterations,
361  {
363 
364  cv::Mat reshaped = points.reshape(3);
365  LineFit3d fit_model(reshaped);
366  LineModel3d model = ransac.FitModel(
367  fit_model, max_error, confidence, min_iterations, max_iterations, good_points, iterations);
368 
369  if (good_points.empty())
370  {
371  return model;
372  }
373 
374  inliers = cv::Mat(good_points.size(), reshaped.cols, reshaped.type());
375  for (size_t i = 0; i < good_points.size(); ++i)
376  {
377  inliers.at<cv::Vec3f>(i, 0) = reshaped.at<cv::Vec3f>(good_points[i], 0);
378  }
379  inliers.reshape(points.channels());
380  return model;
381  }
382 
384  const cv::Mat& points,
385  const LineModel3d& ortho,
386  cv::Mat& inliers,
387  std::vector<uint32_t> &good_points,
388  int32_t& iterations,
389  double max_error,
390  double confidence,
391  int32_t min_iterations,
392  int32_t max_iterations,
394  {
396 
397  cv::Mat reshaped = points.reshape(3);
398  OrthoLineFit3d fit_model(reshaped, ortho);
399  LineModel3d model = ransac.FitModel(
400  fit_model, max_error, confidence, min_iterations, max_iterations, good_points, iterations);
401 
402  if (good_points.empty())
403  {
404  return model;
405  }
406 
407  inliers = cv::Mat(good_points.size(), reshaped.cols, reshaped.type());
408  for (size_t i = 0; i < good_points.size(); ++i)
409  {
410  inliers.at<cv::Vec3f>(i, 0) = reshaped.at<cv::Vec3f>(good_points[i], 0);
411  }
412  inliers.reshape(points.channels());
413  return model;
414  }
415 
416  LineModel3d FitLine3d(const cv::Mat& points)
417  {
418  LineModel3d model;
419  if (points.rows < 2)
420  {
421  return model;
422  }
423  cv::Mat centroid;
424  cv::reduce(points.reshape(3), centroid, 0, cv::REDUCE_AVG);
425 
426  cv::Scalar c(centroid.at<float>(0, 0), centroid.at<float>(0, 1), centroid.at<float>(0, 2));
427 
428  cv::Mat A;
429  cv::subtract(points, c, A);
430 
431  cv::SVD svd;
432  cv::Mat w, u, vt;
433  cv::Mat At;
434  cv::transpose(A.reshape(1), At);
435  svd.compute(At, w, u, vt);
436 
437  cv::Point max_w_loc;
438  cv::minMaxLoc(w, NULL, NULL, NULL, &max_w_loc);
439 
440  model.x = centroid.at<float>(0, 0);
441  model.y = centroid.at<float>(0, 1);
442  model.z = centroid.at<float>(0, 2);
443  model.i = u.at<float>(0, max_w_loc.y);
444  model.j = u.at<float>(1, max_w_loc.y);
445  model.k = u.at<float>(2, max_w_loc.y);
446 
447  return model;
448  }
449 
451  const cv::Mat& points,
452  cv::Mat& inliers,
453  std::vector<uint32_t> &good_points,
454  int32_t& iterations,
455  double max_error,
456  double confidence,
457  int32_t min_iterations,
458  int32_t max_iterations,
460  {
462 
463  cv::Mat reshaped = points.reshape(3);
464  CrossFit3d fit_model(reshaped);
465  CrossModel3d model = ransac.FitModel(
466  fit_model, max_error, confidence, min_iterations, max_iterations, good_points, iterations);
467 
468  if (good_points.empty())
469  {
470  return model;
471  }
472 
473  inliers = cv::Mat(good_points.size(), reshaped.cols, reshaped.type());
474  for (size_t i = 0; i < good_points.size(); ++i)
475  {
476  inliers.at<cv::Vec3f>(i, 0) = reshaped.at<cv::Vec3f>(good_points[i], 0);
477  }
478  inliers.reshape(points.channels());
479  return model;
480  }
481 }
swri_opencv_util::Valid3dPointCorrespondences
bool Valid3dPointCorrespondences(const cv::Mat &points1, const cv::Mat &points2)
Definition: models.cpp:292
swri_opencv_util::PlaneFit
Definition: models.h:205
swri_opencv_util::FindOrthoLine3d
LineModel3d FindOrthoLine3d(const cv::Mat &points, const LineModel3d &ortho, cv::Mat &inliers, std::vector< uint32_t > &good_points, int32_t &iterations, double max_error=1.0, double confidence=0.9, int32_t min_iterations=1, int32_t max_iterations=1000, swri_math_util::RandomGeneratorPtr rng=swri_math_util::RandomGeneratorPtr())
Definition: model_fit.cpp:383
boost::shared_ptr
swri_opencv_util::PerpendicularPlaneWithPointFit
Definition: models.h:225
ros.h
swri_opencv_util::PlaneModel::x
float x
Definition: models.h:202
swri_opencv_util::CrossModel3d
Definition: models.h:294
swri_opencv_util::Valid2dPointCorrespondences
bool Valid2dPointCorrespondences(const cv::Mat &points1, const cv::Mat &points2)
Definition: models.cpp:265
swri_opencv_util::LineModel3d::i
float i
Definition: models.h:258
swri_opencv_util::FitAffineTransform2d
cv::Mat FitAffineTransform2d(const cv::Mat &points1, const cv::Mat &points2)
Definition: model_fit.cpp:193
swri_opencv_util::PlaneModel::z
float z
Definition: models.h:202
swri_opencv_util::LineModel3d::y
float y
Definition: models.h:258
swri_opencv_util::FindPlane
PlaneModel FindPlane(const cv::Mat &points, cv::Mat &inliers, std::vector< uint32_t > &good_points, int32_t &iterations, double max_error=1.0, double confidence=0.9, int32_t min_iterations=1, int32_t max_iterations=1000, swri_math_util::RandomGeneratorPtr rng=swri_math_util::RandomGeneratorPtr())
Definition: model_fit.cpp:285
swri_opencv_util::LineModel3d::k
float k
Definition: models.h:258
swri_opencv_util::PlaneModel::y
float y
Definition: models.h:202
model_fit.h
swri_opencv_util::LineModel3d
Definition: models.h:250
swri_opencv_util::FitRigidTransform2d
cv::Mat FitRigidTransform2d(const cv::Mat &points1, const cv::Mat &points2)
Definition: model_fit.cpp:72
swri_opencv_util::PlaneModel::j
float j
Definition: models.h:202
swri_opencv_util
Definition: blend.h:35
d
d
swri_opencv_util::FitPlane
PlaneModel FitPlane(const cv::Mat &points)
Definition: model_fit.cpp:317
swri_opencv_util::FindRigidTransform2d
cv::Mat FindRigidTransform2d(const cv::Mat &points1, const cv::Mat &points2, cv::Mat &inliers1, cv::Mat &inliers2, std::vector< uint32_t > &good_points, int32_t &iterations, double max_error=1.0, double confidence=0.9, int32_t max_iterations=1000, swri_math_util::RandomGeneratorPtr rng=swri_math_util::RandomGeneratorPtr())
Definition: model_fit.cpp:55
swri_opencv_util::PlaneModel
Definition: models.h:194
swri_opencv_util::FindTranslation2d
cv::Mat FindTranslation2d(const cv::Mat &points1, const cv::Mat &points2, cv::Mat &inliers1, cv::Mat &inliers2, std::vector< uint32_t > &good_points, int32_t &iterations, double max_error=1.0, double confidence=0.9, int32_t max_iterations=1000, swri_math_util::RandomGeneratorPtr rng=swri_math_util::RandomGeneratorPtr())
Definition: model_fit.cpp:38
swri_opencv_util::FitRotation3d
cv::Mat FitRotation3d(const cv::Mat &points1, const cv::Mat &points2)
Definition: model_fit.cpp:127
swri_opencv_util::FindHomography
cv::Mat FindHomography(const cv::Mat &points1, const cv::Mat &points2, cv::Mat &inliers1, cv::Mat &inliers2, std::vector< uint32_t > &good_points, int32_t &iterations, double max_error=1.0, double confidence=0.9, int32_t max_iterations=1000, swri_math_util::RandomGeneratorPtr rng=swri_math_util::RandomGeneratorPtr())
Definition: model_fit.cpp:176
swri_opencv_util::CrossFit3d
Definition: models.h:305
swri_opencv_util::FindPerpendicularPlaneWithPoint
PlaneModel FindPerpendicularPlaneWithPoint(const cv::Vec3f &point_on_plane, const cv::Vec3f &perp_axis, double max_angle_from_perp, const cv::Mat &points, cv::Mat &inliers, std::vector< uint32_t > &good_points, int32_t &iterations, double max_error, double confidence, int32_t min_iterations, int32_t max_iterations, swri_math_util::RandomGeneratorPtr rng=swri_math_util::RandomGeneratorPtr())
Definition: model_fit.cpp:249
NULL
#define NULL
swri_math_util::Ransac
swri_opencv_util::LineModel3d::x
float x
Definition: models.h:258
swri_math_util::Ransac::FitModel
ModelType FitModel(Model &model, double max_error, double confidence, int32_t min_iterations, int32_t max_iterations, std::vector< uint32_t > &inliers, int32_t &iterations)
swri_opencv_util::FindLine3d
LineModel3d FindLine3d(const cv::Mat &points, cv::Mat &inliers, std::vector< uint32_t > &good_points, int32_t &iterations, double max_error=1.0, double confidence=0.9, int32_t min_iterations=1, int32_t max_iterations=1000, swri_math_util::RandomGeneratorPtr rng=swri_math_util::RandomGeneratorPtr())
Definition: model_fit.cpp:351
swri_opencv_util::OrthoLineFit3d
Definition: models.h:282
swri_opencv_util::FindCross3d
CrossModel3d FindCross3d(const cv::Mat &points, cv::Mat &inliers, std::vector< uint32_t > &good_points, int32_t &iterations, double max_error=1.0, double confidence=0.9, int32_t min_iterations=1, int32_t max_iterations=1000, swri_math_util::RandomGeneratorPtr rng=swri_math_util::RandomGeneratorPtr())
Definition: model_fit.cpp:450
swri_opencv_util::PlaneModel::i
float i
Definition: models.h:202
swri_opencv_util::FindAffineTransform2d
cv::Mat FindAffineTransform2d(const cv::Mat &points1, const cv::Mat &points2, cv::Mat &inliers1, cv::Mat &inliers2, std::vector< uint32_t > &good_points, int32_t &iterations, double max_error=1.0, double confidence=0.9, int32_t max_iterations=1000, swri_math_util::RandomGeneratorPtr rng=swri_math_util::RandomGeneratorPtr())
Definition: model_fit.cpp:159
swri_opencv_util::FitLine3d
LineModel3d FitLine3d(const cv::Mat &points)
Definition: model_fit.cpp:416
swri_opencv_util::LineModel3d::z
float z
Definition: models.h:258
swri_opencv_util::LineModel3d::j
float j
Definition: models.h:258
swri_opencv_util::LineFit3d
Definition: models.h:261
swri_opencv_util::PlaneModel::k
float k
Definition: models.h:202


swri_opencv_util
Author(s): Marc Alban
autogenerated on Fri Aug 2 2024 08:39:13