[FIX #371] Fixing surface segmentation bug

Previous commitment had some bug fixes. But it also said that some bugs dealing
with the surface segmentation still remained. This commit fixes them. The
errors consisted of not initialized member variables in some classes regarding
point instances of parameter space clustering and boundary detection.

LiDARToolbox/src/common/datastructures/kdtree/WKdTreeND.cpp

@@ -225,20 +225,18 @@ bool WKdTreeND::removePoint( WKdPointND* removablePoint )
 {
     if( m_points->size() > 0 && m_lowerChild == 0 && m_lowerChild == 0 )
     {
-        size_t removeCount = 0;
+        size_t keptNodeCount = 0;
         size_t size = m_points->size();
         for(size_t index = 0; index < size; index++)
-        {
-            if( m_points->at( index ) == removablePoint )
+            if( m_points->at( index ) != removablePoint )
             {
-                for( size_t index2 = index + 1; index2 < size - removeCount; index2++ )
-                    m_points->at( index ) = m_points->at( index + 1 );
-                removeCount++;
+                if( index > keptNodeCount )
+                    m_points->at( keptNodeCount ) = m_points->at( index );
+                keptNodeCount++;
             }
-        }
-        m_points->resize( size - removeCount );
-        m_points->reserve( size - removeCount );
-        return removeCount > 0;
+        m_points->resize( keptNodeCount );
+        m_points->reserve( keptNodeCount );
+        return keptNodeCount < size;
     }
     else
     {

LiDARToolbox/src/surfaceDetectionByLari/WLariBoundaryDetector.cpp

@@ -113,6 +113,7 @@ void WLariBoundaryDetector::detectInputCluster( vector<WSpatialDomainKdPoint*>*
         clusterPoint->setSpatialPoint( currentPoint );
         clusterPoints->push_back( clusterPoint );
         currentPoint->setClusterID( 9 );
+        clusterPoint->setIsAddedToPlane( false );
         delete coordinate;
     }
 

LiDARToolbox/src/surfaceDetectionByLari/structure/WBoundaryDetectPoint.cpp

@@ -30,11 +30,13 @@ WBoundaryDetectPoint::WBoundaryDetectPoint( vector<double> coordinate ) : WKdPoi
 {
     if( coordinate.size() >= 3 )
         m_zCoordinate = coordinate[2];
+    m_isAddedToPlane = false;
 }
 
 WBoundaryDetectPoint::WBoundaryDetectPoint( double x, double y, double z ) : WKdPointND( x, y )
 {
     m_zCoordinate = z;
+    m_isAddedToPlane = false;
 }
 
 WBoundaryDetectPoint::~WBoundaryDetectPoint()

LiDARToolbox/src/surfaceDetectionByLari/structure/WParameterDomainKdPoint.cpp

@@ -28,12 +28,14 @@
 
 WParameterDomainKdPoint::WParameterDomainKdPoint( vector<double> coordinate ) : WKdPointND( coordinate )
 {
+    m_isAddedToPlane = false;
     m_markedToRefresh = true;
     m_extentPointCount = 0;
 }
 
 WParameterDomainKdPoint::WParameterDomainKdPoint( double x0, double y0, double z0 ) : WKdPointND( x0, y0, z0 )
 {
+    m_isAddedToPlane = false;
     m_markedToRefresh = true;
     m_extentPointCount = 0;
 }