include/svo/point.h

Go to the documentation of this file.

// 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 <svo/global.h>

namespace g2o {
  class VertexSBAPointXYZ;
}
typedef g2o::VertexSBAPointXYZ g2oPoint;

namespace svo {

class Feature;

typedef Matrix<double, 2, 3> Matrix23d;

class Point : boost::noncopyable
{
public:

  enum PointType {
    TYPE_DELETED,
    TYPE_CANDIDATE,
    TYPE_UNKNOWN,
    TYPE_GOOD
  };

  static int                  point_counter_;           
  int                         id_;                      
  Vector3d                    pos_;                     
  Vector3d                    normal_;                  
  Matrix3d                    normal_information_;      
  bool                        normal_set_;              
  list<Feature*>              obs_;                     
  size_t                      n_obs_;                   
  g2oPoint*                   v_pt_;                    
  int                         last_published_ts_;       
  int                         last_projected_kf_id_;    
  PointType                   type_;                    
  int                         n_failed_reproj_;         
  int                         n_succeeded_reproj_;      
  int                         last_structure_optim_;    

  Point(const Vector3d& pos);
  Point(const Vector3d& pos, Feature* ftr);
  ~Point();

  void addFrameRef(Feature* ftr);

  bool deleteFrameRef(Frame* frame);

  void initNormal();

  Feature* findFrameRef(Frame* frame);

  bool getCloseViewObs(const Vector3d& pos, Feature*& obs) const;

  inline size_t nRefs() const { return obs_.size(); }

  void optimize(const size_t n_iter);

  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_