mrpt_ekf_slam_2d.h

Go to the documentation of this file.

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

#pragma once

#include <mrpt/utils/CConfigFile.h>
#include <mrpt/utils/CFileGZInputStream.h>
#include <mrpt/utils/CFileGZOutputStream.h>
#include <mrpt/system/os.h>
#include <mrpt/system/string_utils.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/slam/CRangeBearingKFSLAM2D.h>

#include <mrpt/math/ops_containers.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/opengl/stock_objects.h>

#include <mrpt/gui/CDisplayWindow3D.h>

#include <mrpt/version.h>
#if MRPT_VERSION >= 0x130
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/obs/CObservationBearingRange.h>
#include <mrpt/obs/CRawlog.h>
using namespace mrpt::maps;
using namespace mrpt::obs;
#else
#include <mrpt/slam/CActionRobotMovement2D.h>
#include <mrpt/slam/CActionCollection.h>
#include <mrpt/slam/CObservationOdometry.h>
#include <mrpt/slam/CSensoryFrame.h>
#include <mrpt/slam/CMultiMetricMap.h>
#include <mrpt/slam/CObservationBearingRange.h>
#include <mrpt/slam/CRawlog.h>
#endif

using namespace mrpt;
using namespace mrpt::slam;
using namespace mrpt::opengl;
using namespace mrpt::system;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace mrpt::utils;
using namespace std;
using namespace mrpt::gui;
class EKFslam
{
public:
  EKFslam();
  virtual ~EKFslam();
  void init3Dwindow();
  void run3Dwindow();
  void landmark_to_3d(const CRangeBearingKFSLAM2D::KFArray_FEAT &lm, TPoint3D &p);
  void read_iniFile(std::string ini_filename);
  void observation(CSensoryFrame::Ptr _sf, CObservationOdometry::Ptr _odometry);

protected:
  CRangeBearingKFSLAM2D mapping;  

  mrpt::system::TTimeStamp timeLastUpdate_;  

  CActionCollection::Ptr action;  
  CSensoryFrame::Ptr sf;          

  mrpt::poses::CPose2D odomLastObservation_;                                  
  bool use_motion_model_default_options_;                                     
  CActionRobotMovement2D::TMotionModelOptions motion_model_default_options_;  
  CActionRobotMovement2D::TMotionModelOptions motion_model_options_;          

  CPosePDFGaussian robotPose_;                             
  std::vector<mrpt::math::TPoint2D> LMs_;                  
  std::map<unsigned int, CLandmark::TLandmarkID> LM_IDs_;  
  CMatrixDouble fullCov_;                                  
  CVectorDouble fullState_;                                

  mrpt::gui::CDisplayWindow3D::Ptr win3d;  
  bool SHOW_3D_LIVE;
  bool CAMERA_3DSCENE_FOLLOWS_ROBOT;
  vector<TPose3D> meanPath;
};