vslam_system: vo.h Source File

Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #ifndef VSLAM_SYSTEM_VO_H
00036 #define VSLAM_SYSTEM_VO_H
00037 
00038 //
00039 // A class for visual odometry
00040 // Moves a sliding window along a trajectory
00041 //
00042 
00043 #ifndef EIGEN_USE_NEW_STDVECTOR
00044 #define EIGEN_USE_NEW_STDVECTOR
00045 #endif // EIGEN_USE_NEW_STDVECTOR
00046 
00047 #include <posest/pe3d.h>
00048 #include <posest/pe2d.h>
00049 #include <sba/sba.h>
00050 #include <frame_common/frame.h>
00051 #include <cstdio>
00052 
00053 namespace fc  = frame_common;
00054 
00055 namespace vslam
00056 {
00060   class voSt
00061   {
00062   public:
00070     voSt(boost::shared_ptr<pe::PoseEstimator> pose_estimator, int ws=20, int wf=5, int mininls=800, double mind=0.1, double mina=0.1);
00071 
00073     sba::SysSBA sba;
00074     
00076     vector<int> ipts;
00077 
00078     int wsize; 
00079     int wfixed; 
00080 
00081     // criteria for adding a frame
00082     double mindist; 
00083     double minang;  
00084     int    mininls; 
00085 
00089     bool addFrame(const fc::Frame &fnew); 
00090 
00092     void removeFrame();
00093 
00097     void transferLatestFrame(std::vector<fc::Frame, Eigen::aligned_allocator<fc::Frame> > &eframes,
00098                              sba::SysSBA &esba);
00099 
00106     int findTransform(int frameId, int n, Vector4d &trans, Quaterniond &qrot, Matrix<double,6,6> &prec);
00107 
00109     int findNode(int frameId);
00110 
00112     std::vector<fc::Frame, Eigen::aligned_allocator<fc::Frame> > frames;
00113 
00115     boost::shared_ptr<pe::PoseEstimator> pose_estimator_;
00116     
00118     boost::shared_ptr<frame_common::PointcloudProc> pointcloud_proc_;
00119     bool doPointPlane;
00120     
00121     EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
00122   
00123   private:
00125     std::vector<cv::DMatch> pointcloud_matches_;
00126   };
00127 
00142   void addProjections(fc::Frame &f0, fc::Frame &f1, 
00143                       std::vector<fc::Frame, Eigen::aligned_allocator<fc::Frame> > &frames,
00144                       sba::SysSBA &sba, const std::vector<cv::DMatch> &inliers,
00145                       const Matrix<double,3,4>& f2w, int ndi0, int ndi1, std::vector<int>* ipts = NULL);
00146   
00161   void addPointCloudProjections(fc::Frame &f0, fc::Frame &f1, 
00162                   sba::SysSBA &sba, const std::vector<cv::DMatch> &inliers,
00163                     const Matrix<double,3,4>& f2w_frame0, 
00164                     const Matrix<double,3,4>& f2w_frame1, 
00165                     int ndi0, int ndi1, std::vector<int>* ipts = NULL);
00166   
00168   void substPointRef(std::vector<int> &ipts, int tri0, int tri1);
00169   
00171   Vector3d getProjection(fc::Frame &frame, int index);
00172 } // end namespace vslam
00173 
00174 #endif