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