debug gpu version done

2021-05-08 11:53:15 +02:00
parent 5a693259d6
commit 9dbc540a5d
60 changed files with 1249 additions and 141 deletions
@@ -0,0 +1,130 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_MATCHER_H_
+#define SVO_MATCHER_H_
+
+#include <gpu_svo/global.h>
+
+namespace vk {
+  class AbstractCamera;
+  namespace patch_score {
+    template<int HALF_PATCH_SIZE> class ZMSSD;
+  }
+}
+
+namespace svo {
+
+class Point;
+class Frame;
+class Feature;
+
+/// Warp a patch from the reference view to the current view.
+namespace warp {
+
+void getWarpMatrixAffine(
+    const vk::AbstractCamera& cam_ref,
+    const vk::AbstractCamera& cam_cur,
+    const Vector2d& px_ref,
+    const Vector3d& f_ref,
+    const double depth_ref,
+    const SE3& T_cur_ref,
+    const int level_ref,
+    Matrix2d& A_cur_ref);
+
+int getBestSearchLevel(
+    const Matrix2d& A_cur_ref,
+    const int max_level);
+
+void warpAffine(
+    const Matrix2d& A_cur_ref,
+    const cv::Mat& img_ref,
+    const Vector2d& px_ref,
+    const int level_ref,
+    const int level_cur,
+    const int halfpatch_size,
+    uint8_t* patch);
+
+} // namespace warp
+
+/// Patch-matcher for reprojection-matching and epipolar search in triangulation.
+class Matcher
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  static const int halfpatch_size_ = 4;
+  static const int patch_size_ = 8;
+
+  typedef vk::patch_score::ZMSSD<halfpatch_size_> PatchScore;
+
+  struct Options
+  {
+    bool align_1d;              //!< in epipolar search: align patch 1D along epipolar line
+    int align_max_iter;         //!< number of iterations for aligning the feature patches in gauss newton
+    double max_epi_length_optim;//!< max length of epipolar line to skip epipolar search and directly go to img align
+    size_t max_epi_search_steps;//!< max number of evaluations along epipolar line
+    bool subpix_refinement;     //!< do gauss newton feature patch alignment after epipolar search
+    bool epi_search_edgelet_filtering;
+    double epi_search_edgelet_max_angle;
+    Options() :
+      align_1d(false),
+      align_max_iter(10),
+      max_epi_length_optim(2.0),
+      max_epi_search_steps(1000),
+      subpix_refinement(true),
+      epi_search_edgelet_filtering(true),
+      epi_search_edgelet_max_angle(0.7)
+    {}
+  } options_;
+
+  uint8_t patch_[patch_size_*patch_size_] __attribute__ ((aligned (16)));
+  uint8_t patch_with_border_[(patch_size_+2)*(patch_size_+2)] __attribute__ ((aligned (16)));
+  Matrix2d A_cur_ref_;          //!< affine warp matrix
+  Vector2d epi_dir_;
+  double epi_length_;           //!< length of epipolar line segment in pixels (only used for epipolar search)
+  double h_inv_;                //!< hessian of 1d image alignment along epipolar line
+  int search_level_;
+  bool reject_;
+  Feature* ref_ftr_;
+  Vector2d px_cur_;
+
+  Matcher() = default;
+  ~Matcher() = default;
+
+  /// Find a match by directly applying subpix refinement.
+  /// IMPORTANT! This function assumes that px_cur is already set to an estimate that is within ~2-3 pixel of the final result!
+  bool findMatchDirect(
+      const Point& pt,
+      const Frame& frame,
+      Vector2d& px_cur);
+
+  /// Find a match by searching along the epipolar line without using any features.
+  bool findEpipolarMatchDirect(
+      const Frame& ref_frame,
+      const Frame& cur_frame,
+      const Feature& ref_ftr,
+      const double d_estimate,
+      const double d_min,
+      const double d_max,
+      double& depth);
+
+  void createPatchFromPatchWithBorder();
+};
+
+} // namespace svo
+
+#endif // SVO_MATCHER_H_