AdaptiveKSearchSurface.hpp

Go to the documentation of this file.

 
 /*
 * AdaptiveKSearchSurface.h
 *
 *  Created on: 07.02.2011
 *      Author: Thomas Wiemann
 *   co-Author: Dominik Feldschnieders (dofeldsc@uos.de)
 */
 
#ifndef LVR2_RECONSTRUCTION_ADAPTIVEKSEARCHSURFACE_H_
#define LVR2_RECONSTRUCTION_ADAPTIVEKSEARCHSURFACE_H_
 
#include <cassert>
#include <fstream>
#include <iostream>
#include <limits>
#include <memory>
#include <string>
#include <chrono>
#include <cmath>
 
#include "lvr2/io/PointBuffer.hpp"
#include "lvr2/geometry/Normal.hpp"
#include "lvr2/geometry/Plane.hpp"
#include "lvr2/io/Progress.hpp"
#include "lvr2/geometry/BaseVector.hpp"
 
 
 
#include "PointsetSurface.hpp"
 
// #ifdef LVR2_USE_STANN
// // SearchTreeStann
// #include "SearchTreeStann.hpp"
// #endif
 
// #include "SearchTreeNanoflann.hpp"
#include "SearchTreeFlann.hpp"
 
// // SearchTreePCL
// #ifdef LVR2_USE_PCL
//     #include "SearchTreeFlannPCL.hpp"
// #endif
 
// // SearchTreeNabo
// #ifdef LVR2_USE_NABO
//     #include "SearchTreeNabo.hpp"
// #endif
 
 
// using std::cout;
// using std::endl;
// using std::ifstream;
using std::numeric_limits;
// using std::ofstream;
// using std::shared_ptr;
using std::string;
using std::unique_ptr;
using std::isnan;
 
 
namespace lvr2
{
 
template<typename BaseVecT>
class AdaptiveKSearchSurface : public PointsetSurface<BaseVecT>
{
public:
 
    // 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();
 
    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();
 
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(float dx, float dy, float dz);
 
    // /**
    //  * @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,
        int k,
        const vector<size_t> &id
    );
 
    Plane<BaseVecT> calcPlaneRANSAC(
        const BaseVecT &queryPoint,
        int k,
        const vector<size_t> &id,
        bool &ok
    );
 
    Plane<BaseVecT> calcPlaneIterative(
        const BaseVecT &queryPoint,
        int k,
        const vector<size_t> &id
    );
 
 
 
 
    BaseVecT m_centroid;
 
    // 0: PCA
    // 1: RANSAC
    // 2: Iterative
    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;
 
    string m_searchTreeName;
 
};
 
 
} // namespace lvr2
 
#include "lvr2/reconstruction/AdaptiveKSearchSurface.tcc"
 
#endif // LVR2_RECONSTRUCTION_ADAPTIVEKSEARCHSURFACE_H_