Program Listing for File AdaptiveKSearchSurface.hpp

↰ Return to documentation for file (include/lvr2/reconstruction/AdaptiveKSearchSurface.hpp)

 /*
 * AdaptiveKSearchSurface.h
 *
 *  Created on: 07.02.2011
 *      Author: Thomas Wiemann
 *   co-Author: Dominik Feldschnieders (dofeldsc@uos.de)
 */

#ifndef ADAPTIVEKSEARCHSURFACE
#define ADAPTIVEKSEARCHSURFACE

#include <cassert>
#include <fstream>
#include <iostream>
#include <limits>
#include <memory>
#include <string>
#include <chrono>
#include <cmath>

#include "SearchTreeFlann.hpp"
#include "PointsetSurface.hpp"

#include "lvr2/types/PointBuffer.hpp"
#include "lvr2/geometry/Normal.hpp"
#include "lvr2/geometry/Plane.hpp"
#include "lvr2/geometry/BaseVector.hpp"
#include "lvr2/util/Progress.hpp"


// #include "SearchTreeNanoflann.hpp"

#include "lvr2/algorithm/KDTree.hpp"

// // SearchTreePCL
// #ifdef LVR2_USE_PCL
//     #include "SearchTreeFlannPCL.hpp"
// #endif

using std::numeric_limits;
using std::string;
using std::unique_ptr;
using std::isnan;


namespace lvr2
{

template<typename BaseVecT>
class AdaptiveKSearchSurface : public PointsetSurface<BaseVecT>
{
public:

    using Base = PointsetSurface<BaseVecT>;

    // typedef shared_ptr<AdaptiveKSearchSurface<BaseVecT>> Ptr;

    AdaptiveKSearchSurface(
        PointBufferPtr loader,
        std::string searchTreeName,
        int kn = 10,
        int ki = 10,
        int kd = 10,
        int calcMethod = 0,
        string poseFile = ""
    );

    AdaptiveKSearchSurface() = delete;

    virtual ~AdaptiveKSearchSurface() {};


    virtual pair<typename BaseVecT::CoordType, typename BaseVecT::CoordType>
        distance(BaseVecT v) const;

    virtual void calculateSurfaceNormals();


    // /**
    //  * @brief Returns the number of managed points
    //  */
    // virtual size_t getNumPoints();

    // /**
    //  * @brief Calculates a tangent plane for the query point using the provided
    //  *        k-neighborhood
    //  *
    //  * @param queryPoint    The point for which the tangent plane is created
    //  * @param k             The size of the used k-neighborhood
    //  * @param points        The neighborhood points
    //  * @param ok            True, if RANSAC interpolation was succesfull
    //  *
    //  * @return the resulting plane
    //  */
    // Plane<BaseVecT> calcPlaneRANSACfromPoints(
    //     const BaseVecT& queryPoint,
    //     int k,
    //     vector<BaseVecT> points,
    //     NormalT c_normal,
    //     bool& ok
    // );



    // /**
    //  * @brief Returns the point at the given \ref{index}.
    //  */
    // virtual const BaseVecT operator[](size_t index) const;


    // virtual void colorizePointCloud( typename AdaptiveKSearchSurface<VertexT, NormalT>::Ptr pcm,
    //       const float &sqrtMaxDist = std::numeric_limits<float>::max(),
    //       const unsigned char* blankColor = NULL );

    // /**
    //  * @brief If set to true, normals will be calculated using RANSAC instead of
    //  *        plane fitting
    //  */
    // void useRansac(bool use_it) { m_useRANSAC = use_it; }


    // /// Color information for points public: TODO: This is not the best idea!
    // color3bArr                  m_colors;

    void interpolateSurfaceNormals();

protected:
    using Base::m_points;

private:

     void parseScanPoses(string posefile);

    // /**
    //  * @brief Returns the k closest neighbor vertices to a given queryy point
    //  *
    //  * @param v         A query vertex
    //  * @param k         The (max) number of returned closest points to v
    //  * @param nb        A vector containing the determined closest points
    //  */
    // virtual void getkClosestVertices(const BaseVecT &v,
    //         const size_t &k, vector<BaseVecT> &nb);


    void init();

    bool boundingBoxOK(const BoundingBox<BaseVecT>& bb);

    // /**
    //  * @brief Returns the mean distance of the given point set from
    //  *        the given plane
    //  *
    //  * @param p             The query plane
    //  * @param id            A list of point id's
    //  * @param k             The number of points in the list
    //  */
    // float meanDistance(const Plane<VertexT, NormalT> &p, const vector<unsigned long> &id, const int &k);

    // /**
    //  * @brief Returns a vertex representation of the given point in the
    //  *        point array
    //  *
    //  * @param i             A id of a point in the current point set
    //  * @return              A vertex representation of the given point
    //  */
    // VertexT fromID(int i);

    // /**
    //  * @brief Returns the distance between vertex v and plane p
    //  */
    // float distance(VertexT v, Plane<VertexT, NormalT> p);


    // void radiusSearch(const VertexT &v, double r, vector<VertexT> &resV, vector<NormalT> &resN){};

    Plane<BaseVecT> calcPlane(
        const BaseVecT &queryPoint,
        const vector<size_t> &id
    );

    Plane<BaseVecT> calcPlaneRANSAC(
        const BaseVecT &queryPoint,
        const vector<size_t> &id,
        bool &ok
    );

    Plane<BaseVecT> calcPlaneIterative(
        const BaseVecT &queryPoint,
        const vector<size_t> &id
    );

    Plane<BaseVecT> calcPlaneIPCAExact(
        const BaseVecT &queryPoint,
        const vector<size_t> &id
    );

    // 0: PCA
    // 1: RANSAC
    // 2: Iterative
    // 3: IPCA Exact
    int m_calcMethod;

    // /// The currently stored points
    // coord3fArr                  m_points;

    // /// The point normals
    // coord3fArr                  m_normals;

    // /// A model of the current pointcloud
    // boost::shared_ptr<Model>    m_model;

    // size_t                      m_numPoints;

    std::shared_ptr<SearchTree<BaseVecT> > m_poseTree;

    std::string m_searchTreeName;
};

template<typename BaseVecT>
using AdaptiveKSearchSurfacePtr = std::shared_ptr<AdaptiveKSearchSurface<BaseVecT>>;


} // namespace lvr2

#include "lvr2/reconstruction/AdaptiveKSearchSurface.tcc"

#endif // ADAPTIVEKSEARCHSURFACE