ekf_slam.h

Go to the documentation of this file.

#ifndef EKF_SLAM_H
#define EKF_SLAM_H

#include "tuw_marker_slam/slam_technique.h"
#include "tuw_marker_slam/measurement_marker.h"
#include "tuw_marker_slam/EKFSLAMConfig.h"

namespace tuw {

class EKFSLAM;
typedef std::shared_ptr< EKFSLAM > EKFSLAMPtr;
typedef std::shared_ptr< EKFSLAM const> EKFSLAMConstPtr;

class EKFSLAM : public SLAMTechnique {
public:
    EKFSLAM( const std::vector<double> beta );

    void cycle ( std::vector<Pose2D> &yt, cv::Mat_<double> &C_Yt, const Command &ut, const MeasurementConstPtr &zt );

    void setConfig ( const void *config );
private:
    enum DataAssociationMode {
        ID = 0,
        NNSF_LOCAL = 1,
        NNSF_GLOBAL = 2,
    };

    enum UpdateMode {
        None = 0,
        Single = 1,
        Combined = 2,
    };

    struct CorrData {
        std::pair<size_t, size_t> ij;           
        cv::Matx<double, 3, 3> Q;               
        cv::Vec<double, 3> v;                   
        cv::Matx<double, 3, 3> dx;              
        cv::Matx<double, 3, 3> dm;              
        cv::Matx<double, 3, 3> S_inv;           
    };
    typedef std::shared_ptr< CorrData > CorrDataPtr;
    typedef std::shared_ptr< CorrData const > CorrDataConstPtr;

    void init();

    void prediction ( const Command &ut );

    void data_association ( const MeasurementMarkerConstPtr &zt );

    void NNSF_local ( const MeasurementMarkerConstPtr &zt, const double gamma );

    void NNSF_global ( const MeasurementMarkerConstPtr &zt, const double gamma );

    void measurement ( const MeasurementMarkerConstPtr &zt, const CorrDataPtr &corr );

    cv::Matx<double, 3, 3> measurement_noise ( const MeasurementMarker::Marker zi );

    void update();

    void update_single();

    void update_combined();

    void integration ( const MeasurementMarkerConstPtr &zt );

    tuw_marker_slam::EKFSLAMConfig config_;     
    const std::vector<double> beta_;            

    cv::Mat_<double> y;                         
    cv::Mat_<double> x;                         
    cv::Mat_<double> C_Y;                       
    cv::Mat_<double> C_X;                       

    std::map<int, size_t> f_kj;                 

    std::vector<CorrDataPtr> c_ij;              
    std::vector<CorrDataPtr> c_ji;              

    std::vector<size_t> z_known;                
    std::vector<size_t> z_new;                  
};
};

#endif // EKF_SLAM_H