109 lines
4.2 KiB
C++
109 lines
4.2 KiB
C++
// 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_
|