add banchmark image alignment 2

2021-08-23 15:35:30 +02:00
parent 8df5e42a5b
commit 90e452f0c9
34 changed files with 3294 additions and 0 deletions
@@ -0,0 +1,75 @@
+// 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 VIO_SPARSE_IMG_ALIGN_GPU_H_
+#define VIO_SPARSE_IMG_ALIGN_GPU_H_
+
+#include <nlls_solver.h>
+#include <cl_class.h>
+#include <math_utils.h>
+#include <algorithm>
+#include <sophus/se2.h>
+
+namespace vio {
+
+class Feature;
+
+/// Optimize the pose of the frame by minimizing the photometric error of feature patches.
+class SparseImgAlignGpu : public vk::NLLSSolver<3, Sophus::SE2>
+{
+  static const int patch_halfsize_ = 12;
+  static const int patch_size_ = 12*patch_halfsize_;
+  static const int patch_area_ = patch_size_*patch_size_;
+public:
+
+  SparseImgAlignGpu(
+      int max_level,
+      int min_level,
+      int n_iter,
+      Method method,
+      bool verbose,
+      opencl* residual);
+
+  size_t run();
+  void data_reader(std::string filename, float *rp, float *cp, float *c, float *fts, float* fts_px);
+  int feature_counter(std::string filename);
+   // FILE* data= nullptr;
+
+protected:
+  int level_;                     //!< current pyramid level on which the optimization runs.
+  int max_level_;                 //!< coarsest pyramid level for the alignment.
+  int min_level_;                 //!< finest pyramid level for the alignment.
+  size_t feature_counter_=0;
+  opencl* residual_= nullptr;
+
+  // cache:
+  std::vector<bool> visible_fts_;
+  virtual double computeResiduals(const ModelType& model,
+                           bool linearize_system,
+                           bool compute_weight_scale) {
+        return 0.0;
+    };
+  virtual double computeResiduals(bool linearize_system, bool compute_weight_scale = false);
+  virtual int solve();
+  virtual void update();
+  virtual void update(const ModelType& old_model, ModelType& new_model) { };
+  virtual void startIteration();
+  virtual void finishIteration();
+};
+
+} // namespace vio
+
+#endif // VIO_SPARSE_IMG_ALIGN_GPU_H_