Program Listing for File SLAMScanWrapper.hpp

↰ Return to documentation for file (include/lvr2/registration/SLAMScanWrapper.hpp)

#ifndef SLAMSCANWRAPPER_HPP_
#define SLAMSCANWRAPPER_HPP_

#include "lvr2/types/ScanTypes.hpp"
#include "lvr2/algorithm/KDTree.hpp"

#include <Eigen/Dense>
#include <vector>

namespace lvr2
{

enum class FrameUse
{
    INVALID = 0,
    UPDATED = 1,
    UNUSED = 2,
    GRAPHSLAM = 3,
    LOOPCLOSE = 4,
};

class SLAMScanWrapper
{
public:
    SLAMScanWrapper(ScanPtr scan);

    virtual ~SLAMScanWrapper() = default;

    ScanPtr innerScan();


    virtual void transform(const Transformd& transform, bool writeFrame = true, FrameUse use = FrameUse::UPDATED);

    void addFrame(FrameUse use = FrameUse::UNUSED);


    void reduce(double voxelSize, int maxLeafSize);

    void setMinDistance(double minDistance);

    void setMaxDistance(double maxDistance);

    void trim();


    virtual Vector3d point(size_t index) const;

    const Vector3f& rawPoint(size_t index) const;

    size_t numPoints() const;


    const Transformd& pose() const;

    const Transformd& deltaPose() const;

    const Transformd& initialPose() const;

    Vector3d getPosition() const;

    size_t frameCount() const;

    const std::pair<Transformd, FrameUse>& frame(size_t index) const;

    void writeFrames(std::string path) const;

    KDTreePtr<Vector3f> createKDTree(size_t maxLeafSize = 20) const;

    static size_t nearestNeighbors(KDTreePtr<Vector3f> tree, std::shared_ptr<SLAMScanWrapper> scan,
                                   std::vector<Vector3f*>& neighbors, double maxDistance,
                                   Vector3d& centroid_m, Vector3d& centroid_d);

    static size_t nearestNeighbors(KDTreePtr<Vector3f> tree, std::shared_ptr<SLAMScanWrapper> scan,
                                   std::vector<Vector3f*>& neighbors, double maxDistance);

protected:
    ScanPtr               m_scan;

    std::vector<Vector3f> m_points;
    size_t                m_numPoints;

    Transformd            m_deltaPose;

    std::vector<std::pair<Transformd, FrameUse>> m_frames;
};

using SLAMScanPtr = std::shared_ptr<SLAMScanWrapper>;

} /* namespace lvr2 */

#endif /* SLAMSCANWRAPPER_HPP_ */