From 4ee462c961fc991caf6fefd22fa72b7fcc5e23a7 Mon Sep 17 00:00:00 2001 From: Alexey Spizhevoy Date: Wed, 21 Sep 2011 13:22:12 +0000 Subject: [PATCH] Added selction of BA cost function in stitching samples (and added other BA cost func into stitching module) --- .../stitching/detail/motion_estimators.hpp | 16 ++ .../include/opencv2/stitching/stitcher.hpp | 7 + modules/stitching/src/motion_estimators.cpp | 142 +++++++++++++++++- modules/stitching/src/stitcher.cpp | 6 +- samples/cpp/stitching_detailed.cpp | 21 ++- 5 files changed, 185 insertions(+), 7 deletions(-) diff --git a/modules/stitching/include/opencv2/stitching/detail/motion_estimators.hpp b/modules/stitching/include/opencv2/stitching/detail/motion_estimators.hpp index 9bc9becb26..6a913d591c 100644 --- a/modules/stitching/include/opencv2/stitching/detail/motion_estimators.hpp +++ b/modules/stitching/include/opencv2/stitching/detail/motion_estimators.hpp @@ -138,6 +138,22 @@ private: }; +// Minimizes sun of ray-to-ray distances +class CV_EXPORTS BundleAdjusterRay : public BundleAdjusterBase +{ +public: + BundleAdjusterRay() : BundleAdjusterBase(4, 3) {} + +private: + void setUpInitialCameraParams(const std::vector &cameras); + void obtainRefinedCameraParams(std::vector &cameras) const; + void calcError(Mat &err); + void calcJacobian(Mat &jac); + + Mat err1_, err2_; +}; + + void CV_EXPORTS waveCorrect(std::vector &rmats); diff --git a/modules/stitching/include/opencv2/stitching/stitcher.hpp b/modules/stitching/include/opencv2/stitching/stitcher.hpp index c146b69213..072e96f864 100644 --- a/modules/stitching/include/opencv2/stitching/stitcher.hpp +++ b/modules/stitching/include/opencv2/stitching/stitcher.hpp @@ -47,6 +47,7 @@ #include "opencv2/features2d/features2d.hpp" #include "warpers.hpp" #include "detail/matchers.hpp" +#include "detail/motion_estimators.hpp" #include "detail/exposure_compensate.hpp" #include "detail/seam_finders.hpp" #include "detail/blenders.hpp" @@ -90,6 +91,11 @@ public: void setFeaturesMatcher(Ptr features_matcher) { features_matcher_ = features_matcher; } + Ptr bundleAdjuster() { return bundle_adjuster_; } + const Ptr bundleAdjuster() const { return bundle_adjuster_; } + void setBundleAdjuster(Ptr bundle_adjuster) + { bundle_adjuster_ = bundle_adjuster; } + Ptr warper() { return warper_; } const Ptr warper() const { return warper_; } void setWarper(Ptr warper) { warper_ = warper; } @@ -117,6 +123,7 @@ private: bool horiz_stright_; Ptr features_finder_; Ptr features_matcher_; + Ptr bundle_adjuster_; Ptr warper_; Ptr exposure_comp_; Ptr seam_finder_; diff --git a/modules/stitching/src/motion_estimators.cpp b/modules/stitching/src/motion_estimators.cpp index 10cc93c49f..c948209243 100644 --- a/modules/stitching/src/motion_estimators.cpp +++ b/modules/stitching/src/motion_estimators.cpp @@ -277,7 +277,6 @@ void BundleAdjusterReproj::obtainRefinedCameraParams(vector &camer cameras[i].focal = cam_params_.at(i * 6, 0); cameras[i].ppx = cam_params_.at(i * 6 + 1, 0); cameras[i].ppy = cam_params_.at(i * 6 + 2, 0); - cameras[i].aspect = 1.; Mat rvec(3, 1, CV_64F); rvec.at(0, 0) = cam_params_.at(i * 6 + 3, 0); @@ -380,6 +379,147 @@ void BundleAdjusterReproj::calcJacobian(Mat &jac) } +////////////////////////////////////////////////////////////////////////////// + +void BundleAdjusterRay::setUpInitialCameraParams(const vector &cameras) +{ + cam_params_.create(num_images_ * 4, 1, CV_64F); + SVD svd; + for (int i = 0; i < num_images_; ++i) + { + cam_params_.at(i * 4, 0) = cameras[i].focal; + + svd(cameras[i].R, SVD::FULL_UV); + Mat R = svd.u * svd.vt; + if (determinant(R) < 0) + R *= -1; + + Mat rvec; + Rodrigues(R, rvec); + CV_Assert(rvec.type() == CV_32F); + cam_params_.at(i * 4 + 1, 0) = rvec.at(0, 0); + cam_params_.at(i * 4 + 2, 0) = rvec.at(1, 0); + cam_params_.at(i * 4 + 3, 0) = rvec.at(2, 0); + } +} + + +void BundleAdjusterRay::obtainRefinedCameraParams(vector &cameras) const +{ + for (int i = 0; i < num_images_; ++i) + { + cameras[i].focal = cam_params_.at(i * 4, 0); + + Mat rvec(3, 1, CV_64F); + rvec.at(0, 0) = cam_params_.at(i * 4 + 1, 0); + rvec.at(1, 0) = cam_params_.at(i * 4 + 2, 0); + rvec.at(2, 0) = cam_params_.at(i * 4 + 3, 0); + Rodrigues(rvec, cameras[i].R); + + Mat tmp; + cameras[i].R.convertTo(tmp, CV_32F); + cameras[i].R = tmp; + } +} + + +void BundleAdjusterRay::calcError(Mat &err) +{ + err.create(total_num_matches_ * 3, 1, CV_64F); + + int match_idx = 0; + for (size_t edge_idx = 0; edge_idx < edges_.size(); ++edge_idx) + { + int i = edges_[edge_idx].first; + int j = edges_[edge_idx].second; + double f1 = cam_params_.at(i * 4, 0); + double f2 = cam_params_.at(j * 4, 0); + + double R1[9]; + Mat R1_(3, 3, CV_64F, R1); + Mat rvec(3, 1, CV_64F); + rvec.at(0, 0) = cam_params_.at(i * 4 + 1, 0); + rvec.at(1, 0) = cam_params_.at(i * 4 + 2, 0); + rvec.at(2, 0) = cam_params_.at(i * 4 + 3, 0); + Rodrigues(rvec, R1_); + + double R2[9]; + Mat R2_(3, 3, CV_64F, R2); + rvec.at(0, 0) = cam_params_.at(j * 4 + 1, 0); + rvec.at(1, 0) = cam_params_.at(j * 4 + 2, 0); + rvec.at(2, 0) = cam_params_.at(j * 4 + 3, 0); + Rodrigues(rvec, R2_); + + const ImageFeatures& features1 = features_[i]; + const ImageFeatures& features2 = features_[j]; + const MatchesInfo& matches_info = pairwise_matches_[i * num_images_ + j]; + + Mat_ K1 = Mat::eye(3, 3, CV_64F); + K1(0,0) = f1; K1(0,2) = features1.img_size.width * 0.5; + K1(1,1) = f1; K1(1,2) = features1.img_size.height * 0.5; + + Mat_ K2 = Mat::eye(3, 3, CV_64F); + K2(0,0) = f2; K2(0,2) = features2.img_size.width * 0.5; + K2(1,1) = f2; K2(1,2) = features2.img_size.height * 0.5; + + Mat_ H1 = R1_ * K1.inv(); + Mat_ H2 = R2_ * K2.inv(); + + for (size_t k = 0; k < matches_info.matches.size(); ++k) + { + if (!matches_info.inliers_mask[k]) + continue; + + const DMatch& m = matches_info.matches[k]; + + Point2f p1 = features1.keypoints[m.queryIdx].pt; + double x1 = H1(0,0)*p1.x + H1(0,1)*p1.y + H1(0,2); + double y1 = H1(1,0)*p1.x + H1(1,1)*p1.y + H1(1,2); + double z1 = H1(2,0)*p1.x + H1(2,1)*p1.y + H1(2,2); + double len = sqrt(x1*x1 + y1*y1 + z1*z1); + x1 /= len; y1 /= len; z1 /= len; + + Point2f p2 = features2.keypoints[m.trainIdx].pt; + double x2 = H2(0,0)*p2.x + H2(0,1)*p2.y + H2(0,2); + double y2 = H2(1,0)*p2.x + H2(1,1)*p2.y + H2(1,2); + double z2 = H2(2,0)*p2.x + H2(2,1)*p2.y + H2(2,2); + len = sqrt(x2*x2 + y2*y2 + z2*z2); + x2 /= len; y2 /= len; z2 /= len; + + double mult = sqrt(f1 * f2); + err.at(3 * match_idx, 0) = mult * (x1 - x2); + err.at(3 * match_idx + 1, 0) = mult * (y1 - y2); + err.at(3 * match_idx + 2, 0) = mult * (z1 - z2); + + match_idx++; + } + } +} + + +void BundleAdjusterRay::calcJacobian(Mat &jac) +{ + jac.create(total_num_matches_ * 3, num_images_ * 4, CV_64F); + + double val; + const double step = 1e-3; + + for (int i = 0; i < num_images_; ++i) + { + for (int j = 0; j < 4; ++j) + { + val = cam_params_.at(i * 4 + j, 0); + cam_params_.at(i * 4 + j, 0) = val - step; + calcError(err1_); + cam_params_.at(i * 4 + j, 0) = val + step; + calcError(err2_); + calcDeriv(err1_, err2_, 2 * step, jac.col(i * 4 + j)); + cam_params_.at(i * 4 + j, 0) = val; + } + } +} + + ////////////////////////////////////////////////////////////////////////////// // TODO replace SVD with eigen diff --git a/modules/stitching/src/stitcher.cpp b/modules/stitching/src/stitcher.cpp index 4cbcc7c07d..38a26c5bed 100644 --- a/modules/stitching/src/stitcher.cpp +++ b/modules/stitching/src/stitcher.cpp @@ -55,6 +55,7 @@ Stitcher Stitcher::createDefault(bool try_use_gpu) stitcher.setPanoConfidenceThresh(1); stitcher.setHorizontalStrightening(true); stitcher.setFeaturesMatcher(new detail::BestOf2NearestMatcher(try_use_gpu)); + stitcher.setBundleAdjuster(new detail::BundleAdjusterRay()); #ifndef ANDROID if (try_use_gpu && gpu::getCudaEnabledDeviceCount() > 0) @@ -189,9 +190,8 @@ Stitcher::Status Stitcher::stitch(InputArray imgs_, OutputArray pano_) LOGLN("Initial intrinsic parameters #" << indices[i]+1 << ":\n " << cameras[i].K()); } - detail::BundleAdjusterReproj adjuster; - adjuster.setConfThresh(conf_thresh_); - adjuster(features, pairwise_matches, cameras); + bundle_adjuster_->setConfThresh(conf_thresh_); + (*bundle_adjuster_)(features, pairwise_matches, cameras); // Find median focal length vector focals; diff --git a/samples/cpp/stitching_detailed.cpp b/samples/cpp/stitching_detailed.cpp index 150af5ad44..8d6c05593d 100644 --- a/samples/cpp/stitching_detailed.cpp +++ b/samples/cpp/stitching_detailed.cpp @@ -79,6 +79,8 @@ void printUsage() " --conf_thresh \n" " Threshold for two images are from the same panorama confidence.\n" " The default is 1.0.\n" + " --ba (reproj|ray)\n" + " Bundle adjustment cost function. The default is ray.\n" " --wave_correct (no|yes)\n" " Perform wave effect correction. The default is 'yes'.\n" " --save_graph \n" @@ -114,6 +116,7 @@ double work_megapix = 0.6; double seam_megapix = 0.1; double compose_megapix = -1; float conf_thresh = 1.f; +string ba_cost_func = "ray"; bool wave_correct = true; bool save_graph = false; std::string save_graph_to; @@ -186,6 +189,11 @@ int parseCmdArgs(int argc, char** argv) conf_thresh = static_cast(atof(argv[i + 1])); i++; } + else if (string(argv[i]) == "--ba") + { + ba_cost_func = argv[i + 1]; + i++; + } else if (string(argv[i]) == "--wave_correct") { if (string(argv[i + 1]) == "no") @@ -413,9 +421,16 @@ int main(int argc, char* argv[]) LOGLN("Initial intrinsics #" << indices[i]+1 << ":\n" << cameras[i].K()); } - BundleAdjusterReproj adjuster; - adjuster.setConfThresh(conf_thresh); - adjuster(features, pairwise_matches, cameras); + Ptr adjuster; + if (ba_cost_func == "reproj") adjuster = new detail::BundleAdjusterReproj(); + else if (ba_cost_func == "ray") adjuster = new detail::BundleAdjusterRay(); + else + { + cout << "Unknown bundle adjustment cost function: '" << ba_cost_func << "'.\n"; + return -1; + } + adjuster->setConfThresh(conf_thresh); + (*adjuster)(features, pairwise_matches, cameras); // Find median focal length vector focals;