107 lines
4.2 KiB
C++
107 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_FRAME_HANDLER_H_
|
|
#define SVO_FRAME_HANDLER_H_
|
|
|
|
#include <set>
|
|
#include <gpu_svo/abstract_camera.h>
|
|
#include <gpu_svo/frame_handler_base.h>
|
|
#include <gpu_svo/reprojector.h>
|
|
#include <gpu_svo/initialization.h>
|
|
#include <gpu_svo/imu_integration.h>
|
|
#include <gpu_svo/cl_class.h>
|
|
|
|
namespace svo {
|
|
|
|
/// Monocular Visual Odometry Pipeline as described in the SVO paper.
|
|
class FrameHandlerMono : public FrameHandlerBase
|
|
{
|
|
public:
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
|
|
|
FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init);
|
|
virtual ~FrameHandlerMono();
|
|
|
|
/// Provide an image.
|
|
void addImage(const cv::Mat& img, double timestamp);
|
|
|
|
/// Set the first frame (used for synthetic datasets in benchmark node)
|
|
void setFirstFrame(const FramePtr& first_frame);
|
|
|
|
/// Get the last frame that has been processed.
|
|
FramePtr lastFrame() { return last_frame_; }
|
|
|
|
/// Get the set of spatially closest keyframes of the last frame.
|
|
const set<FramePtr>& coreKeyframes() { return core_kfs_; }
|
|
|
|
/// Return the feature track to visualize the KLT tracking during initialization.
|
|
const vector<cv::Point2f>& initFeatureTrackRefPx() const { return klt_homography_init_->px_ref_; }
|
|
const vector<cv::Point2f>& initFeatureTrackCurPx() const { return klt_homography_init_->px_cur_; }
|
|
|
|
/// Access the depth filter.
|
|
DepthFilter* depthFilter() const { return depth_filter_; }
|
|
|
|
/// An external place recognition module may know where to relocalize.
|
|
bool relocalizeFrameAtPose(
|
|
const int keyframe_id,
|
|
const SE3& T_kf_f,
|
|
const cv::Mat& img,
|
|
const double timestamp);
|
|
std::shared_ptr<Imu_Integration> imu_integPtr_; //Integration of IMU
|
|
|
|
protected:
|
|
vk::AbstractCamera* cam_; //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit).
|
|
Reprojector reprojector_; //!< Projects points from other keyframes into the current frame
|
|
FramePtr new_frame_; //!< Current frame.
|
|
FramePtr last_frame_; //!< Last frame, not necessarily a keyframe.
|
|
set<FramePtr> core_kfs_; //!< Keyframes in the closer neighbourhood.
|
|
vector< pair<FramePtr,size_t> > overlap_kfs_; //!< All keyframes with overlapping field of view. the paired number specifies how many common mappoints are observed TODO: why vector!?
|
|
initialization::KltHomographyInit* klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography.
|
|
DepthFilter* depth_filter_; //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.
|
|
opencl* gpu_fast_;
|
|
uint image_count_=0;
|
|
|
|
/// Initialize the visual odometry algorithm.
|
|
virtual void initialize();
|
|
|
|
/// Processes the first frame and sets it as a keyframe.
|
|
virtual UpdateResult processFirstFrame();
|
|
|
|
/// Processes all frames after the first frame until a keyframe is selected.
|
|
virtual UpdateResult processSecondFrame();
|
|
|
|
/// Processes all frames after the first two keyframes.
|
|
virtual UpdateResult processFrame();
|
|
|
|
/// Try relocalizing the frame at relative position to provided keyframe.
|
|
virtual UpdateResult relocalizeFrame(
|
|
const SE3& T_cur_ref,
|
|
FramePtr ref_keyframe);
|
|
|
|
/// Reset the frame handler. Implement in derived class.
|
|
virtual void resetAll();
|
|
|
|
/// Keyframe selection criterion.
|
|
virtual bool needNewKf(double scene_depth_mean);
|
|
|
|
void setCoreKfs(size_t n_closest);
|
|
};
|
|
|
|
} // namespace svo
|
|
|
|
#endif // SVO_FRAME_HANDLER_H_
|