mrpt_rbpf_slam: mrpt_rbpf_slam.h Source File

Go to the documentation of this file.
00001 /*
00002  *  File: mrpt_slam.h
00003  *  Author: Vladislav Tananaev
00004  *
00005  *
00006  */
00007 
00008 #ifndef MRPT_RBPF_SLAM_H
00009 #define MRPT_RBPF_SLAM_H
00010 
00011 #include <ros/console.h>
00012 
00013 #include <mrpt/slam/CMetricMapBuilderRBPF.h>
00014 #include <mrpt/utils/CFileGZInputStream.h>
00015 #include <mrpt/utils/CFileGZOutputStream.h>
00016 #include <mrpt/utils/CConfigFile.h>
00017 #include <mrpt/gui/CDisplayWindow3D.h>
00018 #include <mrpt/random.h>
00019 #include <mrpt/system/threads.h>
00020 #include <mrpt/system/filesystem.h>
00021 #include <mrpt/system/os.h>
00022 #include <mrpt/poses/CPosePDFGaussian.h>
00023 #include <mrpt/poses/CPose3DPDF.h>
00024 #include <mrpt/opengl/CSetOfLines.h>
00025 #include <mrpt/opengl/CGridPlaneXY.h>
00026 #include <mrpt/opengl/CEllipsoid.h>
00027 #include <mrpt/opengl/stock_objects.h>
00028 
00029 #include <mrpt/gui/CDisplayWindow3D.h>
00030 
00031 #include <mrpt/version.h>
00032 #if MRPT_VERSION >= 0x130
00033 #include <mrpt/obs/CActionRobotMovement2D.h>
00034 #include <mrpt/obs/CObservationBeaconRanges.h>
00035 #include <mrpt/obs/CActionRobotMovement3D.h>
00036 #include <mrpt/obs/CActionCollection.h>
00037 #include <mrpt/obs/CObservationOdometry.h>
00038 #include <mrpt/obs/CSensoryFrame.h>
00039 #include <mrpt/maps/CMultiMetricMap.h>
00040 #include <mrpt/obs/CObservationBearingRange.h>
00041 #include <mrpt/obs/CRawlog.h>
00042 using namespace mrpt::maps;
00043 using namespace mrpt::obs;
00044 #else
00045 #include <mrpt/slam/CActionRobotMovement2D.h>
00046 #include <mrpt/slam/CObservationBeaconRanges.h>
00047 #include <mrpt/slam/CActionRobotMovement3D.h>
00048 #include <mrpt/slam/CActionCollection.h>
00049 #include <mrpt/slam/CObservationOdometry.h>
00050 #include <mrpt/slam/CSensoryFrame.h>
00051 #include <mrpt/slam/CMultiMetricMap.h>
00052 #include <mrpt/slam/CObservationBearingRange.h>
00053 #include <mrpt/slam/CRawlog.h>
00054 #endif
00055 
00056 using namespace mrpt;
00057 using namespace mrpt::slam;
00058 using namespace mrpt::opengl;
00059 using namespace mrpt::gui;
00060 using namespace mrpt::math;
00061 using namespace mrpt::utils;
00062 using namespace mrpt::system;
00063 using namespace mrpt::random;
00064 using namespace mrpt::poses;
00065 
00070 class PFslam
00071 {
00072 public:
00076   PFslam();
00080   virtual ~PFslam();
00081 
00082   void init3Dwindow();
00083 
00084   void run3Dwindow();
00085 
00091   void read_iniFile(std::string ini_filename);
00092 
00096   void init_slam();
00097 
00104   void read_rawlog(std::vector<std::pair<CActionCollection, CSensoryFrame>>& data, std::string rawlog_filename);
00105 
00112   void observation(CSensoryFramePtr _sf, CObservationOdometryPtr _odometry);
00113 
00114 protected:
00115   CMetricMapBuilderRBPF* mapBuilder;  
00116   CActionCollectionPtr action;        
00117   CSensoryFramePtr sf;                
00118 
00119   mrpt::poses::CPose2D odomLastObservation_;                                  
00120   bool use_motion_model_default_options_;                                     
00121   CActionRobotMovement2D::TMotionModelOptions motion_model_default_options_;  
00122   CActionRobotMovement2D::TMotionModelOptions motion_model_options_;          
00123 
00124   CMetricMapBuilderRBPF::TConstructionOptions rbpfMappingOptions;  
00125   mrpt::system::TTimeStamp timeLastUpdate_;                        
00126 
00127   const CMultiMetricMap* metric_map_;  
00128   CPose3DPDFParticles curPDF;    
00129 
00130   mrpt::gui::CDisplayWindow3DPtr win3D;  
00131   bool CAMERA_3DSCENE_FOLLOWS_ROBOT;
00132   bool SHOW_PROGRESS_IN_WINDOW;
00133   int SHOW_PROGRESS_IN_WINDOW_DELAY_MS;
00134   int PROGRESS_WINDOW_WIDTH, PROGRESS_WINDOW_HEIGHT;
00135 };
00136 
00137 #endif /*MRPT_RBPF_SLAM_H*/