KDTree.hpp

Go to the documentation of this file.

#ifndef KDTREE_HPP_
#define KDTREE_HPP_

#include "TreeUtils.hpp"
#include "SLAMScanWrapper.hpp"

#include <memory>
#include <limits>
#include <boost/shared_array.hpp>

namespace lvr2
{

class KDTree
{
public:
    using PointT = float;
    using Point = Vector3<PointT>;
    using Neighbor = Point*;
    using KDTreePtr = std::shared_ptr<KDTree>;

    static std::shared_ptr<KDTree> create(SLAMScanPtr scan, int maxLeafSize = 20);

    template<typename T>
    bool nearestNeighbor(
        const Vector3<T>& point,
        Neighbor& neighbor,
        double& distance,
        double maxDistance = std::numeric_limits<double>::infinity()
    ) const
    {
        neighbor = nullptr;
        distance = maxDistance;
        nnInternal(point.template cast<PointT>(), neighbor, distance);

        return neighbor != nullptr;
    }

    virtual ~KDTree() = default;

    static size_t nearestNeighbors(KDTreePtr tree, SLAMScanPtr scan, Neighbor* neighbors, double maxDistance, Vector3d& centroid_m, Vector3d& centroid_d);

    static size_t nearestNeighbors(KDTreePtr tree, SLAMScanPtr scan, KDTree::Neighbor* neighbors, double maxDistance);

protected:
    KDTree() = default;
    KDTree(const KDTree&&) = delete;

    virtual void nnInternal(const Point& point, Neighbor& neighbor, double& maxDist) const = 0;

    friend class KDNode;

    boost::shared_array<Point> points;
};

using KDTreePtr = std::shared_ptr<KDTree>;

} /* namespace lvr2 */

#endif /* KDTREE_HPP_ */