The EKFslam class provides EKF SLAM 2d from MRPT libraries. More...
#include <mrpt_ekf_slam_2d.h>
Public Member Functions | |
EKFslam () | |
constructor | |
void | init3Dwindow () |
init 3D window from mrpt lib | |
void | landmark_to_3d (const CRangeBearingKFSLAM2D::KFArray_FEAT &lm, TPoint3D &p) |
convert landmark to 3d point | |
void | observation (CSensoryFramePtr _sf, CObservationOdometryPtr _odometry) |
calculate the actions from odometry model for current observation | |
void | read_iniFile (std::string ini_filename) |
read ini file | |
void | run3Dwindow () |
run 3D window update from mrpt lib | |
virtual | ~EKFslam () |
destructor | |
Protected Attributes | |
CActionCollectionPtr | action |
actions | |
bool | CAMERA_3DSCENE_FOLLOWS_ROBOT |
CMatrixDouble | fullCov_ |
full covariance matrix | |
CVectorDouble | fullState_ |
full state vector | |
std::map< unsigned int, CLandmark::TLandmarkID > | LM_IDs_ |
vector of the landmarks ID | |
std::vector< mrpt::math::TPoint2D > | LMs_ |
vector of the landmarks | |
CRangeBearingKFSLAM2D | mapping |
EKF slam 2d class. | |
vector< TPose3D > | meanPath |
CActionRobotMovement2D::TMotionModelOptions | motion_model_default_options_ |
used if there are is not odom | |
CActionRobotMovement2D::TMotionModelOptions | motion_model_options_ |
used with odom value motion noise | |
mrpt::poses::CPose2D | odomLastObservation_ |
last observation of odometry | |
CPosePDFGaussian | robotPose_ |
current robot pose | |
CSensoryFramePtr | sf |
observations | |
bool | SHOW_3D_LIVE |
mrpt::system::TTimeStamp | timeLastUpdate_ |
last update of the pose and map | |
bool | use_motion_model_default_options_ |
used default odom_params | |
mrpt::gui::CDisplayWindow3DPtr | win3d |
MRPT window. |
The EKFslam class provides EKF SLAM 2d from MRPT libraries.
Definition at line 59 of file mrpt_ekf_slam_2d.h.
EKFslam::EKFslam | ( | ) |
constructor
Definition at line 12 of file mrpt_ekf_slam_2d.cpp.
EKFslam::~EKFslam | ( | ) | [virtual] |
destructor
Definition at line 36 of file mrpt_ekf_slam_2d.cpp.
void EKFslam::init3Dwindow | ( | ) |
init 3D window from mrpt lib
Definition at line 87 of file mrpt_ekf_slam_2d.cpp.
void EKFslam::landmark_to_3d | ( | const CRangeBearingKFSLAM2D::KFArray_FEAT & | lm, |
TPoint3D & | p | ||
) |
convert landmark to 3d point
Definition at line 184 of file mrpt_ekf_slam_2d.cpp.
void EKFslam::observation | ( | CSensoryFramePtr | _sf, |
CObservationOdometryPtr | _odometry | ||
) |
calculate the actions from odometry model for current observation
_sf | current observation |
_odometry | raw odometry |
Definition at line 62 of file mrpt_ekf_slam_2d.cpp.
void EKFslam::read_iniFile | ( | std::string | ini_filename | ) |
read ini file
ini_filename | the name of the ini file to read |
Definition at line 40 of file mrpt_ekf_slam_2d.cpp.
void EKFslam::run3Dwindow | ( | ) |
run 3D window update from mrpt lib
Definition at line 97 of file mrpt_ekf_slam_2d.cpp.
CActionCollectionPtr EKFslam::action [protected] |
actions
Definition at line 101 of file mrpt_ekf_slam_2d.h.
bool EKFslam::CAMERA_3DSCENE_FOLLOWS_ROBOT [protected] |
Definition at line 117 of file mrpt_ekf_slam_2d.h.
CMatrixDouble EKFslam::fullCov_ [protected] |
full covariance matrix
Definition at line 112 of file mrpt_ekf_slam_2d.h.
CVectorDouble EKFslam::fullState_ [protected] |
full state vector
Definition at line 113 of file mrpt_ekf_slam_2d.h.
std::map<unsigned int, CLandmark::TLandmarkID> EKFslam::LM_IDs_ [protected] |
vector of the landmarks ID
Definition at line 111 of file mrpt_ekf_slam_2d.h.
std::vector<mrpt::math::TPoint2D> EKFslam::LMs_ [protected] |
vector of the landmarks
Definition at line 110 of file mrpt_ekf_slam_2d.h.
CRangeBearingKFSLAM2D EKFslam::mapping [protected] |
EKF slam 2d class.
Definition at line 97 of file mrpt_ekf_slam_2d.h.
vector<TPose3D> EKFslam::meanPath [protected] |
Definition at line 118 of file mrpt_ekf_slam_2d.h.
CActionRobotMovement2D::TMotionModelOptions EKFslam::motion_model_default_options_ [protected] |
used if there are is not odom
Definition at line 106 of file mrpt_ekf_slam_2d.h.
CActionRobotMovement2D::TMotionModelOptions EKFslam::motion_model_options_ [protected] |
used with odom value motion noise
Definition at line 107 of file mrpt_ekf_slam_2d.h.
mrpt::poses::CPose2D EKFslam::odomLastObservation_ [protected] |
last observation of odometry
Definition at line 104 of file mrpt_ekf_slam_2d.h.
CPosePDFGaussian EKFslam::robotPose_ [protected] |
current robot pose
Definition at line 109 of file mrpt_ekf_slam_2d.h.
CSensoryFramePtr EKFslam::sf [protected] |
observations
Definition at line 102 of file mrpt_ekf_slam_2d.h.
bool EKFslam::SHOW_3D_LIVE [protected] |
Definition at line 116 of file mrpt_ekf_slam_2d.h.
mrpt::system::TTimeStamp EKFslam::timeLastUpdate_ [protected] |
last update of the pose and map
Definition at line 99 of file mrpt_ekf_slam_2d.h.
bool EKFslam::use_motion_model_default_options_ [protected] |
used default odom_params
Definition at line 105 of file mrpt_ekf_slam_2d.h.
mrpt::gui::CDisplayWindow3DPtr EKFslam::win3d [protected] |
MRPT window.
Definition at line 115 of file mrpt_ekf_slam_2d.h.