mrpt_rbpf_slam: mrpt_rbpf_slam.h Source File

Go to the documentation of this file.
00001 /*
00002  *  File: mrpt_rbpf_slam.h
00003  *  Author: Vladislav Tananaev
00004  */
00005 
00006 #pragma once
00007 #include <ros/console.h>
00008 
00009 #include <mrpt/gui/CDisplayWindow3D.h>
00010 #include <mrpt/random.h>
00011 #include <mrpt/slam/CMetricMapBuilderRBPF.h>
00012 #if MRPT_VERSION >= 0x199
00013 #include <mrpt/config/CConfigFile.h>
00014 #include <mrpt/io/CFileGZInputStream.h>
00015 #include <mrpt/io/CFileGZOutputStream.h>
00016 #else
00017 #include <mrpt/utils/CConfigFile.h>
00018 #include <mrpt/utils/CFileGZInputStream.h>
00019 #include <mrpt/utils/CFileGZOutputStream.h>
00020 #endif
00021 #include <mrpt/opengl/CEllipsoid.h>
00022 #include <mrpt/opengl/CGridPlaneXY.h>
00023 #include <mrpt/opengl/CSetOfLines.h>
00024 #include <mrpt/opengl/stock_objects.h>
00025 #include <mrpt/poses/CPose3DPDF.h>
00026 #include <mrpt/poses/CPosePDFGaussian.h>
00027 #include <mrpt/system/filesystem.h>
00028 #include <mrpt/system/os.h>
00029 #include <mrpt/gui/CDisplayWindow3D.h>
00030 #include <mrpt/version.h>
00031 #include <mrpt/maps/CMultiMetricMap.h>
00032 #include <mrpt/obs/CActionCollection.h>
00033 #include <mrpt/obs/CActionRobotMovement2D.h>
00034 #include <mrpt/obs/CActionRobotMovement3D.h>
00035 #include <mrpt/obs/CObservationBeaconRanges.h>
00036 #include <mrpt/obs/CObservationBearingRange.h>
00037 #include <mrpt/obs/CObservationOdometry.h>
00038 #include <mrpt/obs/CRawlog.h>
00039 #include <mrpt/obs/CSensoryFrame.h>
00040 
00041 namespace mrpt_rbpf_slam
00042 {
00047 class PFslam
00048 {
00049 public:
00050   struct Options
00051   {
00052     mrpt::obs::CActionRobotMovement2D::TMotionModelOptions motion_model_options_;  
00053 
00054     mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions rbpfMappingOptions_;   
00055     bool CAMERA_3DSCENE_FOLLOWS_ROBOT_;
00056     bool SHOW_PROGRESS_IN_WINDOW_;
00057     int SHOW_PROGRESS_IN_WINDOW_DELAY_MS_;
00058     int PROGRESS_WINDOW_WIDTH_, PROGRESS_WINDOW_HEIGHT_;
00059     std::string simplemap_path_prefix;
00060   } options_;
00061 
00062   PFslam() = default;
00063   virtual ~PFslam();
00064 
00065   void init3Dwindow();
00066 
00067   void run3Dwindow();
00068 
00074   void readIniFile(const std::string& ini_filename);
00075 
00079   void initSlam(Options options);
00080 
00087   void readRawlog(const std::string& rawlog_filename,
00088                   std::vector<std::pair<mrpt::obs::CActionCollection, mrpt::obs::CSensoryFrame>>& data);
00089 
00096   void observation(const mrpt::obs::CSensoryFrame::ConstPtr sensory_frame,
00097                    const mrpt::obs::CObservationOdometry::ConstPtr odometry);
00098 
00099 protected:
00100   mrpt::slam::CMetricMapBuilderRBPF mapBuilder_;  
00101   mrpt::obs::CActionCollection::Ptr action_;      
00102   mrpt::obs::CSensoryFrame::Ptr sensory_frame_;   
00103 
00104   mrpt::poses::CPose2D odomLastObservation_;  
00105   bool use_motion_model_default_options_;     
00106   mrpt::system::TTimeStamp timeLastUpdate_;   
00107 
00108   const mrpt::maps::CMultiMetricMap* metric_map_;  
00109   mrpt::poses::CPose3DPDFParticles curPDF;         
00110 
00111   mrpt::gui::CDisplayWindow3D::Ptr win3D_;  
00112 };
00113 }  // namespace mrpt_rbpf_slam