image_warp_util.h
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 #ifndef IMAGE_UTIL_IMAGE_WARP_UTIL_H_
00031 #define IMAGE_UTIL_IMAGE_WARP_UTIL_H_
00032 
00033 #include <vector>
00034 
00035 // Boost Libraries
00036 #include <boost/circular_buffer.hpp>
00037 
00038 // ROS Libraries
00039 #include <ros/ros.h>
00040 
00041 // OpenCV Libraries
00042 #include <opencv2/core/core.hpp>
00043 #include <opencv2/highgui/highgui.hpp>
00044 #include <opencv2/features2d/features2d.hpp>
00045 #include <opencv2/calib3d/calib3d.hpp>
00046 #include <opencv2/stitching/detail/warpers.hpp>
00047 
00048 // RANGER Libraries
00049 #include <swri_image_util/image_matching.h>
00050 
00051 namespace swri_image_util
00052 {
00053   cv::Mat WarpImage(const cv::Mat& image, double roll, double pitch);
00054 
00064   void WarpPoints(
00065       double pitch,
00066       double roll,
00067       const cv::Size& image_size,
00068       const cv::Mat& pts_in,
00069       cv::Mat& pts_out);
00070 
00080   void WarpPoints(
00081       double pitch,
00082       double roll,
00083       const cv::Size& image_size,
00084       const std::vector<cv::KeyPoint>& pts_in,
00085       std::vector<cv::KeyPoint>& pts_out);
00086 
00097   cv::Mat GetR(double pitch, double roll, double yaw = 0.0);
00098 
00099 
00105   class PitchAndRollEstimator
00106   {
00107   public:
00111     PitchAndRollEstimator() {}
00112 
00113 
00120     cv::Mat EstimateNominalAngle(double& nominal_pitch,
00121                                  double& nominal_roll,
00122                                  bool show_image_diff = false);
00123 
00124 
00131     static cv::Mat EstimateNominalAngle(const cv::Mat& points1,
00132                                         const cv::Mat& points2,
00133                                         const cv::Size& image_size,
00134                                         double& nominal_pitch,
00135                                         double& nominal_roll);
00136 
00137   private:
00138     cv::Mat im1_;
00139     cv::Mat im2_;
00140 
00141     cv::Mat K_;
00142     cv::Mat T_;
00143 
00144     std::vector<cv::KeyPoint> kp1_;
00145     std::vector<cv::KeyPoint> kp2_;
00146     cv::Mat descriptors1_;
00147     cv::Mat descriptors2_;
00148 
00149     cv::Mat kp1_matched_;
00150     cv::Mat kp2_matched_;
00151 
00152     cv::detail::PlaneWarper warper_;
00153 
00160     bool ComputeGeometricMatches();
00161 
00162 
00176     static bool EstimateTransforms(cv::Mat& pts1,
00177                                    cv::Mat& pts2,
00178                                    cv::Mat& T_affine,
00179                                    cv::Mat& T_rigid,
00180                                    double& rms_error);
00181 
00190     void WarpPoints(double pitch,
00191                     double roll,
00192                     const cv::Mat& pts_in,
00193                     cv::Mat& pts_out);
00194 
00202     void WarpAffinePoints(const cv::Mat& T,
00203                           const cv::Mat& pts_in,
00204                           cv::Mat& pts_out);
00205   };
00206 
00212   class PitchAndRollEstimatorQueue
00213   {
00214   public:
00218     PitchAndRollEstimatorQueue();
00219 
00220     ~PitchAndRollEstimatorQueue() {}
00221 
00227     void SetBufferSize(int32_t buff_size = 50);
00228 
00232     void Clear();
00233 
00243     void WarpPoints(const cv::Mat& points_in,
00244                     cv::Mat& points_out,
00245                     const cv::Size& image_size,
00246                     bool use_median = true);
00247 
00256     void GenerateNewEstimate(const cv::Mat& points1,
00257                              const cv::Mat& points2,
00258                              const cv::Size& image_size);
00259 
00266     void LoadNewData(double new_pitch,
00267                      double new_roll);
00268 
00279     bool GetMeanPitchAndRoll(double& pitch,
00280                              double& roll);
00281 
00282 
00293     bool GetMedianPitchAndRoll(double& pitch,
00294                                double& roll);
00295 
00296   private:
00297     boost::circular_buffer<double> pitches_;
00298     boost::circular_buffer<double> rolls_;
00299 
00300     double mean_pitch_;
00301     double mean_roll_;
00302     double median_pitch_;
00303     double median_roll_;
00304 
00308     void ComputeStats();
00309   };
00310 }
00311 
00312 #endif  // IMAGE_UTIL_IMAGE_WARP_UTIL_H_


swri_image_util
Author(s): Kris Kozak
autogenerated on Tue Oct 3 2017 03:19:34