devbox
从前慢现在也慢
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

高翔《视觉SLAM十四讲》第8讲ch8视觉里程计2实践代码报错解决方法-不用安装opencv4_视觉slam14讲ch8 报错

作者：从前慢现在也慢 | 2024-03-23 03:19:13

赞

踩

视觉slam14讲ch8 报错

 一、终端报错

 在运行14讲第二版的第8讲的程序时，出现了以下错误：

原因是 parallel_for_函数出现错误，这是因为opencv3不支持std::bind函数，作者写书的时候忘提这件事了，后来在勘误里加上了，可以自己安装opencv 4，或者自行改写下parallel_for_函数。(高翔回答https://github.com/gaoxiang12/slambook2/issues/57)

二、解决方法

有两种解决方法：

1.安装opencv4

2.修改optical_flow.cpp与direct_method.cpp程序（本人采用第二种方法）

     继续深入查询发现已经有人分享能使用opencv3运行的direct_method.cpp（https://github.com/gaoxiang12/slambook2/issues/32），但没有找到修改好的optical_flow.cpp。本人对optical_flow.cpp进行修改，下面分享修改好的两个文件：     

     direct_method.cpp（转自https://github.com/rancheng/slambook2/blob/opencv3_2_0/ch8/direct_method.cpp）：

  1. #include <opencv2/opencv.hpp>
  2. #include <sophus/se3.hpp>
  3. #include <boost/format.hpp>
  4. #include <pangolin/pangolin.h>
  5.  
  6. using namespace std;
  7.  
  8. typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
  9.  
  10. // Camera intrinsics
  11. double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
  12. // baseline
  13. double baseline = 0.573;
  14. // paths
  15. string left_file = "./left.png";
  16. string disparity_file = "./disparity.png";
  17. boost::format fmt_others("./%06d.png");    // other files
  18.  
  19. // useful typedefs
  20. typedef Eigen::Matrix<double, 6, 6> Matrix6d;
  21. typedef Eigen::Matrix<double, 2, 6> Matrix26d;
  22. typedef Eigen::Matrix<double, 6, 1> Vector6d;
  23.  
  24. /**
  25.  * pose estimation using direct method
  26.  * @param img1
  27.  * @param img2
  28.  * @param px_ref
  29.  * @param depth_ref
  30.  * @param T21
  31.  */
  32. void DirectPoseEstimationMultiLayer(
  33.         const cv::Mat &img1,
  34.         const cv::Mat &img2,
  35.         const VecVector2d &px_ref,
  36.         const vector<double> depth_ref,
  37.         Sophus::SE3d &T21
  38. );
  39.  
  40. /**
  41.  * pose estimation using direct method
  42.  * @param img1
  43.  * @param img2
  44.  * @param px_ref
  45.  * @param depth_ref
  46.  * @param T21
  47.  */
  48. void DirectPoseEstimationSingleLayer(
  49.         const cv::Mat &img1,
  50.         const cv::Mat &img2,
  51.         const VecVector2d &px_ref,
  52.         const vector<double> depth_ref,
  53.         Sophus::SE3d &T21
  54. );
  55.  
  56. // bilinear interpolation
  57. inline float GetPixelValue(const cv::Mat &img, float x, float y) {
  58.     // boundary check
  59.     if (x < 0) x = 0;
  60.     if (y < 0) y = 0;
  61.     if (x >= img.cols) x = img.cols - 1;
  62.     if (y >= img.rows) y = img.rows - 1;
  63.     uchar *data = &img.data[int(y) * img.step + int(x)];
  64.     float xx = x - floor(x);
  65.     float yy = y - floor(y);
  66.     return float(
  67.             (1 - xx) * (1 - yy) * data[0] +
  68.             xx * (1 - yy) * data[1] +
  69.             (1 - xx) * yy * data[img.step] +
  70.             xx * yy * data[img.step + 1]
  71.     );
  72. }
  73.  
  74. /// class for accumulator jacobians in parallel
  75. class JacobianAccumulator: public cv::ParallelLoopBody {
  76. private:
  77.     const cv::Mat &img1;
  78.     const cv::Mat &img2;
  79.     const VecVector2d &px_ref;
  80.     const vector<double> depth_ref;
  81.     Sophus::SE3d &T21;
  82.     mutable VecVector2d projection; // projected points
  83.  
  84.     mutable std::mutex hessian_mutex;
  85.     mutable Matrix6d H = Matrix6d::Zero();
  86.     mutable Vector6d b = Vector6d::Zero();
  87.     mutable double cost = 0;
  88.  
  89. public:
  90.     JacobianAccumulator(
  91.         const cv::Mat &img1_,
  92.         const cv::Mat &img2_,
  93.         const VecVector2d &px_ref_,
  94.         const vector<double> depth_ref_,
  95.         Sophus::SE3d &T21_) :
  96.         img1(img1_), img2(img2_), px_ref(px_ref_), depth_ref(depth_ref_), T21(T21_) {
  97.         projection = VecVector2d(px_ref.size(), Eigen::Vector2d(0, 0));
  98.     }
  99.  
  100.     /// accumulate jacobians in a range
  101. //    void accumulate_jacobian(const cv::Range &range);
  102.  
  103.  
  104.     /// get hessian matrix
  105.     Matrix6d hessian() const { return H; }
  106.  
  107.     /// get bias
  108.     Vector6d bias() const { return b; }
  109.  
  110.     /// get total cost
  111.     double cost_func() const { return cost; }
  112.  
  113.     /// get projected points
  114.     VecVector2d projected_points() const { return projection; }
  115.  
  116.     /// reset h, b, cost to zero
  117.     void reset() {
  118.         H = Matrix6d::Zero();
  119.         b = Vector6d::Zero();
  120.         cost = 0;
  121.     }
  122.  
  123.     virtual void operator()(const cv::Range& range) const {
  124.  
  125.         // parameters
  126.         const int half_patch_size = 1;
  127.         int cnt_good = 0;
  128.         Matrix6d hessian = Matrix6d::Zero();
  129.         Vector6d bias = Vector6d::Zero();
  130.         double cost_tmp = 0;
  131.  
  132.         for (size_t i = range.start; i < range.end; i++) {
  133.  
  134.             // compute the projection in the second image
  135.             Eigen::Vector3d point_ref =
  136.                     depth_ref[i] * Eigen::Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
  137.             Eigen::Vector3d point_cur = T21 * point_ref;
  138.             if (point_cur[2] < 0)   // depth invalid
  139.                 continue;
  140.  
  141.             float u = fx * point_cur[0] / point_cur[2] + cx, v = fy * point_cur[1] / point_cur[2] + cy;
  142.             if (u < half_patch_size || u > img2.cols - half_patch_size || v < half_patch_size ||
  143.                 v > img2.rows - half_patch_size)
  144.                 continue;
  145.  
  146.             projection[i] = Eigen::Vector2d(u, v);
  147.             double X = point_cur[0], Y = point_cur[1], Z = point_cur[2],
  148.                     Z2 = Z * Z, Z_inv = 1.0 / Z, Z2_inv = Z_inv * Z_inv;
  149.             cnt_good++;
  150.  
  151.             // and compute error and jacobian
  152.             for (int x = -half_patch_size; x <= half_patch_size; x++)
  153.                 for (int y = -half_patch_size; y <= half_patch_size; y++) {
  154.  
  155.                     double error = GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y) -
  156.                                    GetPixelValue(img2, u + x, v + y);
  157.                     Matrix26d J_pixel_xi;
  158.                     Eigen::Vector2d J_img_pixel;
  159.  
  160.                     J_pixel_xi(0, 0) = fx * Z_inv;
  161.                     J_pixel_xi(0, 1) = 0;
  162.                     J_pixel_xi(0, 2) = -fx * X * Z2_inv;
  163.                     J_pixel_xi(0, 3) = -fx * X * Y * Z2_inv;
  164.                     J_pixel_xi(0, 4) = fx + fx * X * X * Z2_inv;
  165.                     J_pixel_xi(0, 5) = -fx * Y * Z_inv;
  166.  
  167.                     J_pixel_xi(1, 0) = 0;
  168.                     J_pixel_xi(1, 1) = fy * Z_inv;
  169.                     J_pixel_xi(1, 2) = -fy * Y * Z2_inv;
  170.                     J_pixel_xi(1, 3) = -fy - fy * Y * Y * Z2_inv;
  171.                     J_pixel_xi(1, 4) = fy * X * Y * Z2_inv;
  172.                     J_pixel_xi(1, 5) = fy * X * Z_inv;
  173.  
  174.                     J_img_pixel = Eigen::Vector2d(
  175.                             0.5 * (GetPixelValue(img2, u + 1 + x, v + y) - GetPixelValue(img2, u - 1 + x, v + y)),
  176.                             0.5 * (GetPixelValue(img2, u + x, v + 1 + y) - GetPixelValue(img2, u + x, v - 1 + y))
  177.                     );
  178.  
  179.                     // total jacobian
  180.                     Vector6d J = -1.0 * (J_img_pixel.transpose() * J_pixel_xi).transpose();
  181.  
  182.                     hessian += J * J.transpose();
  183.                     bias += -error * J;
  184.                     cost_tmp += error * error;
  185.                 }
  186.         }
  187.  
  188.         if (cnt_good) {
  189.             // set hessian, bias and cost
  190.             unique_lock<mutex> lck(hessian_mutex);
  191.             H += hessian;
  192.             b += bias;
  193.             cost += cost_tmp / cnt_good;
  194.         }
  195.     }
  196.  
  197.  
  198. };
  199.  
  200.  
  201.  
  202. int main(int argc, char **argv) {
  203.  
  204.     cv::Mat left_img = cv::imread(left_file, 0);
  205.     cv::Mat disparity_img = cv::imread(disparity_file, 0);
  206.     if (left_img.empty() || disparity_img.empty())
  207.     {
  208.         std::cout << "!!! Failed imread(): image not found" << std::endl;
  209.         return 1;
  210.     }
  211.     // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
  212.     cv::RNG rng;
  213.     int nPoints = 2000;
  214.     int boarder = 20;
  215.     VecVector2d pixels_ref;
  216.     vector<double> depth_ref;
  217.     cout << "left_img.cols" << left_img.cols << endl;
  218.     cout << "left_img: " << left_img << endl;
  219.     // generate pixels in ref and load depth data
  220.     for (int i = 0; i < nPoints; i++) {
  221.         int x = rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
  222.         int y = rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
  223.         int disparity = disparity_img.at<uchar>(y, x);
  224.         double depth = fx * baseline / disparity; // you know this is disparity to depth
  225.         depth_ref.push_back(depth);
  226.         pixels_ref.push_back(Eigen::Vector2d(x, y));
  227.     }
  228.  
  229.     // estimates 01~05.png's pose using this information
  230.     Sophus::SE3d T_cur_ref;
  231.  
  232.     for (int i = 1; i < 6; i++) {  // 1~10
  233.         cv::Mat img = cv::imread((fmt_others % i).str(), 0);
  234.         // try single layer by uncomment this line
  235.         // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
  236.         DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
  237.     }
  238.     return 0;
  239. }
  240.  
  241. void DirectPoseEstimationSingleLayer(
  242.     const cv::Mat &img1,
  243.     const cv::Mat &img2,
  244.     const VecVector2d &px_ref,
  245.     const vector<double> depth_ref,
  246.     Sophus::SE3d &T21) {
  247.  
  248.     const int iterations = 10;
  249.     double cost = 0, lastCost = 0;
  250.     auto t1 = chrono::steady_clock::now();
  251.     JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);
  252.  
  253.     for (int iter = 0; iter < iterations; iter++) {
  254.         jaco_accu.reset();
  255.         cv::parallel_for_(cv::Range(0, px_ref.size()), jaco_accu);
  256.         Matrix6d H = jaco_accu.hessian();
  257.         Vector6d b = jaco_accu.bias();
  258.  
  259.         // solve update and put it into estimation
  260.         Vector6d update = H.ldlt().solve(b);;
  261.         T21 = Sophus::SE3d::exp(update) * T21;
  262.         cost = jaco_accu.cost_func();
  263.  
  264.         if (std::isnan(update[0])) {
  265.             // sometimes occurred when we have a black or white patch and H is irreversible
  266.             cout << "update is nan" << endl;
  267.             break;
  268.         }
  269.         if (iter > 0 && cost > lastCost) {
  270.             cout << "cost increased: " << cost << ", " << lastCost << endl;
  271.             break;
  272.         }
  273.         if (update.norm() < 1e-3) {
  274.             // converge
  275.             break;
  276.         }
  277.  
  278.         lastCost = cost;
  279.         cout << "iteration: " << iter << ", cost: " << cost << endl;
  280.     }
  281.  
  282.     cout << "T21 = \n" << T21.matrix() << endl;
  283.     auto t2 = chrono::steady_clock::now();
  284.     auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  285.     cout << "direct method for single layer: " << time_used.count() << endl;
  286.  
  287.     // plot the projected pixels here
  288.     cv::Mat img2_show;
  289.     cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
  290.     VecVector2d projection = jaco_accu.projected_points();
  291.     for (size_t i = 0; i < px_ref.size(); ++i) {
  292.         auto p_ref = px_ref[i];
  293.         auto p_cur = projection[i];
  294.         if (p_cur[0] > 0 && p_cur[1] > 0) {
  295.             cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
  296.             cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
  297.                      cv::Scalar(0, 250, 0));
  298.         }
  299.     }
  300.     cv::imshow("current", img2_show);
  301.     cv::waitKey();
  302. }
  303.  
  304.  
  305. void DirectPoseEstimationMultiLayer(
  306.     const cv::Mat &img1,
  307.     const cv::Mat &img2,
  308.     const VecVector2d &px_ref,
  309.     const vector<double> depth_ref,
  310.     Sophus::SE3d &T21) {
  311.  
  312.     // parameters
  313.     int pyramids = 4;
  314.     double pyramid_scale = 0.5;
  315.     double scales[] = {1.0, 0.5, 0.25, 0.125};
  316.  
  317.     // create pyramids
  318.     vector<cv::Mat> pyr1, pyr2; // image pyramids
  319.     for (int i = 0; i < pyramids; i++) {
  320.         if (i == 0) {
  321.             pyr1.push_back(img1);
  322.             pyr2.push_back(img2);
  323.         } else {
  324.             cv::Mat img1_pyr, img2_pyr;
  325.             cv::resize(pyr1[i - 1], img1_pyr,
  326.                        cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
  327.             cv::resize(pyr2[i - 1], img2_pyr,
  328.                        cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));
  329.             pyr1.push_back(img1_pyr);
  330.             pyr2.push_back(img2_pyr);
  331.         }
  332.     }
  333.  
  334.     double fxG = fx, fyG = fy, cxG = cx, cyG = cy;  // backup the old values
  335.     for (int level = pyramids - 1; level >= 0; level--) {
  336.         VecVector2d px_ref_pyr; // set the keypoints in this pyramid level
  337.         for (auto &px: px_ref) {
  338.             px_ref_pyr.push_back(scales[level] * px);
  339.         }
  340.  
  341.         // scale fx, fy, cx, cy in different pyramid levels
  342.         fx = fxG * scales[level];
  343.         fy = fyG * scales[level];
  344.         cx = cxG * scales[level];
  345.         cy = cyG * scales[level];
  346.         DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
  347.     }
  348.  
  349. }

     optical_flow.cpp(修改后)

  1.  
  2. #include <opencv2/opencv.hpp>
  3. #include <string>
  4. #include <chrono>
  5. #include <Eigen/Core>
  6. #include <Eigen/Dense>
  7.  
  8. using namespace std;
  9. using namespace cv;
  10.  
  11. string file_1 = "./LK1.png";  // first image
  12. string file_2 = "./LK2.png";  // second image
  13.  
  14. /**
  15.  * single level optical flow
  16.  * @param [in] img1 the first image
  17.  * @param [in] img2 the second image
  18.  * @param [in] kp1 keypoints in img1
  19.  * @param [in|out] kp2 keypoints in img2, if empty, use initial guess in kp1
  20.  * @param [out] success true if a keypoint is tracked successfully
  21.  * @param [in] inverse use inverse formulation?
  22.  */
  23. void OpticalFlowSingleLevel(
  24.     const Mat &img1,
  25.     const Mat &img2,
  26.     const vector<KeyPoint> &kp1,
  27.     vector<KeyPoint> &kp2,
  28.     vector<bool> &success,
  29.     bool inverse = false,
  30.     bool has_initial_guess = false
  31. );
  32.  
  33. /**
  34.  * multi level optical flow, scale of pyramid is set to 2 by default
  35.  * the image pyramid will be create inside the function
  36.  * @param [in] img1 the first pyramid
  37.  * @param [in] img2 the second pyramid
  38.  * @param [in] kp1 keypoints in img1
  39.  * @param [out] kp2 keypoints in img2
  40.  * @param [out] success true if a keypoint is tracked successfully
  41.  * @param [in] inverse set true to enable inverse formulation
  42.  */
  43. void OpticalFlowMultiLevel(
  44.     const Mat &img1,
  45.     const Mat &img2,
  46.     const vector<KeyPoint> &kp1,
  47.     vector<KeyPoint> &kp2,
  48.     vector<bool> &success,
  49.     bool inverse = false
  50. );
  51.  
  52. /**
  53.  * get a gray scale value from reference image (bi-linear interpolated)
  54.  * @param img
  55.  * @param x
  56.  * @param y
  57.  * @return the interpolated value of this pixel
  58.  */
  59. inline float GetPixelValue(const cv::Mat &img, float x, float y) {
  60.     // boundary check
  61.     if (x < 0) x = 0;
  62.     if (y < 0) y = 0;
  63.     if (x >= img.cols) x = img.cols - 1;
  64.     if (y >= img.rows) y = img.rows - 1;
  65.     uchar *data = &img.data[int(y) * img.step + int(x)];
  66.     float xx = x - floor(x);
  67.     float yy = y - floor(y);
  68.     return float(
  69.         (1 - xx) * (1 - yy) * data[0] +
  70.         xx * (1 - yy) * data[1] +
  71.         (1 - xx) * yy * data[img.step] +
  72.         xx * yy * data[img.step + 1]
  73.     );
  74. }
  75.  
  76. /// Optical flow tracker and interface
  77. class OpticalFlowTracker: public cv::ParallelLoopBody  {
  78. private:
  79.     const Mat &img1;
  80.     const Mat &img2;
  81.     const vector<KeyPoint> &kp1;
  82.     vector<KeyPoint> &kp2;
  83.     vector<bool> &success;
  84.     bool inverse = true;
  85.     bool has_initial = false;
  86.  
  87. public:
  88.     OpticalFlowTracker(
  89.         const Mat &img1_,
  90.         const Mat &img2_,
  91.         const vector<KeyPoint> &kp1_,
  92.         vector<KeyPoint> &kp2_,
  93.         vector<bool> &success_,
  94.         bool inverse_ = true, bool has_initial_ = false) :
  95.         img1(img1_), img2(img2_), kp1(kp1_), kp2(kp2_), success(success_), inverse(inverse_),
  96.         has_initial(has_initial_) {}
  97.  
  98.     //  void calculateOpticalFlow(const Range &range);
  99.  
  100.     virtual void operator()(const Range &range) const {
  101.         // parameters
  102.         int half_patch_size = 4;
  103.         int iterations = 10;
  104.         for (size_t i = range.start; i < range.end; i++) {
  105.             auto kp = kp1[i];
  106.             double dx = 0, dy = 0; // dx,dy need to be estimated
  107.             if (has_initial) {
  108.                 dx = kp2[i].pt.x - kp.pt.x;
  109.                 dy = kp2[i].pt.y - kp.pt.y;
  110.             }
  111.  
  112.             double cost = 0, lastCost = 0;
  113.             bool succ = true; // indicate if this point succeeded
  114.  
  115.             // Gauss-Newton iterations
  116.             Eigen::Matrix2d H = Eigen::Matrix2d::Zero();    // hessian
  117.             Eigen::Vector2d b = Eigen::Vector2d::Zero();    // bias
  118.             Eigen::Vector2d J;  // jacobian
  119.             for (int iter = 0; iter < iterations; iter++) {
  120.                 if (inverse == false) {
  121.                     H = Eigen::Matrix2d::Zero();
  122.                     b = Eigen::Vector2d::Zero();
  123.                 } else {
  124.                     // only reset b
  125.                     b = Eigen::Vector2d::Zero();
  126.                 }
  127.  
  128.                 cost = 0;
  129.  
  130.                 // compute cost and jacobian
  131.                 for (int x = -half_patch_size; x < half_patch_size; x++)
  132.                     for (int y = -half_patch_size; y < half_patch_size; y++) {
  133.                         double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y) -
  134.                                     GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);;  // Jacobian
  135.                         if (inverse == false) {
  136.                             J = -1.0 * Eigen::Vector2d(
  137.                                 0.5 * (GetPixelValue(img2, kp.pt.x + dx + x + 1, kp.pt.y + dy + y) -
  138.                                     GetPixelValue(img2, kp.pt.x + dx + x - 1, kp.pt.y + dy + y)),
  139.                                 0.5 * (GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y + 1) -
  140.                                     GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y - 1))
  141.                             );
  142.                         } else if (iter == 0) {
  143.                             // in inverse mode, J keeps same for all iterations
  144.                             // NOTE this J does not change when dx, dy is updated, so we can store it and only compute error
  145.                             J = -1.0 * Eigen::Vector2d(
  146.                                 0.5 * (GetPixelValue(img1, kp.pt.x + x + 1, kp.pt.y + y) -
  147.                                     GetPixelValue(img1, kp.pt.x + x - 1, kp.pt.y + y)),
  148.                                 0.5 * (GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y + 1) -
  149.                                     GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y - 1))
  150.                             );
  151.                         }
  152.                         // compute H, b and set cost;
  153.                         b += -error * J;
  154.                         cost += error * error;
  155.                         if (inverse == false || iter == 0) {
  156.                             // also update H
  157.                             H += J * J.transpose();
  158.                         }
  159.                     }
  160.  
  161.                 // compute update
  162.                 Eigen::Vector2d update = H.ldlt().solve(b);
  163.  
  164.                 if (std::isnan(update[0])) {
  165.                     // sometimes occurred when we have a black or white patch and H is irreversible
  166.                     cout << "update is nan" << endl;
  167.                     succ = false;
  168.                     break;
  169.                 }
  170.  
  171.                 if (iter > 0 && cost > lastCost) {
  172.                     break;
  173.                 }
  174.  
  175.                 // update dx, dy
  176.                 dx += update[0];
  177.                 dy += update[1];
  178.                 lastCost = cost;
  179.                 succ = true;
  180.  
  181.                 if (update.norm() < 1e-2) {
  182.                     // converge
  183.                     break;
  184.                 }
  185.             }
  186.  
  187.             success[i] = succ;
  188.  
  189.             // set kp2
  190.             kp2[i].pt = kp.pt + Point2f(dx, dy);
  191.         }
  192.     }
  193. };
  194.  
  195.  
  196. int main(int argc, char **argv) {
  197.  
  198.     // images, note they are CV_8UC1, not CV_8UC3
  199.     Mat img1 = imread(file_1, 0);
  200.     Mat img2 = imread(file_2, 0);
  201.  
  202.     // key points, using GFTT here.
  203.     vector<KeyPoint> kp1;
  204.     Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20); // maximum 500 keypoints
  205.     detector->detect(img1, kp1);
  206.  
  207.     // now lets track these key points in the second image
  208.     // first use single level LK in the validation picture
  209.     vector<KeyPoint> kp2_single;
  210.     vector<bool> success_single;
  211.     OpticalFlowSingleLevel(img1, img2, kp1, kp2_single, success_single);
  212.  
  213.     // then test multi-level LK
  214.     vector<KeyPoint> kp2_multi;
  215.     vector<bool> success_multi;
  216.     chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  217.     OpticalFlowMultiLevel(img1, img2, kp1, kp2_multi, success_multi, true);
  218.     chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  219.     auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  220.     cout << "optical flow by gauss-newton: " << time_used.count() << endl;
  221.  
  222.     // use opencv's flow for validation
  223.     vector<Point2f> pt1, pt2;
  224.     for (auto &kp: kp1) pt1.push_back(kp.pt);
  225.     vector<uchar> status;
  226.     vector<float> error;
  227.     t1 = chrono::steady_clock::now();
  228.     cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error);
  229.     t2 = chrono::steady_clock::now();
  230.     time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  231.     cout << "optical flow by opencv: " << time_used.count() << endl;
  232.  
  233.     // plot the differences of those functions
  234.     Mat img2_single;
  235.     cv::cvtColor(img2, img2_single, CV_GRAY2BGR);
  236.     for (int i = 0; i < kp2_single.size(); i++) {
  237.         if (success_single[i]) {
  238.             cv::circle(img2_single, kp2_single[i].pt, 2, cv::Scalar(0, 250, 0), 2);
  239.             cv::line(img2_single, kp1[i].pt, kp2_single[i].pt, cv::Scalar(0, 250, 0));
  240.         }
  241.     }
  242.  
  243.     Mat img2_multi;
  244.     cv::cvtColor(img2, img2_multi, CV_GRAY2BGR);
  245.     for (int i = 0; i < kp2_multi.size(); i++) {
  246.         if (success_multi[i]) {
  247.             cv::circle(img2_multi, kp2_multi[i].pt, 2, cv::Scalar(0, 250, 0), 2);
  248.             cv::line(img2_multi, kp1[i].pt, kp2_multi[i].pt, cv::Scalar(0, 250, 0));
  249.         }
  250.     }
  251.  
  252.     Mat img2_CV;
  253.     cv::cvtColor(img2, img2_CV, CV_GRAY2BGR);
  254.     for (int i = 0; i < pt2.size(); i++) {
  255.         if (status[i]) {
  256.             cv::circle(img2_CV, pt2[i], 2, cv::Scalar(0, 250, 0), 2);
  257.             cv::line(img2_CV, pt1[i], pt2[i], cv::Scalar(0, 250, 0));
  258.         }
  259.     }
  260.  
  261.     cv::imshow("tracked single level", img2_single);
  262.     cv::imshow("tracked multi level", img2_multi);
  263.     cv::imshow("tracked by opencv", img2_CV);
  264.     cv::waitKey(0);
  265.  
  266.     return 0;
  267. }
  268.  
  269. void OpticalFlowSingleLevel(
  270.     const Mat &img1,
  271.     const Mat &img2,
  272.     const vector<KeyPoint> &kp1,
  273.     vector<KeyPoint> &kp2,
  274.     vector<bool> &success,
  275.     bool inverse, bool has_initial) {
  276.     kp2.resize(kp1.size());
  277.     success.resize(kp1.size());
  278.     OpticalFlowTracker tracker(img1, img2, kp1, kp2, success, inverse, has_initial);
  279.     parallel_for_(Range(0, kp1.size()), tracker);
  280. }
  281.  
  282.  
  283.  
  284. void OpticalFlowMultiLevel(
  285.     const Mat &img1,
  286.     const Mat &img2,
  287.     const vector<KeyPoint> &kp1,
  288.     vector<KeyPoint> &kp2,
  289.     vector<bool> &success,
  290.     bool inverse) {
  291.  
  292.     // parameters
  293.     int pyramids = 4;
  294.     double pyramid_scale = 0.5;
  295.     double scales[] = {1.0, 0.5, 0.25, 0.125};
  296.  
  297.     // create pyramids
  298.     chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  299.     vector<Mat> pyr1, pyr2; // image pyramids
  300.     for (int i = 0; i < pyramids; i++) {
  301.         if (i == 0) {
  302.             pyr1.push_back(img1);
  303.             pyr2.push_back(img2);
  304.         } else {
  305.             Mat img1_pyr, img2_pyr;
  306.             cv::resize(pyr1[i - 1], img1_pyr,
  307.                        cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
  308.             cv::resize(pyr2[i - 1], img2_pyr,
  309.                        cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));
  310.             pyr1.push_back(img1_pyr);
  311.             pyr2.push_back(img2_pyr);
  312.         }
  313.     }
  314.     chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  315.     auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  316.     cout << "build pyramid time: " << time_used.count() << endl;
  317.  
  318.     // coarse-to-fine LK tracking in pyramids
  319.     vector<KeyPoint> kp1_pyr, kp2_pyr;
  320.     for (auto &kp:kp1) {
  321.         auto kp_top = kp;
  322.         kp_top.pt *= scales[pyramids - 1];
  323.         kp1_pyr.push_back(kp_top);
  324.         kp2_pyr.push_back(kp_top);
  325.     }
  326.  
  327.     for (int level = pyramids - 1; level >= 0; level--) {
  328.         // from coarse to fine
  329.         success.clear();
  330.         t1 = chrono::steady_clock::now();
  331.         OpticalFlowSingleLevel(pyr1[level], pyr2[level], kp1_pyr, kp2_pyr, success, inverse, true);
  332.         t2 = chrono::steady_clock::now();
  333.         auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  334.         cout << "track pyr " << level << " cost time: " << time_used.count() << endl;
  335.  
  336.         if (level > 0) {
  337.             for (auto &kp: kp1_pyr)
  338.                 kp.pt /= pyramid_scale;
  339.             for (auto &kp: kp2_pyr)
  340.                 kp.pt /= pyramid_scale;
  341.         }
  342.     }
  343.  
  344.     for (auto &kp: kp2_pyr)
  345.         kp2.push_back(kp);
  346. }

将两个文件修改后可以在不安装opencv4的情况下编译通过。

三、结果

编译：

 运行：optical_flow

./build/optical_flow

  运行：direct_method

./build/direct_method

声明：本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：【wpsshop博客】
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号