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,83 @@
+// 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_SPARSE_IMG_ALIGN_H_
+#define SVO_SPARSE_IMG_ALIGN_H_
+
+#include <vikit/nlls_solver.h>
+#include <vikit/performance_monitor.h>
+#include <gpu_svo/global.h>
+
+namespace vk {
+class AbstractCamera;
+}
+
+namespace svo {
+
+class Feature;
+
+/// Optimize the pose of the frame by minimizing the photometric error of feature patches.
+class SparseImgAlign : public vk::NLLSSolver<6, SE3>
+{
+  static const int patch_halfsize_ = 2;
+  static const int patch_size_ = 2*patch_halfsize_;
+  static const int patch_area_ = patch_size_*patch_size_;
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  cv::Mat resimg_;
+
+  SparseImgAlign(
+      int n_levels,
+      int min_level,
+      int n_iter,
+      Method method,
+      bool display,
+      bool verbose);
+
+  size_t run(
+      FramePtr ref_frame,
+      FramePtr cur_frame);
+
+  /// Return fisher information matrix, i.e. the Hessian of the log-likelihood
+  /// at the converged state.
+  Matrix<double, 6, 6> getFisherInformation();
+
+protected:
+  FramePtr ref_frame_;            //!< reference frame, has depth for gradient pixels.
+  FramePtr cur_frame_;            //!< only the image is known!
+  int level_;                     //!< current pyramid level on which the optimization runs.
+  bool display_;                  //!< display residual image.
+  int max_level_;                 //!< coarsest pyramid level for the alignment.
+  int min_level_;                 //!< finest pyramid level for the alignment.
+
+  // cache:
+  Matrix<double, 6, Dynamic, ColMajor> jacobian_cache_;
+  bool have_ref_patch_cache_;
+  cv::Mat ref_patch_cache_;
+  std::vector<bool> visible_fts_;
+
+  void precomputeReferencePatches();
+  virtual double computeResiduals(const SE3& model, bool linearize_system, bool compute_weight_scale = false);
+  virtual int solve();
+  virtual void update (const ModelType& old_model, ModelType& new_model);
+  virtual void startIteration();
+  virtual void finishIteration();
+};
+
+} // namespace svo
+
+#endif // SVO_SPARSE_IMG_ALIGN_H_