- 微信
- 微博
  
  分享文章到微博
- 复制链接
  
  复制链接到剪贴板

视觉SLAM十四讲从理论到实践第二版源码调试笔记（实践应用7-14章）

zhangrelay 发表于 2021/07/15 04:10:32 2021/07/15

【摘要】视觉SLAM十四讲从理论到实践第二版源码调试笔记（理论基础1-6章）第七章和第八章：视觉里程计 1+2 使用示例，需要OpenCV4，报错如下： ROS:~/SLAM/slambook2/ch8/build$ cmake ..-- The C compiler identification is GNU 7.4.0-- The CXX compi...

视觉SLAM十四讲从理论到实践第二版源码调试笔记（理论基础1-6章）

第七章和第八章：视觉里程计 1+2

使用示例，需要OpenCV4，报错如下：


  
   
    
     
    
    
     
      ROS:~/SLAM/slambook2/ch8/build$ cmake ..
     
    
   
    
     
    
    
     
      -- The C compiler identification is GNU 7.4.0
     
    
   
    
     
    
    
     
      -- The CXX compiler identification is GNU 7.4.0
     
    
   
    
     
    
    
     
      -- Check for working C compiler: /usr/bin/cc
     
    
   
    
     
    
    
     
      -- Check for working C compiler: /usr/bin/cc -- works
     
    
   
    
     
    
    
     
      -- Detecting C compiler ABI info
     
    
   
    
     
    
    
     
      -- Detecting C compiler ABI info - done
     
    
   
    
     
    
    
     
      -- Detecting C compile features
     
    
   
    
     
    
    
     
      -- Detecting C compile features - done
     
    
   
    
     
    
    
     
      -- Check for working CXX compiler: /usr/bin/c++
     
    
   
    
     
    
    
     
      -- Check for working CXX compiler: /usr/bin/c++ -- works
     
    
   
    
     
    
    
     
      -- Detecting CXX compiler ABI info
     
    
   
    
     
    
    
     
      -- Detecting CXX compiler ABI info - done
     
    
   
    
     
    
    
     
      -- Detecting CXX compile features
     
    
   
    
     
    
    
     
      -- Detecting CXX compile features - done
     
    
   
    
     
    
    
     
      CMake Error at CMakeLists.txt:8 (find_package):
     
    
   
    
     
    
    
     
        Could not find a configuration file for package "OpenCV" that is compatible
     
    
   
    
     
    
    
     
        with requested version "4".
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
        The following configuration files were considered but not accepted:
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
       /usr/share/OpenCV/OpenCVConfig.cmake, version: 3.2.0
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      -- Configuring incomplete, errors occurred!
     
    
   
    
     
    
    
     
      See also "/home/relaybot/SLAM/slambook2/ch8/build/CMakeFiles/CMakeOutput.log".

安装OpenCV4参考：Ubuntu安装OpenCV4记录


  
   
    
     
    
    
     
      ROS:~/SLAM/slambook2/ch8/build$ cmake ..
     
    
   
    
     
    
    
     
      -- The C compiler identification is GNU 7.4.0
     
    
   
    
     
    
    
     
      -- The CXX compiler identification is GNU 7.4.0
     
    
   
    
     
    
    
     
      -- Check for working C compiler: /usr/bin/cc
     
    
   
    
     
    
    
     
      -- Check for working C compiler: /usr/bin/cc -- works
     
    
   
    
     
    
    
     
      -- Detecting C compiler ABI info
     
    
   
    
     
    
    
     
      -- Detecting C compiler ABI info - done
     
    
   
    
     
    
    
     
      -- Detecting C compile features
     
    
   
    
     
    
    
     
      -- Detecting C compile features - done
     
    
   
    
     
    
    
     
      -- Check for working CXX compiler: /usr/bin/c++
     
    
   
    
     
    
    
     
      -- Check for working CXX compiler: /usr/bin/c++ -- works
     
    
   
    
     
    
    
     
      -- Detecting CXX compiler ABI info
     
    
   
    
     
    
    
     
      -- Detecting CXX compiler ABI info - done
     
    
   
    
     
    
    
     
      -- Detecting CXX compile features
     
    
   
    
     
    
    
     
      -- Detecting CXX compile features - done
     
    
   
    
     
    
    
     
      -- Found OpenCV: /usr/local (found suitable version "4.1.2", minimum required is "4") 
     
    
   
    
     
    
    
     
      -- Configuring done
     
    
   
    
     
    
    
     
      -- Generating done
     
    
   
    
     
    
    
     
      -- Build files have been written to: /home/relaybot/SLAM/slambook2/ch8/build
     
    
   
    
     
    
    
     
      ROS:~/SLAM/slambook2/ch8/build$ make

编译时如果报错，分别如下：

optical_flow.cpp：


  
   
    
     
    
    
     
      $ make
     
    
   
    
     
    
    
     
      Scanning dependencies of target optical_flow
     
    
   
    
     
    
    
     
      [ 25%] Building CXX object CMakeFiles/optical_flow.dir/optical_flow.cpp.o
     
    
   
    
     
    
    
     
      /home/relaybot/SLAM/slambook2/ch8/optical_flow.cpp: In function ‘int main(int, char**)’:
     
    
   
    
     
    
    
     
      /home/relaybot/SLAM/slambook2/ch8/optical_flow.cpp:143:37: error: ‘CV_GRAY2BGR’ was not declared in this scope
     
    
   
    
     
    
    
     
       cv::cvtColor(img2, img2_single, CV_GRAY2BGR);
     
    
   
    
     
    
    
     
       ^~~~~~~~~~~
     
    
   
    
     
    
    
     
      CMakeFiles/optical_flow.dir/build.make:62: recipe for target 'CMakeFiles/optical_flow.dir/optical_flow.cpp.o' failed
     
    
   
    
     
    
    
     
      make[2]: *** [CMakeFiles/optical_flow.dir/optical_flow.cpp.o] Error 1
     
    
   
    
     
    
    
     
      CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/optical_flow.dir/all' failed
     
    
   
    
     
    
    
     
      make[1]: *** [CMakeFiles/optical_flow.dir/all] Error 2
     
    
   
    
     
    
    
     
      Makefile:83: recipe for target 'all' failed
     
    
   
    
     
    
    
     
      make: *** [all] Error 2

将CV_GRAY2BGR，更新为COLOR_GRAY2BGR。


  
   
    
     
    
    
     
      //
     
    
   
    
     
    
    
     
      // Created by Xiang on 2017/12/19.
     
    
   
    
     
    
    
     
      //
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      #include <opencv2/opencv.hpp>
     
    
   
    
     
    
    
     
      #include <string>
     
    
   
    
     
    
    
     
      #include <chrono>
     
    
   
    
     
    
    
     
      #include <Eigen/Core>
     
    
   
    
     
    
    
     
      #include <Eigen/Dense>
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      using namespace std;
     
    
   
    
     
    
    
     
      using namespace cv;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      string file_1 = "./LK1.png";  // first image
     
    
   
    
     
    
    
     
      string file_2 = "./LK2.png";  // second image
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      /// Optical flow tracker and interface
     
    
   
    
     
    
    
     
      class OpticalFlowTracker {
     
    
   
    
     
    
    
     
      public:
     
    
   
    
     
    
    
     
       OpticalFlowTracker(
     
    
   
    
     
    
    
      const Mat &img1_,
     
    
   
    
     
    
    
      const Mat &img2_,
     
    
   
    
     
    
    
      const vector<KeyPoint> &kp1_,
     
    
   
    
     
    
    
      vector<KeyPoint> &kp2_,
     
    
   
    
     
    
    
      vector<bool> &success_,
     
    
   
    
     
    
    
      bool inverse_ = true, bool has_initial_ = false) :
     
    
   
    
     
    
    
     
       img1(img1_), img2(img2_), kp1(kp1_), kp2(kp2_), success(success_), inverse(inverse_),
     
    
   
    
     
    
    
     
       has_initial(has_initial_) {}
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      void calculateOpticalFlow(const Range &range);
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      private:
     
    
   
    
     
    
    
      const Mat &img1;
     
    
   
    
     
    
    
      const Mat &img2;
     
    
   
    
     
    
    
      const vector<KeyPoint> &kp1;
     
    
   
    
     
    
    
      vector<KeyPoint> &kp2;
     
    
   
    
     
    
    
      vector<bool> &success;
     
    
   
    
     
    
    
      bool inverse = true;
     
    
   
    
     
    
    
      bool has_initial = false;
     
    
   
    
     
    
    
     
      };
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      /**
     
    
   
    
     
    
    
     
       * single level optical flow
     
    
   
    
     
    
    
     
       * @param [in] img1 the first image
     
    
   
    
     
    
    
     
       * @param [in] img2 the second image
     
    
   
    
     
    
    
     
       * @param [in] kp1 keypoints in img1
     
    
   
    
     
    
    
     
       * @param [in|out] kp2 keypoints in img2, if empty, use initial guess in kp1
     
    
   
    
     
    
    
     
       * @param [out] success true if a keypoint is tracked successfully
     
    
   
    
     
    
    
     
       * @param [in] inverse use inverse formulation?
     
    
   
    
     
    
    
     
       */
     
    
   
    
     
    
    
     
      void OpticalFlowSingleLevel(
     
    
   
    
     
    
    
      const Mat &img1,
     
    
   
    
     
    
    
      const Mat &img2,
     
    
   
    
     
    
    
      const vector<KeyPoint> &kp1,
     
    
   
    
     
    
    
      vector<KeyPoint> &kp2,
     
    
   
    
     
    
    
      vector<bool> &success,
     
    
   
    
     
    
    
      bool inverse = false,
     
    
   
    
     
    
    
      bool has_initial_guess = false
     
    
   
    
     
    
    
     
      );
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      /**
     
    
   
    
     
    
    
     
       * multi level optical flow, scale of pyramid is set to 2 by default
     
    
   
    
     
    
    
     
       * the image pyramid will be create inside the function
     
    
   
    
     
    
    
     
       * @param [in] img1 the first pyramid
     
    
   
    
     
    
    
     
       * @param [in] img2 the second pyramid
     
    
   
    
     
    
    
     
       * @param [in] kp1 keypoints in img1
     
    
   
    
     
    
    
     
       * @param [out] kp2 keypoints in img2
     
    
   
    
     
    
    
     
       * @param [out] success true if a keypoint is tracked successfully
     
    
   
    
     
    
    
     
       * @param [in] inverse set true to enable inverse formulation
     
    
   
    
     
    
    
     
       */
     
    
   
    
     
    
    
     
      void OpticalFlowMultiLevel(
     
    
   
    
     
    
    
      const Mat &img1,
     
    
   
    
     
    
    
      const Mat &img2,
     
    
   
    
     
    
    
      const vector<KeyPoint> &kp1,
     
    
   
    
     
    
    
      vector<KeyPoint> &kp2,
     
    
   
    
     
    
    
      vector<bool> &success,
     
    
   
    
     
    
    
      bool inverse = false
     
    
   
    
     
    
    
     
      );
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      /**
     
    
   
    
     
    
    
     
       * get a gray scale value from reference image (bi-linear interpolated)
     
    
   
    
     
    
    
     
       * @param img
     
    
   
    
     
    
    
     
       * @param x
     
    
   
    
     
    
    
     
       * @param y
     
    
   
    
     
    
    
     
       * @return the interpolated value of this pixel
     
    
   
    
     
    
    
     
       */
     
    
   
    
     
    
    
     
      inline float GetPixelValue(const cv::Mat &img, float x, float y) {
     
    
   
    
     
    
    
      // boundary check
     
    
   
    
     
    
    
      if (x < 0) x = 0;
     
    
   
    
     
    
    
      if (y < 0) y = 0;
     
    
   
    
     
    
    
      if (x >= img.cols) x = img.cols - 1;
     
    
   
    
     
    
    
      if (y >= img.rows) y = img.rows - 1;
     
    
   
    
     
    
    
     
       uchar *data = &img.data[int(y) * img.step + int(x)];
     
    
   
    
     
    
    
      float xx = x - floor(x);
     
    
   
    
     
    
    
      float yy = y - floor(y);
     
    
   
    
     
    
    
      return float(
     
    
   
    
     
    
    
     
       (1 - xx) * (1 - yy) * data[0] +
     
    
   
    
     
    
    
     
       xx * (1 - yy) * data[1] +
     
    
   
    
     
    
    
     
       (1 - xx) * yy * data[img.step] +
     
    
   
    
     
    
    
     
       xx * yy * data[img.step + 1]
     
    
   
    
     
    
    
     
       );
     
    
   
    
     
    
    
     
      }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      int main(int argc, char **argv) {
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // images, note they are CV_8UC1, not CV_8UC3
     
    
   
    
     
    
    
     
       Mat img1 = imread(file_1, 0);
     
    
   
    
     
    
    
     
       Mat img2 = imread(file_2, 0);
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // key points, using GFTT here.
     
    
   
    
     
    
    
      vector<KeyPoint> kp1;
     
    
   
    
     
    
    
     
       Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20); // maximum 500 keypoints
     
    
   
    
     
    
    
     
       detector->detect(img1, kp1);
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // now lets track these key points in the second image
     
    
   
    
     
    
    
      // first use single level LK in the validation picture
     
    
   
    
     
    
    
      vector<KeyPoint> kp2_single;
     
    
   
    
     
    
    
      vector<bool> success_single;
     
    
   
    
     
    
    
     
       OpticalFlowSingleLevel(img1, img2, kp1, kp2_single, success_single);
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // then test multi-level LK
     
    
   
    
     
    
    
      vector<KeyPoint> kp2_multi;
     
    
   
    
     
    
    
      vector<bool> success_multi;
     
    
   
    
     
    
    
     
       chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
     
    
   
    
     
    
    
     
       OpticalFlowMultiLevel(img1, img2, kp1, kp2_multi, success_multi, true);
     
    
   
    
     
    
    
     
       chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
     
    
   
    
     
    
    
      auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
     
    
   
    
     
    
    
      cout << "optical flow by gauss-newton: " << time_used.count() << endl;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // use opencv's flow for validation
     
    
   
    
     
    
    
      vector<Point2f> pt1, pt2;
     
    
   
    
     
    
    
      for (auto &kp: kp1) pt1.push_back(kp.pt);
     
    
   
    
     
    
    
      vector<uchar> status;
     
    
   
    
     
    
    
      vector<float> error;
     
    
   
    
     
    
    
     
       t1 = chrono::steady_clock::now();
     
    
   
    
     
    
    
     
       cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error);
     
    
   
    
     
    
    
     
       t2 = chrono::steady_clock::now();
     
    
   
    
     
    
    
     
       time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
     
    
   
    
     
    
    
      cout << "optical flow by opencv: " << time_used.count() << endl;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // plot the differences of those functions
     
    
   
    
     
    
    
     
       Mat img2_single;
     
    
   
    
     
    
    
     
       cv::cvtColor(img2, img2_single, COLOR_GRAY2BGR);
     
    
   
    
     
    
    
      for (int i = 0; i < kp2_single.size(); i++) {
     
    
   
    
     
    
    
      if (success_single[i]) {
     
    
   
    
     
    
    
     
       cv::circle(img2_single, kp2_single[i].pt, 2, cv::Scalar(0, 250, 0), 2);
     
    
   
    
     
    
    
     
       cv::line(img2_single, kp1[i].pt, kp2_single[i].pt, cv::Scalar(0, 250, 0));
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
       Mat img2_multi;
     
    
   
    
     
    
    
     
       cv::cvtColor(img2, img2_multi, COLOR_GRAY2BGR);
     
    
   
    
     
    
    
      for (int i = 0; i < kp2_multi.size(); i++) {
     
    
   
    
     
    
    
      if (success_multi[i]) {
     
    
   
    
     
    
    
     
       cv::circle(img2_multi, kp2_multi[i].pt, 2, cv::Scalar(0, 250, 0), 2);
     
    
   
    
     
    
    
     
       cv::line(img2_multi, kp1[i].pt, kp2_multi[i].pt, cv::Scalar(0, 250, 0));
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
       Mat img2_CV;
     
    
   
    
     
    
    
     
       cv::cvtColor(img2, img2_CV, COLOR_GRAY2BGR);
     
    
   
    
     
    
    
      for (int i = 0; i < pt2.size(); i++) {
     
    
   
    
     
    
    
      if (status[i]) {
     
    
   
    
     
    
    
     
       cv::circle(img2_CV, pt2[i], 2, cv::Scalar(0, 250, 0), 2);
     
    
   
    
     
    
    
     
       cv::line(img2_CV, pt1[i], pt2[i], cv::Scalar(0, 250, 0));
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
       cv::imshow("tracked single level", img2_single);
     
    
   
    
     
    
    
     
       cv::imshow("tracked multi level", img2_multi);
     
    
   
    
     
    
    
     
       cv::imshow("tracked by opencv", img2_CV);
     
    
   
    
     
    
    
     
       cv::waitKey(0);
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      return 0;
     
    
   
    
     
    
    
     
      }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      void OpticalFlowSingleLevel(
     
    
   
    
     
    
    
      const Mat &img1,
     
    
   
    
     
    
    
      const Mat &img2,
     
    
   
    
     
    
    
      const vector<KeyPoint> &kp1,
     
    
   
    
     
    
    
      vector<KeyPoint> &kp2,
     
    
   
    
     
    
    
      vector<bool> &success,
     
    
   
    
     
    
    
      bool inverse, bool has_initial) {
     
    
   
    
     
    
    
     
       kp2.resize(kp1.size());
     
    
   
    
     
    
    
     
       success.resize(kp1.size());
     
    
   
    
     
    
    
      OpticalFlowTracker tracker(img1, img2, kp1, kp2, success, inverse, has_initial);
     
    
   
    
     
    
    
     
       parallel_for_(Range(0, kp1.size()),
     
    
   
    
     
    
    
      std::bind(&OpticalFlowTracker::calculateOpticalFlow, &tracker, placeholders::_1));
     
    
   
    
     
    
    
     
      }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      void OpticalFlowTracker::calculateOpticalFlow(const Range &range) {
     
    
   
    
     
    
    
      // parameters
     
    
   
    
     
    
    
      int half_patch_size = 4;
     
    
   
    
     
    
    
      int iterations = 10;
     
    
   
    
     
    
    
      for (size_t i = range.start; i < range.end; i++) {
     
    
   
    
     
    
    
      auto kp = kp1[i];
     
    
   
    
     
    
    
      double dx = 0, dy = 0; // dx,dy need to be estimated
     
    
   
    
     
    
    
      if (has_initial) {
     
    
   
    
     
    
    
     
       dx = kp2[i].pt.x - kp.pt.x;
     
    
   
    
     
    
    
     
       dy = kp2[i].pt.y - kp.pt.y;
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      double cost = 0, lastCost = 0;
     
    
   
    
     
    
    
      bool succ = true; // indicate if this point succeeded
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // Gauss-Newton iterations
     
    
   
    
     
    
    
     
       Eigen::Matrix2d H = Eigen::Matrix2d::Zero(); // hessian
     
    
   
    
     
    
    
     
       Eigen::Vector2d b = Eigen::Vector2d::Zero(); // bias
     
    
   
    
     
    
    
     
       Eigen::Vector2d J;  // jacobian
     
    
   
    
     
    
    
      for (int iter = 0; iter < iterations; iter++) {
     
    
   
    
     
    
    
      if (inverse == false) {
     
    
   
    
     
    
    
     
       H = Eigen::Matrix2d::Zero();
     
    
   
    
     
    
    
     
       b = Eigen::Vector2d::Zero();
     
    
   
    
     
    
    
     
       } else {
     
    
   
    
     
    
    
      // only reset b
     
    
   
    
     
    
    
     
       b = Eigen::Vector2d::Zero();
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
       cost = 0;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // compute cost and jacobian
     
    
   
    
     
    
    
      for (int x = -half_patch_size; x < half_patch_size; x++)
     
    
   
    
     
    
    
      for (int y = -half_patch_size; y < half_patch_size; y++) {
     
    
   
    
     
    
    
      double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y) -
     
    
   
    
     
    
    
     
       GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);;  // Jacobian
     
    
   
    
     
    
    
      if (inverse == false) {
     
    
   
    
     
    
    
     
       J = -1.0 * Eigen::Vector2d(
     
    
   
    
     
    
    
      0.5 * (GetPixelValue(img2, kp.pt.x + dx + x + 1, kp.pt.y + dy + y) -
     
    
   
    
     
    
    
     
       GetPixelValue(img2, kp.pt.x + dx + x - 1, kp.pt.y + dy + y)),
     
    
   
    
     
    
    
      0.5 * (GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y + 1) -
     
    
   
    
     
    
    
     
       GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y - 1))
     
    
   
    
     
    
    
     
       );
     
    
   
    
     
    
    
     
       } else if (iter == 0) {
     
    
   
    
     
    
    
      // in inverse mode, J keeps same for all iterations
     
    
   
    
     
    
    
      // NOTE this J does not change when dx, dy is updated, so we can store it and only compute error
     
    
   
    
     
    
    
     
       J = -1.0 * Eigen::Vector2d(
     
    
   
    
     
    
    
      0.5 * (GetPixelValue(img1, kp.pt.x + x + 1, kp.pt.y + y) -
     
    
   
    
     
    
    
     
       GetPixelValue(img1, kp.pt.x + x - 1, kp.pt.y + y)),
     
    
   
    
     
    
    
      0.5 * (GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y + 1) -
     
    
   
    
     
    
    
     
       GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y - 1))
     
    
   
    
     
    
    
     
       );
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      // compute H, b and set cost;
     
    
   
    
     
    
    
     
       b += -error * J;
     
    
   
    
     
    
    
     
       cost += error * error;
     
    
   
    
     
    
    
      if (inverse == false || iter == 0) {
     
    
   
    
     
    
    
      // also update H
     
    
   
    
     
    
    
     
       H += J * J.transpose();
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // compute update
     
    
   
    
     
    
    
     
       Eigen::Vector2d update = H.ldlt().solve(b);
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      if (std::isnan(update[0])) {
     
    
   
    
     
    
    
      // sometimes occurred when we have a black or white patch and H is irreversible
     
    
   
    
     
    
    
      cout << "update is nan" << endl;
     
    
   
    
     
    
    
     
       succ = false;
     
    
   
    
     
    
    
      break;
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      if (iter > 0 && cost > lastCost) {
     
    
   
    
     
    
    
      break;
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // update dx, dy
     
    
   
    
     
    
    
     
       dx += update[0];
     
    
   
    
     
    
    
     
       dy += update[1];
     
    
   
    
     
    
    
     
       lastCost = cost;
     
    
   
    
     
    
    
     
       succ = true;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      if (update.norm() < 1e-2) {
     
    
   
    
     
    
    
      // converge
     
    
   
    
     
    
    
      break;
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
       success[i] = succ;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // set kp2
     
    
   
    
     
    
    
     
       kp2[i].pt = kp.pt + Point2f(dx, dy);
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
     
      }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      void OpticalFlowMultiLevel(
     
    
   
    
     
    
    
      const Mat &img1,
     
    
   
    
     
    
    
      const Mat &img2,
     
    
   
    
     
    
    
      const vector<KeyPoint> &kp1,
     
    
   
    
     
    
    
      vector<KeyPoint> &kp2,
     
    
   
    
     
    
    
      vector<bool> &success,
     
    
   
    
     
    
    
      bool inverse) {
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // parameters
     
    
   
    
     
    
    
      int pyramids = 4;
     
    
   
    
     
    
    
      double pyramid_scale = 0.5;
     
    
   
    
     
    
    
      double scales[] = {1.0, 0.5, 0.25, 0.125};
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // create pyramids
     
    
   
    
     
    
    
     
       chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
     
    
   
    
     
    
    
      vector<Mat> pyr1, pyr2; // image pyramids
     
    
   
    
     
    
    
      for (int i = 0; i < pyramids; i++) {
     
    
   
    
     
    
    
      if (i == 0) {
     
    
   
    
     
    
    
     
       pyr1.push_back(img1);
     
    
   
    
     
    
    
     
       pyr2.push_back(img2);
     
    
   
    
     
    
    
     
       } else {
     
    
   
    
     
    
    
     
       Mat img1_pyr, img2_pyr;
     
    
   
    
     
    
    
     
       cv::resize(pyr1[i - 1], img1_pyr,
     
    
   
    
     
    
    
     
       cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
     
    
   
    
     
    
    
     
       cv::resize(pyr2[i - 1], img2_pyr,
     
    
   
    
     
    
    
     
       cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));
     
    
   
    
     
    
    
     
       pyr1.push_back(img1_pyr);
     
    
   
    
     
    
    
     
       pyr2.push_back(img2_pyr);
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
     
       chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
     
    
   
    
     
    
    
      auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
     
    
   
    
     
    
    
      cout << "build pyramid time: " << time_used.count() << endl;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // coarse-to-fine LK tracking in pyramids
     
    
   
    
     
    
    
      vector<KeyPoint> kp1_pyr, kp2_pyr;
     
    
   
    
     
    
    
      for (auto &kp:kp1) {
     
    
   
    
     
    
    
      auto kp_top = kp;
     
    
   
    
     
    
    
     
       kp_top.pt *= scales[pyramids - 1];
     
    
   
    
     
    
    
     
       kp1_pyr.push_back(kp_top);
     
    
   
    
     
    
    
     
       kp2_pyr.push_back(kp_top);
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      for (int level = pyramids - 1; level >= 0; level--) {
     
    
   
    
     
    
    
      // from coarse to fine
     
    
   
    
     
    
    
     
       success.clear();
     
    
   
    
     
    
    
     
       t1 = chrono::steady_clock::now();
     
    
   
    
     
    
    
     
       OpticalFlowSingleLevel(pyr1[level], pyr2[level], kp1_pyr, kp2_pyr, success, inverse, true);
     
    
   
    
     
    
    
     
       t2 = chrono::steady_clock::now();
     
    
   
    
     
    
    
      auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
     
    
   
    
     
    
    
      cout << "track pyr " << level << " cost time: " << time_used.count() << endl;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      if (level > 0) {
     
    
   
    
     
    
    
      for (auto &kp: kp1_pyr)
     
    
   
    
     
    
    
     
       kp.pt /= pyramid_scale;
     
    
   
    
     
    
    
      for (auto &kp: kp2_pyr)
     
    
   
    
     
    
    
     
       kp.pt /= pyramid_scale;
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      for (auto &kp: kp2_pyr)
     
    
   
    
     
    
    
     
       kp2.push_back(kp);
     
    
   
    
     
    
    
     
      }

但是，direct_method.cpp依然报错，如下：


  
   
    
     
    
    
     
      make
     
    
   
    
     
    
    
     
      [ 50%] Built target optical_flow
     
    
   
    
     
    
    
     
      Scanning dependencies of target direct_method
     
    
   
    
     
    
    
     
      [ 75%] Building CXX object CMakeFiles/direct_method.dir/direct_method.cpp.o
     
    
   
    
     
    
    
     
      /home/relaybot/SLAM/slambook2/ch8/direct_method.cpp: In function ‘void DirectPoseEstimationSingleLayer(const cv::Mat&, const cv::Mat&, const VecVector2d&, std::vector<double, std::allocator<double> >, Sophus::SE3d&)’:
     
    
   
    
     
    
    
     
      /home/relaybot/SLAM/slambook2/ch8/direct_method.cpp:206:35: error: ‘COLOR_GRAY2BGR’ was not declared in this scope
     
    
   
    
     
    
    
     
       cv::cvtColor(img2, img2_show, COLOR_GRAY2BGR);
     
    
   
    
     
    
    
     
       ^~~~~~~~~~~~~~
     
    
   
    
     
    
    
     
      /home/relaybot/SLAM/slambook2/ch8/direct_method.cpp:206:35: note: suggested alternative:
     
    
   
    
     
    
    
     
      In file included from /usr/local/include/opencv4/opencv2/opencv.hpp:74:0,
     
    
   
    
     
    
    
     
       from /home/relaybot/SLAM/slambook2/ch8/direct_method.cpp:1:
     
    
   
    
     
    
    
     
      /usr/local/include/opencv4/opencv2/imgproc.hpp:542:5: note:   ‘COLOR_GRAY2BGR’
     
    
   
    
     
    
    
     
       COLOR_GRAY2BGR = 8,
     
    
   
    
     
    
    
     
       ^~~~~~~~~~~~~~
     
    
   
    
     
    
    
     
      CMakeFiles/direct_method.dir/build.make:62: recipe for target 'CMakeFiles/direct_method.dir/direct_method.cpp.o' failed
     
    
   
    
     
    
    
     
      make[2]: *** [CMakeFiles/direct_method.dir/direct_method.cpp.o] Error 1
     
    
   
    
     
    
    
     
      CMakeFiles/Makefile2:104: recipe for target 'CMakeFiles/direct_method.dir/all' failed
     
    
   
    
     
    
    
     
      make[1]: *** [CMakeFiles/direct_method.dir/all] Error 2
     
    
   
    
     
    
    
     
      Makefile:83: recipe for target 'all' failed
     
    
   
    
     
    
    
     
      make: *** [all] Error 2

在程序中，添加using namespace cv;

修正后的程序如下：


  
   
    
     
    
    
     
      #include <opencv2/opencv.hpp>
     
    
   
    
     
    
    
     
      #include <sophus/se3.hpp>
     
    
   
    
     
    
    
     
      #include <boost/format.hpp>
     
    
   
    
     
    
    
     
      #include <pangolin/pangolin.h>
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      using namespace std;
     
    
   
    
     
    
    
     
      using namespace cv;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      // Camera intrinsics
     
    
   
    
     
    
    
     
      double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
     
    
   
    
     
    
    
     
      // baseline
     
    
   
    
     
    
    
     
      double baseline = 0.573;
     
    
   
    
     
    
    
     
      // paths
     
    
   
    
     
    
    
     
      string left_file = "./left.png";
     
    
   
    
     
    
    
     
      string disparity_file = "./disparity.png";
     
    
   
    
     
    
    
     
      boost::format fmt_others("./%06d.png"); // other files
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      // useful typedefs
     
    
   
    
     
    
    
     
      typedef Eigen::Matrix<double, 6, 6> Matrix6d;
     
    
   
    
     
    
    
     
      typedef Eigen::Matrix<double, 2, 6> Matrix26d;
     
    
   
    
     
    
    
     
      typedef Eigen::Matrix<double, 6, 1> Vector6d;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      /// class for accumulator jacobians in parallel
     
    
   
    
     
    
    
     
      class JacobianAccumulator {
     
    
   
    
     
    
    
     
      public:
     
    
   
    
     
    
    
     
       JacobianAccumulator(
     
    
   
    
     
    
    
      const cv::Mat &img1_,
     
    
   
    
     
    
    
      const cv::Mat &img2_,
     
    
   
    
     
    
    
      const VecVector2d &px_ref_,
     
    
   
    
     
    
    
      const vector<double> depth_ref_,
     
    
   
    
     
    
    
     
       Sophus::SE3d &T21_) :
     
    
   
    
     
    
    
     
       img1(img1_), img2(img2_), px_ref(px_ref_), depth_ref(depth_ref_), T21(T21_) {
     
    
   
    
     
    
    
     
       projection = VecVector2d(px_ref.size(), Eigen::Vector2d(0, 0));
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      /// accumulate jacobians in a range
     
    
   
    
     
    
    
      void accumulate_jacobian(const cv::Range &range);
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      /// get hessian matrix
     
    
   
    
     
    
    
      Matrix6d hessian() const { return H; }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      /// get bias
     
    
   
    
     
    
    
      Vector6d bias() const { return b; }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      /// get total cost
     
    
   
    
     
    
    
      double cost_func() const { return cost; }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      /// get projected points
     
    
   
    
     
    
    
      VecVector2d projected_points() const { return projection; }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      /// reset h, b, cost to zero
     
    
   
    
     
    
    
      void reset() {
     
    
   
    
     
    
    
     
       H = Matrix6d::Zero();
     
    
   
    
     
    
    
     
       b = Vector6d::Zero();
     
    
   
    
     
    
    
     
       cost = 0;
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      private:
     
    
   
    
     
    
    
      const cv::Mat &img1;
     
    
   
    
     
    
    
      const cv::Mat &img2;
     
    
   
    
     
    
    
      const VecVector2d &px_ref;
     
    
   
    
     
    
    
      const vector<double> depth_ref;
     
    
   
    
     
    
    
     
       Sophus::SE3d &T21;
     
    
   
    
     
    
    
     
       VecVector2d projection; // projected points
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      std::mutex hessian_mutex;
     
    
   
    
     
    
    
     
       Matrix6d H = Matrix6d::Zero();
     
    
   
    
     
    
    
     
       Vector6d b = Vector6d::Zero();
     
    
   
    
     
    
    
      double cost = 0;
     
    
   
    
     
    
    
     
      };
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      /**
     
    
   
    
     
    
    
     
       * pose estimation using direct method
     
    
   
    
     
    
    
     
       * @param img1
     
    
   
    
     
    
    
     
       * @param img2
     
    
   
    
     
    
    
     
       * @param px_ref
     
    
   
    
     
    
    
     
       * @param depth_ref
     
    
   
    
     
    
    
     
       * @param T21
     
    
   
    
     
    
    
     
       */
     
    
   
    
     
    
    
     
      void DirectPoseEstimationMultiLayer(
     
    
   
    
     
    
    
     
       const cv::Mat &img1,
     
    
   
    
     
    
    
     
       const cv::Mat &img2,
     
    
   
    
     
    
    
     
       const VecVector2d &px_ref,
     
    
   
    
     
    
    
     
       const vector<double> depth_ref,
     
    
   
    
     
    
    
     
       Sophus::SE3d &T21
     
    
   
    
     
    
    
     
      );
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      /**
     
    
   
    
     
    
    
     
       * pose estimation using direct method
     
    
   
    
     
    
    
     
       * @param img1
     
    
   
    
     
    
    
     
       * @param img2
     
    
   
    
     
    
    
     
       * @param px_ref
     
    
   
    
     
    
    
     
       * @param depth_ref
     
    
   
    
     
    
    
     
       * @param T21
     
    
   
    
     
    
    
     
       */
     
    
   
    
     
    
    
     
      void DirectPoseEstimationSingleLayer(
     
    
   
    
     
    
    
     
       const cv::Mat &img1,
     
    
   
    
     
    
    
     
       const cv::Mat &img2,
     
    
   
    
     
    
    
     
       const VecVector2d &px_ref,
     
    
   
    
     
    
    
     
       const vector<double> depth_ref,
     
    
   
    
     
    
    
     
       Sophus::SE3d &T21
     
    
   
    
     
    
    
     
      );
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      // bilinear interpolation
     
    
   
    
     
    
    
     
      inline float GetPixelValue(const cv::Mat &img, float x, float y) {
     
    
   
    
     
    
    
      // boundary check
     
    
   
    
     
    
    
      if (x < 0) x = 0;
     
    
   
    
     
    
    
      if (y < 0) y = 0;
     
    
   
    
     
    
    
      if (x >= img.cols) x = img.cols - 1;
     
    
   
    
     
    
    
      if (y >= img.rows) y = img.rows - 1;
     
    
   
    
     
    
    
     
       uchar *data = &img.data[int(y) * img.step + int(x)];
     
    
   
    
     
    
    
      float xx = x - floor(x);
     
    
   
    
     
    
    
      float yy = y - floor(y);
     
    
   
    
     
    
    
      return float(
     
    
   
    
     
    
    
     
       (1 - xx) * (1 - yy) * data[0] +
     
    
   
    
     
    
    
     
       xx * (1 - yy) * data[1] +
     
    
   
    
     
    
    
     
       (1 - xx) * yy * data[img.step] +
     
    
   
    
     
    
    
     
       xx * yy * data[img.step + 1]
     
    
   
    
     
    
    
     
       );
     
    
   
    
     
    
    
     
      }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      int main(int argc, char **argv) {
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
       cv::Mat left_img = cv::imread(left_file, 0);
     
    
   
    
     
    
    
     
       cv::Mat disparity_img = cv::imread(disparity_file, 0);
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
     
    
   
    
     
    
    
     
       cv::RNG rng;
     
    
   
    
     
    
    
      int nPoints = 2000;
     
    
   
    
     
    
    
      int boarder = 20;
     
    
   
    
     
    
    
     
       VecVector2d pixels_ref;
     
    
   
    
     
    
    
      vector<double> depth_ref;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // generate pixels in ref and load depth data
     
    
   
    
     
    
    
      for (int i = 0; i < nPoints; i++) {
     
    
   
    
     
    
    
      int x = rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
     
    
   
    
     
    
    
      int y = rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
     
    
   
    
     
    
    
      int disparity = disparity_img.at<uchar>(y, x);
     
    
   
    
     
    
    
      double depth = fx * baseline / disparity; // you know this is disparity to depth
     
    
   
    
     
    
    
     
       depth_ref.push_back(depth);
     
    
   
    
     
    
    
     
       pixels_ref.push_back(Eigen::Vector2d(x, y));
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // estimates 01~05.png's pose using this information
     
    
   
    
     
    
    
     
       Sophus::SE3d T_cur_ref;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      for (int i = 1; i < 6; i++) {  // 1~10
     
    
   
    
     
    
    
     
       cv::Mat img = cv::imread((fmt_others % i).str(), 0);
     
    
   
    
     
    
    
      // try single layer by uncomment this line
     
    
   
    
     
    
    
      // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
     
    
   
    
     
    
    
     
       DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      return 0;
     
    
   
    
     
    
    
     
      }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      void DirectPoseEstimationSingleLayer(
     
    
   
    
     
    
    
     
       const cv::Mat &img1,
     
    
   
    
     
    
    
     
       const cv::Mat &img2,
     
    
   
    
     
    
    
     
       const VecVector2d &px_ref,
     
    
   
    
     
    
    
     
       const vector<double> depth_ref,
     
    
   
    
     
    
    
     
       Sophus::SE3d &T21) {
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      const int iterations = 10;
     
    
   
    
     
    
    
      double cost = 0, lastCost = 0;
     
    
   
    
     
    
    
      auto t1 = chrono::steady_clock::now();
     
    
   
    
     
    
    
      JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      for (int iter = 0; iter < iterations; iter++) {
     
    
   
    
     
    
    
     
       jaco_accu.reset();
     
    
   
    
     
    
    
     
       cv::parallel_for_(cv::Range(0, px_ref.size()),
     
    
   
    
     
    
    
      std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, std::placeholders::_1));
     
    
   
    
     
    
    
     
       Matrix6d H = jaco_accu.hessian();
     
    
   
    
     
    
    
     
       Vector6d b = jaco_accu.bias();
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // solve update and put it into estimation
     
    
   
    
     
    
    
     
       Vector6d update = H.ldlt().solve(b);;
     
    
   
    
     
    
    
     
       T21 = Sophus::SE3d::exp(update) * T21;
     
    
   
    
     
    
    
     
       cost = jaco_accu.cost_func();
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      if (std::isnan(update[0])) {
     
    
   
    
     
    
    
      // sometimes occurred when we have a black or white patch and H is irreversible
     
    
   
    
     
    
    
      cout << "update is nan" << endl;
     
    
   
    
     
    
    
      break;
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      if (iter > 0 && cost > lastCost) {
     
    
   
    
     
    
    
      cout << "cost increased: " << cost << ", " << lastCost << endl;
     
    
   
    
     
    
    
      break;
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      if (update.norm() < 1e-3) {
     
    
   
    
     
    
    
      // converge
     
    
   
    
     
    
    
      break;
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
       lastCost = cost;
     
    
   
    
     
    
    
      cout << "iteration: " << iter << ", cost: " << cost << endl;
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      cout << "T21 = \n" << T21.matrix() << endl;
     
    
   
    
     
    
    
      auto t2 = chrono::steady_clock::now();
     
    
   
    
     
    
    
      auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
     
    
   
    
     
    
    
      cout << "direct method for single layer: " << time_used.count() << endl;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // plot the projected pixels here
     
    
   
    
     
    
    
     
       cv::Mat img2_show;
     
    
   
    
     
    
    
      //Mat img2_show;
     
    
   
    
     
    
    
     
       cv::cvtColor(img2, img2_show, COLOR_GRAY2BGR);
     
    
   
    
     
    
    
     
       VecVector2d projection = jaco_accu.projected_points();
     
    
   
    
     
    
    
      for (size_t i = 0; i < px_ref.size(); ++i) {
     
    
   
    
     
    
    
      auto p_ref = px_ref[i];
     
    
   
    
     
    
    
      auto p_cur = projection[i];
     
    
   
    
     
    
    
      if (p_cur[0] > 0 && p_cur[1] > 0) {
     
    
   
    
     
    
    
     
       cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
     
    
   
    
     
    
    
     
       cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
     
    
   
    
     
    
    
     
       cv::Scalar(0, 250, 0));
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
     
       cv::imshow("current", img2_show);
     
    
   
    
     
    
    
     
       cv::waitKey();
     
    
   
    
     
    
    
     
      }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      void JacobianAccumulator::accumulate_jacobian(const cv::Range &range) {
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // parameters
     
    
   
    
     
    
    
      const int half_patch_size = 1;
     
    
   
    
     
    
    
      int cnt_good = 0;
     
    
   
    
     
    
    
     
       Matrix6d hessian = Matrix6d::Zero();
     
    
   
    
     
    
    
     
       Vector6d bias = Vector6d::Zero();
     
    
   
    
     
    
    
      double cost_tmp = 0;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      for (size_t i = range.start; i < range.end; i++) {
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // compute the projection in the second image
     
    
   
    
     
    
    
     
       Eigen::Vector3d point_ref =
     
    
   
    
     
    
    
     
       depth_ref[i] * Eigen::Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
     
    
   
    
     
    
    
     
       Eigen::Vector3d point_cur = T21 * point_ref;
     
    
   
    
     
    
    
      if (point_cur[2] < 0)   // depth invalid
     
    
   
    
     
    
    
      continue;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      float u = fx * point_cur[0] / point_cur[2] + cx, v = fy * point_cur[1] / point_cur[2] + cy;
     
    
   
    
     
    
    
      if (u < half_patch_size || u > img2.cols - half_patch_size || v < half_patch_size ||
     
    
   
    
     
    
    
     
       v > img2.rows - half_patch_size)
     
    
   
    
     
    
    
      continue;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
       projection[i] = Eigen::Vector2d(u, v);
     
    
   
    
     
    
    
      double X = point_cur[0], Y = point_cur[1], Z = point_cur[2],
     
    
   
    
     
    
    
     
       Z2 = Z * Z, Z_inv = 1.0 / Z, Z2_inv = Z_inv * Z_inv;
     
    
   
    
     
    
    
     
       cnt_good++;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // and compute error and jacobian
     
    
   
    
     
    
    
      for (int x = -half_patch_size; x <= half_patch_size; x++)
     
    
   
    
     
    
    
      for (int y = -half_patch_size; y <= half_patch_size; y++) {
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      double error = GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y) -
     
    
   
    
     
    
    
     
       GetPixelValue(img2, u + x, v + y);
     
    
   
    
     
    
    
     
       Matrix26d J_pixel_xi;
     
    
   
    
     
    
    
     
       Eigen::Vector2d J_img_pixel;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
       J_pixel_xi(0, 0) = fx * Z_inv;
     
    
   
    
     
    
    
     
       J_pixel_xi(0, 1) = 0;
     
    
   
    
     
    
    
     
       J_pixel_xi(0, 2) = -fx * X * Z2_inv;
     
    
   
    
     
    
    
     
       J_pixel_xi(0, 3) = -fx * X * Y * Z2_inv;
     
    
   
    
     
    
    
     
       J_pixel_xi(0, 4) = fx + fx * X * X * Z2_inv;
     
    
   
    
     
    
    
     
       J_pixel_xi(0, 5) = -fx * Y * Z_inv;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
       J_pixel_xi(1, 0) = 0;
     
    
   
    
     
    
    
     
       J_pixel_xi(1, 1) = fy * Z_inv;
     
    
   
    
     
    
    
     
       J_pixel_xi(1, 2) = -fy * Y * Z2_inv;
     
    
   
    
     
    
    
     
       J_pixel_xi(1, 3) = -fy - fy * Y * Y * Z2_inv;
     
    
   
    
     
    
    
     
       J_pixel_xi(1, 4) = fy * X * Y * Z2_inv;
     
    
   
    
     
    
    
     
       J_pixel_xi(1, 5) = fy * X * Z_inv;
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
       J_img_pixel = Eigen::Vector2d(
     
    
   
    
     
    
    
      0.5 * (GetPixelValue(img2, u + 1 + x, v + y) - GetPixelValue(img2, u - 1 + x, v + y)),
     
    
   
    
     
    
    
      0.5 * (GetPixelValue(img2, u + x, v + 1 + y) - GetPixelValue(img2, u + x, v - 1 + y))
     
    
   
    
     
    
    
     
       );
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // total jacobian
     
    
   
    
     
    
    
     
       Vector6d J = -1.0 * (J_img_pixel.transpose() * J_pixel_xi).transpose();
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
       hessian += J * J.transpose();
     
    
   
    
     
    
    
     
       bias += -error * J;
     
    
   
    
     
    
    
     
       cost_tmp += error * error;
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      if (cnt_good) {
     
    
   
    
     
    
    
      // set hessian, bias and cost
     
    
   
    
     
    
    
      unique_lock<mutex> lck(hessian_mutex);
     
    
   
    
     
    
    
     
       H += hessian;
     
    
   
    
     
    
    
     
       b += bias;
     
    
   
    
     
    
    
     
       cost += cost_tmp / cnt_good;
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
     
      }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      void DirectPoseEstimationMultiLayer(
     
    
   
    
     
    
    
     
       const cv::Mat &img1,
     
    
   
    
     
    
    
     
       const cv::Mat &img2,
     
    
   
    
     
    
    
     
       const VecVector2d &px_ref,
     
    
   
    
     
    
    
     
       const vector<double> depth_ref,
     
    
   
    
     
    
    
     
       Sophus::SE3d &T21) {
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // parameters
     
    
   
    
     
    
    
      int pyramids = 4;
     
    
   
    
     
    
    
      double pyramid_scale = 0.5;
     
    
   
    
     
    
    
      double scales[] = {1.0, 0.5, 0.25, 0.125};
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // create pyramids
     
    
   
    
     
    
    
      vector<cv::Mat> pyr1, pyr2; // image pyramids
     
    
   
    
     
    
    
      for (int i = 0; i < pyramids; i++) {
     
    
   
    
     
    
    
      if (i == 0) {
     
    
   
    
     
    
    
     
       pyr1.push_back(img1);
     
    
   
    
     
    
    
     
       pyr2.push_back(img2);
     
    
   
    
     
    
    
     
       } else {
     
    
   
    
     
    
    
     
       cv::Mat img1_pyr, img2_pyr;
     
    
   
    
     
    
    
     
       cv::resize(pyr1[i - 1], img1_pyr,
     
    
   
    
     
    
    
     
       cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
     
    
   
    
     
    
    
     
       cv::resize(pyr2[i - 1], img2_pyr,
     
    
   
    
     
    
    
     
       cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));
     
    
   
    
     
    
    
     
       pyr1.push_back(img1_pyr);
     
    
   
    
     
    
    
     
       pyr2.push_back(img2_pyr);
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      double fxG = fx, fyG = fy, cxG = cx, cyG = cy;  // backup the old values
     
    
   
    
     
    
    
      for (int level = pyramids - 1; level >= 0; level--) {
     
    
   
    
     
    
    
     
       VecVector2d px_ref_pyr; // set the keypoints in this pyramid level
     
    
   
    
     
    
    
      for (auto &px: px_ref) {
     
    
   
    
     
    
    
     
       px_ref_pyr.push_back(scales[level] * px);
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      // scale fx, fy, cx, cy in different pyramid levels
     
    
   
    
     
    
    
     
       fx = fxG * scales[level];
     
    
   
    
     
    
    
     
       fy = fyG * scales[level];
     
    
   
    
     
    
    
     
       cx = cxG * scales[level];
     
    
   
    
     
    
    
     
       cy = cyG * scales[level];
     
    
   
    
     
    
    
     
       DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
     
    
   
    
     
    
    
     
       }
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      }

第九章和第十章：后端 1+2

编译示例，不会遇到问题。

第十一章：回环检测

需要先编译第三方功能包：DBoW3。

然后再编译时候，可能出错，信息如下：


  
   
    
     
    
    
     
      ROS:~/SLAM/slambook2/ch11/build$ make
     
    
   
    
     
    
    
     
      Scanning dependencies of target gen_vocab
     
    
   
    
     
    
    
     
      [ 16%] Building CXX object CMakeFiles/gen_vocab.dir/gen_vocab_large.cpp.o
     
    
   
    
     
    
    
     
      make[2]: *** No rule to make target '/usr/local/lib/libDBoW3.a', needed by 'gen_vocab'.  Stop.
     
    
   
    
     
    
    
     
      CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/gen_vocab.dir/all' failed
     
    
   
    
     
    
    
     
      make[1]: *** [CMakeFiles/gen_vocab.dir/all] Error 2
     
    
   
    
     
    
    
     
      Makefile:83: recipe for target 'all' failed
     
    
   
    
     
    
    
     
      make: *** [all] Error 2

原因：

看一下~/slambook2/ch11/CMakeLists.txt，发现如下：


  
   
    
     
    
    
     
      # dbow3 
     
    
   
    
     
    
    
     
      # dbow3 is a simple lib so I assume you installed it in default directory 
     
    
   
    
     
    
    
     
      set( DBoW3_INCLUDE_DIRS "/usr/local/include" )
     
    
   
    
     
    
    
     
      set( DBoW3_LIBS "/usr/local/lib/libDBoW3.a" )

实际是：


  
   
    
     
    
    
     
      ROS:~/SLAM/slambook2/3rdparty/DBoW3/build$ sudo make install
     
    
   
    
     
    
    
     
      [sudo] password for relaybot: 
     
    
   
    
     
    
    
     
      [ 60%] Built target DBoW3
     
    
   
    
     
    
    
     
      [ 73%] Built target create_voc_step0
     
    
   
    
     
    
    
     
      [ 86%] Built target demo_general
     
    
   
    
     
    
    
     
      [100%] Built target create_voc_step1
     
    
   
    
     
    
    
     
      Install the project...
     
    
   
    
     
    
    
     
      -- Install configuration: "Release"
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/lib/cmake/FindDBoW3.cmake
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/lib/cmake/DBoW3/DBoW3Config.cmake
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/lib/libDBoW3.so.0.0.1
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/lib/libDBoW3.so.0.0
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/lib/libDBoW3.so
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/include/DBoW3/BowVector.h
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/include/DBoW3/DBoW3.h
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/include/DBoW3/Database.h
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/include/DBoW3/DescManip.h
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/include/DBoW3/FeatureVector.h
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/include/DBoW3/QueryResults.h
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/include/DBoW3/ScoringObject.h
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/include/DBoW3/Vocabulary.h
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/include/DBoW3/exports.h
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/include/DBoW3/quicklz.h
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/include/DBoW3/timers.h
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/bin/demo_general
     
    
   
    
     
    
    
     
      -- Set runtime path of "/usr/local/bin/demo_general" to ""
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/bin/create_voc_step0
     
    
   
    
     
    
    
     
      -- Set runtime path of "/usr/local/bin/create_voc_step0" to ""
     
    
   
    
     
    
    
     
      -- Installing: /usr/local/bin/create_voc_step1
     
    
   
    
     
    
    
     
      -- Set runtime path of "/usr/local/bin/create_voc_step1" to ""

将/usr/local/lib/libDBoW3.a改为/usr/local/lib/libDBoW3.so！！！

然后就一切ok。

第十二章：建图

正常编译，一切ok！

第十三章：实践：设计SLAM系统

需要先编译3rdparty/googletest，否则会报如下错误：


  
   
    
     
    
    
     
      ROS:~/SLAM/slambook2/ch13/build$ cmake ..
     
    
   
    
     
    
    
     
      -- The C compiler identification is GNU 7.4.0
     
    
   
    
     
    
    
     
      -- The CXX compiler identification is GNU 7.4.0
     
    
   
    
     
    
    
     
      -- Check for working C compiler: /usr/bin/cc
     
    
   
    
     
    
    
     
      -- Check for working C compiler: /usr/bin/cc -- works
     
    
   
    
     
    
    
     
      -- Detecting C compiler ABI info
     
    
   
    
     
    
    
     
      -- Detecting C compiler ABI info - done
     
    
   
    
     
    
    
     
      -- Detecting C compile features
     
    
   
    
     
    
    
     
      -- Detecting C compile features - done
     
    
   
    
     
    
    
     
      -- Check for working CXX compiler: /usr/bin/c++
     
    
   
    
     
    
    
     
      -- Check for working CXX compiler: /usr/bin/c++ -- works
     
    
   
    
     
    
    
     
      -- Detecting CXX compiler ABI info
     
    
   
    
     
    
    
     
      -- Detecting CXX compiler ABI info - done
     
    
   
    
     
    
    
     
      -- Detecting CXX compile features
     
    
   
    
     
    
    
     
      -- Detecting CXX compile features - done
     
    
   
    
     
    
    
     
      -- Found OpenCV: /usr (found suitable version "3.2.0", minimum required is "3.1") 
     
    
   
    
     
    
    
     
      -- Found Glog: /usr/include  
     
    
   
    
     
    
    
     
      CMake Error at /usr/share/cmake-3.10/Modules/FindPackageHandleStandardArgs.cmake:137 (message):
     
    
   
    
     
    
    
     
        Could NOT find GTest (missing: GTEST_LIBRARY GTEST_MAIN_LIBRARY)
     
    
   
    
     
    
    
     
      Call Stack (most recent call first):
     
    
   
    
     
    
    
     
        /usr/share/cmake-3.10/Modules/FindPackageHandleStandardArgs.cmake:378 (_FPHSA_FAILURE_MESSAGE)
     
    
   
    
     
    
    
     
        /usr/share/cmake-3.10/Modules/FindGTest.cmake:196 (FIND_PACKAGE_HANDLE_STANDARD_ARGS)
     
    
   
    
     
    
    
     
        CMakeLists.txt:38 (find_package)
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
      
     
    
   
    
     
    
    
     
      -- Configuring incomplete, errors occurred!
     
    
   
    
     
    
    
     
      See also "/home/relaybot/SLAM/slambook2/ch13/build/CMakeFiles/CMakeOutput.log"

安装好googletest，就一切正常了。

第十四章：SLAM：现在与未来

自学各种SLAM案例，推荐一个网址：OpenSLAM！

附录A和附录B为数学基础，必须掌握

附录C～ROS入门：参考之前一篇博文如下：

沉迷机器人操作系统的一个理由和四种修仙秘籍

这只是将全书案例在自己电脑上复现的过程，重点是：

SLAM理论和实践！！！

每章具体备课内容，在开课前更新。

文章来源: zhangrelay.blog.csdn.net，作者：zhangrelay，版权归原作者所有，如需转载，请联系作者。

原文链接：zhangrelay.blog.csdn.net/article/details/103169383

点赞
收藏
关注作者

0/1000

抱歉，系统识别当前为高风险访问，暂不支持该操作

全部回复

上滑加载中

设置昵称

在此一键设置昵称，即可参与社区互动！

*长度不超过10个汉字或20个英文字符，设置后3个月内不可修改。

确认取消

加入云驻计划，成为创作者

华为云周边好礼
免费体验产品
特殊身份标识
线下官方门票
内部专家零距离
与10000+优质创作者共同成长

立即加入

视觉SLAM十四讲从理论到实践第二版源码调试笔记（实践应用7-14章）

第七章和第八章：视觉里程计 1+2

第九章和第十章：后端 1+2

第十一章：回环检测

第十二章：建图

第十三章：实践：设计SLAM系统

第十四章：SLAM：现在与未来

SLAM理论和实践！！！

SLAM理论和实践！！！

SLAM理论和实践！！！

全部回复

设置昵称

关于作者

目录

加入云驻计划，成为创作者

视觉SLAM十四讲从理论到实践第二版源码调试笔记（实践应用7-14章）

第七章和第八章：视觉里程计 1+2

第九章和第十章：后端 1+2

第十一章：回环检测

第十二章：建图

第十三章：实践：设计SLAM系统

第十四章：SLAM：现在与未来

SLAM理论和实践！！！

SLAM理论和实践！！！

SLAM理论和实践！！！

全部回复

设置昵称

关于作者

目录

热门推荐查看更多

相关文章

加入云驻计划，成为创作者

相关产品