svo: include/svo/initialization.h Source File

Go to the documentation of this file.
00001 // This file is part of SVO - Semi-direct Visual Odometry.
00002 //
00003 // Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
00004 // (Robotics and Perception Group, University of Zurich, Switzerland).
00005 //
00006 // SVO is free software: you can redistribute it and/or modify it under the
00007 // terms of the GNU General Public License as published by the Free Software
00008 // Foundation, either version 3 of the License, or any later version.
00009 //
00010 // SVO is distributed in the hope that it will be useful, but WITHOUT ANY
00011 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00012 // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
00013 //
00014 // You should have received a copy of the GNU General Public License
00015 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016 
00017 #ifndef SVO_INITIALIZATION_H
00018 #define SVO_INITIALIZATION_H
00019 
00020 #include <svo/global.h>
00021 
00022 namespace svo {
00023 
00024 class FrameHandlerMono;
00025 
00027 namespace initialization {
00028 
00029 enum InitResult { FAILURE, NO_KEYFRAME, SUCCESS };
00030 
00032 class KltHomographyInit {
00033   friend class svo::FrameHandlerMono;
00034 public:
00035   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00036 
00037   FramePtr frame_ref_;
00038   KltHomographyInit() {};
00039   ~KltHomographyInit() {};
00040   InitResult addFirstFrame(FramePtr frame_ref);
00041   InitResult addSecondFrame(FramePtr frame_ref);
00042   void reset();
00043 
00044 protected:
00045   vector<cv::Point2f> px_ref_;      
00046   vector<cv::Point2f> px_cur_;      
00047   vector<Vector3d> f_ref_;          
00048   vector<Vector3d> f_cur_;          
00049   vector<double> disparities_;      
00050   vector<int> inliers_;             
00051   vector<Vector3d> xyz_in_cur_;     
00052   SE3 T_cur_from_ref_;              
00053 };
00054 
00056 void detectFeatures(
00057     FramePtr frame,
00058     vector<cv::Point2f>& px_vec,
00059     vector<Vector3d>& f_vec);
00060 
00062 void trackKlt(
00063     FramePtr frame_ref,
00064     FramePtr frame_cur,
00065     vector<cv::Point2f>& px_ref,
00066     vector<cv::Point2f>& px_cur,
00067     vector<Vector3d>& f_ref,
00068     vector<Vector3d>& f_cur,
00069     vector<double>& disparities);
00070 
00071 void computeHomography(
00072     const vector<Vector3d>& f_ref,
00073     const vector<Vector3d>& f_cur,
00074     double focal_length,
00075     double reprojection_threshold,
00076     vector<int>& inliers,
00077     vector<Vector3d>& xyz_in_cur,
00078     SE3& T_cur_from_ref);
00079 
00080 } // namespace initialization
00081 } // namespace nslam
00082 
00083 #endif // NSLAM_INITIALIZATION_H