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;