mrpt_ekf_slam_2d: mrpt_ekf_slam

Go to the documentation of this file.
00001 /*
00002  *  File:mrpt_ekf_slam_3d.h
00003  *  Author: Vladislav Tananaev
00004  *
00005  */
00006 
00007 #ifndef MRPT_EKF_SLAM_2D_H
00008 #define MRPT_EKF_SLAM_2D_H
00009 
00010 #include <mrpt/utils/CConfigFile.h>
00011 #include <mrpt/utils/CFileGZInputStream.h>
00012 #include <mrpt/utils/CFileGZOutputStream.h>
00013 #include <mrpt/system/os.h>
00014 #include <mrpt/system/string_utils.h>
00015 #include <mrpt/system/filesystem.h>
00016 #include <mrpt/slam/CRangeBearingKFSLAM2D.h>
00017 
00018 #include <mrpt/math/ops_containers.h>
00019 #include <mrpt/opengl/CGridPlaneXY.h>
00020 #include <mrpt/opengl/CSetOfLines.h>
00021 #include <mrpt/opengl/stock_objects.h>
00022 
00023 #include <mrpt/gui/CDisplayWindow3D.h>
00024 
00025 #include <mrpt/version.h>
00026 #if MRPT_VERSION >= 0x130
00027 #include <mrpt/obs/CActionRobotMovement2D.h>
00028 #include <mrpt/obs/CActionCollection.h>
00029 #include <mrpt/obs/CObservationOdometry.h>
00030 #include <mrpt/obs/CSensoryFrame.h>
00031 #include <mrpt/maps/CMultiMetricMap.h>
00032 #include <mrpt/obs/CObservationBearingRange.h>
00033 #include <mrpt/obs/CRawlog.h>
00034 using namespace mrpt::maps;
00035 using namespace mrpt::obs;
00036 #else
00037 #include <mrpt/slam/CActionRobotMovement2D.h>
00038 #include <mrpt/slam/CActionCollection.h>
00039 #include <mrpt/slam/CObservationOdometry.h>
00040 #include <mrpt/slam/CSensoryFrame.h>
00041 #include <mrpt/slam/CMultiMetricMap.h>
00042 #include <mrpt/slam/CObservationBearingRange.h>
00043 #include <mrpt/slam/CRawlog.h>
00044 #endif
00045 
00046 using namespace mrpt;
00047 using namespace mrpt::slam;
00048 using namespace mrpt::opengl;
00049 using namespace mrpt::system;
00050 using namespace mrpt::math;
00051 using namespace mrpt::poses;
00052 using namespace mrpt::utils;
00053 using namespace std;
00054 using namespace mrpt::gui;
00059 class EKFslam
00060 {
00061 public:
00065   EKFslam();
00069   virtual ~EKFslam();
00073   void init3Dwindow();
00077   void run3Dwindow();
00081   void landmark_to_3d(const CRangeBearingKFSLAM2D::KFArray_FEAT &lm, TPoint3D &p);
00087   void read_iniFile(std::string ini_filename);
00094   void observation(CSensoryFramePtr _sf, CObservationOdometryPtr _odometry);
00095 
00096 protected:
00097   CRangeBearingKFSLAM2D mapping;  
00098 
00099   mrpt::system::TTimeStamp timeLastUpdate_;  
00100 
00101   CActionCollectionPtr action;  
00102   CSensoryFramePtr sf;          
00103 
00104   mrpt::poses::CPose2D odomLastObservation_;                                  
00105   bool use_motion_model_default_options_;                                     
00106   CActionRobotMovement2D::TMotionModelOptions motion_model_default_options_;  
00107   CActionRobotMovement2D::TMotionModelOptions motion_model_options_;          
00108 
00109   CPosePDFGaussian robotPose_;                             
00110   std::vector<mrpt::math::TPoint2D> LMs_;                  
00111   std::map<unsigned int, CLandmark::TLandmarkID> LM_IDs_;  
00112   CMatrixDouble fullCov_;                                  
00113   CVectorDouble fullState_;                                
00114 
00115   mrpt::gui::CDisplayWindow3DPtr win3d;  
00116   bool SHOW_3D_LIVE;
00117   bool CAMERA_3DSCENE_FOLLOWS_ROBOT;
00118   vector<TPose3D> meanPath;
00119 };
00120 
00121 #endif /* MRPT_EKF_SLAM_2D_H */