Commit a3c5285a authored by Andreas Schwarzkopf's avatar Andreas Schwarzkopf

[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.
parent 570f0b11
......@@ -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 ) );
}
/**
......
......@@ -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;
}
......@@ -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:
/**
......
......@@ -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();
......
......@@ -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.
*/
......
//---------------------------------------------------------------------------
//
// 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;
}
//---------------------------------------------------------------------------
//
// 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;
};
#endif // WMTEMPLEASTSQUARESTEST_H
/* XPM */
static const char * WMTempLeastSquaresTest_xpm[] = {
"32 32 5 1",
" c #FFFFFF",
". c #3E3E3E",
"+ c #000000",
"@ c #000063",
"# c #808080",
" ",
" ",
" ",
" ",
" ",
"............. ..............",
" +. .+ ",
" + . . + ",
" + . . + ",
" + . . + ",
" + . . + ",