mrpt_rbpf_slam.h

Go to the documentation of this file.

/*
 *  File: mrpt_rbpf_slam.h
 *  Author: Vladislav Tananaev
 */

#pragma once
#include <ros/console.h>

#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/random.h>
#include <mrpt/slam/CMetricMapBuilderRBPF.h>
#if MRPT_VERSION >= 0x199
#include <mrpt/config/CConfigFile.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#else
#include <mrpt/utils/CConfigFile.h>
#include <mrpt/utils/CFileGZInputStream.h>
#include <mrpt/utils/CFileGZOutputStream.h>
#endif
#include <mrpt/opengl/CEllipsoid.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/poses/CPose3DPDF.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/version.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CActionRobotMovement3D.h>
#include <mrpt/obs/CObservationBeaconRanges.h>
#include <mrpt/obs/CObservationBearingRange.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/obs/CSensoryFrame.h>

namespace mrpt_rbpf_slam
{
class PFslam
{
public:
  struct Options
  {
    mrpt::obs::CActionRobotMovement2D::TMotionModelOptions motion_model_options_;  
    mrpt::slam::CMetricMapBuilderRBPF::TConstructionOptions rbpfMappingOptions_;   
    bool CAMERA_3DSCENE_FOLLOWS_ROBOT_;
    bool SHOW_PROGRESS_IN_WINDOW_;
    int SHOW_PROGRESS_IN_WINDOW_DELAY_MS_;
    int PROGRESS_WINDOW_WIDTH_, PROGRESS_WINDOW_HEIGHT_;
    std::string simplemap_path_prefix;
  } options_;

  PFslam() = default;
  virtual ~PFslam();

  void init3Dwindow();

  void run3Dwindow();

  void readIniFile(const std::string& ini_filename);

  void initSlam(Options options);

  void readRawlog(const std::string& rawlog_filename,
                  std::vector<std::pair<mrpt::obs::CActionCollection, mrpt::obs::CSensoryFrame>>& data);

  void observation(const mrpt::obs::CSensoryFrame::ConstPtr sensory_frame,
                   const mrpt::obs::CObservationOdometry::ConstPtr odometry);

protected:
  mrpt::slam::CMetricMapBuilderRBPF mapBuilder_;  
  mrpt::obs::CActionCollection::Ptr action_;      
  mrpt::obs::CSensoryFrame::Ptr sensory_frame_;   

  mrpt::poses::CPose2D odomLastObservation_;  
  bool use_motion_model_default_options_;     
  mrpt::system::TTimeStamp timeLastUpdate_;   

  const mrpt::maps::CMultiMetricMap* metric_map_;  
  mrpt::poses::CPose3DPDFParticles curPDF;         

  mrpt::gui::CDisplayWindow3D::Ptr win3D_;  
};
}  // namespace mrpt_rbpf_slam