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 #ifndef MRPT_EKF_SLAM_3D_H
00008 #define MRPT_EKF_SLAM_3D_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/CRangeBearingKFSLAM.h>
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 #include <mrpt/gui/CDisplayWindow3D.h>
00022 
00023 #include <mrpt/version.h>
00024 #if MRPT_VERSION >= 0x130
00025 #include <mrpt/obs/CActionRobotMovement2D.h>
00026 #include <mrpt/obs/CActionRobotMovement3D.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/CActionRobotMovement3D.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 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 
00058 class EKFslam
00059 {
00060 public:
00064   EKFslam();
00068   virtual ~EKFslam();
00072   void init3Dwindow();
00076   void run3Dwindow();
00080   void landmark_to_3d(const CRangeBearingKFSLAM::KFArray_FEAT &lm, TPoint3D &p);
00086   void read_iniFile(std::string ini_filename);
00093   void observation(CSensoryFramePtr _sf, CObservationOdometryPtr _odometry);
00094 
00095 protected:
00096   CRangeBearingKFSLAM mapping;  
00097 
00098   mrpt::system::TTimeStamp timeLastUpdate_;  
00099 
00100   CActionCollectionPtr action;  
00101   CSensoryFramePtr sf;          
00102 
00103   mrpt::poses::CPose3D odomLastObservation_;  
00104 #if MRPT_VERSION >= 0x150
00105   CActionRobotMovement3D::TMotionModelOptions motion_model_options_;  
00106 #endif
00107 
00108   mrpt::gui::CDisplayWindow3DPtr win3d;  
00109   bool SHOW_3D_LIVE;
00110   bool CAMERA_3DSCENE_FOLLOWS_ROBOT;
00111   vector<TPose3D> meanPath;
00112   CPose3DQuatPDFGaussian robotPose_;                       
00113   std::vector<mrpt::math::TPoint3D> LMs_;                  
00114   std::map<unsigned int, CLandmark::TLandmarkID> LM_IDs_;  
00115   CMatrixDouble fullCov_;                                  
00116   CVectorDouble fullState_;                                
00117 };
00118 
00119 #endif /* MRPT_EKF_SLAM_3D_H */