Added selction of BA cost function in stitching samples (and added other BA cost…

Added selction of BA cost function in stitching samples (and added other BA cost func into stitching module)

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<CameraParams> &cameras);
+    void obtainRefinedCameraParams(std::vector<CameraParams> &cameras) const;
+    void calcError(Mat &err);
+    void calcJacobian(Mat &jac);
+
+    Mat err1_, err2_;
+};
+
+
 void CV_EXPORTS waveCorrect(std::vector<Mat> &rmats);
 
 

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<detail::FeaturesMatcher> features_matcher)
         { features_matcher_ = features_matcher; }
 
+    Ptr<detail::BundleAdjusterBase> bundleAdjuster() { return bundle_adjuster_; }
+    const Ptr<detail::BundleAdjusterBase> bundleAdjuster() const { return bundle_adjuster_; }
+    void setBundleAdjuster(Ptr<detail::BundleAdjusterBase> bundle_adjuster)
+        { bundle_adjuster_ = bundle_adjuster; }
+
     Ptr<WarperCreator> warper() { return warper_; }
     const Ptr<WarperCreator> warper() const { return warper_; }
     void setWarper(Ptr<WarperCreator> warper) { warper_ = warper; }
@@ -117,6 +123,7 @@ private:
     bool horiz_stright_;
     Ptr<detail::FeaturesFinder> features_finder_;
     Ptr<detail::FeaturesMatcher> features_matcher_;
+    Ptr<detail::BundleAdjusterBase> bundle_adjuster_;
     Ptr<WarperCreator> warper_;
     Ptr<detail::ExposureCompensator> exposure_comp_;
     Ptr<detail::SeamFinder> seam_finder_;

modules/stitching/src/motion_estimators.cpp

@@ -277,7 +277,6 @@ void BundleAdjusterReproj::obtainRefinedCameraParams(vector<CameraParams> &camer
         cameras[i].focal = cam_params_.at<double>(i * 6, 0);
         cameras[i].ppx = cam_params_.at<double>(i * 6 + 1, 0);
         cameras[i].ppy = cam_params_.at<double>(i * 6 + 2, 0);
-        cameras[i].aspect = 1.;
 
         Mat rvec(3, 1, CV_64F);
         rvec.at<double>(0, 0) = cam_params_.at<double>(i * 6 + 3, 0);
@@ -380,6 +379,147 @@ void BundleAdjusterReproj::calcJacobian(Mat &jac)
 }
 
 
+//////////////////////////////////////////////////////////////////////////////
+
+void BundleAdjusterRay::setUpInitialCameraParams(const vector<CameraParams> &cameras)
+{
+    cam_params_.create(num_images_ * 4, 1, CV_64F);
+    SVD svd;
+    for (int i = 0; i < num_images_; ++i)
+    {
+        cam_params_.at<double>(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<double>(i * 4 + 1, 0) = rvec.at<float>(0, 0);
+        cam_params_.at<double>(i * 4 + 2, 0) = rvec.at<float>(1, 0);
+        cam_params_.at<double>(i * 4 + 3, 0) = rvec.at<float>(2, 0);
+    }
+}
+
+
+void BundleAdjusterRay::obtainRefinedCameraParams(vector<CameraParams> &cameras) const
+{
+    for (int i = 0; i < num_images_; ++i)
+    {
+        cameras[i].focal = cam_params_.at<double>(i * 4, 0);
+
+        Mat rvec(3, 1, CV_64F);
+        rvec.at<double>(0, 0) = cam_params_.at<double>(i * 4 + 1, 0);
+        rvec.at<double>(1, 0) = cam_params_.at<double>(i * 4 + 2, 0);
+        rvec.at<double>(2, 0) = cam_params_.at<double>(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<double>(i * 4, 0);
+        double f2 = cam_params_.at<double>(j * 4, 0);
+
+        double R1[9];
+        Mat R1_(3, 3, CV_64F, R1);
+        Mat rvec(3, 1, CV_64F);
+        rvec.at<double>(0, 0) = cam_params_.at<double>(i * 4 + 1, 0);
+        rvec.at<double>(1, 0) = cam_params_.at<double>(i * 4 + 2, 0);
+        rvec.at<double>(2, 0) = cam_params_.at<double>(i * 4 + 3, 0);
+        Rodrigues(rvec, R1_);
+
+        double R2[9];
+        Mat R2_(3, 3, CV_64F, R2);
+        rvec.at<double>(0, 0) = cam_params_.at<double>(j * 4 + 1, 0);
+        rvec.at<double>(1, 0) = cam_params_.at<double>(j * 4 + 2, 0);
+        rvec.at<double>(2, 0) = cam_params_.at<double>(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_<double> 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_<double> 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_<double> H1 = R1_ * K1.inv();
+        Mat_<double> 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<double>(3 * match_idx, 0) = mult * (x1 - x2);
+            err.at<double>(3 * match_idx + 1, 0) = mult * (y1 - y2);
+            err.at<double>(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<double>(i * 4 + j, 0);
+            cam_params_.at<double>(i * 4 + j, 0) = val - step;
+            calcError(err1_);
+            cam_params_.at<double>(i * 4 + j, 0) = val + step;
+            calcError(err2_);
+            calcDeriv(err1_, err2_, 2 * step, jac.col(i * 4 + j));
+            cam_params_.at<double>(i * 4 + j, 0) = val;
+        }
+    }
+}
+
+
 //////////////////////////////////////////////////////////////////////////////
 
 // TODO replace SVD with eigen

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<double> focals;

samples/cpp/stitching_detailed.cpp

@@ -79,6 +79,8 @@ void printUsage()
         "  --conf_thresh <float>\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 <file_name>\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<float>(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<detail::BundleAdjusterBase> 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<double> focals;