Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
Help
Support
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
OpenWalnut Modules
Project overview
Project overview
Details
Activity
Releases
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Issues
0
Issues
0
List
Boards
Labels
Service Desk
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Operations
Operations
Environments
Analytics
Analytics
CI / CD
Repository
Value Stream
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
OpenWalnut
OpenWalnut Modules
Commits
5337e00e
Commit
5337e00e
authored
Aug 24, 2014
by
Andreas Schwarzkopf
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
[STYLE #42] cleanup
parent
c38980eb
Changes
21
Hide whitespace changes
Inline
Side-by-side
Showing
21 changed files
with
82 additions
and
222 deletions
+82
-222
LiDARToolbox/src/common/math/leastSquares/WLeastSquares.cpp
LiDARToolbox/src/common/math/leastSquares/WLeastSquares.cpp
+28
-28
LiDARToolbox/src/common/math/leastSquares/WLeastSquares.h
LiDARToolbox/src/common/math/leastSquares/WLeastSquares.h
+12
-12
LiDARToolbox/src/pointsCutOutliers/WMPointsCutOutliers.cpp
LiDARToolbox/src/pointsCutOutliers/WMPointsCutOutliers.cpp
+2
-6
LiDARToolbox/src/pointsGroupSelector/WMPointsGroupSelector.cpp
...Toolbox/src/pointsGroupSelector/WMPointsGroupSelector.cpp
+0
-9
LiDARToolbox/src/pointsTransform/WMPointsTransform.cpp
LiDARToolbox/src/pointsTransform/WMPointsTransform.cpp
+0
-10
LiDARToolbox/src/surfaceDetectionByLari/WLariBoundaryDetector.cpp
...lbox/src/surfaceDetectionByLari/WLariBoundaryDetector.cpp
+1
-26
LiDARToolbox/src/surfaceDetectionByLari/WLariOutliner.cpp
LiDARToolbox/src/surfaceDetectionByLari/WLariOutliner.cpp
+1
-7
LiDARToolbox/src/surfaceDetectionByLari/WLariPointClassifier.cpp
...olbox/src/surfaceDetectionByLari/WLariPointClassifier.cpp
+1
-11
LiDARToolbox/src/surfaceDetectionByLari/WLariPointClassifier.h
...Toolbox/src/surfaceDetectionByLari/WLariPointClassifier.h
+6
-0
LiDARToolbox/src/surfaceDetectionByLari/WMSurfaceDetectionByLari.cpp
...x/src/surfaceDetectionByLari/WMSurfaceDetectionByLari.cpp
+1
-16
LiDARToolbox/src/surfaceDetectionByLari/clusterDetection/WLariBruteforceClustering.cpp
...tionByLari/clusterDetection/WLariBruteforceClustering.cpp
+1
-8
LiDARToolbox/src/surfaceDetectionByLari/structure/WParameterDomainKdPoint.h
...urfaceDetectionByLari/structure/WParameterDomainKdPoint.h
+2
-2
LiDARToolbox/src/surfaceDetectionByLari/structure/WSpatialDomainKdPoint.cpp
...urfaceDetectionByLari/structure/WSpatialDomainKdPoint.cpp
+5
-5
LiDARToolbox/src/surfaceDetectionByLari/structure/WSpatialDomainKdPoint.h
.../surfaceDetectionByLari/structure/WSpatialDomainKdPoint.h
+8
-8
LiDARToolbox/src/surfaceDetectionByPCL/WMSurfaceDetectionByPCL.cpp
...box/src/surfaceDetectionByPCL/WMSurfaceDetectionByPCL.cpp
+0
-10
LiDARToolbox/src/tempLeastSquaresTest/WMTempLeastSquaresTest.cpp
...olbox/src/tempLeastSquaresTest/WMTempLeastSquaresTest.cpp
+11
-15
LiDARToolbox/src/tempLeastSquaresTest/WMTempLeastSquaresTest.h
...Toolbox/src/tempLeastSquaresTest/WMTempLeastSquaresTest.h
+2
-2
LiDARToolbox/src/tempRandomPoints/WMTempRandomPoints.cpp
LiDARToolbox/src/tempRandomPoints/WMTempRandomPoints.cpp
+1
-10
LiDARToolbox/src/wallDetectionByPCA/WMWallDetectionByPCA.cpp
LiDARToolbox/src/wallDetectionByPCA/WMWallDetectionByPCA.cpp
+0
-10
LiDARToolbox/src/wallDetectionByPCA/WPCAWallDetector.cpp
LiDARToolbox/src/wallDetectionByPCA/WPCAWallDetector.cpp
+0
-4
LiDARToolbox/src/wallDetectionByPCA/structure/WWallDetectOctree.cpp
...ox/src/wallDetectionByPCA/structure/WWallDetectOctree.cpp
+0
-23
No files found.
LiDARToolbox/src/common/math/leastSquares/WLeastSquares.cpp
View file @
5337e00e
...
...
@@ -46,17 +46,17 @@ void WLeastSquares::analyzeData( vector<WPosition>* data )
calculatePerpendicularDimension
();
clearMatrices
();
calculateMatrices
();
calculateHesse
sche
NormalForm
();
calculateHesseNormalForm
();
}
vector
<
double
>
WLeastSquares
::
getHesse
sche
NormalForm
()
vector
<
double
>
WLeastSquares
::
getHesseNormalForm
()
{
return
m_hesse
sche
NormalForm
;
return
m_hesseNormalForm
;
}
vector
<
double
>
WLeastSquares
::
getNormalVectorNotNormalized
()
{
vector
<
double
>
normalVector
;
for
(
size_t
dimension
=
0
;
dimension
<
m_hesse
sche
NormalForm
.
size
()
-
1
;
dimension
++
)
normalVector
.
push_back
(
m_hesse
sche
NormalForm
[
dimension
]
);
for
(
size_t
dimension
=
0
;
dimension
<
m_hesseNormalForm
.
size
()
-
1
;
dimension
++
)
normalVector
.
push_back
(
m_hesseNormalForm
[
dimension
]
);
return
normalVector
;
}
void
WLeastSquares
::
calculatePerpendicularDimension
()
...
...
@@ -127,39 +127,39 @@ void WLeastSquares::calculateMatrices()
}
}
}
void
WLeastSquares
::
calculateHesse
sche
NormalForm
()
void
WLeastSquares
::
calculateHesseNormalForm
()
{
MatrixXd
matrixX
=
m_matrixA
.
inverse
()
*
m_matrixB
;
m_hesse
sche
NormalForm
.
reserve
(
m_dimensions
+
1
);
m_hesse
sche
NormalForm
.
resize
(
m_dimensions
+
1
);
for
(
size_t
index
=
0
;
index
<
m_hesse
sche
NormalForm
.
size
();
index
++
)
m_hesseNormalForm
.
reserve
(
m_dimensions
+
1
);
m_hesseNormalForm
.
resize
(
m_dimensions
+
1
);
for
(
size_t
index
=
0
;
index
<
m_hesseNormalForm
.
size
();
index
++
)
{
size_t
indexMat
=
index
<=
m_verticalDimension
?
index
:
index
-
1
;
m_hesse
sche
NormalForm
[
index
]
=
m_hesseNormalForm
[
index
]
=
index
!=
m_verticalDimension
?-
matrixX
(
indexMat
,
0
)
:
1
;
}
double
sumSquared
=
0
;
for
(
size_t
index
=
0
;
index
<
m_dimensions
;
index
++
)
sumSquared
+=
pow
(
m_hesse
sche
NormalForm
[
index
],
2.0
);
sumSquared
+=
pow
(
m_hesseNormalForm
[
index
],
2.0
);
sumSquared
=
pow
(
sumSquared
,
0.5
);
//for( size_t index = 0; index < m_dimensions; index++ )
// m_hesse
sche
NormalForm[index] /= sumSquared;
// m_hesseNormalForm[index] /= sumSquared;
}
vector
<
double
>
WLeastSquares
::
getParametersXYZ0
()
{
return
getParametersXYZ0
(
m_hesse
sche
NormalForm
);
return
getParametersXYZ0
(
m_hesseNormalForm
);
}
vector
<
double
>
WLeastSquares
::
getParametersXYZ0
(
vector
<
double
>
hesse
sche
NormalForm
)
vector
<
double
>
WLeastSquares
::
getParametersXYZ0
(
vector
<
double
>
hesseNormalForm
)
{
vector
<
double
>
parameters
;
double
a
=
hesse
sche
NormalForm
[
0
];
double
b
=
hesse
sche
NormalForm
[
1
];
double
c
=
hesse
sche
NormalForm
[
2
];
double
d
=
hesse
sche
NormalForm
[
3
];
double
a
=
hesseNormalForm
[
0
];
double
b
=
hesseNormalForm
[
1
];
double
c
=
hesseNormalForm
[
2
];
double
d
=
hesseNormalForm
[
3
];
double
a2
=
a
*
a
;
double
b2
=
b
*
b
;
double
c2
=
c
*
c
;
...
...
@@ -174,32 +174,32 @@ double WLeastSquares::getDistanceToPlane( WPosition point )
{
double
normalAbsolute
=
0
;
for
(
size_t
dimension
=
0
;
dimension
<
m_dimensions
;
dimension
++
)
normalAbsolute
+=
pow
(
m_hesse
sche
NormalForm
[
dimension
],
2.0
);
normalAbsolute
+=
pow
(
m_hesseNormalForm
[
dimension
],
2.0
);
normalAbsolute
=
pow
(
normalAbsolute
,
0.5
);
double
distance
=
0
;
for
(
size_t
dimension
=
0
;
dimension
<
m_dimensions
;
dimension
++
)
distance
+=
point
[
dimension
]
*
m_hesse
sche
NormalForm
[
dimension
];
return
(
distance
+
m_hesse
sche
NormalForm
[
m_dimensions
]
)
/
normalAbsolute
;
distance
+=
point
[
dimension
]
*
m_hesseNormalForm
[
dimension
];
return
(
distance
+
m_hesseNormalForm
[
m_dimensions
]
)
/
normalAbsolute
;
}
WPosition
WLeastSquares
::
getNearestPointTo
(
WPosition
point
)
{
return
getNearestPointTo
(
m_hesse
sche
NormalForm
,
point
);
return
getNearestPointTo
(
m_hesseNormalForm
,
point
);
}
WPosition
WLeastSquares
::
getNearestPointTo
(
vector
<
double
>
planeHesse
sche
NormalForm
,
WPosition
point
)
WPosition
WLeastSquares
::
getNearestPointTo
(
vector
<
double
>
planeHesseNormalForm
,
WPosition
point
)
{
double
dimensions
=
planeHesse
sche
NormalForm
.
size
()
-
1
;
double
amountN
=
planeHesse
sche
NormalForm
[
dimensions
];
double
dimensions
=
planeHesseNormalForm
.
size
()
-
1
;
double
amountN
=
planeHesseNormalForm
[
dimensions
];
double
amountR
=
0
;
for
(
size_t
dimension
=
0
;
dimension
<
dimensions
;
dimension
++
)
{
amountN
+=
planeHesse
sche
NormalForm
[
dimension
]
*
point
[
dimension
];
amountR
+=
planeHesse
scheNormalForm
[
dimension
]
*
planeHessesch
eNormalForm
[
dimension
];
amountN
+=
planeHesseNormalForm
[
dimension
]
*
point
[
dimension
];
amountR
+=
planeHesse
NormalForm
[
dimension
]
*
planeHess
eNormalForm
[
dimension
];
}
double
r
=
-
amountN
/
amountR
;
WPosition
cuttingPoint
(
point
[
0
],
point
[
1
],
point
[
2
]
);
for
(
size_t
dimension
=
0
;
dimension
<
dimensions
;
dimension
++
)
cuttingPoint
[
dimension
]
+=
planeHesse
sche
NormalForm
[
dimension
]
*
r
;
cuttingPoint
[
dimension
]
+=
planeHesseNormalForm
[
dimension
]
*
r
;
return
cuttingPoint
;
}
LiDARToolbox/src/common/math/leastSquares/WLeastSquares.h
View file @
5337e00e
...
...
@@ -64,12 +64,12 @@ public:
void
analyzeData
(
vector
<
WPosition
>*
data
);
/**
* Returns the plane formula for the best fitted plane.
* \return The
hessesche Normal Form of the best fitted plane. First n numbers (by
*
the
dimensions coordinate count) represent the normal vector of the best
* \return The
Hesse normal form of the best fitted plane. First n numbers (by the
* dimensions coordinate count) represent the normal vector of the best
* fitted plane. The last one is the perpendicular euclidian distance to the
* coordinate system orign.
*/
vector
<
double
>
getHesse
sche
NormalForm
();
vector
<
double
>
getHesseNormalForm
();
/**
* Returns a not normalized normal vector of the least squares adjustment result.
* \return A not normalized normal vector of the least squares adjustment result.
...
...
@@ -84,12 +84,12 @@ public:
/**
* Calculates a plane formula of the parameter domain using the approach of
* Lari/Habib using the Hesse
sche
normal form as input plane formula.
* \param hesse
scheNormalForm The Hessesch
e normal formula of a plane.
* Lari/Habib using the Hesse normal form as input plane formula.
* \param hesse
NormalForm The Hess
e 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
>
hesse
sche
NormalForm
);
static
vector
<
double
>
getParametersXYZ0
(
vector
<
double
>
hesseNormalForm
);
/**
* Returns a point distance to the currently calculated plane.
* analyzeData() must have been executes.
...
...
@@ -108,11 +108,11 @@ public:
/**
* Returns a nearest point to the input point's coordinate that lies on the
* calculated plane.
* \param planeHesse
sche
NormalForm Plane on which the nearest point is found.
* \param planeHesseNormalForm 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
>
planeHesse
sche
NormalForm
,
WPosition
point
);
static
WPosition
getNearestPointTo
(
vector
<
double
>
planeHesseNormalForm
,
WPosition
point
);
private:
/**
...
...
@@ -131,9 +131,9 @@ private:
void
calculateMatrices
();
/**
* The algorithm solves the matrix formula A*x=B. This method at first calculates
* the matrix x and then the Hesse
sche
Normal Form of the best fitted plane.
* the matrix x and then the Hesse Normal Form of the best fitted plane.
*/
void
calculateHesse
sche
NormalForm
();
void
calculateHesseNormalForm
();
/**
* Dimension definition of the coordinate system.
...
...
@@ -144,9 +144,9 @@ private:
*/
vector
<
WPosition
>*
m_positions
;
/**
* Space for the calculated Hesse
sche Normal F
orm.
* Space for the calculated Hesse
normal f
orm.
*/
vector
<
double
>
m_hesse
sche
NormalForm
;
vector
<
double
>
m_hesseNormalForm
;
/**
* Dimension with the biggest eigen vector coordonate extent of the smallest Eigen Value.
*/
...
...
LiDARToolbox/src/pointsCutOutliers/WMPointsCutOutliers.cpp
View file @
5337e00e
...
...
@@ -79,14 +79,13 @@ const std::string WMPointsCutOutliers::getDescription() const
void
WMPointsCutOutliers
::
connectors
()
{
m_input
=
WModuleInputData
<
WDataSetPoints
>::
createAndAdd
(
shared_from_this
(),
"input"
,
"
The mesh to display
"
);
m_input
=
WModuleInputData
<
WDataSetPoints
>::
createAndAdd
(
shared_from_this
(),
"input"
,
""
);
m_output
=
boost
::
shared_ptr
<
WModuleOutputData
<
WDataSetPoints
>
>
(
new
WModuleOutputData
<
WDataSetPoints
>
(
shared_from_this
(),
"points"
,
"
The loaded points.
"
)
);
shared_from_this
(),
"points"
,
""
)
);
addConnector
(
m_output
);
// addConnector( m_buildings );
WModule
::
connectors
();
}
...
...
@@ -109,7 +108,6 @@ void WMPointsCutOutliers::requirements()
void
WMPointsCutOutliers
::
moduleMain
()
{
// get notified about data changes
m_moduleState
.
setResetable
(
true
,
true
);
m_moduleState
.
add
(
m_input
->
getDataChangedCondition
()
);
m_moduleState
.
add
(
m_propCondition
);
...
...
@@ -151,8 +149,6 @@ void WMPointsCutOutliers::moduleMain()
{
continue
;
}
// ---> Insert code doing the real stuff here
}
WKernel
::
getRunningKernel
()
->
getGraphicsEngine
()
->
getScene
()
->
remove
(
m_rootNode
);
...
...
LiDARToolbox/src/pointsGroupSelector/WMPointsGroupSelector.cpp
View file @
5337e00e
...
...
@@ -106,9 +106,6 @@ void WMPointsGroupSelector::properties()
m_zMax
=
m_infoProperties
->
addProperty
(
"Z max.: "
,
"Maximal z coordinate of all input points."
,
0.0
);
// ---> Put the code for your properties here. See "src/modules/template/" for an extensively documented example.
m_detailDepth
=
m_properties
->
addProperty
(
"Detail Depth 2^n m: "
,
"Resulting 2^n meters detail "
"depth for the octree search tree."
,
0
,
m_propCondition
);
...
...
@@ -139,9 +136,6 @@ void WMPointsGroupSelector::requirements()
void
WMPointsGroupSelector
::
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
);
...
...
@@ -155,7 +149,6 @@ void WMPointsGroupSelector::moduleMain()
// main loop
while
(
!
m_shutdownFlag
()
)
{
//infoLog() << "Waiting ...";
m_moduleState
.
wait
();
boost
::
shared_ptr
<
WDataSetPointsGrouped
>
points
=
m_input
->
getData
();
...
...
@@ -244,8 +237,6 @@ void WMPointsGroupSelector::moduleMain()
{
continue
;
}
// ---> Insert code doing the real stuff here
}
WKernel
::
getRunningKernel
()
->
getGraphicsEngine
()
->
getScene
()
->
remove
(
m_rootNode
);
...
...
LiDARToolbox/src/pointsTransform/WMPointsTransform.cpp
View file @
5337e00e
...
...
@@ -101,8 +101,6 @@ void WMPointsTransform::connectors()
void
WMPointsTransform
::
properties
()
{
// ---> Put the code for your properties here. See "src/modules/template/" for an extensively documented example.
m_infoRenderTimeSeconds
=
m_infoProperties
->
addProperty
(
"Wall time (s): "
,
"Time in seconds that the "
"whole render process took."
,
0.0
);
...
...
@@ -171,8 +169,6 @@ void WMPointsTransform::requirements()
void
WMPointsTransform
::
moduleMain
()
{
infoLog
()
<<
"Thrsholding example main routine started"
;
// get notified about data changes
m_moduleState
.
setResetable
(
true
,
true
);
for
(
size_t
pointset
=
0
;
pointset
<
m_input
.
size
();
pointset
++
)
...
...
@@ -188,7 +184,6 @@ void WMPointsTransform::moduleMain()
// main loop
while
(
!
m_shutdownFlag
()
)
{
//infoLog() << "Waiting ...";
m_moduleState
.
wait
();
WRealtimeTimer
timer
;
...
...
@@ -203,7 +198,6 @@ void WMPointsTransform::moduleMain()
for
(
size_t
pointset
=
0
;
pointset
<
m_input
.
size
();
pointset
++
)
{
boost
::
shared_ptr
<
WDataSetPoints
>
points
=
m_input
[
pointset
]
->
getData
();
// std::cout << "Execute cycle\r\n";
if
(
points
)
{
m_inVerts
=
points
->
getVertices
();
...
...
@@ -229,8 +223,6 @@ void WMPointsTransform::moduleMain()
}
m_infoRenderTimeSeconds
->
set
(
timer
.
elapsed
()
);
// std::cout << "this is WOTree " << std::endl;
// woke up since the module is requested to finish?
if
(
m_shutdownFlag
()
)
{
...
...
@@ -241,8 +233,6 @@ void WMPointsTransform::moduleMain()
{
continue
;
}
// ---> Insert code doing the real stuff here
}
WKernel
::
getRunningKernel
()
->
getGraphicsEngine
()
->
getScene
()
->
remove
(
m_rootNode
);
...
...
LiDARToolbox/src/surfaceDetectionByLari/WLariBoundaryDetector.cpp
View file @
5337e00e
...
...
@@ -48,7 +48,6 @@ void WLariBoundaryDetector::detectBoundaries( WKdTreeND* parameterDomain )
cout
<<
"detectBoundaries()"
<<
endl
;
initSpatialDomain
(
parameterDomain
);
//TODO(aschwarzkopf): Remove apply only for biggest cluster:
for
(
size_t
cluster
=
0
;
cluster
<
m_spatialInputClusters
->
size
();
cluster
++
)
detectInputCluster
(
m_spatialInputClusters
->
at
(
cluster
)
);
}
...
...
@@ -105,7 +104,6 @@ void WLariBoundaryDetector::detectInputCluster( vector<WSpatialDomainKdPoint*>*
WBoundaryDetectPoint
*
clusterPoint
=
new
WBoundaryDetectPoint
(
*
coordinate
);
clusterPoint
->
setSpatialPoint
(
currentPoint
);
clusterPoints
->
push_back
(
clusterPoint
);
//currentPoint->setCoordinate( coordinate );
currentPoint
->
setClusterID
(
9
);
delete
coordinate
;
}
...
...
@@ -155,7 +153,6 @@ void WLariBoundaryDetector::detectInputCluster( vector<WSpatialDomainKdPoint*>*
initOneOutsidePoint
();
for
(
size_t
index
=
0
;
index
<
clusterPoints
->
size
();
index
++
)
{
//cout << "Detecting whether in bounds[" << m_currentBoundary->size() << "] P " << index << "/" << clusterPoints->size() << endl;
WBoundaryDetectPoint
*
currentPoint
=
clusterPoints
->
at
(
index
);
const
vector
<
double
>&
coordinate
=
currentPoint
->
getCoordinate
();
if
(
pointBelongsToBoundingBox
(
coordinate
)
&&
pointIsInBounds
(
coordinate
)
)
...
...
@@ -182,18 +179,10 @@ void WLariBoundaryDetector::initTransformationCoordinateSystem( vector<WSpatialD
vector
<
double
>*
meanNormalVector
=
new
vector
<
double
>
(
3
,
0.0
);
for
(
size_t
index
=
0
;
index
<
currentPoints
->
size
();
index
++
)
{
//cout << "init[" << index << "] (" << meanNormalVector[0] << ", " << meanNormalVector[1] << ", " << meanNormalVector[2] << endl;
vector
<
double
>*
currentPoint
=
WVectorMaths
::
copyVectorForPointer
(
currentPoints
->
at
(
index
)
->
getParametersXYZ0
()
);
WVectorMaths
::
normalizeVector
(
currentPoint
);
if
(
WVectorMaths
::
isValidVector
(
*
currentPoint
)
)
//{
WVectorMaths
::
addVector
(
meanNormalVector
,
*
currentPoint
);
//}
//else
//{
//cout << "!!! FOUND INVALID EXTENT !!! P[" << index << "](" << currentPoint[0] << ", "
// << currentPoint[1] << ", " << currentPoint[2] << ")" << endl;
//}
delete
currentPoint
;
}
WVectorMaths
::
normalizeVector
(
meanNormalVector
);
...
...
@@ -204,9 +193,6 @@ void WLariBoundaryDetector::initTransformationCoordinateSystem( vector<WSpatialD
}
WBoundaryDetectPoint
*
WLariBoundaryDetector
::
getNextBoundPoint
()
{
//if( m_currentBoundary->size() == 5 )
// cout << "The garbled point breakpoint" << endl;
WBoundaryDetectPoint
*
previousPoint
=
m_currentBoundary
->
size
()
>=
2
?
m_currentBoundary
->
at
(
m_currentBoundary
->
size
()
-
2
)
:
new
WBoundaryDetectPoint
(
m_currentBoundary
->
at
(
0
)
->
getCoordinate
()[
0
]
-
1.0
,
...
...
@@ -215,7 +201,6 @@ WBoundaryDetectPoint* WLariBoundaryDetector::getNextBoundPoint()
m_clusterSearcher
.
setSearchedPoint
(
currentPoint
->
getCoordinate
()
);
vector
<
WPointDistance
>*
nearestPoints
=
m_clusterSearcher
.
getNearestPoints
();
//cout << "getNextBoundPoint(): m_clusterSearcher.getNearestPoints() - " << nearestPoints->size() << " nearest points found" << endl;
WBoundaryDetectPoint
*
nextPoint
=
0
;
double
narrowestAngle
=
361
;
for
(
size_t
index
=
0
;
index
<
nearestPoints
->
size
();
index
++
)
...
...
@@ -233,9 +218,6 @@ WBoundaryDetectPoint* WLariBoundaryDetector::getNextBoundPoint()
if
(
m_currentBoundary
->
size
()
<
2
)
delete
previousPoint
;
//if( nextPoint != 0 )
// cout << "getNextBoundPoint() - P" << m_currentBoundary->size() << "(" << nextPoint->getCoordinate()[0]
// << ", " << nextPoint->getCoordinate()[1] << ") @" << narrowestAngle << "°" << endl;
return
nextPoint
;
}
bool
WLariBoundaryDetector
::
pointCanProceedBound
(
WPointDistance
nextPointDistance
)
...
...
@@ -325,24 +307,18 @@ bool WLariBoundaryDetector::pointIsInBounds( const vector<double>& point )
bool
isInBounds
=
false
;
for
(
size_t
bound
=
0
;
bound
<
m_currentBoundary
->
size
()
-
1
;
bound
++
)
{
//if( bound == 114 )
// cout << "garbled point" << endl;
//cout << " pointIsInBounds() [" << bound << "]" << endl;
if
(
pointLiesOnBound
(
point
,
bound
)
)
return
true
;
if
(
pointHitsBound
(
point
,
bound
)
)
{
isInBounds
=
!
isInBounds
;
//TODO(aschwarzkopf): Implement an appropriate routine to handle when intersection hits exactly on points.
//TODO(aschwarzkopf): Possible solution: Move m_oneOutsidePoint until bound points do not hit exactly.
isInBounds
=
!
isInBounds
;
}
}
return
isInBounds
;
}
bool
WLariBoundaryDetector
::
pointLiesOnBound
(
const
vector
<
double
>&
point
,
size_t
boundNr
)
{
//cout << " pointLiesOnBound()" << endl;
const
vector
<
double
>
boundPoint1
=
m_currentBoundary
->
at
(
boundNr
)
->
getCoordinate
();
double
secondIndex
=
boundNr
+
1
;
while
(
boundNr
>=
m_currentBoundary
->
size
()
)
...
...
@@ -354,7 +330,6 @@ bool WLariBoundaryDetector::pointLiesOnBound( const vector<double>& point, size_
}
bool
WLariBoundaryDetector
::
pointHitsBound
(
const
vector
<
double
>&
point
,
size_t
boundNr
)
{
//cout << " pointHitsBound()" << endl;
const
vector
<
double
>
boundPoint1
=
m_currentBoundary
->
at
(
boundNr
)
->
getCoordinate
();
size_t
secondIndex
=
boundNr
+
1
;
while
(
secondIndex
>=
m_currentBoundary
->
size
()
)
...
...
LiDARToolbox/src/surfaceDetectionByLari/WLariOutliner.cpp
View file @
5337e00e
...
...
@@ -98,12 +98,6 @@ boost::shared_ptr< WDataSetPoints > WLariOutliner::outlineSpatialDomainCategorie
vector
<
double
>
spatialCoordinate
=
spatialPoint
->
getCoordinate
();
vector
<
double
>
eigenValues
=
spatialPoint
->
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_pointClassifier
->
calculateIsPlanarPoint
(
eigenValues
);
...
...
@@ -135,7 +129,7 @@ boost::shared_ptr< WTriangleMesh > WLariOutliner::outlineLeastSquaresPlanes( dou
WPosition
spatialPoint
(
0.0
,
0.0
,
0.0
);
for
(
size_t
dimension
=
0
;
dimension
<
spatialCoordinate
.
size
();
dimension
++
)
spatialPoint
[
dimension
]
=
spatialCoordinate
[
dimension
];
vector
<
double
>
planeFormula
=
spatialDomainPoint
->
getHesse
sche
NormalForm
();
vector
<
double
>
planeFormula
=
spatialDomainPoint
->
getHesseNormalForm
();
WMTempLeastSquaresTest
::
outlineNormalPlane
(
planeFormula
,
spatialPoint
,
squaresWidth
/
2.0
,
outputMesh
);
}
...
...
LiDARToolbox/src/surfaceDetectionByLari/WLariPointClassifier.cpp
View file @
5337e00e
...
...
@@ -52,7 +52,6 @@ WLariPointClassifier::~WLariPointClassifier()
void
WLariPointClassifier
::
analyzeData
(
vector
<
WSpatialDomainKdPoint
*>*
inputPoints
)
{
//TODO(aschwarzkopf): After draft stage refine in several methods and remove comments
cout
<<
"Attempting to analyze "
<<
inputPoints
->
size
()
<<
" points"
<<
endl
;
delete
m_spatialDomain
;
delete
m_parameterDomain
;
...
...
@@ -63,7 +62,6 @@ void WLariPointClassifier::analyzeData( vector<WSpatialDomainKdPoint*>* inputPoi
classifyPoints
(
inputPoints
,
parameterPoints
);
//Generating properties for each existing point
cout
<<
"Adding "
<<
parameterPoints
->
size
()
<<
" parameter points"
<<
endl
;
m_parameterDomain
->
add
(
reinterpret_cast
<
vector
<
WKdPointND
*>*>
(
parameterPoints
)
);
}
...
...
@@ -100,17 +98,11 @@ void WLariPointClassifier::classifyPoints( vector<WSpatialDomainKdPoint*>* spati
}
void
WLariPointClassifier
::
classifyPointsAtThread
(
vector
<
WSpatialDomainKdPoint
*>*
spatialPoints
,
size_t
threadIndex
)
{
//cout << (spatialPoints->size()) << " " << (parameterPoints->size()) << " " << threadIndex << endl;
WPointSearcher
spatialSearcher
(
m_spatialDomain
);
spatialSearcher
.
setMaxResultPointCount
(
m_numberPointsK
);
spatialSearcher
.
setMaxSearchDistance
(
m_maxPointDistanceR
);
for
(
size_t
index
=
threadIndex
;
index
<
spatialPoints
->
size
();
index
+=
m_cpuThreadCount
)
{
if
(
index
==
1061
)
cout
<<
"Garbled classified point - "
<<
index
<<
endl
;
//cout << "Thread " << threadIndex << ", index " << index << endl;
//Getting nearest n points
WSpatialDomainKdPoint
*
spatialPoint
=
spatialPoints
->
at
(
index
);
vector
<
double
>
spatialCoordinate
=
spatialPoint
->
getCoordinate
();
spatialSearcher
.
setSearchedPoint
(
spatialCoordinate
);
...
...
@@ -119,17 +111,15 @@ void WLariPointClassifier::classifyPointsAtThread( vector<WSpatialDomainKdPoint*
spatialPoint
->
setKNearestPoints
(
points
->
size
()
);
spatialPoint
->
setDistanceToNthNearestNeighbor
(
nearestPoints
->
at
(
points
->
size
()
-
1
).
getDistance
()
);
//Calculating Eigen properties
WPrincipalComponentAnalysis
pca
;
pca
.
analyzeData
(
points
);
spatialPoint
->
setEigenVectors
(
pca
.
getDirections
()
);
vector
<
double
>
eigenValues
=
pca
.
getEigenValues
();
spatialPoint
->
setEigenValues
(
eigenValues
);
//Adding parameter space information
WLeastSquares
leastSquares
;
leastSquares
.
analyzeData
(
points
);
spatialPoint
->
setHesse
scheNormalForm
(
leastSquares
.
getHessesch
eNormalForm
()
);
spatialPoint
->
setHesse
NormalForm
(
leastSquares
.
getHess
eNormalForm
()
);
delete
nearestPoints
;
delete
points
;
...
...
LiDARToolbox/src/surfaceDetectionByLari/WLariPointClassifier.h
View file @
5337e00e
...
...
@@ -46,6 +46,12 @@ using std::endl;
using
std
::
vector
;
/**
* Class that adds classification meta to points in relation to its neighbor points. The
* meta contains such things as eigenvalues, eigenvectors (Both calculated by Principal
* Component Analysis) and the Hesse normal form (calculated using least squares
* adjustment).
*/
class
WLariPointClassifier
{
public:
...
...
LiDARToolbox/src/surfaceDetectionByLari/WMSurfaceDetectionByLari.cpp
View file @
5337e00e
...
...
@@ -97,7 +97,6 @@ void WMSurfaceDetectionByLari::connectors()
addConnector
(
m_outputSpatialDomainCategories
);
addConnector
(
m_outputParameterDomain
);
addConnector
(
m_outputLeastSquaresPlanes
);
// addConnector( m_buildings );
WModule
::
connectors
();
}
...
...
@@ -127,7 +126,6 @@ void WMSurfaceDetectionByLari::properties()
m_squareWidth
=
m_properties
->
addProperty
(
"Plane outline size: "
,
""
,
0.2
,
m_propCondition
);
//TODO(aschwarzkopf): Resolve changing the thread count during execution.
m_cpuThreadCount
=
m_properties
->
addProperty
(
"CPU threads: "
,
""
,
8
,
m_propCondition
);
m_cpuThreadCount
->
setMin
(
4
);
m_cpuThreadCount
->
setMax
(
24
);
...
...
@@ -180,11 +178,6 @@ void WMSurfaceDetectionByLari::properties()
m_cylNLambda3Max
->
setMin
(
m_cylNLambda3Min
->
get
()
);
m_cylNLambda3Max
->
setMax
(
1.0
);
// ---> Put the code for your properties here. See "src/modules/template/" for an extensively documented example.
WModule
::
properties
();
}
...
...
@@ -194,23 +187,18 @@ void WMSurfaceDetectionByLari::requirements()
void
WMSurfaceDetectionByLari
::
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_surfaceNLambda1Max
->
setMin
(
m_surfaceNLambda1Min
->
get
()
);
...
...
@@ -300,7 +288,6 @@ void WMSurfaceDetectionByLari::moduleMain()
m_reloadData
->
get
(
true
);
// woke up since the module is requested to finish?
if
(
m_shutdownFlag
()
)
{
break
;
...
...
@@ -311,8 +298,6 @@ void WMSurfaceDetectionByLari::moduleMain()
{
continue
;
}
// ---> Insert code doing the real stuff here
}
WKernel
::
getRunningKernel
()
->
getGraphicsEngine
()
->
getScene
()
->
remove
(
m_rootNode
);
...
...
LiDARToolbox/src/surfaceDetectionByLari/clusterDetection/WLariBruteforceClustering.cpp
View file @
5337e00e
...
...
@@ -43,15 +43,13 @@ WLariBruteforceClustering::~WLariBruteforceClustering()
void
WLariBruteforceClustering
::
detectClustersByBruteForce
()
{
vector
<
WKdPointND
*>*
parameterNodes
=
m_parameterDomain
->
getAllPoints
();
//AW: D A S H E I S S T K E I N E W A R T E S C H L A N G E ! ! !
vector
<
WKdPointND
*>*
parameterNodes
=
m_parameterDomain
->
getAllPoints
();
size_t
currentClusterID
=
0
;
while
(
parameterNodes
->
size
()
>
0
)
{
initExtentSizes
(
parameterNodes
);
//cout << "Detecting cluster - " << parameterNodes->size() << " left" << endl;
WParameterDomainKdPoint
*
biggestExtentPoint
=
0
;
int
mostNeighborsCount
=
0
;
// cout << "Looking for biggest extent" << endl;
for
(
size_t
index
=
0
;
index
<
parameterNodes
->
size
();
index
++
)
{
WParameterDomainKdPoint
*
parameterPoint
=
static_cast
<
WParameterDomainKdPoint
*>
(
parameterNodes
->
at
(
index
)
);
...
...
@@ -61,7 +59,6 @@ void WLariBruteforceClustering::detectClustersByBruteForce()
mostNeighborsCount
=
parameterPoint
->
getExtentPointCount
();
}
}
// cout << "Looking for neighbors of biggest extent" << endl;
addExtentCluster
(
biggestExtentPoint
,
currentClusterID
++
);
vector
<
WKdPointND
*>*
oldParameterNodes
=
parameterNodes
;
...
...
@@ -92,7 +89,6 @@ vector<WParameterDomainKdPoint*>* WLariBruteforceClustering::getParametersOfExte
parameterSearcher
.
setMaxSearchDistance
(
maxParameterDistance
);
parameterSearcher
.
setSearchedPoint
(
parametersXYZ0
);
vector
<
WPointDistance
>*
nearestPoints
=
parameterSearcher
.
getNearestPoints
();
//cout << "Considering "<<nearestPoints->size()<<" nearest points\t";
vector
<
WParameterDomainKdPoint
*>*
neighborParameters
=
new
vector
<
WParameterDomainKdPoint
*>
();
for
(
size_t
index
=
0
;
index
<
nearestPoints
->
size
();
index
++
)
if
(
isParameterOfSameExtent
(
parametersXYZ0
,
nearestPoints
->
at
(
index
).
getComparedCoordinate
()
)
)
...
...
@@ -178,14 +174,11 @@ double WLariBruteforceClustering::getMaxParameterDistance( const vector<double>&
parameterFar
[
1
]
=
rotationY
*
distanceFar
;
distanceNear
=
WPointDistance
::
getPointDistance
(
extent
,
parameterNear
);
distanceFar
=
WPointDistance
::
getPointDistance
(
extent
,
parameterFar
);
//cout << " )\tDistance: " << distanceNear<<", "<<distanceFar<<"\t";
return
distanceNear
>
distanceFar
?
distanceNear
:
distanceFar
;
}
bool
WLariBruteforceClustering
::
isParameterOfSameExtent
(
const
vector
<
double
>&
parameters1
,
const
vector
<
double
>&
parameters2
)
{
vector
<
double
>
origin
(
3
,
0
);
//for( size_t index = 0; index < parameters1.size(); index++ )
// origin.push_back( 0.0 );
double
distance1
=
WPointDistance
::
getPointDistance
(
origin
,
parameters1
);
double
distance2
=
WPointDistance
::
getPointDistance
(
origin
,
parameters2
);