SLAMAlign.hpp

Go to the documentation of this file.

#ifndef SLAMALIGN_HPP_
#define SLAMALIGN_HPP_

#include "SLAMScanWrapper.hpp"
#include "SLAMOptions.hpp"
#include "GraphSLAM.hpp"

namespace lvr2
{

class SLAMAlign
{

public:
    SLAMAlign(const SLAMOptions& options, const std::vector<SLAMScanPtr>& scans, std::vector<bool> new_scans = std::vector<bool>());

    SLAMAlign(const SLAMOptions& options = SLAMOptions(), std::vector<bool> new_scans = std::vector<bool>());

    virtual ~SLAMAlign() = default;

    void addScan(const SLAMScanPtr& scan, bool match = false);

    void addScan(const ScanPtr& scan, bool match = false);

    SLAMScanPtr scan(size_t index) const;

    void match();

    void finish();

    void setOptions(const SLAMOptions& options);

    SLAMOptions& options();

    const SLAMOptions& options() const;

protected:

    void reduceScan(const SLAMScanPtr& scan);

    void applyTransform(SLAMScanPtr scan, const Matrix4d& transform);

    void checkLoopClose(size_t last);

    void loopClose(size_t first, size_t last);

    void graphSLAM(size_t last);


    void checkLoopCloseOtherOrder(size_t last);

    void createIcpGraph();

    SLAMOptions              m_options;

    std::vector<SLAMScanPtr> m_scans;

    SLAMScanPtr              m_metascan;

    GraphSLAM                m_graph;
    bool                     m_foundLoop;
    int                      m_loopIndexCount;

    std::vector<bool>        m_new_scans;

    std::vector<std::pair<int, int>> m_icp_graph;
};

} /* namespace lvr2 */

#endif /* SLAMALIGN_HPP_ */