.. _program_listing_file__tmp_ws_src_marti_common_swri_image_util_include_swri_image_util_image_warp_util.h:

Program Listing for File image_warp_util.h
==========================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_marti_common_swri_image_util_include_swri_image_util_image_warp_util.h>` (``/tmp/ws/src/marti_common/swri_image_util/include/swri_image_util/image_warp_util.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // *****************************************************************************
   //
   // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
   // All rights reserved.
   //
   // Redistribution and use in source and binary forms, with or without
   // modification, are permitted provided that the following conditions are met:
   //     * Redistributions of source code must retain the above copyright
   //       notice, this list of conditions and the following disclaimer.
   //     * Redistributions in binary form must reproduce the above copyright
   //       notice, this list of conditions and the following disclaimer in the
   //       documentation and/or other materials provided with the distribution.
   //     * Neither the name of Southwest Research Institute® (SwRI®) nor the
   //       names of its contributors may be used to endorse or promote products
   //       derived from this software without specific prior written permission.
   //
   // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
   // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
   // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
   // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
   // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
   // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
   // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
   // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
   // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
   // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
   //
   // *****************************************************************************
   
   #ifndef IMAGE_UTIL_IMAGE_WARP_UTIL_H_
   #define IMAGE_UTIL_IMAGE_WARP_UTIL_H_
   
   #include <vector>
   
   // Boost Libraries
   #include <boost/circular_buffer.hpp>
   
   // OpenCV Libraries
   #include <opencv2/core/core.hpp>
   #include <opencv2/highgui/highgui.hpp>
   #include <opencv2/features2d/features2d.hpp>
   #include <opencv2/calib3d/calib3d.hpp>
   #include <opencv2/stitching/detail/warpers.hpp>
   
   // RANGER Libraries
   #include <swri_image_util/image_matching.h>
   
   #include <rclcpp/logger.hpp>
   
   namespace swri_image_util
   {
     cv::Mat WarpImage(const cv::Mat& image, double roll, double pitch);
   
     void WarpPoints(
         double pitch,
         double roll,
         const cv::Size& image_size,
         const cv::Mat& pts_in,
         cv::Mat& pts_out);
   
     void WarpPoints(
         double pitch,
         double roll,
         const cv::Size& image_size,
         const std::vector<cv::KeyPoint>& pts_in,
         std::vector<cv::KeyPoint>& pts_out);
   
     cv::Mat GetR(double pitch, double roll, double yaw = 0.0);
   
   
     class PitchAndRollEstimator
     {
     public:
       PitchAndRollEstimator() {}
   
   
       cv::Mat EstimateNominalAngle(double& nominal_pitch,
                                    double& nominal_roll,
                                    bool show_image_diff,
                                    rclcpp::Logger logger=rclcpp::get_logger("swri_image_util"));
   
   
       static cv::Mat EstimateNominalAngle(const cv::Mat& points1,
                                           const cv::Mat& points2,
                                           const cv::Size& image_size,
                                           double& nominal_pitch,
                                           double& nominal_roll);
   
     private:
       cv::Mat im1_;
       cv::Mat im2_;
   
       cv::Mat K_;
       cv::Mat T_;
   
       std::vector<cv::KeyPoint> kp1_;
       std::vector<cv::KeyPoint> kp2_;
       cv::Mat descriptors1_;
       cv::Mat descriptors2_;
   
       cv::Mat kp1_matched_;
       cv::Mat kp2_matched_;
   
       cv::detail::PlaneWarper warper_;
   
       bool ComputeGeometricMatches(rclcpp::Logger logger=rclcpp::get_logger("swri_image_util"));
   
   
       static bool EstimateTransforms(cv::Mat& pts1,
                                      cv::Mat& pts2,
                                      cv::Mat& T_affine,
                                      cv::Mat& T_rigid,
                                      double& rms_error);
   
       void WarpPoints(double pitch,
                       double roll,
                       const cv::Mat& pts_in,
                       cv::Mat& pts_out,
                       rclcpp::Logger logger=rclcpp::get_logger("swri_image_util"));
   
       void WarpAffinePoints(const cv::Mat& T,
                             const cv::Mat& pts_in,
                             cv::Mat& pts_out);
     };
   
     class PitchAndRollEstimatorQueue
     {
     public:
       PitchAndRollEstimatorQueue();
   
       ~PitchAndRollEstimatorQueue() {}
   
       void SetBufferSize(int32_t buff_size = 50);
   
       void Clear();
   
       void WarpPoints(const cv::Mat& points_in,
                       cv::Mat& points_out,
                       const cv::Size& image_size,
                       bool use_median = true);
   
       void GenerateNewEstimate(const cv::Mat& points1,
                                const cv::Mat& points2,
                                const cv::Size& image_size);
   
       void LoadNewData(double new_pitch,
                        double new_roll);
   
       bool GetMeanPitchAndRoll(double& pitch,
                                double& roll);
   
   
       bool GetMedianPitchAndRoll(double& pitch,
                                  double& roll);
   
     private:
       boost::circular_buffer<double> pitches_;
       boost::circular_buffer<double> rolls_;
   
       double mean_pitch_;
       double mean_roll_;
       double median_pitch_;
       double median_roll_;
   
       void ComputeStats();
     };
   }
   
   #endif  // IMAGE_UTIL_IMAGE_WARP_UTIL_H_