mrpt_ekf_slam_3d.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/CRangeBearingKFSLAM.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/CActionRobotMovement3D.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/CActionRobotMovement3D.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;

class EKFslam
{
public:
  EKFslam();
  virtual ~EKFslam();
  void init3Dwindow();
  void run3Dwindow();
  void landmark_to_3d(const CRangeBearingKFSLAM::KFArray_FEAT &lm, TPoint3D &p);
  void read_iniFile(std::string ini_filename);
  void observation(CSensoryFrame::Ptr _sf, CObservationOdometry::Ptr _odometry);

protected:
  CRangeBearingKFSLAM mapping;  

  mrpt::system::TTimeStamp timeLastUpdate_;  

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

  mrpt::poses::CPose3D odomLastObservation_;  
#if MRPT_VERSION >= 0x150
  CActionRobotMovement3D::TMotionModelOptions motion_model_options_;  
#endif

  mrpt::gui::CDisplayWindow3D::Ptr win3d;  
  bool SHOW_3D_LIVE;
  bool CAMERA_3DSCENE_FOLLOWS_ROBOT;
  vector<TPose3D> meanPath;
  CPose3DQuatPDFGaussian robotPose_;                       
  std::vector<mrpt::math::TPoint3D> LMs_;                  
  std::map<unsigned int, CLandmark::TLandmarkID> LM_IDs_;  
  CMatrixDouble fullCov_;                                  
  CVectorDouble fullState_;                                
};