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,108 @@
+// 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_POINT_H_
+#define SVO_POINT_H_
+
+#include <boost/noncopyable.hpp>
+#include <gpu_svo/global.h>
+
+namespace g2o {
+  class VertexSBAPointXYZ;
+}
+typedef g2o::VertexSBAPointXYZ g2oPoint;
+
+namespace svo {
+
+class Feature;
+
+typedef Matrix<double, 2, 3> Matrix23d;
+
+/// A 3D point on the surface of the scene.
+class Point : boost::noncopyable
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  
+  enum PointType {
+    TYPE_DELETED,
+    TYPE_CANDIDATE,
+    TYPE_UNKNOWN,
+    TYPE_GOOD
+  };
+
+  static int                  point_counter_;           //!< Counts the number of created points. Used to set the unique id.
+  int                         id_;                      //!< Unique ID of the point.
+  Vector3d                    pos_;                     //!< 3d pos of the point in the world coordinate frame.
+  Vector3d                    normal_;                  //!< Surface normal at point.
+  Matrix3d                    normal_information_;      //!< Inverse covariance matrix of normal estimation.
+  bool                        normal_set_;              //!< Flag whether the surface normal was estimated or not.
+  list<Feature*>              obs_;                     //!< References to keyframes which observe the point.
+  size_t                      n_obs_;                   //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames.
+  g2oPoint*                   v_pt_;                    //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
+  int                         last_published_ts_;       //!< Timestamp of last publishing.
+  int                         last_projected_kf_id_;    //!< Flag for the reprojection: don't reproject a pt twice.
+  PointType                   type_;                    //!< Quality of the point.
+  int                         n_failed_reproj_;         //!< Number of failed reprojections. Used to assess the quality of the point.
+  int                         n_succeeded_reproj_;      //!< Number of succeeded reprojections. Used to assess the quality of the point.
+  int                         last_structure_optim_;    //!< Timestamp of last point optimization
+
+  Point(const Vector3d& pos);
+  Point(const Vector3d& pos, Feature* ftr);
+  ~Point();
+
+  /// Add a reference to a frame.
+  void addFrameRef(Feature* ftr);
+
+  /// Remove reference to a frame.
+  bool deleteFrameRef(Frame* frame);
+
+  /// Initialize point normal. The inital estimate will point towards the frame.
+  void initNormal();
+
+  /// Check whether mappoint has reference to a frame.
+  Feature* findFrameRef(Frame* frame);
+
+  /// Get Frame with similar viewpoint.
+  bool getCloseViewObs(const Vector3d& pos, Feature*& obs) const;
+
+  /// Get number of observations.
+  inline size_t nRefs() const { return obs_.size(); }
+
+  /// Optimize point position through minimizing the reprojection error.
+  void optimize(const size_t n_iter);
+
+  /// Jacobian of point projection on unit plane (focal length = 1) in frame (f).
+  inline static void jacobian_xyz2uv(
+      const Vector3d& p_in_f,
+      const Matrix3d& R_f_w,
+      Matrix23d& point_jac)
+  {
+    const double z_inv = 1.0/p_in_f[2];
+    const double z_inv_sq = z_inv*z_inv;
+    point_jac(0, 0) = z_inv;
+    point_jac(0, 1) = 0.0;
+    point_jac(0, 2) = -p_in_f[0] * z_inv_sq;
+    point_jac(1, 0) = 0.0;
+    point_jac(1, 1) = z_inv;
+    point_jac(1, 2) = -p_in_f[1] * z_inv_sq;
+    point_jac = - point_jac * R_f_w;
+  }
+};
+
+} // namespace svo
+
+#endif // SVO_POINT_H_