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_ */