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/config/CConfigFile.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/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/COpenGLScene.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#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>

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

   protected:
        mrpt::slam::CRangeBearingKFSLAM mapping;  

        mrpt::system::TTimeStamp
                timeLastUpdate_;  

        mrpt::obs::CActionCollection::Ptr action;  
        mrpt::obs::CSensoryFrame::Ptr sf;  

        mrpt::poses::CPose3D
                odomLastObservation_;  
        mrpt::obs::CActionRobotMovement3D::TMotionModelOptions
                motion_model_options_;  

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