mrpt_ekf_slam_3d: 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/CRangeBearingKFSLAM.h>
00016 #include <mrpt/math/ops_containers.h>
00017 #include <mrpt/opengl/CGridPlaneXY.h>
00018 #include <mrpt/opengl/CSetOfLines.h>
00019 #include <mrpt/opengl/stock_objects.h>
00020 #include <mrpt/gui/CDisplayWindow3D.h>
00021 
00022 #include <mrpt/version.h>
00023 #if MRPT_VERSION >= 0x130
00024 #include <mrpt/obs/CActionRobotMovement2D.h>
00025 #include <mrpt/obs/CActionRobotMovement3D.h>
00026 #include <mrpt/obs/CActionCollection.h>
00027 #include <mrpt/obs/CObservationOdometry.h>
00028 #include <mrpt/obs/CSensoryFrame.h>
00029 #include <mrpt/maps/CMultiMetricMap.h>
00030 #include <mrpt/obs/CObservationBearingRange.h>
00031 #include <mrpt/obs/CRawlog.h>
00032 using namespace mrpt::maps;
00033 using namespace mrpt::obs;
00034 #else
00035 #include <mrpt/slam/CActionRobotMovement2D.h>
00036 #include <mrpt/slam/CActionRobotMovement3D.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 using namespace mrpt;
00045 using namespace mrpt::slam;
00046 using namespace mrpt::opengl;
00047 using namespace mrpt::system;
00048 using namespace mrpt::math;
00049 using namespace mrpt::poses;
00050 using namespace mrpt::utils;
00051 using namespace std;
00052 
00057 class EKFslam
00058 {
00059 public:
00063   EKFslam();
00067   virtual ~EKFslam();
00071   void init3Dwindow();
00075   void run3Dwindow();
00079   void landmark_to_3d(const CRangeBearingKFSLAM::KFArray_FEAT &lm, TPoint3D &p);
00085   void read_iniFile(std::string ini_filename);
00092   void observation(CSensoryFrame::Ptr _sf, CObservationOdometry::Ptr _odometry);
00093 
00094 protected:
00095   CRangeBearingKFSLAM mapping;  
00096 
00097   mrpt::system::TTimeStamp timeLastUpdate_;  
00098 
00099   CActionCollection::Ptr action;  
00100   CSensoryFrame::Ptr sf;          
00101 
00102   mrpt::poses::CPose3D odomLastObservation_;  
00103 #if MRPT_VERSION >= 0x150
00104   CActionRobotMovement3D::TMotionModelOptions motion_model_options_;  
00105 #endif
00106 
00107   mrpt::gui::CDisplayWindow3D::Ptr win3d;  
00108   bool SHOW_3D_LIVE;
00109   bool CAMERA_3DSCENE_FOLLOWS_ROBOT;
00110   vector<TPose3D> meanPath;
00111   CPose3DQuatPDFGaussian robotPose_;                       
00112   std::vector<mrpt::math::TPoint3D> LMs_;                  
00113   std::map<unsigned int, CLandmark::TLandmarkID> LM_IDs_;  
00114   CMatrixDouble fullCov_;                                  
00115   CVectorDouble fullState_;                                
00116 };
00117