GraphSLAM.hpp

Go to the documentation of this file.

 
#ifndef GRAPHSLAM_HPP_
#define GRAPHSLAM_HPP_
 
#include "SLAMScanWrapper.hpp"
#include "SLAMOptions.hpp"
#include "KDTree.hpp"
 
#include <Eigen/SparseCore>
 
namespace lvr2
{
 
bool findCloseScans(const std::vector<SLAMScanPtr>& scans, size_t scan, const SLAMOptions& options, std::vector<size_t>& output);
 
class GraphSLAM
{
public:
 
    using GraphMatrix = Eigen::SparseMatrix<double>;
    using GraphVector = Eigen::VectorXd;
    using Graph = std::vector<std::pair<int, int>>;
 
    GraphSLAM(const SLAMOptions* options);
 
    virtual ~GraphSLAM() = default;
 
    void doGraphSLAM(const std::vector<SLAMScanPtr>& scans, size_t last, const std::vector<bool>& new_scans = std::vector<bool>()) const;
 
protected:
    void createGraph(const std::vector<SLAMScanPtr>& scans, size_t last, Graph& graph) const;
 
    void fillEquation(const std::vector<SLAMScanPtr>& scans, const Graph& graph, GraphMatrix& mat, GraphVector& vec) const;
    
    void eulerCovariance(KDTreePtr tree, SLAMScanPtr scan, Matrix6d& outMat, Vector6d& outVec) const;
 
    const SLAMOptions*     m_options;
};
 
} /* namespace lvr2 */
 
#endif /* GRAPHSLAM_HPP_ */