[ADD ##368] Surface detection using the Region Growing Segmentation of the Point Cloud Library

 * Currently the module Surface detection by PCL only gives the ability to detect
 * surfaces using point clouds (WDataSetPoints) what is a method of a fair quality.
 *
 * Some few changes across other modules:
 *   - Improvement on the octree neighbor search algorithm. Now you can look for
 *     neighbors using the neighborship of 6, 18 or 26
 *   - Very first steps on the surface detection algorithm "An adaptive approach for the
 *     segmentation and extraction of planar and linear/cylindrical features from laser
 *     scanning data" of Lari/Habib (2014).
 *   - Added an unidimensional kd tree structures with some processing methods such as
 *     looking for nearest nghbors.

LiDARToolbox/README

@@ -5,4 +5,8 @@ This toolbox is a collection of modules for dealing with LiDAR data. More inform
 
 This toolbox is currently under development by
 - Andreas Schwarzkopf
-- Alexander Wiebel
\ No newline at end of file
+- Alexander Wiebel
+
+Requirements:
+- libLAS (http://www.liblas.org/ or packages liblas-dev and liblas-c-dev)
+- Point Cloud Library (http://pointclouds.org/)
\ No newline at end of file

LiDARToolbox/VERSION

@@ -15,4 +15,4 @@
 #VERSION=1.2.0
 #VERSION=1.2.0+hgX  -> replaces the X with the current revision number
 #VERSUIB=1.2.0+hg1234 -> here, revision number was set by the build scripts for example.
-VERSION=0.0.2+hgX
+VERSION=0.0.3+hgX

LiDARToolbox/src/CMakeLists.txt

@@ -59,6 +59,20 @@ INCLUDE( OpenWalnut )
 
 # IMPORTANT: NEVER add any commands bore the line INCLUDE( OpenWalnut ). This might cause problems!
 
+
+# -----------------------------------------------------------------------------------------------------------------------------------------------
+# Add Point Cloud Library for Nearest Neighbor Search
+# -----------------------------------------------------------------------------------------------------------------------------------------------
+# Code has been written with PCL v1.5
+# FIND_PACKAGE( PCL 1.5 REQUIRED COMPONENTS common kdtree registration )
+
+FIND_PACKAGE( PCL REQUIRED COMPONENTS common kdtree search registration segmentation )
+INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS} )
+LINK_DIRECTORIES( ${PCL_LIBRARY_DIRS} )
+ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
+LIST( APPEND ADDITIONAL_LIBS ${PCL_SEGMENTATION_LIBRARIES};)
+
+
 # ---------------------------------------------------------------------------------------------------------------------------------------------------
 #
 # Setup all modules
@@ -73,10 +87,10 @@ INCLUDE( OpenWalnut )
 # Please see the documentation of SETUP_MODULE as it shows how to add custom dependencies (third-party libs)
 
 SETUP_MODULE( 
-    ${PROJECT_NAME}   # use project name as module(-toolbox) name
-    "."               # where to find the sources
-    "liblas.so"       # no additonal dependencies
-    ""                # no sources to exclude
+    ${PROJECT_NAME}                 # use project name as module(-toolbox) name
+    "."                             # where to find the sources
+    "liblas.so;${ADDITIONAL_LIBS}"  # no additonal dependencies
+    ""                              # no sources to exclude
 )
 
 # Tell CMake that we want to use the WToolboxVersion header in our Modules. It gets created automatically if you keep this line.

LiDARToolbox/src/WToolkit.cpp

@@ -37,6 +37,8 @@
 #include "pointsCutOutliers/WMPointsCutOutliers.h"
 #include "pointsGroupSelector/WMPointsGroupSelector.h"
 #include "readLAS/WMReadLAS.h"
+#include "surfaceDetectionByLari/WMSurfaceDetectionByLari.h"
+#include "surfaceDetectionByPCL/WMSurfaceDetectionByPCL.h"
 #include "wallDetectionByPCA/WMWallDetectionByPCA.h"
 
 // #include "WToolkit.h"
@@ -60,6 +62,8 @@ extern "C" void WLoadModule( WModuleList& m ) // NOLINT
     m.push_back( boost::shared_ptr< WModule >( new WMPointsCutOutliers ) );
     m.push_back( boost::shared_ptr< WModule >( new WMPointsGroupSelector ) );
     m.push_back( boost::shared_ptr< WModule >( new WMReadLAS ) );
+    m.push_back( boost::shared_ptr< WModule >( new WMSurfaceDetectionByLari ) );
+    m.push_back( boost::shared_ptr< WModule >( new WMSurfaceDetectionByPCL ) );
     m.push_back( boost::shared_ptr< WModule >( new WMWallDetectionByPCA ) );
 }
 

LiDARToolbox/src/datastructures/kdtree/WKdTreeND.cpp

+//---------------------------------------------------------------------------
+//
+// Project: OpenWalnut ( http://www.openwalnut.org )
+//
+// Copyright 2009 OpenWalnut Community, BSV-Leipzig and CNCF-CBS
+// For more information see http://www.openwalnut.org/copying
+//
+// This file is part of OpenWalnut.
+//
+// OpenWalnut is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// OpenWalnut is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with OpenWalnut. If not, see <http://www.gnu.org/licenses/>.
+//
+//---------------------------------------------------------------------------
+
+#include <iostream>
+#include <algorithm>
+#include <vector>
+#include "WKdTreeND.h"
+
+using std::cout;
+using std::endl;
+WKdTreeND::WKdTreeND()
+{
+}
+WKdTreeND::WKdTreeND( size_t dimensions )
+{
+    m_dimensions = dimensions;
+//    areaMin.reserve( dimensions );
+//    areaMin.resize( dimensions );
+//    areaMax.reserve( dimensions );
+//    areaMax.resize( dimensions );
+//    hasBoundMin.reserve( dimensions );
+//    hasBoundMin.resize( dimensions );
+//    hasBoundMax.reserve( dimensions );
+//    hasBoundMax.resize( dimensions );
+//    for( size_t index = 0; index < dimensions; index++ )
+//    {
+//        areaMin[index] = 0;
+//        areaMax[index] = 0;
+//        hasBoundMin[index] = false;
+//        hasBoundMax[index] = false;
+//    }
+    m_splittingDimension = 0;
+    m_splittingPosition = 0.0;
+    m_allowDoubles = true;
+    m_points = new vector<vector<double> >();
+    m_parentSplittingDimension = dimensions;
+    m_higherChild = 0;
+    m_lowerChild = 0;
+}
+WKdTreeND::~WKdTreeND()
+{
+    delete m_points;
+    if( m_lowerChild != 0 )
+    {
+        delete m_lowerChild;
+    }
+    if( m_higherChild != 0 )
+    {
+        delete m_higherChild;
+    }
+}
+void WKdTreeND::add( vector<vector<double> >* addables )
+{
+    if( addables->size() == 0 )
+        return;
+    for( size_t index = 0; index < addables->size(); index++ )
+        m_points->push_back( addables->at( index ) );
+
+    if( m_lowerChild == 0 && m_lowerChild == 0 )
+    {
+        determineNewSplittingDimension( m_points );
+        if( canSplit() )
+        {
+            if( m_points->size() == 2 )
+            {
+                m_splittingPosition = ( m_points->at( 0 )[m_splittingDimension]
+                        + m_points->at( 1 )[m_splittingDimension] ) / 2.0;
+            }
+            else
+            {
+                calculateSplittingPosition( m_points );
+            }
+            m_lowerChild = getNewInstance( m_dimensions );
+            m_higherChild = getNewInstance( m_dimensions );
+            initSubNodes();
+            addPointsToChildren( m_points );
+            m_points->resize( 0 );
+            m_points->reserve( 0 );
+        }
+    }
+    else
+    {
+        if( m_lowerChild != 0 && m_lowerChild != 0 )
+        {
+            addPointsToChildren( m_points );
+            m_points->resize( 0 );
+            m_points->reserve( 0 );
+        }
+        else
+        {
+            cout << "!!!UNKNOWN EXCEPTION!!! - adding items" << endl;
+        }
+    }
+}
+bool WKdTreeND::canSplit()
+{
+    return m_splittingDimension < m_dimensions;
+}
+void WKdTreeND::fetchPoints( vector<vector<double> >* targetPointSet )
+{
+    if( m_points->size() == 0 && m_lowerChild == 0 && m_lowerChild == 0 )
+    {
+    }
+    else
+    {
+        if( m_points->size() > 0 && m_lowerChild == 0 && m_lowerChild == 0 )
+        {
+            for(size_t index = 0; index < m_points->size(); index++)
+                targetPointSet->push_back( m_points->at( index ) );
+        }
+        else
+        {
+            if( m_points->size() == 0 && m_lowerChild != 0 && m_lowerChild != 0 )
+            {
+                m_lowerChild->fetchPoints( targetPointSet );
+                m_higherChild->fetchPoints( targetPointSet );
+            }
+            else
+            {
+                cout << "!!!UNKNOWN EXCEPTION!!!" << endl;
+            }
+        }
+    }
+}
+vector<WKdTreeND*>* WKdTreeND::getAllLeafNodes()
+{
+    vector<WKdTreeND*>* outputLeafNodes = new vector<WKdTreeND*>();
+    fetchAllLeafNodes( outputLeafNodes );
+    return outputLeafNodes;
+}
+//double WKdTreeND::getAreaMin(size_t dimension)
+//{
+//    return areaMin[dimension];
+//}
+//double WKdTreeND::getAreaMax(size_t dimension)
+//{
+//    return areaMax[dimension];
+//}
+size_t WKdTreeND::getDimensions()
+{
+    return m_dimensions;
+}
+//bool WKdTreeND::getHasBoundMin(size_t dimension)
+//{
+//    return hasBoundMin[dimension];
+//}
+//bool WKdTreeND::getHasBoundMax(size_t dimension)
+//{
+//    return hasBoundMax[dimension];
+//}
+WKdTreeND* WKdTreeND::getHigherChild()
+{
+    return m_higherChild;
+}
+WKdTreeND* WKdTreeND::getLowerChild()
+{
+    return m_lowerChild;
+}
+vector<vector<double> >* WKdTreeND::getNodePoints()
+{
+    return m_points;
+}
+size_t WKdTreeND::getSplittingDimension()
+{
+    return m_splittingDimension;
+}
+double WKdTreeND::getSplittingPosition()
+{
+    return m_splittingPosition;
+}
+WKdTreeND* WKdTreeND::getNewInstance( size_t dimensions )
+{
+    return new WKdTreeND( dimensions );
+}
+void WKdTreeND::addPointsToChildren( vector<vector<double> >* newPoints )
+{
+    vector<vector<double> >* lowerPoints = new vector<vector<double> >();
+    vector<vector<double> >* higherPoints = new vector<vector<double> >();
+    for( size_t index = 0; index < newPoints->size(); index++ )
+    {
+        vector<double> newPoint = newPoints->at( index );
+        double position = newPoint.at( m_splittingDimension );
+        if( position < m_splittingPosition )
+        {
+            lowerPoints->push_back( newPoint );
+        }
+        else
+        {
+            higherPoints->push_back( newPoint );
+        }
+    }
+    m_lowerChild->add( lowerPoints );
+    m_higherChild->add( higherPoints );
+    delete lowerPoints;
+    delete higherPoints;
+}
+void WKdTreeND::calculateSplittingPosition( vector<vector<double> >* inputPoints )
+{
+    if( !canSplit() )
+        return;
+    size_t count = inputPoints->size();
+    vector<double>* line = new vector<double>();
+    line->reserve( count );
+    line->resize( count );
+    for( size_t index = 0; index < inputPoints->size(); index++ )
+        line->at( index ) = inputPoints->at( index ).at( m_splittingDimension );
+    std::sort( line->begin(), line->end() );
+    size_t medianIdx = count / 2;
+    m_splittingPosition = line->at( medianIdx );
+    size_t medianLeftIdx = medianIdx - 1;
+    double medianLeft = line->at( medianLeftIdx );
+    bool leftIdxValid = true;
+    size_t medianRightIdx = medianIdx;
+    double medianRight = line->at( medianRightIdx );
+    bool rightIdxValid = true;
+    while( rightIdxValid && medianRight == medianLeft )
+    {
+        if( medianRightIdx + 1 < inputPoints->size() )
+        {
+            medianRightIdx++;
+            medianRight = line->at( medianRightIdx );
+        }
+        else
+        {
+            rightIdxValid = false;
+        }
+    }
+    while( leftIdxValid && medianLeftIdx < inputPoints->size() && medianLeft == m_splittingPosition )
+    {
+        if( medianLeftIdx > 0 )
+        {
+            medianLeftIdx--;
+            medianLeft = line->at( medianLeftIdx );
+        }
+        else
+        {
+            leftIdxValid = false;
+        }
+    }
+    if( leftIdxValid && rightIdxValid )
+    {
+        if( medianRightIdx - medianIdx < medianIdx - medianLeftIdx - 1 )
+        {
+            medianIdx = medianRightIdx;
+        }
+        else
+        {
+            medianIdx = medianLeftIdx + 1;
+        }
+    }
+    else
+    {
+        if( leftIdxValid )
+        {
+            medianIdx = medianLeftIdx + 1;
+        }
+        else
+        {
+            if( rightIdxValid )
+            {
+                medianIdx = medianRightIdx;
+            }
+            else
+            {
+                cout << "!!!UNKNOWN EXCEPTION!!!" << endl;
+            }
+        }
+    }
+
+
+    m_splittingPosition = ( line->at( medianIdx ) + line->at( medianIdx - 1 ) ) / 2.0;
+    line->resize( 0 );
+    line->reserve( 0 );
+    delete line;
+}
+bool WKdTreeND::determineNewSplittingDimension( vector<vector<double> >* inputPoints )
+{
+    m_splittingDimension = m_dimensions;
+    if( inputPoints->size() < 2 )
+        return false;
+    vector<double>* boundingBoxMin = new vector<double>();
+    vector<double>* boundingBoxMax = new vector<double>();
+    boundingBoxMin->reserve( m_dimensions );
+    boundingBoxMin->resize( m_dimensions );
+    boundingBoxMax->reserve( m_dimensions );
+    boundingBoxMax->resize( m_dimensions );
+    for( size_t index = 0; index < inputPoints->size(); index++ )
+        for( size_t dimension = 0; dimension < getDimensions(); dimension++ )
+        {
+            double position = inputPoints->at( index ).at( dimension );
+            if( index == 0 || position < boundingBoxMin->at( dimension ) )
+                boundingBoxMin->at( dimension ) = position;
+            if( index == 0 || position > boundingBoxMax->at( dimension ) )
+                boundingBoxMax->at( dimension ) = position;
+        }
+
+    double spreadMax = 0.0;
+    for( size_t dimension = 0; dimension < getDimensions(); dimension++ )
+    {
+        double currentSpread = boundingBoxMax->at( dimension ) - boundingBoxMin->at( dimension );
+        if( dimension != m_parentSplittingDimension && currentSpread > spreadMax )
+        {
+            spreadMax = currentSpread;
+            m_splittingDimension = dimension;
+        }
+    }
+    if( m_splittingDimension >= m_dimensions && m_parentSplittingDimension < m_dimensions
+            && boundingBoxMax->at( m_parentSplittingDimension ) - boundingBoxMin->at( m_parentSplittingDimension ) > 0.0 )
+        m_splittingDimension = m_parentSplittingDimension;
+    boundingBoxMin->resize( 0 );
+    boundingBoxMin->reserve( 0 );
+    boundingBoxMin->resize( 0 );
+    boundingBoxMin->reserve( 0 );
+    delete boundingBoxMin;
+    delete boundingBoxMax;
+    return m_splittingDimension < m_dimensions;
+}
+void WKdTreeND::fetchAllLeafNodes( vector<WKdTreeND*>* targetNodeList )
+{
+    if( m_points->size() > 0 && m_lowerChild == 0 && m_higherChild == 0 )
+    {
+        targetNodeList->push_back( this );
+    }
+    else
+    {
+        if( m_points->size() == 0 && m_lowerChild != 0 && m_higherChild != 0 )
+        {
+            m_lowerChild->fetchAllLeafNodes( targetNodeList );
+            m_higherChild->fetchAllLeafNodes( targetNodeList );
+        }
+        else
+        {
+            cout << "!!!UNKNOWN EXCEPTION!!! - fetching leaf nodes" << endl;
+        }
+    }
+}
+bool WKdTreeND::hasParent()
+{
+    return m_parentSplittingDimension < m_dimensions;
+}
+void WKdTreeND::initSubNodes()
+{
+    m_lowerChild->m_parentSplittingDimension = m_splittingDimension;
+    m_higherChild->m_parentSplittingDimension = m_splittingDimension;
+//    for(size_t dimension = 0; dimension < getDimensions(); dimension++){
+//        lowerChild->hasBoundMin[dimension] = hasBoundMin[dimension];
+//        lowerChild->hasBoundMax[dimension] = hasBoundMax[dimension];
+//        lowerChild->areaMin[dimension] = areaMin[dimension];
+//        lowerChild->areaMax[dimension] = areaMax[dimension];
+//        higherChild->hasBoundMin[dimension] = hasBoundMin[dimension];
+//        higherChild->hasBoundMax[dimension] = hasBoundMax[dimension];
+//        higherChild->areaMin[dimension] = areaMin[dimension];
+//        higherChild->areaMax[dimension] = areaMax[dimension];
+//    }
+//    lowerChild->hasBoundMax[splittingDimension] = true;
+//    lowerChild->areaMax[splittingDimension] = splittingPosition;
+//    higherChild->hasBoundMin[splittingDimension] = true;
+//    higherChild->areaMin[splittingDimension] = splittingPosition;
+}

LiDARToolbox/src/datastructures/kdtree/WKdTreeND.h

+//---------------------------------------------------------------------------
+//
+// Project: OpenWalnut ( http://www.openwalnut.org )
+//
+// Copyright 2009 OpenWalnut Community, BSV-Leipzig and CNCF-CBS
+// For more information see http://www.openwalnut.org/copying
+//
+// This file is part of OpenWalnut.
+//
+// OpenWalnut is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// OpenWalnut is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with OpenWalnut. If not, see <http://www.gnu.org/licenses/>.
+//
+//---------------------------------------------------------------------------
+
+#ifndef WKDTREEND_H
+#define WKDTREEND_H
+
+#include <vector>
+#include <algorithm>
+
+using std::vector;
+using std::size_t;
+using std::cout;
+using std::endl;
+
+/**
+ * This is a unidimensional kd tree compound. It is proposed not only to work with two 
+ * or three dimensional kd trees but also with a single or more than three dimensions.
+ */
+class WKdTreeND
+{
+public:
+    WKdTreeND();
+    /**
+     * Instantiates a kd tree of a arbitrary dimension count.
+     * \param dimensions The dimension count of the kd tree.
+     */
+    explicit WKdTreeND( size_t dimensions );
+    virtual ~WKdTreeND();
+    /**
+     * Adds points to the kd tree. It's better to add all the points into an empty tree.
+     * The tree is compatible to add one by one point but it will result a heavily 
+     * unbalanced binary tree inside adding a larger amount of points.
+     * \param addables Points to add.
+     */
+    void add( vector<vector<double> >* addables );
+    /**
+     * Says whether a kd tree node can be split or not. Firstly the method to determine 
+     * the splitting dimension should be executed. A true value is returned if the 
+     * calculated splitting dimension is within the total dimensions count.
+     * \return The kd tree node can be split or not.
+     */
+    bool canSplit();
+    /**
+     * Fetches all the points from a kd tree node into a list.
+     * \param targetPointSet Target point list to put points to.
+     */
+    void fetchPoints( vector<vector<double> >* targetPointSet );
+//    double getAreaMin(size_t dimension); //TODO(aschwarzkopf): Consider to purge in future
+//    double getAreaMax(size_t dimension); //TODO(aschwarzkopf): Consider to purge in future
+    /**
+     * Returns all the leaf nodes of the kd tree.
+     * \return All the leaf nodes of the kd tree.
+     */
+    vector<WKdTreeND*>* getAllLeafNodes();
+    /**
+     * Returns the dimension count of the kd tree.
+     * \return The dimension count of the kd tree.
+     */
+    size_t getDimensions();
+//    bool getHasBoundMin(size_t dimension); //TODO(aschwarzkopf): Consider to purge in future
+//    bool getHasBoundMax(size_t dimension); //TODO(aschwarzkopf): Consider to purge in future
+    /**
+     * Returns the child node that lies on the lower scale across the splitting 
+     * dimension.
+     * \return The child node that lies on the lower scale across the splitting 
+     * dimension.
+     */
+    WKdTreeND* getLowerChild();
+    /**
+     * Returns the child node that lies on the higher scale across the splitting 
+     * dimension.
+     * \return The child node that lies on the higher scale across the splitting 
+     * dimension.
+     */
+    WKdTreeND* getHigherChild();
+    /**
+     * Returns the points that are registered to the kd tree node. Usually all the 
+     * poinnts have the same coordinate if nodes are subdivided to the lowest point 
+     * count and no points of the same coordinates exist. Only leaf nodes can have and 
+     * return points.
+     * \return The points of a kd tree node.
+     */
+    vector<vector<double> >* getNodePoints();
+    /**
+     * Returns the dimension across which the current node is split.
+     * \return The dimension across which the current node is split.
+     */
+    size_t getSplittingDimension();
+    /**
+     * Returns the splitting position of the splitted dimension.
+     * \return The splitting position of the splitted dimension.
+     */
+    double getSplittingPosition();
+
+protected:
+    /**
+     * Returns a new instance of the kd tree. You should overwrite this method when you 
+     * derive your own class from this so that the kd tree can instantiate new objects 
+     * of your type.
+     * \param dimensions The dimensions count of the new node.
+     * \return A new kd tree node instance of your node type.
+     */
+    virtual WKdTreeND* getNewInstance( size_t dimensions );
+
+private:
+    /**
+     * This method is used by the method to add new points. It puts points either to the 
+     * node of the lower or higher position across the splitting dimension. The method is 
+     * executed after the splitting dimension and position are calculated.
+     * \param newPoints Points to append to child nodes.
+     */
+    void addPointsToChildren( vector<vector<double> >* newPoints );
+    /**
+     * Calculates the splitting position between the two child nodes. It calculates the 
+     * median of all input points across the splitting dimension between two children. 
+     * Always check out the splitting dimension before that.
+     * \param points Input points to calculate the median across the splitted dimension.
+     */
+    void calculateSplittingPosition( vector<vector<double> >* points );
+    /**
+     * Determines the new splitting dimension between two child nodes. Afterwards the 
+     * splitting position can be calculated across that dimension after executing that 
+     * method.
+     * \param inputPoints Input points to analyse the most optimal splitting dimension.
+     * \return The kd tree node can be split or not.
+     */