[ADD #371] Lari/Habib approach module can display point's best fitted planes...

[ADD #371] Lari/Habib approach module can display point's best fitted planes and the parameter domain
 * Added features:
 *   - Displaying the best for each pointfitted plane (calculated using the least
 *     squares method in relation th neighbor points)
 *   - The parameter domain points can be displayed now. Each oof them displays the best
 *   - fitted plane properties
 *
 * Bug fix:
 *   - Fixed the Hessesche normal formula calculation. Previously A, B and C were
 *     normalized just like a normal vector (Actually they depict a normal vector)

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

@@ -27,12 +27,13 @@
 
 #include "WPointSearcher.h"
 
-WPointSearcher::WPointSearcher()
+WPointSearcher::WPointSearcher( WKdTreeND* kdTree )
 {
     m_distanceSteps = 4;
     m_examinedKdTree = 0;
     m_maxResultPointCount = 50;
     m_maxSearchDistance = 0.5;
+    m_examinedKdTree = kdTree;
 }
 WPointSearcher::~WPointSearcher()
 {

LiDARToolbox/src/common/datastructures/kdtree/WPointSearcher.h

@@ -45,8 +45,9 @@ class WPointSearcher
 public:
     /**
      * Instantiates the points searcher.
+     * \param kdTree Assigned source kd tree to search points.
      */
-    WPointSearcher();
+    explicit WPointSearcher( WKdTreeND* kdTree );
     /**
      * Destroys the points searcher
      */

LiDARToolbox/src/common/math/leastSquares/WLeastSquares.cpp

@@ -139,18 +139,28 @@ void WLeastSquares::calculateHessescheNormalForm()
     for( size_t index = 0; index < m_dimensions; index++ )
         sumSquared += pow( m_hessescheNormalForm[index], 2.0 );
     sumSquared = pow( sumSquared, 0.5 );
-    for( size_t index = 0; index < m_dimensions; index++ )
-        m_hessescheNormalForm[index] /= sumSquared;
+    //for( size_t index = 0; index < m_dimensions; index++ )
+    //    m_hessescheNormalForm[index] /= sumSquared;
 }
 vector<double> WLeastSquares::getParametersXYZ0()
+{
+    return getParametersXYZ0( m_hessescheNormalForm );
+}
+vector<double> WLeastSquares::getParametersXYZ0( vector<double> hessescheNormalForm )
 {
     vector<double> parameters;
-    double squaredSum = 0;
-    for( size_t index = 0; index < m_dimensions; index++ )
-        squaredSum += pow( m_hessescheNormalForm[index], 2.0 );
-    for( size_t index = 0; index < m_dimensions; index++ )
-        parameters.push_back( -m_hessescheNormalForm[index] *
-                m_hessescheNormalForm[ m_dimensions] / squaredSum );
+    double a = hessescheNormalForm[0];
+    double b = hessescheNormalForm[1];
+    double c = hessescheNormalForm[2];
+    double d = hessescheNormalForm[3];
+    double a2 = a * a;
+    double b2 = b * b;
+    double c2 = c * c;
+    double sum = a2 + b2 + c2;
+
+    parameters.push_back( -a * d / sum );
+    parameters.push_back( -b * d / sum );
+    parameters.push_back( -c * d / sum );
     return parameters;
 }
 double WLeastSquares::getDistanceToPlane( WPosition point )
@@ -167,17 +177,22 @@ double WLeastSquares::getDistanceToPlane( WPosition point )
 }
 WPosition WLeastSquares::getNearestPointTo( WPosition point )
 {
-    double amountN = m_hessescheNormalForm[m_dimensions];
+    return getNearestPointTo( m_hessescheNormalForm, point );
+}
+WPosition WLeastSquares::getNearestPointTo( vector<double> planeHessescheNormalForm, WPosition point )
+{
+    double dimensions = planeHessescheNormalForm.size() - 1;
+    double amountN = planeHessescheNormalForm[dimensions];
     double amountR = 0;
-    for(size_t dimension = 0; dimension < m_dimensions; dimension++ )
+    for(size_t dimension = 0; dimension < dimensions; dimension++ )
     {
-        amountN += m_hessescheNormalForm[dimension] * point[dimension];
-        amountR += m_hessescheNormalForm[dimension] * m_hessescheNormalForm[dimension];
+        amountN += planeHessescheNormalForm[dimension] * point[dimension];
+        amountR += planeHessescheNormalForm[dimension] * planeHessescheNormalForm[dimension];
     }
     double r = -amountN / amountR;
 
     WPosition cuttingPoint( point[0], point[1], point[2] );
-    for( size_t dimension = 0; dimension < m_dimensions; dimension++ )
-        cuttingPoint[dimension] += m_hessescheNormalForm[dimension] * r;
+    for( size_t dimension = 0; dimension < dimensions; dimension++ )
+        cuttingPoint[dimension] += planeHessescheNormalForm[dimension] * r;
     return cuttingPoint;
 }

LiDARToolbox/src/common/math/leastSquares/WLeastSquares.h

@@ -73,6 +73,15 @@ public:
      * \return Parameter space coordinates corresponding to Lari/Habib 2014.
      */
     vector<double> getParametersXYZ0();
+
+    /**
+     * Calculates a plane formula of the parameter domain using the approach of 
+     * Lari/Habib using the Hessesche normal form as input plane formula.
+     * \param hessescheNormalForm The Hessesche normal formula of a plane.
+     * \return Parameters X_0, Y_0 and Z_0 that describe a plane using the approach of 
+     *         Lari/Habib.
+     */
+    static vector<double> getParametersXYZ0( vector<double> hessescheNormalForm );
     /**
      * Returns a point distance to the currently calculated plane.
      * analyzeData() must have been executes.
@@ -88,6 +97,14 @@ public:
      * \return The nearest coordinate on the calculates plane of an arbitrary point.
      */
     WPosition getNearestPointTo( WPosition point );
+    /**
+     * Returns a nearest point to the input point's coordinate that lies on the 
+     * calculated plane.
+     * \param planeHessescheNormalForm Plane on which the nearest point is found.
+     * \param point Point to get the nearest coordinate on the plane of.
+     * \return The nearest coordinate on the calculates plane of an arbitrary point.
+     */
+    static WPosition getNearestPointTo( vector<double> planeHessescheNormalForm, WPosition point );
 
 private:
     /**

LiDARToolbox/src/surfaceDetectionByLari/WLariOutliner.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 <vector>
+#include "WLariOutliner.h"
+#include "../common/math/leastSquares/WLeastSquares.h"
+
+
+WLariOutliner::WLariOutliner( WSurfaceDetectorLari* surfaceDetector )
+{
+    m_surfaceDetector = surfaceDetector;
+}
+
+WLariOutliner::~WLariOutliner()
+{
+}
+boost::shared_ptr< WDataSetPoints > WLariOutliner::outlineParameterDomain()
+{
+    WDataSetPoints::VertexArray outVertices(
+            new WDataSetPoints::VertexArray::element_type() );
+    WDataSetPoints::ColorArray outColors(
+            new WDataSetPoints::ColorArray::element_type() );
+    vector<WKdTreeND*>* parameterNodes = m_surfaceDetector->getParameterDomain()->getAllLeafNodes();
+    for( size_t index = 0; index < parameterNodes->size(); index++ )
+    {
+        WParameterDomainKdNode* parameterDomainNode = ( WParameterDomainKdNode* )( parameterNodes->at( index ) );
+        vector<double> parameterCoordinate = parameterDomainNode->getNodePoints()->at( 0 );
+        for( size_t dimension = 0; dimension < parameterCoordinate.size(); dimension++ )
+            outVertices->push_back( parameterCoordinate[dimension] );
+        for( size_t colorCh = 0; colorCh < 3; colorCh++ )
+            outColors->push_back( 0.50 );
+    }
+    boost::shared_ptr< WDataSetPoints > outputPoints(
+            new WDataSetPoints( outVertices, outColors ) );
+    return outputPoints;
+}
+boost::shared_ptr< WDataSetPoints > WLariOutliner::outlineSpatialDomain()
+{
+    WDataSetPoints::VertexArray outVertices(
+            new WDataSetPoints::VertexArray::element_type() );
+    WDataSetPoints::ColorArray outColors(
+            new WDataSetPoints::ColorArray::element_type() );
+    vector<WKdTreeND*>* spatialNodes = m_surfaceDetector->getSpatialDomain()->getAllLeafNodes();
+    for( size_t index = 0; index < spatialNodes->size(); index++ )
+    {
+        WSpatialDomainKdNode* spatialDomainNode = ( WSpatialDomainKdNode* )( spatialNodes->at( index ) );
+        vector<double> spatialCoordinate = spatialDomainNode->getNodePoints()->at( 0 );
+        vector<double> eigenValues = spatialDomainNode->getEigenValues();
+
+        double sumLambda = 0;        //TODO(aschwarzkopf): Unused
+        for( size_t i = 0; i < eigenValues.size(); i++ )
+            sumLambda += eigenValues[i];
+        vector<double> nLambdaI;    //TODO(aschwarzkopf): Unused
+        for( size_t i = 0; i < eigenValues.size(); i++ )
+            nLambdaI.push_back( eigenValues[i] / sumLambda );
+        for( size_t dimension = 0; dimension < spatialCoordinate.size(); dimension++ )
+            outVertices->push_back( spatialCoordinate[dimension] );
+        bool isPlanar = m_surfaceDetector->calculateIsPlanarPoint( eigenValues );
+        bool isCylindrical = m_surfaceDetector->calculateIsCylindricalPoint( eigenValues );
+        if( isPlanar || isCylindrical )
+        {
+            outColors->push_back( isPlanar ?1 :0 );
+            outColors->push_back( isCylindrical ?0.5 :0 );
+            outColors->push_back( isCylindrical ?1.0 :0 );
+        }
+        else
+        {
+            for( size_t colorCh = 0; colorCh < 3; colorCh++ )
+                outColors->push_back( 0.50 );
+        }
+    }
+    boost::shared_ptr< WDataSetPoints > outputPoints(
+            new WDataSetPoints( outVertices, outColors ) );
+    return outputPoints;
+}
+boost::shared_ptr< WTriangleMesh > WLariOutliner::outlineLeastSquaresPlanes( double squaresWidth )
+{
+    boost::shared_ptr< WTriangleMesh > outputMesh( new WTriangleMesh( 0, 0 ) );
+    vector<WKdTreeND*>* spatialNodes = m_surfaceDetector->getSpatialDomain()->getAllLeafNodes();
+    for( size_t index = 0; index < spatialNodes->size(); index++ )
+    {
+        WSpatialDomainKdNode* spatialDomainNode = ( WSpatialDomainKdNode* )( spatialNodes->at( index ) );
+        vector<double> spatialCoordinate = spatialDomainNode->getNodePoints()->at( 0 );
+        WPosition spatialPoint( 0.0, 0.0, 0.0 );
+        for( size_t dimension = 0; dimension < spatialCoordinate.size(); dimension++ )
+            spatialPoint[dimension] = spatialCoordinate[dimension];
+        vector<double> planeFormula = spatialDomainNode->getHessescheNormalForm();
+
+        WMTempLeastSquaresTest::outlineNormalPlane( planeFormula, spatialPoint, squaresWidth / 2.0, outputMesh );
+    }
+    return outputMesh;
+}

LiDARToolbox/src/surfaceDetectionByLari/WLariOutliner.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 WLARIOUTLINER_H
+#define WLARIOUTLINER_H
+
+#include <iostream>
+#include <vector>
+
+#include "WSurfaceDetectorLari.h"
+#include "core/dataHandler/WDataSetPoints.h"
+#include "structure/WParameterDomainKdNode.h"
+#include "structure/WSpatialDomainKdNode.h"
+#include "../common/datastructures/kdtree/WPointSearcher.h"
+#include "core/common/math/principalComponentAnalysis/WPrincipalComponentAnalysis.h"
+#include "core/common/WRealtimeTimer.h"
+#include "../common/math/leastSquares/WLeastSquares.h"
+#include "../tempLeastSquaresTest/WMTempLeastSquaresTest.h"
+
+
+using std::cout;
+using std::endl;
+using std::vector;
+
+/**
+ * Class that is used to outline the segmentation results of the approach of Lari/Habib.
+ */
+class WLariOutliner
+{
+public:
+    /**
+     * Creates the instance that is used to segment surfaces using the approach of 
+     * Lari/Habib.
+     * \param surfaceDetector Surface detection instance that uses the approach of 
+     *                        Lari/Habib.
+     */
+    explicit WLariOutliner( WSurfaceDetectorLari* surfaceDetector );
+    /**
+     * Destroys the result outlining instance.
+     */
+    virtual ~WLariOutliner();
+
+    /**
+     * Puts out points of the spatial domain.
+     * \return Points of the spatiial domain. Red ones are planar and blue points belong 
+     *         to the linear/cylindrical features.
+     */
+    boost::shared_ptr< WDataSetPoints > outlineParameterDomain();
+    /**
+     * Returns points of the parameter domain (Plane describing features of each 
+     * corresponding spatial point).
+     * \return Points of the parameter domain that describe each spatial point's fitted 
+     *         plane.
+     */
+    boost::shared_ptr< WDataSetPoints > outlineSpatialDomain();
+    /**
+     * Outlines each point's best fitted plane.
+     * \param squaresWidth Square width of the outlined best fitted planes of input 
+     *                     points (in relation to its neighbors).
+     * \return output triangle mesh that depicts best fitted planes of each input point.
+     */
+    boost::shared_ptr< WTriangleMesh > outlineLeastSquaresPlanes( double squaresWidth );
+
+private:
+    /**
+     * Assigned Surface detection instance that uses the approach of Lari/Habib. It's 
+     * used to retirieve a corresponding spatial or parameter domain.
+     */
+    WSurfaceDetectorLari* m_surfaceDetector;
+};
+
+#endif  // WLARIOUTLINER_H

LiDARToolbox/src/surfaceDetectionByLari/WMSurfaceDetectionByLari.cpp

@@ -80,10 +80,19 @@ void WMSurfaceDetectionByLari::connectors()
 {
     m_input = WModuleInputData< WDataSetPoints >::createAndAdd( shared_from_this(), "input", "The mesh to display" );
 
-    m_outputPoints = boost::shared_ptr< WModuleOutputData< WDataSetPoints > >(
-                new WModuleOutputData< WDataSetPoints >( shared_from_this(), "points", "The loaded mesh." ) );
-
-    addConnector( m_outputPoints );
+    m_outputSpatialDomain = boost::shared_ptr< WModuleOutputData< WDataSetPoints > >(
+            new WModuleOutputData< WDataSetPoints >( shared_from_this(),
+                    "Spatial domain points", "//TODO: description" ) );
+    m_outputParameterDomain = boost::shared_ptr< WModuleOutputData< WDataSetPoints > >(
+            new WModuleOutputData< WDataSetPoints >( shared_from_this(),
+                    "Parameter domain points", "//TODO: description" ) );
+    m_outputLeastSquaresPlanes = boost::shared_ptr< WModuleOutputData< WTriangleMesh > >(
+            new WModuleOutputData< WTriangleMesh >( shared_from_this(),
+                    "Least Squares Planes", "//TODO: description" ) );
+
+    addConnector( m_outputSpatialDomain );
+    addConnector( m_outputParameterDomain );
+    addConnector( m_outputLeastSquaresPlanes );
 //    addConnector( m_buildings );
     WModule::connectors();
 }
@@ -108,6 +117,8 @@ void WMSurfaceDetectionByLari::properties()
     m_numberPointsK = m_properties->addProperty( "Number points K=", "", 12, m_propCondition );
     m_maxPointDistanceR = m_properties->addProperty( "Max point distance r=", "", 1.0, m_propCondition );
 
+    m_squareWidth = m_properties->addProperty( "Plane outline size: ", "", 0.2, m_propCondition );
+
     m_planarGroup = m_properties->addPropertyGroup( "Planar feature properties",
                                             "All conditions must be met to detect as a surface." );
     m_surfaceNLambda1Min = m_planarGroup->addProperty( "N Lambda 1 >=", "", 0.3, m_propCondition );
@@ -224,20 +235,24 @@ void WMSurfaceDetectionByLari::moduleMain()
                 m_progressStatus->increment( 1 );
             }
 
-            WSurfaceDetectorLari detector = WSurfaceDetectorLari();
-            detector.setNumberPointsK( m_numberPointsK->get() );
-            detector.setMaxPointDistanceR( m_maxPointDistanceR->get() );
-            detector.setPlanarNLambdaRange( 0, m_surfaceNLambda1Min->get(), m_surfaceNLambda1Max->get() );
-            detector.setPlanarNLambdaRange( 1, m_surfaceNLambda2Min->get(), m_surfaceNLambda2Max->get() );
-            detector.setPlanarNLambdaRange( 2, m_surfaceNLambda3Min->get(), m_surfaceNLambda3Max->get() );
-            detector.setCylindricalNLambdaRange( 0, m_cylNLambda1Min->get(), m_cylNLambda1Max->get() );
-            detector.setCylindricalNLambdaRange( 1, m_cylNLambda2Min->get(), m_cylNLambda2Max->get() );
-            detector.setCylindricalNLambdaRange( 2, m_cylNLambda3Min->get(), m_cylNLambda3Max->get() );
-            boost::shared_ptr< WDataSetPoints > pointsLari = detector.detectSurfaces( inputPointsVector );
-            //delete detector;
+            WSurfaceDetectorLari* detector = new WSurfaceDetectorLari();
+            detector->setNumberPointsK( m_numberPointsK->get() );
+            detector->setMaxPointDistanceR( m_maxPointDistanceR->get() );
+            detector->setPlanarNLambdaRange( 0, m_surfaceNLambda1Min->get(), m_surfaceNLambda1Max->get() );
+            detector->setPlanarNLambdaRange( 1, m_surfaceNLambda2Min->get(), m_surfaceNLambda2Max->get() );
+            detector->setPlanarNLambdaRange( 2, m_surfaceNLambda3Min->get(), m_surfaceNLambda3Max->get() );
+            detector->setCylindricalNLambdaRange( 0, m_cylNLambda1Min->get(), m_cylNLambda1Max->get() );
+            detector->setCylindricalNLambdaRange( 1, m_cylNLambda2Min->get(), m_cylNLambda2Max->get() );
+            detector->setCylindricalNLambdaRange( 2, m_cylNLambda3Min->get(), m_cylNLambda3Max->get() );
+
+            detector->analyzeData( inputPointsVector );
+            WLariOutliner outliner( detector );
+            m_outputSpatialDomain->updateData( outliner.outlineSpatialDomain() );
+            m_outputParameterDomain->updateData( outliner.outlineParameterDomain() );
+            m_outputLeastSquaresPlanes->updateData( outliner.outlineLeastSquaresPlanes( m_squareWidth->get() ) );
 
+            //delete detector;
             delete inputPointsVector;
-            m_outputPoints->updateData( pointsLari );
 
             m_nbPoints->set( count );
             m_infoRenderTimeSeconds->set( timer.elapsed() );

LiDARToolbox/src/surfaceDetectionByLari/WMSurfaceDetectionByLari.h

@@ -67,6 +67,7 @@
 #include "core/common/WItemSelectionItemTyped.h"
 #include "core/graphicsEngine/WGEUtils.h"
 #include "core/graphicsEngine/WGERequirement.h"
+#include "WLariOutliner.h"
 
 // forward declarations to reduce compile dependencies
 template< class T > class WModuleInputData;
@@ -160,11 +161,22 @@ private:
      * WDataSetPoints data input (proposed for LiDAR data).
      */
     boost::shared_ptr< WModuleInputData< WDataSetPoints > > m_input;
-
     /**
-     * WDataSetPointsGrouped data output as point groups depicting each building
+     * Output point set that depicts the input points of the spatial domains. Points are 
+     * hilighted if they belong to the planar or linear/cylindrical domain.
+     */
+    boost::shared_ptr< WModuleOutputData< WDataSetPoints > > m_outputSpatialDomain;
+    /**
+     * Output point set that depicts the parameter domain. Each point of that depicts 
+     * a planar formula (in relation to its neighbors) of each input point of the 
+     * spatial domain.
+     */
+    boost::shared_ptr< WModuleOutputData< WDataSetPoints > > m_outputParameterDomain;
+    /**
+     * Output connector that depicts best fitted planes of each input point in relation 
+     * to its neighbors.
      */
-    boost::shared_ptr< WModuleOutputData< WDataSetPoints > > m_outputPoints;
+    boost::shared_ptr< WModuleOutputData< WTriangleMesh > > m_outputLeastSquaresPlanes;
 
     /**
      * The OSG root node for this module. All other geodes or OSG nodes will be attached on this single node.
@@ -297,6 +309,11 @@ private:
      */
     WPropDouble m_cylNLambda3Max;
 
+    /**
+     * Width of each square that depichts a best fitted plane of each input point.
+     */
+    WPropDouble m_squareWidth;    //TODO(aschwarzkopf): Remove in future
+
 
     WPropTrigger  m_reloadData; //!< This property triggers the actual reading,
 

LiDARToolbox/src/surfaceDetectionByLari/WSurfaceDetectorLari.cpp

@@ -39,95 +39,84 @@ WSurfaceDetectorLari::WSurfaceDetectorLari()
     m_cylindricalNLambdaMin.resize( 3 );
     m_cylindricalNLambdaMax.reserve( 3 );
     m_cylindricalNLambdaMax.resize( 3 );
+    m_spadialDomain = new WSpatialDomainKdNode( 3 );
 }
 
 WSurfaceDetectorLari::~WSurfaceDetectorLari()
 {
 }
 
-boost::shared_ptr< WDataSetPoints > WSurfaceDetectorLari::detectSurfaces( vector<vector<double> >* inputPoints )
+void WSurfaceDetectorLari::analyzeData( vector<vector<double> >* inputPoints )
 {
     //TODO(aschwarzkopf): After draft stage refine in several methods and remove comments
     cout << "Attempting to analyze " << inputPoints->size() << " points" << endl;
-    WRealtimeTimer timer;
-    timer.reset();
-    WKdTreePcaProps* deletableTree = new WKdTreePcaProps( 3 );
-    deletableTree->add( inputPoints );
-    cout << "WKdTreeND took " << timer.elapsed() << " seconds." << endl;
+    delete m_spadialDomain;
+    m_spadialDomain = new WSpatialDomainKdNode( 3 );
+    m_parameterDomain = new WParameterDomainKdNode( 3 );
+    m_spadialDomain->add( inputPoints );
+    vector<vector<double> >* parameterPoints = new vector<vector<double> >();
 
-    WDataSetPoints::VertexArray outVertices(
-            new WDataSetPoints::VertexArray::element_type() );
-    WDataSetPoints::ColorArray outColors(
-            new WDataSetPoints::ColorArray::element_type() );
-
-    timer.reset();
-    WPointSearcher pointSearcher;
-    pointSearcher.setExaminedKdTree( deletableTree );
+    WPointSearcher pointSearcher( m_spadialDomain );
     pointSearcher.setMaxResultPointCount( m_numberPointsK );
     pointSearcher.setMaxSearchDistance( m_maxPointDistanceR );
-    WPrincipalComponentAnalysis pca;
-    vector<WKdTreeND*>* processedPoints = deletableTree->getAllLeafNodes();
-    cout << "Attempting PCA for " << processedPoints->size() << " points" << endl;
+    vector<WKdTreeND*>* spatialNodes = m_spadialDomain->getAllLeafNodes();
 
     //Generating properties for each existing point
-    for( size_t index = 0; index < processedPoints->size(); index++ )
+    for( size_t index = 0; index < spatialNodes->size(); index++ )
     {
         //Getting nearest n points
-        WKdTreePcaProps* pointProps = ( WKdTreePcaProps*)(processedPoints->at( index ) );
-        vector<double> pointOfInterest = pointProps->getNodePoints()->at( 0 );
-        pointSearcher.setSearchedPoint( pointOfInterest );
+        WSpatialDomainKdNode* spatialDomainNode = ( WSpatialDomainKdNode* )( spatialNodes->at( index ) );
+        vector<double> spatialCoordinate = spatialDomainNode->getNodePoints()->at( 0 );
+        pointSearcher.setSearchedPoint( spatialCoordinate );
         vector<WPointDistance>* nearestPoints = pointSearcher.getNearestPoints();
         vector<WPosition>* points = WPointSearcher::convertToPointSet( nearestPoints );
 
         //Calculating Eigen properties
+        WPrincipalComponentAnalysis pca;
         pca.analyzeData( points );
-        pointProps->createGeoProps();
-        WLariGeoProps* geoProps = pointProps->getGeoProps();
-        geoProps->setEigenVectors( pca.getDirections() );
+        spatialDomainNode->setEigenVectors( pca.getDirections() );
         vector<double> eigenValues = pca.getEigenValues();
-        vector<double> nLambdaI;    //TODO(aschwarzkopf): Unused
-        double sumLambda = 0;        //TODO(aschwarzkopf): Unused
-        geoProps->setEigenValues( eigenValues );
+        spatialDomainNode->setEigenValues( eigenValues );
 
         //Adding parameter space information
         WLeastSquares leastSquares;
         leastSquares.analyzeData( points );
-        geoProps->setParametersXYZ0( leastSquares.getParametersXYZ0() );
-        //for(size_t index = 0; index < geoProps->getParametersXYZ0().size(); index++)
-        //    cout << geoProps->getParametersXYZ0()[index] << "    ";
-        //cout << endl;
-
-        //Displaying points and classification
-        for( size_t i = 0; i < eigenValues.size(); i++ )
-            sumLambda += eigenValues[i];
-        for( size_t i = 0; i < eigenValues.size(); i++ )
-            nLambdaI.push_back( eigenValues[i] / sumLambda );
-        for( size_t dimension = 0; dimension < pointOfInterest.size(); dimension++ )
-            outVertices->push_back( pointOfInterest[dimension] );
-        bool isPlanar = calculateIsPlanarPoint( eigenValues );
-        bool isCylindrical = calculateIsCylindricalPoint( eigenValues );
-        if( isPlanar || isCylindrical )
-        {
-            outColors->push_back( isPlanar ?1 :0 );
-            outColors->push_back( isCylindrical ?0.5 :0 );
-            outColors->push_back( isCylindrical ?1.0 :0 );
-        }
-        else
-        {
-            for( size_t colorCh = 0; colorCh < 3; colorCh++ )
-                outColors->push_back( 0.50 );
-        }
+        spatialDomainNode->setHessescheNormalForm( leastSquares.getHessescheNormalForm() );
+        parameterPoints->push_back( leastSquares.getParametersXYZ0() );
 
         delete nearestPoints;
         delete points;
-        //outputPointSet.printNearestPoints(nearestPoints, "Default point set");
     }
-    cout << "WPointSearcher took " << timer.elapsed() << " seconds." << endl;
-    delete deletableTree;
-
-    boost::shared_ptr< WDataSetPoints > outputPoints(
-            new WDataSetPoints( outVertices, outColors ) );
-    return outputPoints;
+    m_parameterDomain->add( parameterPoints );
+    linkSpatialAndParameterDomain();
+}
+void WSurfaceDetectorLari::linkSpatialAndParameterDomain()
+{
+    WPointSearcher pointSearcher( m_spadialDomain );
+    pointSearcher.setMaxResultPointCount( 1 );
+    pointSearcher.setMaxSearchDistance( 0.0 );
+    vector<WKdTreeND*>* parameterNodes = m_parameterDomain->getAllLeafNodes();
+    size_t noSuccess = 0;
+    size_t success = 0;
+    for( size_t index = 0; index < parameterNodes->size(); index++ )
+    {
+        WParameterDomainKdNode* parameterDomainNode = ( WParameterDomainKdNode* )( parameterNodes->at( index ) );
+        vector<double> parameterCoordinate = parameterDomainNode->getNodePoints()->at( 0 );
+        pointSearcher.setSearchedPoint( parameterCoordinate );
+        vector<WPointDistance>* nearestPoints = pointSearcher.getNearestPoints();
+        noSuccess += nearestPoints->size() == 0 ?1 :0;
+        success += nearestPoints->size() > 0 ?1 :0;
+    }
+    std::cout << "sussess: " << success << std::endl;
+    std::cout << "no sussess: " << noSuccess << std::endl;
+}
+WParameterDomainKdNode* WSurfaceDetectorLari::getParameterDomain()
+{
+    return m_parameterDomain;
+}
+WSpatialDomainKdNode* WSurfaceDetectorLari::getSpatialDomain()
+{
+    return m_spadialDomain;
 }
 bool WSurfaceDetectorLari::calculateIsPlanarPoint( vector<double> eigenValues )
 {

LiDARToolbox/src/surfaceDetectionByLari/WSurfaceDetectorLari.h

@@ -29,7 +29,8 @@
 #include <vector>
 
 #include "core/dataHandler/WDataSetPoints.h"
-#include "structure/WKdTreePcaProps.h"