image_warp_util.cpp
Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
00004 // All rights reserved.
00005 //
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from this software without specific prior written permission.
00016 //
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //
00028 // *****************************************************************************
00029 
00030 #include <swri_image_util/image_warp_util.h>
00031 #include <swri_opencv_util/model_fit.h>
00032 
00033 #include <algorithm>
00034 
00035 namespace swri_image_util
00036 {
00037   cv::Mat WarpImage(const cv::Mat& image, double roll, double pitch)
00038   {
00039     cv::Mat warped;
00040 
00041     // Initialize the camera matrix:
00042     cv::Mat K = cv::Mat::eye(cv::Size(3, 3), CV_32F);
00043     K.at<float>(0, 2) = static_cast<double>(image.cols - 1) / 2.0;
00044     K.at<float>(1, 2) = static_cast<double>(image.rows - 1) / 2.0;
00045 
00046     cv::Mat T = cv::Mat::zeros(cv::Size(3, 1), CV_32F);
00047 
00048     cv::Mat R = GetR(pitch, roll);
00049 
00050     cv::detail::PlaneWarper warper;
00051     warper.warp(image, K, R, T, cv::INTER_LANCZOS4, 0, warped);
00052 
00053     // TODO(malban): This warp can cause problems because it can change the
00054     //               image size.  The result should be cropped or padded.  The
00055     //               warp points function will need to be modified accordingly.
00056 
00057     return warped;
00058   }
00059 
00060   void WarpPoints(
00061       double pitch,
00062       double roll,
00063       const cv::Size& image_size,
00064       const cv::Mat& pts_in,
00065       cv::Mat& pts_out)
00066   {
00067     // Initialize the camera matrix:
00068     cv::Mat K = cv::Mat::eye(cv::Size(3, 3), CV_32F);
00069     K.at<float>(0, 2) = static_cast<double>(image_size.width - 1) / 2.0;
00070     K.at<float>(1, 2) = static_cast<double>(image_size.height - 1) / 2.0;
00071 
00072     cv::detail::PlaneWarper warper;
00073 
00074     cv::Mat T = cv::Mat::zeros(cv::Size(3, 1), CV_32F);
00075     cv::Mat R = GetR(pitch, roll);
00076     pts_in.copyTo(pts_out);
00077     for (int32_t i = 0; i < pts_in.rows; ++i)
00078     {
00079       cv::Point2f pt;
00080       pt.x = pts_in.at<cv::Vec2f>(i, 0).val[0];
00081       pt.y = pts_in.at<cv::Vec2f>(i, 0).val[1];
00082       cv::Point2f pt2 = warper.warpPoint(pt, K, R, T);
00083       pts_out.at<cv::Vec2f>(i, 0).val[0] = pt2.x + K.at<float>(0, 2);
00084       pts_out.at<cv::Vec2f>(i, 0).val[1] = pt2.y + K.at<float>(1, 2);
00085     }
00086   }
00087 
00088   void WarpPoints(
00089       double pitch,
00090       double roll,
00091       const cv::Size& image_size,
00092       const std::vector<cv::KeyPoint>& pts_in,
00093       std::vector<cv::KeyPoint>& pts_out)
00094   {
00095     pts_out = pts_in;
00096 
00097     // Initialize the camera matrix:
00098     cv::Mat K = cv::Mat::eye(cv::Size(3, 3), CV_32F);
00099     K.at<float>(0, 2) = static_cast<double>(image_size.width - 1) / 2.0;
00100     K.at<float>(1, 2) = static_cast<double>(image_size.height - 1) / 2.0;
00101 
00102     cv::detail::PlaneWarper warper;
00103 
00104     cv::Mat T = cv::Mat::zeros(cv::Size(3, 1), CV_32F);
00105     cv::Mat R = GetR(pitch, roll);
00106 
00107     for (uint32_t i = 0; i < pts_in.size(); i++)
00108     {
00109       pts_out[i].pt = warper.warpPoint(pts_in[i].pt, K, R, T);
00110       pts_out[i].pt.x += K.at<float>(0, 2);
00111       pts_out[i].pt.y += K.at<float>(1, 2);
00112     }
00113   }
00114 
00115 
00116   cv::Mat GetR(double pitch, double roll, double yaw)
00117   {
00118     cv::Mat R1 = cv::Mat::eye(cv::Size(3, 3), CV_32F);
00119     cv::Mat R2 = cv::Mat::eye(cv::Size(3, 3), CV_32F);
00120     cv::Mat R3 = cv::Mat::eye(cv::Size(3, 3), CV_32F);
00121 
00122     // do pitch first:
00123     R1.at<float>(0, 0) = std::cos(pitch);
00124     R1.at<float>(0, 2) = -std::sin(pitch);
00125     R1.at<float>(2, 0) = std::sin(pitch);
00126     R1.at<float>(2, 2) = std::cos(pitch);
00127 
00128     // Then roll
00129     R2.at<float>(1, 1) = std::cos(roll);
00130     R2.at<float>(1, 2) = std::sin(roll);
00131     R2.at<float>(2, 1) = -std::sin(roll);
00132     R2.at<float>(2, 2) = std::cos(roll);
00133 
00134     // Finally yaw
00135     R3.at<float>(0, 0) = std::cos(yaw);
00136     R3.at<float>(0, 1) = std::sin(yaw);
00137     R3.at<float>(1, 0) = -std::sin(yaw);
00138     R3.at<float>(1, 1) = std::cos(yaw);
00139 
00140     cv::Mat R = R3 * R2 * R1;
00141 
00142     return R;
00143   }
00144 
00145   cv::Mat PitchAndRollEstimator::EstimateNominalAngle(
00146       double& nominal_pitch,
00147       double& nominal_roll,
00148       bool show_image_diff)
00149   {
00150     if (kp1_matched_.empty() || kp2_matched_.empty())
00151     {
00152       return cv::Mat();
00153     }
00154 
00155     ros::WallTime T1 = ros::WallTime::now();
00156     cv::Mat T_rigid = EstimateNominalAngle(kp1_matched_,
00157                                            kp2_matched_,
00158                                            cv::Size(im1_.cols, im1_.rows),
00159                                            nominal_pitch,
00160                                            nominal_roll);
00161 
00162     ros::WallTime T2 = ros::WallTime::now();
00163 
00164     ROS_ERROR("Estimate Nominal Angle time = %g", (T2 - T1).toSec());
00165     cv::Mat R = GetR(nominal_pitch, nominal_roll);
00166 
00167     if (show_image_diff)
00168     {
00169       // Do the warping and transformation and show the results
00170       cv::Mat warped_im1;
00171       cv::Mat warped_im2;
00172 
00173       warper_.warp(im1_, K_, R, T_, cv::INTER_LANCZOS4, 0, warped_im1);
00174       warper_.warp(im2_, K_, R, T_, cv::INTER_LANCZOS4, 0, warped_im2);
00175 
00176       cv::Mat temp_im;
00177       cv::warpAffine(warped_im1,
00178                      temp_im,
00179                      T_rigid,
00180                      cv::Size(warped_im1.cols, warped_im1.rows));
00181 
00182       cv::Mat sub = warped_im2 - temp_im;
00183 
00184       cv::namedWindow("Warped Subtraction");
00185       cv::imshow("Warped Subtraction", sub);
00186 
00187       // Now compare the result to the unwarped, transformed result:
00188 
00189       cv::warpAffine(im1_,
00190                      temp_im,
00191                      T_rigid,
00192                      cv::Size(im1_.cols, im1_.rows));
00193 
00194       cv::Mat sub2 = im2_ - temp_im;
00195       cv::namedWindow("Subtraction");
00196       cv::imshow("Subtraction", sub2);
00197 
00198       cv::namedWindow("im2_");
00199       cv::imshow("im2_", im2_);
00200       cv::waitKey(0);
00201     }
00202 
00203     return R;
00204   }
00205 
00206   cv::Mat PitchAndRollEstimator::EstimateNominalAngle(
00207       const cv::Mat& points1,
00208       const cv::Mat& points2,
00209       const cv::Size& image_size,
00210       double& nominal_pitch,
00211       double& nominal_roll)
00212   {
00213     // Max number of iterations per angle, per scale
00214     const int32_t max_iterations = 5;
00215     const int32_t num_octaves = 5;
00216 
00217     double pitch_range = 0.02 * 3.14159/180.0;
00218     double min_pitch = -std::abs(pitch_range / 2.0);
00219     double max_pitch = std::abs(pitch_range / 2.0);
00220 
00221     double roll_range = 0.02*3.14159/180.0;
00222     double min_roll = -std::abs(roll_range / 2.0);
00223     double max_roll = std::abs(roll_range / 2.0);
00224 
00225     cv::Mat T_rigid_final = cv::Mat();
00226     for (int32_t octave_idx = 0; octave_idx < num_octaves; ++octave_idx)
00227     {
00228       double dp = (max_pitch - min_pitch) /
00229           static_cast<double>(max_iterations - 1);
00230       double dr = (max_roll - min_roll) /
00231           static_cast<double>(max_iterations - 1);
00232 
00233       double min_diff = 1e20;
00234       nominal_pitch = 0.0;
00235       nominal_roll = 0.0;
00236 
00237 
00238       for (int32_t pitch_idx = 0; pitch_idx < max_iterations; ++pitch_idx)
00239       {
00240         double cur_pitch = min_pitch + dp * pitch_idx;
00241         for (int32_t roll_idx = 0; roll_idx < max_iterations; ++roll_idx)
00242         {
00243           double cur_roll = min_roll + dr * roll_idx;
00244 
00245           cv::Mat kp1_warped;
00246           swri_image_util::WarpPoints(cur_pitch,
00247                      cur_roll,
00248                      image_size,
00249                      points1,
00250                      kp1_warped);
00251 
00252           cv::Mat kp2_warped;
00253           swri_image_util::WarpPoints(cur_pitch,
00254                      cur_roll,
00255                      image_size,
00256                      points2,
00257                      kp2_warped);
00258 
00259           cv::Mat T_rigid;
00260           cv::Mat T_affine;
00261           double rms_error;
00262           bool success = EstimateTransforms(kp1_warped,
00263                                             kp2_warped,
00264                                             T_affine,
00265                                             T_rigid,
00266                                             rms_error);
00267           if (!success)
00268           {
00269             continue;
00270           }
00271 
00272           double cur_diff = rms_error;
00273 
00274           if (cur_diff < min_diff)
00275           {
00276             min_diff = cur_diff;
00277 
00278             nominal_pitch = cur_pitch;
00279             nominal_roll = cur_roll;
00280             T_rigid_final = T_rigid;
00281           }
00282         }
00283       }
00284 
00285       min_pitch = nominal_pitch - std::abs(dp * 2 / 3);
00286       max_pitch = nominal_pitch + std::abs(dp * 2 / 3);
00287 
00288 
00289       min_roll = nominal_roll - std::abs(dr * 2 / 3);
00290       max_roll = nominal_roll + std::abs(dr * 2 / 3);
00291     }
00292 
00293     cv::Mat R = GetR(nominal_pitch,
00294                      nominal_roll);
00295 
00296     return T_rigid_final;
00297   }
00298 
00299   bool PitchAndRollEstimator::ComputeGeometricMatches()
00300   {
00301     if (im1_.empty() || im2_.empty())
00302     {
00303       ROS_ERROR("No images defined");
00304       return false;
00305     }
00306 
00307     // perform the matching first:
00308     // Compute the matching features between this frame and the previous one.
00309     std::vector<cv::DMatch> matches;
00310     cv::BFMatcher matcher;
00311     matcher.match(descriptors1_, descriptors2_, matches);
00312 
00313     cv::Mat points1;
00314     cv::Mat points2;
00315     ConvertMatches(kp1_, kp2_, matches, points1, points2);
00316 
00317     // Compute the fundamental matrix which describes the camera motion
00318     // between the frames using a RANSAC process and get the set of inlier
00319     // matches which agree with the motion.
00320     cv::Mat fundamental_matrix;
00321     cv::Mat fund_inliers1;
00322     cv::Mat fund_inliers2;
00323     try
00324     {
00325       GetFundamentalInliers(points1,
00326                             points2,
00327                             fundamental_matrix,
00328                             fund_inliers1,
00329                             fund_inliers2);
00330     }
00331     catch (const std::exception& e)
00332     {
00333       ROS_ERROR("Caught an exception when computing fundamental inliers:"
00334                 " %s", e.what());
00335       return false;
00336     }
00337 
00338     ROS_INFO("Found %d fundamental inliers.", fund_inliers1.rows);
00339 
00340     cv::Mat inliers1;
00341     cv::Mat inliers2;
00342     std::vector<uint32_t> good_points;
00343     
00344     int32_t iterations;
00345     cv::Mat affine = swri_opencv_util::FindAffineTransform2d(
00346       fund_inliers1, fund_inliers2, inliers1, inliers2, good_points, iterations, 30.0);
00347 
00348     if (affine.empty())
00349     {
00350       ROS_ERROR("Failed to compute 2D affine transform.");
00351       return false;
00352     }
00353 
00354     kp1_matched_ = inliers1;
00355     kp2_matched_ = inliers2;
00356 
00357     return true;
00358   }
00359 
00360   bool PitchAndRollEstimator::EstimateTransforms(
00361       cv::Mat& pts1,
00362       cv::Mat& pts2,
00363       cv::Mat& T_affine,
00364       cv::Mat& T_rigid,
00365       double& rms_error)
00366   {
00367     cv::Mat inliers1;
00368     cv::Mat inliers2;
00369     std::vector<uint32_t> good_points;
00370     
00371     int32_t iterations;
00372     T_affine = swri_opencv_util::FindAffineTransform2d(
00373       pts1, pts2, inliers1, inliers2, good_points, iterations, 30.0);
00374     
00375     T_rigid = swri_opencv_util::FindRigidTransform2d(
00376       pts1, pts2, inliers1, inliers2, good_points, iterations, 30.0);
00377     
00378     cv::Mat inliers1_t;
00379     cv::transform(inliers1, inliers1_t, T_rigid);
00380     double n = good_points.size();
00381     rms_error = cv::norm(inliers2, inliers1_t, cv::NORM_L2) / std::sqrt(n);
00382 
00383     if (T_rigid.empty())
00384     {
00385       return false;
00386     }
00387 
00388     return true;
00389   }
00390 
00391   void PitchAndRollEstimator::WarpPoints(
00392       double pitch,
00393       double roll,
00394       const cv::Mat& pts_in,
00395       cv::Mat& pts_out)
00396   {
00397     if (im1_.empty() || im2_.empty())
00398     {
00399       ROS_ERROR("Object not initialized. Pitch and roll not computed.  Perhaps"
00400                 "call static implementation instead");
00401       return;
00402     }
00403 
00404     swri_image_util::WarpPoints(pitch,
00405                roll,
00406                cv::Size(im1_.cols, im1_.rows),
00407                pts_in,
00408                pts_out);
00409   }
00410 
00411   void PitchAndRollEstimator::WarpAffinePoints(
00412       const cv::Mat& T,
00413       const cv::Mat& pts_in,
00414       cv::Mat& pts_out)
00415   {
00416     // Create augmented keypoint matrix:
00417     cv::Mat aug_mat(cv::Size(3, pts_in.rows), CV_32F);
00418 
00419     for (int32_t i = 0; i < pts_in.rows; ++i)
00420     {
00421       aug_mat.at<float>(i, 0) = pts_in.at<cv::Vec2f>(0, i)[0];
00422       aug_mat.at<float>(i, 1) = pts_in.at<cv::Vec2f>(0, i)[1];
00423       aug_mat.at<float>(i, 2) = 1.0;
00424     }
00425 
00426     cv::Mat T_temp = T.t();
00427     cv::Mat pts_out_temp = aug_mat * T_temp;
00428 
00429     pts_out.release();
00430     pts_out.create(cv::Size(1, pts_in.rows), CV_32FC2);
00431     // Convert points back to proper form:
00432     for (int32_t i = 0; i < pts_in.rows; ++i)
00433     {
00434       pts_out.at<cv::Vec2f>(0, i)[0] = pts_out_temp.at<float>(i, 0);
00435       pts_out.at<cv::Vec2f>(0, i)[1] = pts_out_temp.at<float>(i, 1);
00436     }
00437   }
00438 
00446 
00448   //
00449   // PitchAndRollEstimatorQueue()
00450   //
00452   PitchAndRollEstimatorQueue::PitchAndRollEstimatorQueue()
00453   {
00454     SetBufferSize();
00455     ComputeStats();
00456   }
00457 
00459   //
00460   // SetBufferSize()
00461   //
00463   void PitchAndRollEstimatorQueue::SetBufferSize(int32_t buff_size)
00464   {
00465     pitches_.set_capacity(buff_size);
00466     rolls_.set_capacity(buff_size);
00467   }
00468 
00469 
00471   //
00472   // WarpPoints()
00473   //
00475   void PitchAndRollEstimatorQueue::WarpPoints(const cv::Mat& points_in,
00476                                               cv::Mat& points_out,
00477                                               const cv::Size& image_size,
00478                                               bool use_median)
00479   {
00480     double pitch;
00481     double roll;
00482     if (use_median)
00483     {
00484       GetMedianPitchAndRoll(pitch, roll);
00485     }
00486     else
00487     {
00488       GetMeanPitchAndRoll(pitch, roll);
00489     }
00490 
00491     swri_image_util::WarpPoints(pitch, roll, image_size, points_in, points_out);
00492   }
00493 
00495   //
00496   // Clear()
00497   //
00499   void PitchAndRollEstimatorQueue::Clear()
00500   {
00501     pitches_.clear();
00502     rolls_.clear();
00503     ComputeStats();
00504   }
00505 
00506 
00508   //
00509   // GenerateNewEstimate()
00510   //
00512   void PitchAndRollEstimatorQueue::GenerateNewEstimate(
00513       const cv::Mat& points1,
00514       const cv::Mat& points2,
00515       const cv::Size& image_size)
00516   {
00517     double pitch = 0.0;
00518     double roll = 0.0;
00519     cv::Mat T = PitchAndRollEstimator::EstimateNominalAngle(points1,
00520                                                             points2,
00521                                                             image_size,
00522                                                             pitch,
00523                                                             roll);
00524 
00525     if (!T.empty())
00526     {
00527       LoadNewData(pitch, roll);
00528       ComputeStats();
00529     }
00530   }
00531 
00533   //
00534   // GetMeanPitchAndRoll()
00535   //
00537   bool PitchAndRollEstimatorQueue::GetMeanPitchAndRoll(double& pitch,
00538                                                        double& roll)
00539   {
00540     pitch = mean_pitch_;
00541     roll = mean_roll_;
00542 
00543     return pitches_.size() > 0;
00544   }
00545 
00547   //
00548   // GetMedianPitchAndRoll()
00549   //
00551   bool PitchAndRollEstimatorQueue::GetMedianPitchAndRoll(double& pitch,
00552                                                          double& roll)
00553   {
00554     pitch = median_pitch_;
00555     roll = median_roll_;
00556 
00557     return pitches_.size() > 0;
00558   }
00559 
00560 
00562   //
00563   // LoadNewData()
00564   //
00566   void PitchAndRollEstimatorQueue::LoadNewData(double new_pitch,
00567                                                double new_roll)
00568   {
00569     pitches_.push_back(new_pitch);
00570     rolls_.push_back(new_roll);
00571     ComputeStats();
00572   }
00573 
00575   //
00576   // ComputeStats()
00577   //
00579   void PitchAndRollEstimatorQueue::ComputeStats()
00580   {
00581     mean_pitch_ = 0.0;
00582     mean_roll_ = 0.0;
00583     median_pitch_ = 0.0;
00584     median_roll_ = 0.0;
00585 
00586     if (pitches_.empty())
00587     {
00588       return;
00589     }
00590 
00591     std::vector<double> temp_pitch;
00592     std::vector<double> temp_roll;
00593     temp_pitch.assign(pitches_.begin(), pitches_.end());
00594     temp_roll.assign(rolls_.begin(), rolls_.end());
00595 
00596     std::sort(temp_pitch.begin(), temp_pitch.end());
00597     std::sort(temp_roll.begin(), temp_roll.end());
00598 
00599     double pitch_sum = 0.0;
00600     double roll_sum = 0.0;
00601 
00602     for (int32_t i = 0; i < (int32_t)temp_pitch.size(); ++i)
00603     {
00604       pitch_sum += temp_pitch[i];
00605       roll_sum += temp_roll[i];
00606     }
00607 
00608     double N = static_cast<double>(temp_pitch.size());
00609     mean_pitch_ = pitch_sum / N;
00610     mean_roll_ = roll_sum / N;
00611 
00612     int32_t mid_idx = static_cast<int32_t>(temp_pitch.size() - 1) / 2;
00613     if (temp_pitch.size() % 2 == 0)
00614     {
00615       median_pitch_ = (temp_pitch[mid_idx] + temp_pitch[mid_idx + 1]) / 2.0;
00616       median_roll_ = (temp_roll[mid_idx] + temp_roll[mid_idx + 1]) / 2.0;
00617     }
00618     else
00619     {
00620       median_pitch_ = temp_pitch[mid_idx];
00621       median_roll_ = temp_roll[mid_idx];
00622     }
00623   }
00624 }


swri_image_util
Author(s): Kris Kozak
autogenerated on Thu Jun 6 2019 20:34:52