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 #pragma once
00008 
00009 #include <mrpt/utils/CConfigFile.h>
00010 #include <mrpt/utils/CFileGZInputStream.h>
00011 #include <mrpt/utils/CFileGZOutputStream.h>
00012 #include <mrpt/system/os.h>
00013 #include <mrpt/system/string_utils.h>
00014 #include <mrpt/system/filesystem.h>
00015 #include <mrpt/slam/CRangeBearingKFSLAM2D.h>
00016 
00017 #include <mrpt/math/ops_containers.h>
00018 #include <mrpt/opengl/CGridPlaneXY.h>
00019 #include <mrpt/opengl/CSetOfLines.h>
00020 #include <mrpt/opengl/stock_objects.h>
00021 
00022 #include <mrpt/gui/CDisplayWindow3D.h>
00023 
00024 #include <mrpt/version.h>
00025 #if MRPT_VERSION >= 0x130
00026 #include <mrpt/obs/CActionRobotMovement2D.h>
00027 #include <mrpt/obs/CActionCollection.h>
00028 #include <mrpt/obs/CObservationOdometry.h>
00029 #include <mrpt/obs/CSensoryFrame.h>
00030 #include <mrpt/maps/CMultiMetricMap.h>
00031 #include <mrpt/obs/CObservationBearingRange.h>
00032 #include <mrpt/obs/CRawlog.h>
00033 using namespace mrpt::maps;
00034 using namespace mrpt::obs;
00035 #else
00036 #include <mrpt/slam/CActionRobotMovement2D.h>
00037 #include <mrpt/slam/CActionCollection.h>
00038 #include <mrpt/slam/CObservationOdometry.h>
00039 #include <mrpt/slam/CSensoryFrame.h>
00040 #include <mrpt/slam/CMultiMetricMap.h>
00041 #include <mrpt/slam/CObservationBearingRange.h>
00042 #include <mrpt/slam/CRawlog.h>
00043 #endif
00044 
00045 using namespace mrpt;
00046 using namespace mrpt::slam;
00047 using namespace mrpt::opengl;
00048 using namespace mrpt::system;
00049 using namespace mrpt::math;
00050 using namespace mrpt::poses;
00051 using namespace mrpt::utils;
00052 using namespace std;
00053 using namespace mrpt::gui;
00058 class EKFslam
00059 {
00060 public:
00064   EKFslam();
00068   virtual ~EKFslam();
00072   void init3Dwindow();
00076   void run3Dwindow();
00080   void landmark_to_3d(const CRangeBearingKFSLAM2D::KFArray_FEAT &lm, TPoint3D &p);
00086   void read_iniFile(std::string ini_filename);
00093   void observation(CSensoryFrame::Ptr _sf, CObservationOdometry::Ptr _odometry);
00094 
00095 protected:
00096   CRangeBearingKFSLAM2D mapping;  
00097 
00098   mrpt::system::TTimeStamp timeLastUpdate_;  
00099 
00100   CActionCollection::Ptr action;  
00101   CSensoryFrame::Ptr sf;          
00102 
00103   mrpt::poses::CPose2D odomLastObservation_;                                  
00104   bool use_motion_model_default_options_;                                     
00105   CActionRobotMovement2D::TMotionModelOptions motion_model_default_options_;  
00106   CActionRobotMovement2D::TMotionModelOptions motion_model_options_;          
00107 
00108   CPosePDFGaussian robotPose_;                             
00109   std::vector<mrpt::math::TPoint2D> LMs_;                  
00110   std::map<unsigned int, CLandmark::TLandmarkID> LM_IDs_;  
00111   CMatrixDouble fullCov_;                                  
00112   CVectorDouble fullState_;                                
00113 
00114   mrpt::gui::CDisplayWindow3D::Ptr win3d;  
00115   bool SHOW_3D_LIVE;
00116   bool CAMERA_3DSCENE_FOLLOWS_ROBOT;
00117   vector<TPose3D> meanPath;
00118 };
00119