vslam.h
Go to the documentation of this file.
00001 #ifndef VSLAM_SYSTEM_VSLAM_H
00002 #define VSLAM_SYSTEM_VSLAM_H
00003 
00004 #include <vslam_system/vo.h>
00005 #include <vslam_system/place_recognizer.h>
00006 #include <posest/pe3d.h>
00007 #include <posest/peh.h>
00008 #include <sba/sba.h>
00009 #include <frame_common/frame.h>
00010 
00011 namespace vslam {
00012 
00015 class VslamSystem
00016 {
00017   public: 
00018     frame_common::FrameProc frame_processor_; 
00019 
00020     std::vector<frame_common::Frame, Eigen::aligned_allocator<frame_common::Frame> > frames_;
00021     sba::SysSBA sba_;    
00022     vslam::voSt vo_;   
00023     vslam::PlaceRecognizer place_recognizer_; 
00024 #ifndef HOWARD
00025     pe::PoseEstimator3d pose_estimator_;      
00026 #else
00027     pe::PoseEstimatorH pose_estimator_;
00028 #endif
00029     
00031     boost::shared_ptr<frame_common::PointcloudProc> pointcloud_processor_;
00033     std::vector<cv::DMatch> pointcloud_matches_;
00034     bool doPointPlane;         // true if we want to process point clouds
00035 
00036   public:
00043     VslamSystem(const std::string& vocab_tree_file, const std::string& vocab_weights_file,
00044                 int min_keyframe_inliers=0, double min_keyframe_distance=0.2, 
00045                 double min_keyframe_angle=0.1);
00046 
00054     bool addFrame(const frame_common::CamParams& camera_parameters,
00055                   const cv::Mat& left, const cv::Mat& right, int nfrac = 0,
00056                   bool setPointCloud = false);
00057     
00065     bool addFrame(const frame_common::CamParams& camera_parameters,
00066              const cv::Mat& left, const cv::Mat& right, 
00067              const pcl::PointCloud<pcl::PointXYZRGB>& ptcloud, int nfrac = 0);
00068              
00069     void addKeyframe(frame_common::Frame& next_frame);
00070 
00073     void refine(int initial_runs=3);
00074 
00075     int prInliers;  
00076     int numPRs;                 
00077     int nSkip;      
00078 
00079     // parameters settings
00080     void setPlaceInliers(int n) { prInliers = n; }; 
00081     void setPRSkip(int n) { nSkip = n; };           
00082     void setKeyInliers(int n) { vo_.mininls = n; }; 
00083     void setKeyDist(double n) { vo_.mindist = n; }; 
00084     void setKeyAngle(double n) { vo_.minang = n; }; 
00085     void setNDisp(int n) { frame_processor_.ndisp = n; }; 
00086     void setPRRansacIt(int n) { pose_estimator_.numRansac = n; }; 
00087     void setPRPolish(bool n) { pose_estimator_.polish = n; }; 
00088     void setVORansacIt(int n) { vo_.pose_estimator_->numRansac = n; }; 
00089     void setVOPolish(bool n) { vo_.pose_estimator_->polish = n; }; 
00090     void setPRWindow(int x, int y) { pose_estimator_.wx = x; pose_estimator_.wy = y; }; 
00091     void setVOWindow(int x, int y) { vo_.pose_estimator_->wx = x; vo_.pose_estimator_->wy = y; }; 
00092     void setHuber(double x) { sba_.huber = x; vo_.sba.huber = x; }
00093     
00094     
00095     
00097     void setPointcloudProc(boost::shared_ptr<frame_common::PointcloudProc> proc)
00098     {
00099       pointcloud_processor_ = proc;
00100       vo_.pointcloud_proc_ = proc;
00101     }
00102 };
00103 
00104 } // namespace vslam
00105 
00106 #endif


vslam_system
Author(s): Kurt Konolige, Patrick Mihelich, Helen Oleynikova
autogenerated on Thu Jan 2 2014 12:12:32