37 cv::Mat
WarpImage(
const cv::Mat& image,
double roll,
double pitch)
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;
46 cv::Mat T = cv::Mat::zeros(cv::Size(3, 1), CV_32F);
48 cv::Mat R =
GetR(pitch, roll);
50 cv::detail::PlaneWarper warper;
51 warper.warp(image, K, R, T, cv::INTER_LANCZOS4, 0, warped);
63 const cv::Size& image_size,
64 const cv::Mat& pts_in,
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;
72 cv::detail::PlaneWarper warper;
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)
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);
91 const cv::Size& image_size,
92 const std::vector<cv::KeyPoint>& pts_in,
93 std::vector<cv::KeyPoint>& pts_out)
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;
102 cv::detail::PlaneWarper warper;
104 cv::Mat T = cv::Mat::zeros(cv::Size(3, 1), CV_32F);
105 cv::Mat R =
GetR(pitch, roll);
107 for (uint32_t i = 0; i < pts_in.size(); i++)
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);
116 cv::Mat
GetR(
double pitch,
double roll,
double yaw)
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);
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);
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);
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);
140 cv::Mat R = R3 * R2 * R1;
146 double& nominal_pitch,
147 double& nominal_roll,
148 bool show_image_diff)
164 ROS_ERROR(
"Estimate Nominal Angle time = %g", (T2 - T1).toSec());
165 cv::Mat R =
GetR(nominal_pitch, nominal_roll);
177 cv::warpAffine(warped_im1,
180 cv::Size(warped_im1.cols, warped_im1.rows));
182 cv::Mat sub = warped_im2 - temp_im;
184 cv::namedWindow(
"Warped Subtraction");
185 cv::imshow(
"Warped Subtraction", sub);
194 cv::Mat sub2 =
im2_ - temp_im;
195 cv::namedWindow(
"Subtraction");
196 cv::imshow(
"Subtraction", sub2);
198 cv::namedWindow(
"im2_");
199 cv::imshow(
"im2_",
im2_);
207 const cv::Mat& points1,
208 const cv::Mat& points2,
209 const cv::Size& image_size,
210 double& nominal_pitch,
211 double& nominal_roll)
214 const int32_t max_iterations = 5;
215 const int32_t num_octaves = 5;
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);
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);
225 cv::Mat T_rigid_final = cv::Mat();
226 for (int32_t octave_idx = 0; octave_idx < num_octaves; ++octave_idx)
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);
233 double min_diff = 1e20;
238 for (int32_t pitch_idx = 0; pitch_idx < max_iterations; ++pitch_idx)
240 double cur_pitch = min_pitch + dp * pitch_idx;
241 for (int32_t roll_idx = 0; roll_idx < max_iterations; ++roll_idx)
243 double cur_roll = min_roll + dr * roll_idx;
272 double cur_diff = rms_error;
274 if (cur_diff < min_diff)
278 nominal_pitch = cur_pitch;
279 nominal_roll = cur_roll;
280 T_rigid_final = T_rigid;
285 min_pitch = nominal_pitch - std::abs(dp * 2 / 3);
286 max_pitch = nominal_pitch + std::abs(dp * 2 / 3);
289 min_roll = nominal_roll - std::abs(dr * 2 / 3);
290 max_roll = nominal_roll + std::abs(dr * 2 / 3);
293 cv::Mat R =
GetR(nominal_pitch,
296 return T_rigid_final;
309 std::vector<cv::DMatch> matches;
310 cv::BFMatcher matcher;
320 cv::Mat fundamental_matrix;
321 cv::Mat fund_inliers1;
322 cv::Mat fund_inliers2;
331 catch (
const std::exception& e)
333 ROS_ERROR(
"Caught an exception when computing fundamental inliers:" 338 ROS_INFO(
"Found %d fundamental inliers.", fund_inliers1.rows);
342 std::vector<uint32_t> good_points;
346 fund_inliers1, fund_inliers2, inliers1, inliers2, good_points, iterations, 30.0);
350 ROS_ERROR(
"Failed to compute 2D affine transform.");
369 std::vector<uint32_t> good_points;
373 pts1, pts2, inliers1, inliers2, good_points, iterations, 30.0);
376 pts1, pts2, inliers1, inliers2, good_points, iterations, 30.0);
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);
394 const cv::Mat& pts_in,
399 ROS_ERROR(
"Object not initialized. Pitch and roll not computed. Perhaps" 400 "call static implementation instead");
413 const cv::Mat& pts_in,
417 cv::Mat aug_mat(cv::Size(3, pts_in.rows), CV_32F);
419 for (int32_t i = 0; i < pts_in.rows; ++i)
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;
426 cv::Mat T_temp = T.t();
427 cv::Mat pts_out_temp = aug_mat * T_temp;
430 pts_out.create(cv::Size(1, pts_in.rows), CV_32FC2);
432 for (int32_t i = 0; i < pts_in.rows; ++i)
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);
465 pitches_.set_capacity(buff_size);
466 rolls_.set_capacity(buff_size);
477 const cv::Size& image_size,
484 GetMedianPitchAndRoll(pitch, roll);
488 GetMeanPitchAndRoll(pitch, roll);
513 const cv::Mat& points1,
514 const cv::Mat& points2,
515 const cv::Size& image_size)
527 LoadNewData(pitch, roll);
543 return pitches_.size() > 0;
554 pitch = median_pitch_;
557 return pitches_.size() > 0;
569 pitches_.push_back(new_pitch);
570 rolls_.push_back(new_roll);
586 if (pitches_.empty())
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());
596 std::sort(temp_pitch.begin(), temp_pitch.end());
597 std::sort(temp_roll.begin(), temp_roll.end());
599 double pitch_sum = 0.0;
600 double roll_sum = 0.0;
602 for (int32_t i = 0; i < (int32_t)temp_pitch.size(); ++i)
604 pitch_sum += temp_pitch[i];
605 roll_sum += temp_roll[i];
608 double N =
static_cast<double>(temp_pitch.size());
609 mean_pitch_ = pitch_sum / N;
610 mean_roll_ = roll_sum / N;
612 int32_t mid_idx =
static_cast<int32_t
>(temp_pitch.size() - 1) / 2;
613 if (temp_pitch.size() % 2 == 0)
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;
620 median_pitch_ = temp_pitch[mid_idx];
621 median_roll_ = temp_roll[mid_idx];
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 Clear()
Clears the buffer.
cv::detail::PlaneWarper warper_
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)
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())
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.
PitchAndRollEstimatorQueue()
Constructor.