Program Listing for File image_warp_util.h
↰ Return to documentation for file (include/swri_image_util/image_warp_util.h
)
// *****************************************************************************
//
// 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_