[ADD #371] Added a module for trying out the least squares algorithm

 * First module: Creates a sphere or cube of random points of the radius of 2.
 * Second module: Visualizes the least squares process by putting the output plane to
 * the triangle mesh output.
 * Transformed the points cropping module in order to be able to stretch and rotate the
 * point set.

LiDARToolbox/src/WToolkit.cpp

@@ -41,6 +41,10 @@
 #include "surfaceDetectionByPCL/WMSurfaceDetectionByPCL.h"
 #include "wallDetectionByPCA/WMWallDetectionByPCA.h"
 
+#include "tempLeastSquaresTest/WMTempLeastSquaresTest.h"
+#include "tempRandomPoints/WMTempRandomPoints.h"
+
+
 // #include "WToolkit.h"
 
 /**
@@ -65,6 +69,9 @@ extern "C" void WLoadModule( WModuleList& m ) // NOLINT
     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 ) );
+
+    m.push_back( boost::shared_ptr< WModule >( new WMTempLeastSquaresTest ) );
+    m.push_back( boost::shared_ptr< WModule >( new WMTempRandomPoints ) );
 }
 
 /**

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

@@ -148,8 +148,36 @@ vector<double> WLeastSquares::getParametersXYZ0()
     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++)
+    for( size_t index = 0; index < m_dimensions; index++ )
         parameters.push_back( -m_hessescheNormalForm[index] *
                 m_hessescheNormalForm[ m_dimensions] / squaredSum );
     return parameters;
 }
+double WLeastSquares::getDistanceToPlane( WPosition point )
+{
+    double normalAbsolute = 0;
+    for( size_t dimension = 0; dimension < m_dimensions; dimension++ )
+        normalAbsolute += pow( m_hessescheNormalForm[dimension], 2.0 );
+    normalAbsolute = pow( normalAbsolute, 0.5 );
+
+    double distance = 0;
+    for( size_t dimension = 0; dimension < m_dimensions; dimension++ )
+        distance += point[dimension] * m_hessescheNormalForm[dimension];
+    return ( distance + m_hessescheNormalForm[m_dimensions] ) / normalAbsolute;
+}
+WPosition WLeastSquares::getNearestPointTo( WPosition point )
+{
+    double amountN = m_hessescheNormalForm[m_dimensions];
+    double amountR = 0;
+    for(size_t dimension = 0; dimension < m_dimensions; dimension++ )
+    {
+        amountN += m_hessescheNormalForm[dimension] * point[dimension];
+        amountR += m_hessescheNormalForm[dimension] * m_hessescheNormalForm[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;
+    return cuttingPoint;
+}

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

@@ -73,6 +73,21 @@ public:
      * \return Parameter space coordinates corresponding to Lari/Habib 2014.
      */
     vector<double> getParametersXYZ0();
+    /**
+     * Returns a point distance to the currently calculated plane.
+     * analyzeData() must have been executes.
+     * \param point Point from which the plane distance is calculated.
+     * \return Perpendicular distance from a point to the plane calculated py the least 
+     *         squares algorithm.
+     */
+    double getDistanceToPlane( WPosition point );
+    /**
+     * Returns a nearest point to the input point's coordinate that lies on the 
+     * calculated plane.
+     * \param point Point to get the nearest coordinate on the plane of.
+     * \return The nearest coordinate on the calculates plane of an arbitrary point.
+     */
+    WPosition getNearestPointTo( WPosition point );
 
 private:
     /**

LiDARToolbox/src/pointsTransform/WMPointsTransform.cpp

@@ -111,6 +111,15 @@ void WMPointsTransform::properties()
     m_translateZ = m_translatePointsGroup->addProperty( "Z offset: ", "Translates the point set across the Z axis by "
                                                         "that offset.", 0.0, m_propCondition  );
 
+    m_groupMultiplyPoints = m_properties->addPropertyGroup( "Coordinate multiplication",
+                                            "Multiplies each X, Y and Z value by a factor." );
+    m_factorX = m_groupMultiplyPoints->addProperty( "X factor: ", "Number which multiplies each X coordinate.",
+                                                    1.0, m_propCondition  );
+    m_factorY = m_groupMultiplyPoints->addProperty( "Y factor: ", "Number which multiplies each X coordinate.",
+                                                    1.0, m_propCondition  );
+    m_factorZ = m_groupMultiplyPoints->addProperty( "Z factor: ", "Number which multiplies each X coordinate.",
+                                                    1.0, m_propCondition  );
+
     m_groupRotation = m_properties->addPropertyGroup( "Rotation options",
                                             "Applis rotation across three planes in sequence." );
     m_rotation1AngleXY = m_groupRotation->addProperty( "1st rot. (plane XY)", "First applied rotation: Along the plane "
@@ -257,6 +266,10 @@ boost::shared_ptr< WDataSetPoints > WMPointsTransform::getTransformedPointSet()
             y += offset[1];
             z += offset[2];
 
+            x *= m_factorX->get();
+            y *= m_factorY->get();
+            z *= m_factorZ->get();
+
             x -= m_rotationAnchorX->get();
             y -= m_rotationAnchorY->get();
             z -= m_rotationAnchorZ->get();

LiDARToolbox/src/pointsTransform/WMPointsTransform.h

@@ -74,7 +74,8 @@ class WDataSetScalar;
 class WGEManagedGroupNode;
 
 /**
- * Crops a point data set to a selection or cuts away a cube area.
+ * Transforms a point set: Available options: Cropping, stretching, translation and 
+ * rotation.
  * \ingroup modules
  */
 class WMPointsTransform: public WModule
@@ -238,6 +239,23 @@ private:
      */
     WPropDouble m_translateZ;
 
+    /**
+     * Group that multiplies each coordinate by a factor.
+     */
+    WPropGroup m_groupMultiplyPoints;
+    /**
+     * Each X coordinate is multiplied by this value
+     */
+    WPropDouble m_factorX;
+    /**
+     * Each Y coordinate is multiplied by this value
+     */
+    WPropDouble m_factorY;
+    /**
+     * Each Z coordinate is multiplied by this value
+     */
+    WPropDouble m_factorZ;
+
     /**
      * Rotation options.
      */

LiDARToolbox/src/tempLeastSquaresTest/WMTempLeastSquaresTest.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 <string>
+
+#include <fstream>  // std::ifstream
+#include <iostream> // std::cout
+#include <vector>
+
+#include <osg/Geometry>
+#include "core/kernel/WModule.h"
+
+#include "core/dataHandler/WDataSetScalar.h"
+#include "core/graphicsEngine/callbacks/WGELinearTranslationCallback.h"
+#include "core/graphicsEngine/shaders/WGEPropertyUniform.h"
+#include "core/graphicsEngine/shaders/WGEShader.h"
+#include "core/graphicsEngine/WGEGeodeUtils.h"
+#include "core/graphicsEngine/WGEManagedGroupNode.h"
+#include "core/kernel/WKernel.h"
+#include "core/kernel/WModuleInputData.h"
+#include "WMTempLeastSquaresTest.xpm"
+#include "WMTempLeastSquaresTest.h"
+#include "../common/datastructures/octree/WOctree.h"
+
+// This line is needed by the module loader to actually find your module.
+//W_LOADABLE_MODULE( WMTempLeastSquaresTest )
+//TODO(aschwarzkopf): Reenable above after solving the toolbox problem
+
+WMTempLeastSquaresTest::WMTempLeastSquaresTest():
+    WModule(),
+    m_propCondition( new WCondition() )
+{
+}
+
+WMTempLeastSquaresTest::~WMTempLeastSquaresTest()
+{
+}
+
+boost::shared_ptr< WModule > WMTempLeastSquaresTest::factory() const
+{
+    return boost::shared_ptr< WModule >( new WMTempLeastSquaresTest() );
+}
+
+const char** WMTempLeastSquaresTest::getXPMIcon() const
+{
+    return WMTempLeastSquaresTest_xpm;
+}
+const std::string WMTempLeastSquaresTest::getName() const
+{
+    return "[Temp.] Least squares test";
+}
+
+const std::string WMTempLeastSquaresTest::getDescription() const
+{
+    return "Crops point data to a selection.";
+}
+
+void WMTempLeastSquaresTest::connectors()
+{
+    m_input = WModuleInputData< WDataSetPoints >::createAndAdd( shared_from_this(), "input", "The mesh to display" );
+
+    m_output = boost::shared_ptr< WModuleOutputData< WTriangleMesh > >(
+                new WModuleOutputData< WTriangleMesh >(
+                        shared_from_this(), "points", "The loaded points." ) );
+
+    addConnector( m_output );
+    WModule::connectors();
+}
+
+void WMTempLeastSquaresTest::properties()
+{
+    // ---> Put the code for your properties here. See "src/modules/template/" for an extensively documented example.
+    WModule::properties();
+}
+
+void WMTempLeastSquaresTest::requirements()
+{
+}
+
+void WMTempLeastSquaresTest::moduleMain()
+{
+    infoLog() << "Thrsholding example main routine started";
+
+    // get notified about data changes
+    m_moduleState.setResetable( true, true );
+    m_moduleState.add( m_input->getDataChangedCondition() );
+    m_moduleState.add( m_propCondition );
+
+    ready();
+
+    // graphics setup
+    m_rootNode = osg::ref_ptr< WGEManagedGroupNode >( new WGEManagedGroupNode( m_active ) );
+    WKernel::getRunningKernel()->getGraphicsEngine()->getScene()->insert( m_rootNode );
+
+    // main loop
+    while( !m_shutdownFlag() )
+    {
+        //infoLog() << "Waiting ...";
+        m_moduleState.wait();
+
+
+        //m_output->updateData( getRandomPoints() );
+        analyzeBestFittedPlane();
+
+//        std::cout << "this is WOTree " << std::endl;
+
+        // woke up since the module is requested to finish?
+        if  ( m_shutdownFlag() )
+        {
+            break;
+        }
+
+        boost::shared_ptr< WDataSetPoints > points2 = m_input->getData();
+        if  ( !points2 )
+        {
+            continue;
+        }
+
+        // ---> Insert code doing the real stuff here
+    }
+
+    WKernel::getRunningKernel()->getGraphicsEngine()->getScene()->remove( m_rootNode );
+}
+void WMTempLeastSquaresTest::setProgressSettings( size_t steps )
+{
+    m_progress->removeSubProgress( m_progressStatus );
+    std::string headerText = "Loading data";
+    m_progressStatus = boost::shared_ptr< WProgress >( new WProgress( headerText, steps ) );
+    m_progress->addSubProgress( m_progressStatus );
+}
+void WMTempLeastSquaresTest::analyzeBestFittedPlane()
+{
+    boost::shared_ptr< WDataSetPoints > points = m_input->getData();
+//    std::cout << "Execute cycle\r\n";
+    if( points )
+    {
+        m_verts = points->getVertices();
+        m_colors = points->getColors();
+        vector<WPosition>* data = new vector<WPosition>();
+        WPosition mean( 0, 0, 0 );
+        for( size_t index = 0; index < m_verts->size() / 3; index++ )
+        {
+            data->push_back( WPosition( m_verts->at( index*3 ),
+                                       m_verts->at( index*3 + 1 ),
+                                       m_verts->at( index*3 + 2 ) ) );
+            for( size_t dimension = 0; dimension < mean.size(); dimension++ )
+                mean[dimension] += m_verts->at( index * 3 + dimension ) * 3.0 / m_verts->size();
+        }
+        WLeastSquares leastSquares;
+        leastSquares.analyzeData( data );
+        WPosition cuttingPoint = leastSquares.getNearestPointTo( mean );
+        double maxDistance = 0.0;
+        for( size_t index = 0; index < data->size(); index++ )
+        {
+            double distance = 0;
+            for( size_t dimension = 0; dimension < cuttingPoint.size(); dimension++ )
+            {
+                distance += pow( cuttingPoint[dimension] - data->at( index )[dimension], 2.0 );
+            }
+            distance = pow( distance, 0.5 );
+            if(distance > maxDistance)
+                maxDistance = distance;
+        }
+        vector<double> planeFormula = leastSquares.getHessescheNormalForm();
+
+        size_t strongestDimension = 0;
+        double strongestDimensionExtent = 0;
+        for( size_t dimension = 0; dimension < cuttingPoint.size(); dimension++ )
+            if( planeFormula[dimension] > strongestDimensionExtent )
+            {
+                strongestDimension = dimension;
+                strongestDimensionExtent = planeFormula[dimension];
+            }
+        size_t dimensionVec1 = strongestDimension == 0 ?1 :0;
+        size_t dimensionVec2 = strongestDimension == 2 ?1 :2;
+        cout << "Vec1: " << dimensionVec1 << "    Vec2: " << dimensionVec2 << "    Strongest: " << strongestDimension << endl;
+        WVector3d vector1( 0, 0, 0 );
+        vector1[dimensionVec1] = planeFormula[strongestDimension];
+        vector1[strongestDimension] = - planeFormula[dimensionVec1]/* / planeFormula[strongestDimension]*/;
+        vector1 = getNormalizedVector( vector1 );
+        WPosition vector2( planeFormula[1] * vector1[2] - planeFormula[2] * vector1[1],
+                           planeFormula[2] * vector1[0] - planeFormula[0] * vector1[2],
+                           planeFormula[0] * vector1[1] - planeFormula[1] * vector1[0] );
+        WPosition vector3( vector1[1] * vector2[2] - vector1[2] * vector2[1],
+                           vector1[2] * vector2[0] - vector1[0] * vector2[2],
+                           vector1[0] * vector2[1] - vector1[1] * vector2[0] );
+        cout << "Reverse transformation: " << vector3 << endl;
+        boost::shared_ptr< WTriangleMesh > outputMesh( new WTriangleMesh( 0, 0 ) );
+        for( double factor = 1.0; factor >= -2.0; factor -= 2.0 )
+            for( double vectorN = 1.0; vectorN >= -2.0; vectorN -= 2.0 )
+            {
+                WPosition vecN( 0.0, 0.0, 0.0 );
+                for( size_t dimension = 0; dimension < cuttingPoint.size(); dimension++ )
+                    vecN[dimension] = maxDistance * 1.5 * ( vector1[dimension] * vectorN + vector2[dimension] * factor );
+                outputMesh->addVertex( vecN );
+            }
+        outputMesh->addTriangle( 2, 0, 1 );
+        outputMesh->addTriangle( 3, 2, 1 );
+        m_output->updateData( outputMesh );
+
+        std::cout << "Mean: " << mean << std::endl;
+
+        std::cout << "Distance to mean: " << leastSquares.getDistanceToPlane( mean ) << std::endl;
+
+        std::cout << "Nearest point:  " << cuttingPoint << std::endl;
+
+        std::cout << "Its distance to plane: " << leastSquares.getDistanceToPlane( cuttingPoint ) << std::endl;
+
+        std::cout << "Plane: ";
+        for( size_t index = 0; index < planeFormula.size(); index++ )
+            std::cout << planeFormula[index] << ", ";
+        std::cout << endl << endl;
+    }
+}
+WPosition WMTempLeastSquaresTest::getNormalizedVector( WVector3d vector )
+{
+    double sumSquared = 0;
+    for( size_t index = 0; index < vector.size(); index++ )
+        sumSquared += pow( vector[index], 2.0 );
+    sumSquared = pow( sumSquared, 0.5 );
+    for( size_t index = 0; index < vector.size(); index++ )
+        vector[index] /= sumSquared;
+    return vector;
+}

LiDARToolbox/src/tempLeastSquaresTest/WMTempLeastSquaresTest.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 WMTEMPLEASTSQUARESTEST_H
+#define WMTEMPLEASTSQUARESTEST_H
+
+
+#include <liblas/liblas.hpp>
+#include <string>
+#include <vector>
+
+#include <fstream>  // std::ifstream
+#include <iostream> // std::cout
+
+#include "core/kernel/WModule.h"
+
+#include "core/graphicsEngine/WGEManagedGroupNode.h"
+#include "core/graphicsEngine/shaders/WGEShader.h"
+#include "core/graphicsEngine/WTriangleMesh.h"
+
+#include <osg/ShapeDrawable>
+#include <osg/Geode>
+#include "core/dataHandler/WDataSetPoints.h"
+#include "../common/datastructures/octree/WOctree.h"
+
+
+
+
+
+//!.Unnecessary imports
+#include "core/common/WItemSelection.h"
+#include "core/common/WItemSelector.h"
+
+#include "core/kernel/WModuleOutputData.h"
+
+#include <osg/Group>
+#include <osg/Material>
+#include <osg/StateAttribute>
+
+#include "core/kernel/WKernel.h"
+#include "core/common/exceptions/WFileNotFound.h"
+#include "core/common/WColor.h"
+#include "core/common/WPathHelper.h"
+#include "core/common/WPropertyHelper.h"
+#include "core/common/WItemSelectionItem.h"
+#include "core/common/WItemSelectionItemTyped.h"
+#include "core/graphicsEngine/WGEUtils.h"
+#include "core/graphicsEngine/WGERequirement.h"
+
+
+#include "core/common/math/linearAlgebra/WVectorFixed.h"
+#include "../common/math/leastSquares/WLeastSquares.h"
+
+
+// forward declarations to reduce compile dependencies
+template< class T > class WModuleInputData;
+class WDataSetScalar;
+class WGEManagedGroupNode;
+
+/**
+ * Test the Visualizes the least squares algorithm by putting out a plane.
+ * \ingroup modules
+ */
+class WMTempLeastSquaresTest: public WModule
+{
+public:
+    /**
+     * Creates the module for drawing contour lines.
+     */
+    WMTempLeastSquaresTest();
+
+    /**
+     * Destroys this module.
+     */
+    virtual ~WMTempLeastSquaresTest();
+
+    /**
+     * Gives back the name of this module.
+     * \return the module's name.
+     */
+    virtual const std::string getName() const;
+
+    /**
+     * Gives back a description of this module.
+     * \return description to module.
+     */
+    virtual const std::string getDescription() const;
+
+    /**
+     * Due to the prototype design pattern used to build modules, this method returns a new instance of this method. NOTE: it
+     * should never be initialized or modified in some other way. A simple new instance is required.
+     * \return the prototype used to create every module in OpenWalnut.
+     */
+    virtual boost::shared_ptr< WModule > factory() const;
+
+    /**
+     * Get the icon for this module in XPM format.
+     * \return The icon.
+     */
+    virtual const char** getXPMIcon() const;
+
+protected:
+    /**
+     * Entry point after loading the module. Runs in separate thread.
+     */
+    virtual void moduleMain();
+
+    /**
+     *Initialize the connectors this module is using.
+     */
+    virtual void connectors();
+
+    /**
+     * Initialize the properties for this module.
+     */
+    virtual void properties();
+
+    /**
+     * Initialize requirements for this module.
+     */
+    virtual void requirements();
+
+private:
+    /**
+     * Initializes progress bar settings.
+     * \param steps Points count as reference to the progress bar.
+     */
+    void setProgressSettings( size_t steps );
+    /**
+     * Transforms a vector so that its euclidian distance becomes 1.0. The directions 
+     * remains the same.
+     * \param vector Vector to normalize.
+     * \return The normalized vector.
+     */
+    static WPosition getNormalizedVector( WVector3d vector );
+
+    /**
+     * The OSG root node for this module. All other geodes or OSG nodes will be attached on this single node.
+     */
+    osg::ref_ptr< WGEManagedGroupNode > m_rootNode;
+    /**
+     * Returns a cropped data set corresponding to the selection. The selection is
+     * set by m_<from/to>_<X/Y/Z>. m_cutInsteadOfCrop determines whether to crop to
+     * a selection or to cut away a cube area.
+     * \return The cropped or cut point data set.
+     */
+    boost::shared_ptr< WDataSetPoints > getRandomPoints();
+    /**
+     * Sets the best fitted plane to the input points to the triangle mesh output.
+     */
+    void analyzeBestFittedPlane();
+
+    /**
+     * WDataSetPoints data input (proposed for LiDAR data).
+     */
+    boost::shared_ptr< WModuleInputData< WDataSetPoints > > m_input;
+    /**
+     * Processed point data with cut off outliers.
+     */
+    boost::shared_ptr< WModuleOutputData< WTriangleMesh > > m_output;
+
+    /**
+     * Needed for recreating the geometry, incase when resolution changes.
+     */
+    boost::shared_ptr< WCondition > m_propCondition;
+
+    /**
+     * Plugin progress status that is shared with the reader.
+     */
+    boost::shared_ptr< WProgress > m_progressStatus;
+
+    /**
+     * Input point coordinates to crop.
+     */
+    WDataSetPoints::VertexArray m_verts;
+    /**
+     * Colors of the input point data set that are also passed through.
+     */
+    WDataSetPoints::ColorArray m_colors;
+};