image_warp_util.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 
32 
33 #include <algorithm>
34 
35 namespace swri_image_util
36 {
37  cv::Mat WarpImage(const cv::Mat& image, double roll, double pitch)
38  {
39  cv::Mat warped;
40 
41  // Initialize the camera matrix:
42  cv::Mat K = cv::Mat::eye(cv::Size(3, 3), CV_32F);
43  K.at<float>(0, 2) = static_cast<double>(image.cols - 1) / 2.0;
44  K.at<float>(1, 2) = static_cast<double>(image.rows - 1) / 2.0;
45 
46  cv::Mat T = cv::Mat::zeros(cv::Size(3, 1), CV_32F);
47 
48  cv::Mat R = GetR(pitch, roll);
49 
50  cv::detail::PlaneWarper warper;
51  warper.warp(image, K, R, T, cv::INTER_LANCZOS4, 0, warped);
52 
53  // TODO(malban): This warp can cause problems because it can change the
54  // image size. The result should be cropped or padded. The
55  // warp points function will need to be modified accordingly.
56 
57  return warped;
58  }
59 
60  void WarpPoints(
61  double pitch,
62  double roll,
63  const cv::Size& image_size,
64  const cv::Mat& pts_in,
65  cv::Mat& pts_out)
66  {
67  // Initialize the camera matrix:
68  cv::Mat K = cv::Mat::eye(cv::Size(3, 3), CV_32F);
69  K.at<float>(0, 2) = static_cast<double>(image_size.width - 1) / 2.0;
70  K.at<float>(1, 2) = static_cast<double>(image_size.height - 1) / 2.0;
71 
72  cv::detail::PlaneWarper warper;
73 
74  cv::Mat T = cv::Mat::zeros(cv::Size(3, 1), CV_32F);
75  cv::Mat R = GetR(pitch, roll);
76  pts_in.copyTo(pts_out);
77  for (int32_t i = 0; i < pts_in.rows; ++i)
78  {
79  cv::Point2f pt;
80  pt.x = pts_in.at<cv::Vec2f>(i, 0).val[0];
81  pt.y = pts_in.at<cv::Vec2f>(i, 0).val[1];
82  cv::Point2f pt2 = warper.warpPoint(pt, K, R, T);
83  pts_out.at<cv::Vec2f>(i, 0).val[0] = pt2.x + K.at<float>(0, 2);
84  pts_out.at<cv::Vec2f>(i, 0).val[1] = pt2.y + K.at<float>(1, 2);
85  }
86  }
87 
88  void WarpPoints(
89  double pitch,
90  double roll,
91  const cv::Size& image_size,
92  const std::vector<cv::KeyPoint>& pts_in,
93  std::vector<cv::KeyPoint>& pts_out)
94  {
95  pts_out = pts_in;
96 
97  // Initialize the camera matrix:
98  cv::Mat K = cv::Mat::eye(cv::Size(3, 3), CV_32F);
99  K.at<float>(0, 2) = static_cast<double>(image_size.width - 1) / 2.0;
100  K.at<float>(1, 2) = static_cast<double>(image_size.height - 1) / 2.0;
101 
102  cv::detail::PlaneWarper warper;
103 
104  cv::Mat T = cv::Mat::zeros(cv::Size(3, 1), CV_32F);
105  cv::Mat R = GetR(pitch, roll);
106 
107  for (uint32_t i = 0; i < pts_in.size(); i++)
108  {
109  pts_out[i].pt = warper.warpPoint(pts_in[i].pt, K, R, T);
110  pts_out[i].pt.x += K.at<float>(0, 2);
111  pts_out[i].pt.y += K.at<float>(1, 2);
112  }
113  }
114 
115 
116  cv::Mat GetR(double pitch, double roll, double yaw)
117  {
118  cv::Mat R1 = cv::Mat::eye(cv::Size(3, 3), CV_32F);
119  cv::Mat R2 = cv::Mat::eye(cv::Size(3, 3), CV_32F);
120  cv::Mat R3 = cv::Mat::eye(cv::Size(3, 3), CV_32F);
121 
122  // do pitch first:
123  R1.at<float>(0, 0) = std::cos(pitch);
124  R1.at<float>(0, 2) = -std::sin(pitch);
125  R1.at<float>(2, 0) = std::sin(pitch);
126  R1.at<float>(2, 2) = std::cos(pitch);
127 
128  // Then roll
129  R2.at<float>(1, 1) = std::cos(roll);
130  R2.at<float>(1, 2) = std::sin(roll);
131  R2.at<float>(2, 1) = -std::sin(roll);
132  R2.at<float>(2, 2) = std::cos(roll);
133 
134  // Finally yaw
135  R3.at<float>(0, 0) = std::cos(yaw);
136  R3.at<float>(0, 1) = std::sin(yaw);
137  R3.at<float>(1, 0) = -std::sin(yaw);
138  R3.at<float>(1, 1) = std::cos(yaw);
139 
140  cv::Mat R = R3 * R2 * R1;
141 
142  return R;
143  }
144 
146  double& nominal_pitch,
147  double& nominal_roll,
148  bool show_image_diff)
149  {
150  if (kp1_matched_.empty() || kp2_matched_.empty())
151  {
152  return cv::Mat();
153  }
154 
156  cv::Mat T_rigid = EstimateNominalAngle(kp1_matched_,
157  kp2_matched_,
158  cv::Size(im1_.cols, im1_.rows),
159  nominal_pitch,
160  nominal_roll);
161 
163 
164  ROS_ERROR("Estimate Nominal Angle time = %g", (T2 - T1).toSec());
165  cv::Mat R = GetR(nominal_pitch, nominal_roll);
166 
167  if (show_image_diff)
168  {
169  // Do the warping and transformation and show the results
170  cv::Mat warped_im1;
171  cv::Mat warped_im2;
172 
173  warper_.warp(im1_, K_, R, T_, cv::INTER_LANCZOS4, 0, warped_im1);
174  warper_.warp(im2_, K_, R, T_, cv::INTER_LANCZOS4, 0, warped_im2);
175 
176  cv::Mat temp_im;
177  cv::warpAffine(warped_im1,
178  temp_im,
179  T_rigid,
180  cv::Size(warped_im1.cols, warped_im1.rows));
181 
182  cv::Mat sub = warped_im2 - temp_im;
183 
184  cv::namedWindow("Warped Subtraction");
185  cv::imshow("Warped Subtraction", sub);
186 
187  // Now compare the result to the unwarped, transformed result:
188 
189  cv::warpAffine(im1_,
190  temp_im,
191  T_rigid,
192  cv::Size(im1_.cols, im1_.rows));
193 
194  cv::Mat sub2 = im2_ - temp_im;
195  cv::namedWindow("Subtraction");
196  cv::imshow("Subtraction", sub2);
197 
198  cv::namedWindow("im2_");
199  cv::imshow("im2_", im2_);
200  cv::waitKey(0);
201  }
202 
203  return R;
204  }
205 
207  const cv::Mat& points1,
208  const cv::Mat& points2,
209  const cv::Size& image_size,
210  double& nominal_pitch,
211  double& nominal_roll)
212  {
213  // Max number of iterations per angle, per scale
214  const int32_t max_iterations = 5;
215  const int32_t num_octaves = 5;
216 
217  double pitch_range = 0.02 * 3.14159/180.0;
218  double min_pitch = -std::abs(pitch_range / 2.0);
219  double max_pitch = std::abs(pitch_range / 2.0);
220 
221  double roll_range = 0.02*3.14159/180.0;
222  double min_roll = -std::abs(roll_range / 2.0);
223  double max_roll = std::abs(roll_range / 2.0);
224 
225  cv::Mat T_rigid_final = cv::Mat();
226  for (int32_t octave_idx = 0; octave_idx < num_octaves; ++octave_idx)
227  {
228  double dp = (max_pitch - min_pitch) /
229  static_cast<double>(max_iterations - 1);
230  double dr = (max_roll - min_roll) /
231  static_cast<double>(max_iterations - 1);
232 
233  double min_diff = 1e20;
234  nominal_pitch = 0.0;
235  nominal_roll = 0.0;
236 
237 
238  for (int32_t pitch_idx = 0; pitch_idx < max_iterations; ++pitch_idx)
239  {
240  double cur_pitch = min_pitch + dp * pitch_idx;
241  for (int32_t roll_idx = 0; roll_idx < max_iterations; ++roll_idx)
242  {
243  double cur_roll = min_roll + dr * roll_idx;
244 
245  cv::Mat kp1_warped;
246  swri_image_util::WarpPoints(cur_pitch,
247  cur_roll,
248  image_size,
249  points1,
250  kp1_warped);
251 
252  cv::Mat kp2_warped;
253  swri_image_util::WarpPoints(cur_pitch,
254  cur_roll,
255  image_size,
256  points2,
257  kp2_warped);
258 
259  cv::Mat T_rigid;
260  cv::Mat T_affine;
261  double rms_error;
262  bool success = EstimateTransforms(kp1_warped,
263  kp2_warped,
264  T_affine,
265  T_rigid,
266  rms_error);
267  if (!success)
268  {
269  continue;
270  }
271 
272  double cur_diff = rms_error;
273 
274  if (cur_diff < min_diff)
275  {
276  min_diff = cur_diff;
277 
278  nominal_pitch = cur_pitch;
279  nominal_roll = cur_roll;
280  T_rigid_final = T_rigid;
281  }
282  }
283  }
284 
285  min_pitch = nominal_pitch - std::abs(dp * 2 / 3);
286  max_pitch = nominal_pitch + std::abs(dp * 2 / 3);
287 
288 
289  min_roll = nominal_roll - std::abs(dr * 2 / 3);
290  max_roll = nominal_roll + std::abs(dr * 2 / 3);
291  }
292 
293  cv::Mat R = GetR(nominal_pitch,
294  nominal_roll);
295 
296  return T_rigid_final;
297  }
298 
300  {
301  if (im1_.empty() || im2_.empty())
302  {
303  ROS_ERROR("No images defined");
304  return false;
305  }
306 
307  // perform the matching first:
308  // Compute the matching features between this frame and the previous one.
309  std::vector<cv::DMatch> matches;
310  cv::BFMatcher matcher;
311  matcher.match(descriptors1_, descriptors2_, matches);
312 
313  cv::Mat points1;
314  cv::Mat points2;
315  ConvertMatches(kp1_, kp2_, matches, points1, points2);
316 
317  // Compute the fundamental matrix which describes the camera motion
318  // between the frames using a RANSAC process and get the set of inlier
319  // matches which agree with the motion.
320  cv::Mat fundamental_matrix;
321  cv::Mat fund_inliers1;
322  cv::Mat fund_inliers2;
323  try
324  {
325  GetFundamentalInliers(points1,
326  points2,
327  fundamental_matrix,
328  fund_inliers1,
329  fund_inliers2);
330  }
331  catch (const std::exception& e)
332  {
333  ROS_ERROR("Caught an exception when computing fundamental inliers:"
334  " %s", e.what());
335  return false;
336  }
337 
338  ROS_INFO("Found %d fundamental inliers.", fund_inliers1.rows);
339 
340  cv::Mat inliers1;
341  cv::Mat inliers2;
342  std::vector<uint32_t> good_points;
343 
344  int32_t iterations;
346  fund_inliers1, fund_inliers2, inliers1, inliers2, good_points, iterations, 30.0);
347 
348  if (affine.empty())
349  {
350  ROS_ERROR("Failed to compute 2D affine transform.");
351  return false;
352  }
353 
354  kp1_matched_ = inliers1;
355  kp2_matched_ = inliers2;
356 
357  return true;
358  }
359 
361  cv::Mat& pts1,
362  cv::Mat& pts2,
363  cv::Mat& T_affine,
364  cv::Mat& T_rigid,
365  double& rms_error)
366  {
367  cv::Mat inliers1;
368  cv::Mat inliers2;
369  std::vector<uint32_t> good_points;
370 
371  int32_t iterations;
373  pts1, pts2, inliers1, inliers2, good_points, iterations, 30.0);
374 
376  pts1, pts2, inliers1, inliers2, good_points, iterations, 30.0);
377 
378  cv::Mat inliers1_t;
379  cv::transform(inliers1, inliers1_t, T_rigid);
380  double n = good_points.size();
381  rms_error = cv::norm(inliers2, inliers1_t, cv::NORM_L2) / std::sqrt(n);
382 
383  if (T_rigid.empty())
384  {
385  return false;
386  }
387 
388  return true;
389  }
390 
392  double pitch,
393  double roll,
394  const cv::Mat& pts_in,
395  cv::Mat& pts_out)
396  {
397  if (im1_.empty() || im2_.empty())
398  {
399  ROS_ERROR("Object not initialized. Pitch and roll not computed. Perhaps"
400  "call static implementation instead");
401  return;
402  }
403 
405  roll,
406  cv::Size(im1_.cols, im1_.rows),
407  pts_in,
408  pts_out);
409  }
410 
412  const cv::Mat& T,
413  const cv::Mat& pts_in,
414  cv::Mat& pts_out)
415  {
416  // Create augmented keypoint matrix:
417  cv::Mat aug_mat(cv::Size(3, pts_in.rows), CV_32F);
418 
419  for (int32_t i = 0; i < pts_in.rows; ++i)
420  {
421  aug_mat.at<float>(i, 0) = pts_in.at<cv::Vec2f>(0, i)[0];
422  aug_mat.at<float>(i, 1) = pts_in.at<cv::Vec2f>(0, i)[1];
423  aug_mat.at<float>(i, 2) = 1.0;
424  }
425 
426  cv::Mat T_temp = T.t();
427  cv::Mat pts_out_temp = aug_mat * T_temp;
428 
429  pts_out.release();
430  pts_out.create(cv::Size(1, pts_in.rows), CV_32FC2);
431  // Convert points back to proper form:
432  for (int32_t i = 0; i < pts_in.rows; ++i)
433  {
434  pts_out.at<cv::Vec2f>(0, i)[0] = pts_out_temp.at<float>(i, 0);
435  pts_out.at<cv::Vec2f>(0, i)[1] = pts_out_temp.at<float>(i, 1);
436  }
437  }
438 
446 
448  //
449  // PitchAndRollEstimatorQueue()
450  //
453  {
454  SetBufferSize();
455  ComputeStats();
456  }
457 
459  //
460  // SetBufferSize()
461  //
464  {
465  pitches_.set_capacity(buff_size);
466  rolls_.set_capacity(buff_size);
467  }
468 
469 
471  //
472  // WarpPoints()
473  //
475  void PitchAndRollEstimatorQueue::WarpPoints(const cv::Mat& points_in,
476  cv::Mat& points_out,
477  const cv::Size& image_size,
478  bool use_median)
479  {
480  double pitch;
481  double roll;
482  if (use_median)
483  {
484  GetMedianPitchAndRoll(pitch, roll);
485  }
486  else
487  {
488  GetMeanPitchAndRoll(pitch, roll);
489  }
490 
491  swri_image_util::WarpPoints(pitch, roll, image_size, points_in, points_out);
492  }
493 
495  //
496  // Clear()
497  //
500  {
501  pitches_.clear();
502  rolls_.clear();
503  ComputeStats();
504  }
505 
506 
508  //
509  // GenerateNewEstimate()
510  //
513  const cv::Mat& points1,
514  const cv::Mat& points2,
515  const cv::Size& image_size)
516  {
517  double pitch = 0.0;
518  double roll = 0.0;
520  points2,
521  image_size,
522  pitch,
523  roll);
524 
525  if (!T.empty())
526  {
527  LoadNewData(pitch, roll);
528  ComputeStats();
529  }
530  }
531 
533  //
534  // GetMeanPitchAndRoll()
535  //
538  double& roll)
539  {
540  pitch = mean_pitch_;
541  roll = mean_roll_;
542 
543  return pitches_.size() > 0;
544  }
545 
547  //
548  // GetMedianPitchAndRoll()
549  //
552  double& roll)
553  {
554  pitch = median_pitch_;
555  roll = median_roll_;
556 
557  return pitches_.size() > 0;
558  }
559 
560 
562  //
563  // LoadNewData()
564  //
567  double new_roll)
568  {
569  pitches_.push_back(new_pitch);
570  rolls_.push_back(new_roll);
571  ComputeStats();
572  }
573 
575  //
576  // ComputeStats()
577  //
580  {
581  mean_pitch_ = 0.0;
582  mean_roll_ = 0.0;
583  median_pitch_ = 0.0;
584  median_roll_ = 0.0;
585 
586  if (pitches_.empty())
587  {
588  return;
589  }
590 
591  std::vector<double> temp_pitch;
592  std::vector<double> temp_roll;
593  temp_pitch.assign(pitches_.begin(), pitches_.end());
594  temp_roll.assign(rolls_.begin(), rolls_.end());
595 
596  std::sort(temp_pitch.begin(), temp_pitch.end());
597  std::sort(temp_roll.begin(), temp_roll.end());
598 
599  double pitch_sum = 0.0;
600  double roll_sum = 0.0;
601 
602  for (int32_t i = 0; i < (int32_t)temp_pitch.size(); ++i)
603  {
604  pitch_sum += temp_pitch[i];
605  roll_sum += temp_roll[i];
606  }
607 
608  double N = static_cast<double>(temp_pitch.size());
609  mean_pitch_ = pitch_sum / N;
610  mean_roll_ = roll_sum / N;
611 
612  int32_t mid_idx = static_cast<int32_t>(temp_pitch.size() - 1) / 2;
613  if (temp_pitch.size() % 2 == 0)
614  {
615  median_pitch_ = (temp_pitch[mid_idx] + temp_pitch[mid_idx + 1]) / 2.0;
616  median_roll_ = (temp_roll[mid_idx] + temp_roll[mid_idx + 1]) / 2.0;
617  }
618  else
619  {
620  median_pitch_ = temp_pitch[mid_idx];
621  median_roll_ = temp_roll[mid_idx];
622  }
623  }
624 }
void GetFundamentalInliers(const cv::Mat points1, const cv::Mat points2, cv::Mat &fundamental_matrix, cv::Mat &inliers1, cv::Mat &inliers2, double max_distance=1.0, double confidence=0.99)
Computes the fundamental matrix for a set of matching points in two different images. The method also returns the inlier keypoints for both frames.
std::vector< cv::KeyPoint > kp2_
void GenerateNewEstimate(const cv::Mat &points1, const cv::Mat &points2, const cv::Size &image_size)
Estimates pitch and roll from corresponding points and loads the pitch and roll data onto the buffer...
void ComputeStats()
Computes the statistics on the data in the buffers.
static bool EstimateTransforms(cv::Mat &pts1, cv::Mat &pts2, cv::Mat &T_affine, cv::Mat &T_rigid, double &rms_error)
Estimates the "nearest" rigid, and corresponding full affine transformation for a set of matching poi...
bool ComputeGeometricMatches()
Matches keypoints using loose geometric constraints and stores them in kp1_matched_ and kp2_matched_...
void WarpPoints(double pitch, double roll, const cv::Mat &pts_in, cv::Mat &pts_out)
Warps a matrix of points (in the same form as the inliers)
void WarpAffinePoints(const cv::Mat &T, const cv::Mat &pts_in, cv::Mat &pts_out)
Warps a matrix of points (in the same form as the inliers)
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())
bool GetMeanPitchAndRoll(double &pitch, double &roll)
Computes the mean pitch and roll.
void WarpPoints(double pitch, double roll, const cv::Size &image_size, const cv::Mat &pts_in, cv::Mat &pts_out)
cv::Mat WarpImage(const cv::Mat &image, double roll, double pitch)
#define ROS_INFO(...)
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())
static WallTime now()
bool GetMedianPitchAndRoll(double &pitch, double &roll)
Computes the median pitch and roll.
void ConvertMatches(const std::vector< cv::KeyPoint > &kp1, const std::vector< cv::KeyPoint > &kp2, const std::vector< cv::DMatch > &matches, cv::Mat &kp1_out, cv::Mat &kp2_out)
Converts keypoints and matches into two cv::Mats in which the the matching keypoints from kp1 and kp2...
cv::Mat GetR(double pitch, double roll, double yaw=0.0)
Gets the rotation matrix associated with the specified pitch and roll values.
std::vector< cv::KeyPoint > kp1_
cv::Mat EstimateNominalAngle(double &nominal_pitch, double &nominal_roll, bool show_image_diff=false)
Estimates the nominal pitch and roll of the camera (from perfectly vertical) from two overlapping ima...
void WarpPoints(const cv::Mat &points_in, cv::Mat &points_out, const cv::Size &image_size, bool use_median=true)
Warps points based on the stored estimated pitch and roll.
void SetBufferSize(int32_t buff_size=50)
Sets the circular buffer capacity for computing statistics.
void LoadNewData(double new_pitch, double new_roll)
Loads new pitch and roll data directly onto the buffer.
#define ROS_ERROR(...)


swri_image_util
Author(s): Kris Kozak
autogenerated on Sat Jan 21 2023 03:13:20