From 0b0e5fea5a54b001ee41c46e484c835d0692e2ad Mon Sep 17 00:00:00 2001
From: Markus Grotz <markus.grotz@kit.edu>
Date: Tue, 31 May 2016 13:51:51 +0200
Subject: [PATCH] moved EarlyVisionGraph lib to a separate component
---
source/RobotAPI/components/CMakeLists.txt | 2 +
.../EarlyVisionGraph/CMakeLists.txt | 34 ++
.../EarlyVisionGraph/GraphGenerator.cpp | 351 +++++++++++++
.../EarlyVisionGraph/GraphGenerator.h | 85 +++
.../EarlyVisionGraph/GraphLookupTable.cpp | 238 +++++++++
.../EarlyVisionGraph/GraphLookupTable.h | 65 +++
.../components/EarlyVisionGraph/GraphMap.cpp | 262 ++++++++++
.../components/EarlyVisionGraph/GraphMap.h | 74 +++
.../EarlyVisionGraph/GraphProcessor.cpp | 307 +++++++++++
.../EarlyVisionGraph/GraphProcessor.h | 50 ++
.../GraphPyramidLookupTable.cpp | 108 ++++
.../GraphPyramidLookupTable.h | 52 ++
.../EarlyVisionGraph/GraphTriangulation.cpp | 423 +++++++++++++++
.../EarlyVisionGraph/GraphTriangulation.h | 52 ++
.../EarlyVisionGraph/IntensityGraph.cpp | 276 ++++++++++
.../EarlyVisionGraph/IntensityGraph.h | 97 ++++
.../components/EarlyVisionGraph/MathTools.cpp | 488 ++++++++++++++++++
.../components/EarlyVisionGraph/MathTools.h | 72 +++
.../EarlyVisionGraph/SphericalGraph.cpp | 314 +++++++++++
.../EarlyVisionGraph/SphericalGraph.h | 165 ++++++
.../components/EarlyVisionGraph/Structs.h | 85 +++
21 files changed, 3600 insertions(+)
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.cpp
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.h
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.cpp
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.h
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/GraphMap.cpp
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/GraphMap.h
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.cpp
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.h
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.cpp
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.cpp
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.h
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.cpp
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.h
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/MathTools.cpp
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/MathTools.h
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.cpp
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.h
create mode 100644 source/RobotAPI/components/EarlyVisionGraph/Structs.h
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index fb09cfb0a..b596e7006 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -3,3 +3,5 @@ add_subdirectory(units)
add_subdirectory(DebugDrawer)
add_subdirectory(RobotState)
+
+add_subdirectory(EarlyVisionGraph)
\ No newline at end of file
diff --git a/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt b/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
new file mode 100644
index 000000000..3398169a5
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
@@ -0,0 +1,34 @@
+armarx_component_set_name("EarlyVisionGraph")
+
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+if(Eigen3_FOUND)
+ include_directories(${Eigen3_INCLUDE_DIR})
+endif()
+
+
+set(COMPONENT_LIBS "")
+
+
+set(SOURCES
+./MathTools.cpp
+./SphericalGraph.cpp
+./IntensityGraph.cpp
+./GraphLookupTable.cpp
+./GraphPyramidLookupTable.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(HEADERS
+./Structs.h
+./MathTools.h
+./SphericalGraph.h
+./IntensityGraph.h
+./GraphLookupTable.h
+./GraphPyramidLookupTable.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.cpp
new file mode 100644
index 000000000..bf424e5b3
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.cpp
@@ -0,0 +1,351 @@
+// *****************************************************************
+// Filename: GraphGenerator.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 08.07.2008
+// *****************************************************************
+
+#include "GraphGenerator.h"
+
+vector<Vec3d> CGraphGenerator::m_Vertices;
+vector<Face> CGraphGenerator::m_Faces;
+
+
+void CGraphGenerator::generateGraph(CSphericalGraph* pPlainGraph, EPolyhedronType nBaseType, int nLevels, float fRadius)
+{
+ initBasePolyhedron(nBaseType);
+ projectToSphere(fRadius);
+
+ for (int L = 0 ; L < nLevels ; L++)
+ {
+ subDivide();
+ projectToSphere(fRadius);
+ }
+
+ buildNodesAndEdges(pPlainGraph);
+}
+
+void CGraphGenerator::initBasePolyhedron(EPolyhedronType nBaseType)
+{
+ m_Vertices.clear();
+ m_Faces.clear();
+
+ switch (nBaseType)
+ {
+ case eTetrahedron:
+ generateTetrahedron();
+ break;
+
+ case eOctahedron:
+ generateOctahedron();
+ break;
+
+ case eIcosahedron:
+ generateIcosahedron();
+ break;
+
+ default:
+ printf("Wrong base type\n");
+ break;
+ }
+}
+
+void CGraphGenerator::generateTetrahedron()
+{
+ // vertices of a tetrahedron
+ float fSqrt3 = sqrt(3.0);
+ Vec3d v[4];
+
+ Math3d::SetVec(v[0], fSqrt3, fSqrt3, fSqrt3);
+ Math3d::SetVec(v[1], -fSqrt3, -fSqrt3, fSqrt3);
+ Math3d::SetVec(v[2], -fSqrt3, fSqrt3, -fSqrt3);
+ Math3d::SetVec(v[3], fSqrt3, -fSqrt3, -fSqrt3);
+
+ for (int i = 0 ; i < 4 ; i++)
+ {
+ m_Vertices.push_back(v[i]);
+ }
+
+ // structure describing a tetrahedron
+ Face f[4];
+ f[0].set(1, 2, 3);
+ f[1].set(1, 4, 2);
+ f[2].set(3, 2, 4);
+ f[3].set(4, 1, 3);
+
+ for (int i = 0 ; i < 4 ; i++)
+ {
+ f[i].m_n1--;
+ f[i].m_n2--;
+ f[i].m_n3--;
+ m_Faces.push_back(f[i]);
+ }
+}
+
+void CGraphGenerator::generateOctahedron()
+{
+ // Six equidistant points lying on the unit sphere
+ Vec3d v[6];
+ Math3d::SetVec(v[0], 1, 0, 0);
+ Math3d::SetVec(v[1], -1, 0, 0);
+ Math3d::SetVec(v[2], 0, 1, 0);
+ Math3d::SetVec(v[3], 0, -1, 0);
+ Math3d::SetVec(v[4], 0, 0, 1);
+ Math3d::SetVec(v[5], 0, 0, -1);
+
+ for (int i = 0 ; i < 6 ; i++)
+ {
+ m_Vertices.push_back(v[i]);
+ }
+
+ // Join vertices to create a unit octahedron
+ Face f[8];
+ f[0].set(1, 5, 3);
+ f[1].set(3, 5, 2);
+ f[2].set(2, 5, 4);
+ f[3].set(4, 5, 1);
+ f[4].set(1, 3, 6);
+ f[5].set(3, 2, 6);
+ f[6].set(2, 4, 6);
+ f[7].set(4, 1, 6);
+
+ for (int i = 0 ; i < 8 ; i++)
+ {
+ f[i].m_n1--;
+ f[i].m_n2--;
+ f[i].m_n3--;
+ m_Faces.push_back(f[i]);
+ }
+}
+
+void CGraphGenerator::generateIcosahedron()
+{
+ // Twelve vertices of icosahedron on unit sphere
+ float tau = 0.8506508084;
+ float one = 0.5257311121;
+
+ Vec3d v[12];
+
+ Math3d::SetVec(v[0], tau, one, 0);
+ Math3d::SetVec(v[1], -tau, one, 0);
+ Math3d::SetVec(v[2], -tau, -one, 0);
+ Math3d::SetVec(v[3], tau, -one, 0);
+ Math3d::SetVec(v[4], one, 0, tau);
+ Math3d::SetVec(v[5], one, 0, -tau);
+ Math3d::SetVec(v[6], -one, 0, -tau);
+ Math3d::SetVec(v[7], -one, 0, tau);
+ Math3d::SetVec(v[8], 0, tau, one);
+ Math3d::SetVec(v[9], 0, -tau, one);
+ Math3d::SetVec(v[10], 0, -tau, -one);
+ Math3d::SetVec(v[11], 0, tau, -one);
+
+ for (int i = 0 ; i < 12 ; i++)
+ {
+ m_Vertices.push_back(v[i]);
+ }
+
+
+ // Structure for unit icosahedron
+ Face f[20];
+ f[0].set(5, 8, 9);
+ f[1].set(5, 10, 8);
+ f[2].set(6, 12, 7);
+ f[3].set(6, 7, 11);
+ f[4].set(1, 4, 5);
+ f[5].set(1, 6, 4);
+ f[6].set(3, 2, 8);
+ f[7].set(3, 7, 2);
+ f[8].set(9, 12, 1);
+ f[9].set(9, 2, 12);
+ f[10].set(10, 4, 11);
+ f[11].set(10, 11, 3);
+ f[12].set(9, 1, 5);
+ f[13].set(12, 6, 1);
+ f[14].set(5, 4, 10);
+ f[15].set(6, 11, 4);
+ f[16].set(8, 2, 9);
+ f[17].set(7, 12, 2);
+ f[18].set(8, 10, 3);
+ f[19].set(7, 3, 11);
+
+ for (int i = 0 ; i < 20 ; i++)
+ {
+ f[i].m_n1--;
+ f[i].m_n2--;
+ f[i].m_n3--;
+ m_Faces.push_back(f[i]);
+ }
+}
+
+void CGraphGenerator::subDivide()
+{
+ // buffer new faces
+ vector<Face> faces;
+
+ // gp through all faces and subdivide
+ for (int f = 0 ; f < int(m_Faces.size()) ; f++)
+ {
+ // get the triangle vertex indices
+ int nA = m_Faces.at(f).m_n1;
+ int nB = m_Faces.at(f).m_n2;
+ int nC = m_Faces.at(f).m_n3;
+
+ // get the triangle vertex coordinates
+ Vec3d a = m_Vertices.at(nA);
+ Vec3d b = m_Vertices.at(nB);
+ Vec3d c = m_Vertices.at(nC);
+
+ // Now find the midpoints between vertices
+ Vec3d a_m, b_m, c_m;
+ Math3d::AddVecVec(a, b, a_m);
+ Math3d::MulVecScalar(a_m, 0.5, a_m);
+ Math3d::AddVecVec(b, c, b_m);
+ Math3d::MulVecScalar(b_m, 0.5, b_m);
+ Math3d::AddVecVec(c, a, c_m);
+ Math3d::MulVecScalar(c_m, 0.5, c_m);
+ /* printf("a: %f,%f,%f\n", a.x,a.y,a.z);
+ printf("b: %f,%f,%f\n", b.x,b.y,b.z);
+ printf("c: %f,%f,%f\n", c.x,c.y,c.z);
+
+ printf("a<->b: %f,%f,%f\n", a_m.x,a_m.y,a_m.z);
+ printf("b<->c: %f,%f,%f\n", b_m.x,b_m.y,b_m.z);
+ printf("c<->a: %f,%f,%f\n", c_m.x,c_m.y,c_m.z);*/
+
+ // add vertices
+ int nA_m, nB_m, nC_m;
+
+ int nIndex;
+ nIndex = findVertex(a_m, 0.01);
+
+ if (nIndex == -1)
+ {
+ m_Vertices.push_back(a_m);
+ nA_m = m_Vertices.size() - 1;
+ }
+ else
+ {
+ nA_m = nIndex;
+ }
+
+ nIndex = findVertex(b_m, 0.01);
+
+ if (nIndex == -1)
+ {
+ m_Vertices.push_back(b_m);
+ nB_m = m_Vertices.size() - 1;
+ }
+ else
+ {
+ nB_m = nIndex;
+ }
+
+ nIndex = findVertex(c_m, 0.01);
+
+ if (nIndex == -1)
+ {
+ m_Vertices.push_back(c_m);
+ nC_m = m_Vertices.size() - 1;
+ }
+ else
+ {
+ nC_m = nIndex;
+ }
+
+ // Create new faces with orig vertices plus midpoints
+ Face f[4];
+ f[0].set(nA, nA_m, nC_m);
+ f[1].set(nA_m, nB, nB_m);
+ f[2].set(nC_m, nB_m, nC);
+ f[3].set(nA_m, nB_m, nC_m);
+
+ for (int i = 0 ; i < 4 ; i++)
+ {
+ faces.push_back(f[i]);
+ }
+ }
+
+ // assign new faces
+ m_Faces = faces;
+}
+
+void CGraphGenerator::projectToSphere(float fRadius)
+{
+ TSphereCoord coord;
+ Vec3d point;
+ coord.fDistance = 1.0f;
+
+ for (int v = 0 ; v < int(m_Vertices.size()) ; v++)
+ {
+ float X = m_Vertices.at(v).x;
+ float Y = m_Vertices.at(v).y;
+ float Z = m_Vertices.at(v).z;
+
+ float x, y, z;
+
+ // Convert Cartesian X,Y,Z to spherical (radians)
+ float theta = atan2(Y, X);
+ float phi = atan2(sqrt(X * X + Y * Y), Z);
+
+ // Recalculate X,Y,Z for constant r, given theta & phi.
+ x = fRadius * sin(phi) * cos(theta);
+ y = fRadius * sin(phi) * sin(theta);
+ z = fRadius * cos(phi);
+
+ m_Vertices.at(v).x = x;
+ m_Vertices.at(v).y = y;
+ m_Vertices.at(v).z = z;
+ }
+}
+
+void CGraphGenerator::buildNodesAndEdges(CSphericalGraph* pGraph)
+{
+ // add nodes
+ TSphereCoord coord;
+
+ for (int v = 0 ; v < int(m_Vertices.size()) ; v++)
+ {
+ // printf("Vertex: %f,%f,%f\n", m_Vertices.at(v).x, m_Vertices.at(v).y, m_Vertices.at(v).z);
+ MathTools::convert(m_Vertices.at(v), coord);
+ CSGNode* pNode = pGraph->getNewNode();
+ pNode->setPosition(coord);
+ pGraph->addNode(pNode);
+ }
+
+ // add edges
+ // go through all faces (assumes check for duplicate edges in spherical graph!?)
+ for (int f = 0 ; f < int(m_Faces.size()) ; f++)
+ {
+ // printf(" %d,%d,%d\n", m_Faces.at(f).m_n1, m_Faces.at(f).m_n2, m_Faces.at(f).m_n3);
+ pGraph->addEdge(m_Faces.at(f).m_n1, m_Faces.at(f).m_n2);
+ pGraph->addEdge(m_Faces.at(f).m_n2, m_Faces.at(f).m_n3);
+ pGraph->addEdge(m_Faces.at(f).m_n3, m_Faces.at(f).m_n1);
+ }
+}
+
+int CGraphGenerator::findVertex(Vec3d position, float fEpsilon)
+{
+ float fDistance;
+ float fMinDistance = FLT_MAX;
+ int nIndex = -1;
+
+ for (int v = 0 ; v < int(m_Vertices.size()) ; v++)
+ {
+ fDistance = Math3d::Angle(position, m_Vertices.at(v));
+
+ if (fDistance < fMinDistance)
+ {
+ fMinDistance = fDistance;
+ nIndex = v;
+ }
+ }
+
+ if (fMinDistance > fEpsilon)
+ {
+ nIndex = -1;
+ }
+
+ return nIndex;
+}
+
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.h b/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.h
new file mode 100644
index 000000000..7904916da
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.h
@@ -0,0 +1,85 @@
+// *****************************************************************
+// Filename: GraphGenerator.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 08.07.2008
+// *****************************************************************
+
+#ifndef _GRAPH_GENERATOR_H_
+#define _GRAPH_GENERATOR_H_
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "Base/DataStructures/Graph/SphericalGraph.h"
+#include "Base/Math/MathTools.h"
+#include "Math/Math3d.h"
+#include <vector>
+#include <float.h>
+
+using namespace std;
+
+// *****************************************************************
+// enums
+// *****************************************************************
+enum EPolyhedronType
+{
+ eTetrahedron,
+ eOctahedron,
+ eIcosahedron
+};
+
+// *****************************************************************
+// implementation of class Face
+// *****************************************************************
+class Face
+{
+public:
+ Face() {};
+ Face(int n1, int n2, int n3)
+ {
+ set(n1, n2, n3);
+ };
+ ~Face() {};
+
+ void set(int n1, int n2, int n3)
+ {
+ m_n1 = n1;
+ m_n2 = n2 ;
+ m_n3 = n3;
+ };
+
+ int m_n1;
+ int m_n2;
+ int m_n3;
+};
+
+// *****************************************************************
+// implementation of CGraphGenerator
+// *****************************************************************
+class CGraphGenerator
+{
+public:
+ static void generateGraph(CSphericalGraph* pPlainGraph, EPolyhedronType nBaseType, int nLevels, float fRadius);
+
+private:
+ static void initBasePolyhedron(EPolyhedronType nBaseType);
+ static void generateTetrahedron();
+ static void generateOctahedron();
+ static void generateIcosahedron();
+ static void subDivide();
+ static void projectToSphere(float fRadius);
+ static void buildNodesAndEdges(CSphericalGraph* pGraph);
+ static int findVertex(Vec3d position, float fEpsilon);
+
+private:
+ CGraphGenerator() {};
+ ~CGraphGenerator() {};
+
+ static vector<Vec3d> m_Vertices;
+ static vector<Face> m_Faces;
+};
+
+#endif /* _GRAPH_GENERATOR_H_ */
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.cpp
new file mode 100644
index 000000000..a6e856c56
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.cpp
@@ -0,0 +1,238 @@
+// *****************************************************************
+// Filename: GraphLookupTable.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 10.10.2008
+// *****************************************************************
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "GraphLookupTable.h"
+#include "MathTools.h"
+#include <float.h>
+
+// *****************************************************************
+// implementation of CGraphLookupTable
+// *****************************************************************
+// construction / destruction
+CGraphLookupTable::CGraphLookupTable(int nMaxZenithBins, int nMaxAzimuthBins)
+{
+ m_nMaxZenithBins = nMaxZenithBins;
+ m_nMaxAzimuthBins = nMaxAzimuthBins;
+
+ buildMemory();
+}
+
+CGraphLookupTable::~CGraphLookupTable()
+{
+ cleanMemory();
+}
+
+void CGraphLookupTable::buildLookupTable(CSphericalGraph* pGraph)
+{
+ reset();
+
+ TNodeList* pNodes = pGraph->getNodes();
+ int nSize = pNodes->size();
+
+ for (int n = 0 ; n < nSize ; n++)
+ {
+ CSGNode* pNode = pNodes->at(n);
+ addEntry(pNode->getPosition(), n);
+ }
+
+ m_pGraph = pGraph;
+}
+
+int CGraphLookupTable::getClosestNode(TSphereCoord position)
+{
+ bool bExact;
+ return getClosestNode(position, bExact);
+}
+
+int CGraphLookupTable::getClosestNode(Eigen::Vector3d position)
+{
+ bool bExact;
+ return getClosestNode(position, bExact);
+}
+
+int CGraphLookupTable::getClosestNode(Eigen::Vector3d position, bool& bExact)
+{
+ TSphereCoord coords;
+ MathTools::convert(position, coords);
+
+ return getClosestNode(coords, bExact);
+}
+
+
+int CGraphLookupTable::getClosestNode(TSphereCoord position, bool& bExact)
+{
+ int nIndex1, nIndex2;
+ nIndex1 = getZenithBinIndex(position.fPhi);
+ nIndex2 = getAzimuthBinIndex(position.fTheta, position.fPhi);
+
+ list<int>::iterator iter = m_ppTable[nIndex1][nIndex2].begin();
+
+ float fMinDistance = FLT_MAX;
+ int nBestIndex = -1;
+ float fDistance;
+
+ while (iter != m_ppTable[nIndex1][nIndex2].end())
+ {
+ fDistance = MathTools::getDistanceOnArc(position, m_pGraph->getNode(*iter)->getPosition());
+
+ if (fDistance < fMinDistance)
+ {
+ fMinDistance = fDistance;
+ nBestIndex = *iter;
+ }
+
+ iter++;
+ }
+
+ // retrieve distance to bounding
+ float fMinPhi, fMaxPhi, fMinTheta, fMaxTheta;
+
+ float fTestValues[4];
+ // printf("Current position: phi %f, theta %f\n", position.fPhi, position.fTheta);
+
+ getBinMinMaxValues(nIndex1, nIndex2, fMinPhi, fMaxPhi, fMinTheta, fMaxTheta);
+ // printf("B Phi: %f - %f\n", fMinPhi, fMaxPhi);
+ // printf("B Theta: %f - %f\n", fMinTheta, fMaxTheta);
+
+ fTestValues[0] = fabs(fMinPhi - position.fPhi) * M_PI / 180.0f;
+ fTestValues[1] = fabs(fMaxPhi - position.fPhi) * M_PI / 180.0f;
+ fTestValues[2] = fabs(fMinTheta - position.fTheta) * M_PI / 180.0f;
+ fTestValues[3] = fabs(fMaxTheta - position.fTheta) * M_PI / 180.0f;
+ bExact = true;
+
+ // printf("AzimuthDistance (min,max,current): (%f,%f,%f)\n", fTestValues[2], fTestValues[3], fMinDistance);
+ // printf("ZenithDistance (min,max,current): (%f,%f,%f)\n", fTestValues[0], fTestValues[1], fMinDistance);
+
+ for (int i = 0 ; i < 4 ; i++)
+ {
+ if (fTestValues[i] < fMinDistance)
+ {
+ bExact = false;
+ }
+ }
+
+ return nBestIndex;
+}
+
+void CGraphLookupTable::reset()
+{
+ float fZenith;
+
+ for (int z = 0 ; z < getNumberZenithBins() ; z++)
+ {
+ fZenith = 180.0f / m_nMaxZenithBins * z;
+
+ for (int a = 0 ; a < getNumberAzimuthBins(fZenith) ; a++)
+ {
+ m_ppTable[z][a].clear();
+ }
+ }
+}
+
+// *****************************************************************
+// private methods
+// *****************************************************************
+void CGraphLookupTable::addEntry(TSphereCoord position, int nIndex)
+{
+ int nIndex1, nIndex2;
+ nIndex1 = getZenithBinIndex(position.fPhi);
+ nIndex2 = getAzimuthBinIndex(position.fTheta, position.fPhi);
+
+ m_ppTable[nIndex1][nIndex2].push_back(nIndex);
+}
+
+void CGraphLookupTable::buildMemory()
+{
+ float fZenith;
+
+ m_ppTable = new list<int>* [m_nMaxZenithBins];
+
+ for (int z = 0 ; z < m_nMaxZenithBins ; z++)
+ {
+ fZenith = 180.0f / m_nMaxZenithBins * z;
+ m_ppTable[z] = new list<int>[getNumberAzimuthBins(fZenith)];
+ }
+}
+
+void CGraphLookupTable::cleanMemory()
+{
+ for (int z = 0 ; z < m_nMaxZenithBins ; z++)
+ {
+ delete [] m_ppTable[z];
+ }
+
+ delete [] m_ppTable;
+}
+
+int CGraphLookupTable::getNumberZenithBins()
+{
+ return m_nMaxZenithBins;
+}
+
+int CGraphLookupTable::getNumberAzimuthBins(float fZenith)
+{
+ int nZenithBin = getZenithBinIndex(fZenith);
+
+ // 0 degree means 1 min bins
+ // 90 degree means max bins
+ int nMinBins = 1;
+ float fNumberBins;
+
+ if (getNumberZenithBins() > 1)
+ {
+ fNumberBins = sin(float(nZenithBin) / (getNumberZenithBins() - 1) * M_PI) * (m_nMaxAzimuthBins - nMinBins) + nMinBins;
+ }
+ else
+ {
+ fNumberBins = 0.5f;
+ }
+
+ return int(fNumberBins + 0.5f);
+}
+
+int CGraphLookupTable::getAzimuthBinIndex(float fAzimuth, float fZenith)
+{
+ float fBinIndex = fAzimuth / 360.0f * (getNumberAzimuthBins(fZenith) - 1) + 0.5f;
+ return int(fBinIndex);
+}
+
+int CGraphLookupTable::getZenithBinIndex(float fZenith)
+{
+ float fBinIndex = fZenith / 180.0f * (m_nMaxZenithBins - 1) + 0.5f;
+ return int(fBinIndex);
+}
+
+void CGraphLookupTable::getBinMinMaxValues(int nZenithBinIndex, int nAzimuthBinIndex, float& fMinZenith, float& fMaxZenith, float& fMinAzimuth, float& fMaxAzimuth)
+{
+ if ((m_nMaxZenithBins - 1) == 0)
+ {
+ fMinZenith = -FLT_MAX;
+ fMaxZenith = FLT_MAX;
+ }
+ else
+ {
+ fMinZenith = (nZenithBinIndex - 0.5f) * 180.0f / (m_nMaxZenithBins - 1);
+ fMaxZenith = (nZenithBinIndex + 0.5f) * 180.0f / (m_nMaxZenithBins - 1);
+ }
+
+ if ((getNumberAzimuthBins((fMinZenith + fMaxZenith) / 2.0f) - 1) == 0)
+ {
+ fMinAzimuth = -FLT_MAX;
+ fMaxAzimuth = FLT_MAX;
+ }
+ else
+ {
+ fMinAzimuth = (nAzimuthBinIndex - 0.5f) * 360.0f / (getNumberAzimuthBins((fMinZenith + fMaxZenith) / 2.0f) - 1);
+ fMaxAzimuth = (nAzimuthBinIndex + 0.5f) * 360.0f / (getNumberAzimuthBins((fMinZenith + fMaxZenith) / 2.0f) - 1);
+ }
+}
+
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.h b/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.h
new file mode 100644
index 000000000..b97a195b3
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.h
@@ -0,0 +1,65 @@
+// *****************************************************************
+// Filename: GraphLookupTable.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 10.10.2008
+// *****************************************************************
+
+#ifndef _GRAPH_LOOKUP_TABLE_H_
+#define _GRAPH_LOOKUP_TABLE_H_
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "SphericalGraph.h"
+#include <list>
+
+// *****************************************************************
+// namespace
+// *****************************************************************
+using namespace std;
+
+// *****************************************************************
+// decleration of CGraphLookupTable
+// *****************************************************************
+class CGraphLookupTable
+{
+public:
+ // construction / destruction
+ CGraphLookupTable(int nMaxZenithBins, int nMaxAzimuthBins);
+ ~CGraphLookupTable();
+
+ // build table
+ void buildLookupTable(CSphericalGraph* pGraph);
+
+ // operation
+ int getClosestNode(Eigen::Vector3d position);
+ int getClosestNode(TSphereCoord position);
+ int getClosestNode(Eigen::Vector3d position, bool& bExact);
+ int getClosestNode(TSphereCoord position, bool& bExact);
+
+private:
+ void buildMemory();
+ void cleanMemory();
+ void addEntry(TSphereCoord position, int nIndex);
+ void reset();
+
+ int getNumberZenithBins();
+ int getNumberAzimuthBins(float fZenith);
+ int getAzimuthBinIndex(float fAzimuth, float fZenith);
+ int getZenithBinIndex(float fZenith);
+
+ void getBinMinMaxValues(int nZenithBinIndex, int nAzimuthBinIndex, float& fMinZenith, float& fMaxZenith, float& fMinAzimuth, float& fMaxAzimuth);
+
+ int m_nMaxZenithBins;
+ int m_nMaxAzimuthBins;
+
+ list<int>** m_ppTable;
+
+ CSphericalGraph* m_pGraph;
+};
+
+
+#endif /* _GRAPH_LOOKUP_TABLE_H_ */
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphMap.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphMap.cpp
new file mode 100644
index 000000000..deaeab33e
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphMap.cpp
@@ -0,0 +1,262 @@
+// *****************************************************************
+// Filename: GraphMap.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 9.08.2007
+// *****************************************************************
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "GraphMap.h"
+#include "Base/Math/MathTools.h"
+#include "Base/DataStructures/Graph/GraphProcessor.h"
+#include <list>
+#include "Helpers/helpers.h"
+
+using namespace std;
+
+// *****************************************************************
+// implementation of CGraphMap
+// *****************************************************************
+// construction / destruction
+CGraphMap::CGraphMap(CIntensityGraph* pGraph, bool bUseLookup)
+{
+ m_pGraph = pGraph;
+ m_nNumberNodes = pGraph->getNodes()->size();
+
+ m_nWidth = (int) sqrt(float(m_nNumberNodes));
+ m_nHeight = m_nNumberNodes / m_nWidth + 1;
+
+ m_bUseLookup = bUseLookup;
+
+ // init lookup table
+ if (m_bUseLookup)
+ {
+ m_pLookupTable = new CGraphPyramidLookupTable(100, 100);
+ m_pLookupTable->buildLookupTable(pGraph);
+ }
+
+ // create float matrix
+ m_pMap = new CFloatMatrix(m_nWidth, m_nHeight);
+ m_bOwnMemory = true;
+ toMap();
+}
+
+CGraphMap::~CGraphMap()
+{
+ if (m_bOwnMemory)
+ {
+ delete m_pMap;
+ }
+
+ if (m_bUseLookup)
+ {
+ delete m_pLookupTable;
+ }
+}
+
+void CGraphMap::setMap(CFloatMatrix* pMap, bool bUpdateGraph)
+{
+ if (m_bOwnMemory)
+ {
+ delete m_pMap;
+ }
+
+ m_bOwnMemory = false;
+
+ m_pMap = pMap;
+ m_nWidth = pMap->columns;
+ m_nHeight = pMap->rows;
+
+ if (bUpdateGraph)
+ {
+ toGraph();
+ }
+}
+
+CFloatMatrix* CGraphMap::getMap()
+{
+ return m_pMap;
+}
+
+int CGraphMap::getWidth()
+{
+ return m_nWidth;
+}
+
+int CGraphMap::getHeight()
+{
+ return m_nHeight;
+}
+
+// manipulation
+void CGraphMap::applyGaussian(EOperation nOperation, Vec3d position, float fAmplitude, float fVariance)
+{
+ TSphereCoord coord;
+ MathTools::convert(position, coord);
+
+ applyGaussian(nOperation, coord, fAmplitude, fVariance);
+}
+
+void CGraphMap::applyGaussian(EOperation nOperation, TSphereCoord coord, float fAmplitude, float fVariance)
+{
+ int nNodeID = 0;
+
+ if (m_bUseLookup)
+ {
+ nNodeID = m_pLookupTable->getClosestNode(coord);
+ }
+ else
+ {
+ nNodeID = GraphProcessor::findClosestNode(m_pGraph, coord);
+ }
+
+ applyGaussian(nOperation, nNodeID, fAmplitude, fVariance);
+}
+
+void CGraphMap::applyGaussian(EOperation nOperation, int nNodeID, float fAmplitude, float fVariance)
+{
+ list<int> nodes;
+ vector<int> accomplished;
+
+ // store initial node
+ int nFirstNode = nNodeID;
+ TSphereCoord coord = m_pGraph->getNodes()->at(nFirstNode)->getPosition();
+ nodes.push_back(nFirstNode);
+
+ // eat nodelist
+ TSphereCoord cur_sc;
+ Vec3d ref, cur;
+ MathTools::convert(coord, ref);
+ float fIntensity;
+
+ while (nodes.size() > 0)
+ {
+ // retrieve first node
+ int nCurrentNode = nodes.front();
+ nodes.pop_front();
+ accomplished.push_back(nCurrentNode);
+
+ // calculate angle
+ cur_sc = m_pGraph->getNodes()->at(nCurrentNode)->getPosition();
+ MathTools::convert(cur_sc, cur);
+ float fAngle = Math3d::Angle(ref, cur);
+
+ // check if maximum angle
+ if (fAngle > 2.0 * fVariance)
+ {
+ continue;
+ }
+
+ // evaluate gaussian at angle
+ fIntensity = evaluateGaussian(fAngle, fAmplitude, fVariance);
+
+ // update graph and map
+ switch (nOperation)
+ {
+ case eSet:
+ m_pMap->data[nCurrentNode] = fIntensity;
+ break;
+
+ case eAdd:
+ m_pMap->data[nCurrentNode] += fIntensity;
+ break;
+
+ case eMultiply:
+ m_pMap->data[nCurrentNode] *= fIntensity;
+ break;
+
+ case eMax:
+ if (m_pMap->data[nCurrentNode] < fIntensity)
+ {
+ m_pMap->data[nCurrentNode] = fIntensity;
+ }
+
+ break;
+
+ default:
+ printf("Illegal operation\n");
+ break;
+ }
+
+ // add neighbours to nodelist
+ vector<int>* pAdjacency = m_pGraph->getNodeAdjacency(nCurrentNode);
+
+ // check for double nodes
+ int nAdjNode;
+
+ for (int a = 0 ; a < int(pAdjacency->size()) ; a++)
+ {
+ nAdjNode = pAdjacency->at(a);
+
+ // check if we already processed this node
+ bool bNewNode = true;
+
+ for (int c = 0 ; c < int(accomplished.size()) ; c++)
+ if (accomplished.at(c) == nAdjNode)
+ {
+ bNewNode = false;
+ }
+
+ list<int>::iterator iter = nodes.begin();
+
+ while (iter != nodes.end())
+ {
+ if (*iter == nAdjNode)
+ {
+ bNewNode = false;
+ }
+
+ iter++;
+ }
+
+
+ if (bNewNode)
+ {
+ nodes.push_back(nAdjNode);
+ }
+ }
+ }
+}
+
+void CGraphMap::updateGraph()
+{
+ toGraph();
+}
+
+void CGraphMap::updateMap()
+{
+ toMap();
+}
+
+float CGraphMap::evaluateGaussian(float fDist, float fAmplitude, float fVariance)
+{
+ return exp(- (fDist / fVariance * 2.0f)) * fAmplitude;
+}
+
+void CGraphMap::toGraph()
+{
+ float fIntensity;
+
+ for (int n = 0 ; n < m_nNumberNodes ; n++)
+ {
+ fIntensity = m_pMap->data[n];
+ ((CIntensityNode*) m_pGraph->getNodes()->at(n))->setIntensity(fIntensity);
+ }
+}
+
+void CGraphMap::toMap()
+{
+ float fIntensity;
+
+ for (int n = 0 ; n < m_nNumberNodes ; n++)
+ {
+ fIntensity = ((CIntensityNode*) m_pGraph->getNodes()->at(n))->getIntensity();
+ m_pMap->data[n] = fIntensity;
+ }
+}
+
+
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphMap.h b/source/RobotAPI/components/EarlyVisionGraph/GraphMap.h
new file mode 100644
index 000000000..e8533388a
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphMap.h
@@ -0,0 +1,74 @@
+// *****************************************************************
+// Filename: GraphMap.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+#ifndef _GRAPH_MAP_H_
+#define _GRAPH_MAP_H_
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "Base/DataStructures/Graph/IntensityGraph.h"
+#include "Base/DataStructures/Graph/GraphPyramidLookupTable.h"
+#include "Math/FloatMatrix.h"
+
+// *****************************************************************
+// enums
+// *****************************************************************
+enum EOperation
+{
+ eSet,
+ eAdd,
+ eMultiply,
+ eMax
+};
+
+// *****************************************************************
+// decleration of CGraphMap
+// *****************************************************************
+class CGraphMap
+{
+public:
+ // construction / destruction
+ CGraphMap(CIntensityGraph* pGraph, bool bUseLookup = false);
+ ~CGraphMap();
+
+ // getter / setter
+ void setMap(CFloatMatrix* pMap, bool bUpdateGraph = true);
+ CFloatMatrix* getMap();
+ int getWidth();
+ int getHeight();
+
+ // manipulation
+ void applyGaussian(EOperation nOperation, Vec3d position, float fAmplitude, float fVariance);
+ void applyGaussian(EOperation nOperation, TSphereCoord coord, float fAmplitude, float fVariance);
+ void applyGaussian(EOperation nOperation, int nNodeID, float fAmplitude, float fVariance);
+
+ // update
+ void updateGraph();
+ void updateMap();
+
+private:
+ float evaluateGaussian(float fDist, float fAmplitude, float fVariance);
+ void toGraph(); // update intensitygraph
+ void toMap(); // update intensitymap
+
+ CSphericalGraph* m_pGraph;
+ int m_nNumberNodes;
+ CFloatMatrix* m_pMap;
+ bool m_bOwnMemory;
+
+ int m_nWidth;
+ int m_nHeight;
+
+ bool m_bUseLookup;
+ CGraphPyramidLookupTable* m_pLookupTable;
+};
+
+
+#endif /* _GRAPH_MAP_H_ */
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.cpp
new file mode 100644
index 000000000..c5faffd65
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.cpp
@@ -0,0 +1,307 @@
+// *****************************************************************
+// Filename: GraphProcessor.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+
+// *****************************************************************
+// includes
+// *****************************************************************
+// system
+#include <math.h>
+#include <float.h>
+
+// local includes
+#include "GraphProcessor.h"
+#include "Base/Math/MathTools.h"
+#include "Base/Tools/DebugMemory.h"
+
+void GraphProcessor::transform(CSphericalGraph* pGraph, TTransform transform)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+
+ TSphereCoord position, position_tr;
+
+ for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ {
+ CSGNode* pNode = pNodes->at(n);
+ position = pNode->getPosition();
+ position_tr = MathTools::transform(position, transform);
+ pNode->setPosition(position_tr);
+ }
+}
+
+void GraphProcessor::inverseTransform(CSphericalGraph* pGraph, TTransform transform)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+
+ TSphereCoord position, position_tr;
+
+ for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ {
+ CSGNode* pNode = pNodes->at(n);
+ position = pNode->getPosition();
+ position_tr = MathTools::inverseTransform(position, transform);
+ pNode->setPosition(position_tr);
+ }
+}
+
+void GraphProcessor::invertPhi(CSphericalGraph* pGraph)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+
+ TSphereCoord position;
+
+ for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ {
+ CSGNode* pNode = pNodes->at(n);
+ position = pNode->getPosition();
+ position.fPhi = 360.0f - position.fPhi;
+ pNode->setPosition(position);
+ }
+}
+
+void GraphProcessor::invertTheta(CSphericalGraph* pGraph)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+
+ TSphereCoord position;
+
+ for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ {
+ CSGNode* pNode = pNodes->at(n);
+ position = pNode->getPosition();
+ position.fTheta = 180.0f - position.fTheta;
+ pNode->setPosition(position);
+ }
+}
+
+void GraphProcessor::copyGraphNodesAndEdges(CSphericalGraph* pSrcGraph, CSphericalGraph* pDstGraph)
+{
+ pDstGraph->clear();
+
+ TNodeList* pNodes = pSrcGraph->getNodes();
+ TEdgeList* pEdges = pSrcGraph->getEdges();
+
+ for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ {
+ CSGNode* pNode = pNodes->at(n);
+ CSGNode* pNewNode = pDstGraph->getNewNode();
+ pNewNode->setPosition(pNode->getPosition());
+ pDstGraph->addNode(pNewNode);
+ }
+
+ for (int e = 0 ; e < int(pEdges->size()) ; e++)
+ {
+ CSGEdge* pEdge = pEdges->at(e);
+ pDstGraph->addEdge(pEdge->nIndex1, pEdge->nIndex2, pEdge->nLeftFace, pEdge->nRightFace);
+ }
+}
+
+int GraphProcessor::findClosestNode(CSphericalGraph* pGraph, TSphereCoord coord)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+
+ float fMinDistance = FLT_MAX;
+ float fDistance = 0.0f;
+ int nIndex = -1;
+
+ for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ {
+ CSGNode* pNode = pNodes->at(n);
+ fDistance = MathTools::getDistanceOnArc(pNode->getPosition(), coord);
+
+ if (fDistance < fMinDistance)
+ {
+ fMinDistance = fDistance;
+ nIndex = n;
+ }
+ }
+
+ return nIndex;
+}
+
+void GraphProcessor::findClosestNeighbours(CSphericalGraph* pGraph, int& s, int& t)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+
+ float fMinDistance = FLT_MAX;
+ int nIndex1 = -1;
+ int nIndex2 = -1;
+ float fDistance;
+
+ for (int i = 0 ; i < int(pNodes->size()); i++)
+ {
+ for (int j = 0 ; j < int(pNodes->size()); j++)
+ {
+ if (i != j)
+ {
+ fDistance = MathTools::getDistanceOnArc(pNodes->at(i)->getPosition(), pNodes->at(j)->getPosition());
+
+ if (fDistance < fMinDistance)
+ {
+ fMinDistance = fDistance;
+ nIndex1 = i;
+ nIndex2 = j;
+ }
+ }
+ }
+ }
+
+ s = nIndex1;
+ t = nIndex2;
+}
+
+int GraphProcessor::findEdge(CSphericalGraph* pGraph, int nIndex1, int nIndex2)
+{
+ TEdgeList* pEdges = pGraph->getEdges();
+
+ for (int e = 0 ; e < int(pEdges->size()) ; e++)
+ {
+ if ((pEdges->at(e)->nIndex1 == nIndex1) && (pEdges->at(e)->nIndex2 == nIndex2))
+ {
+ return e;
+ }
+
+ if ((pEdges->at(e)->nIndex1 == nIndex2) && (pEdges->at(e)->nIndex2 == nIndex1))
+ {
+ return e;
+ }
+ }
+
+ // not found
+ return -1;
+}
+
+bool GraphProcessor::insideCircumcircle(CSphericalGraph* pGraph, int nIndex1, int nIndex2, int nIndex3, int nPointIndex, bool& bInside)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+
+ TSphereCoord center;
+ float fRadius;
+
+ if (!getCircumcircle(pGraph, nIndex1, nIndex2, nIndex3, center, fRadius))
+ {
+ return false;
+ }
+
+ TSphereCoord point = pNodes->at(nPointIndex)->getPosition();
+
+ // check with give coordinates
+ float fDistance = sqrt((center.fPhi - point.fPhi) * (center.fPhi - point.fPhi) + (center.fTheta - point.fTheta) * (center.fTheta - point.fTheta));
+
+ bInside = (fDistance < (fRadius + 0.0001));
+
+ return true;
+}
+
+bool GraphProcessor::getCircumcircle(CSphericalGraph* pGraph, int nIndex1, int nIndex2, int nIndex3, TSphereCoord& center, float& fRadius)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+ float cp;
+
+ TSphereCoord s1, s2, s3;
+ s1 = pNodes->at(nIndex1)->getPosition();
+ s2 = pNodes->at(nIndex2)->getPosition();
+ s3 = pNodes->at(nIndex3)->getPosition();
+
+ cp = MathTools::CalculateCrossProductFromDifference(s1, s2, s3);
+
+ if (cp != 0.0)
+ {
+ float p1Sq, p2Sq, p3Sq;
+ float num;
+ float cx, cy;
+
+ p1Sq = s1.fPhi * s1.fPhi + s1.fTheta * s1.fTheta;
+ p2Sq = s2.fPhi * s2.fPhi + s2.fTheta * s2.fTheta;
+ p3Sq = s3.fPhi * s3.fPhi + s3.fTheta * s3.fTheta;
+
+ num = p1Sq * (s2.fTheta - s3.fTheta) + p2Sq * (s3.fTheta - s1.fTheta) + p3Sq * (s1.fTheta - s2.fTheta);
+ cx = num / (2.0f * cp);
+ num = p1Sq * (s3.fPhi - s2.fPhi) + p2Sq * (s1.fPhi - s3.fPhi) + p3Sq * (s2.fPhi - s1.fPhi);
+ cy = num / (2.0f * cp);
+
+ center.fPhi = cx;
+ center.fTheta = cy;
+
+ fRadius = sqrt((center.fPhi - s1.fPhi) * (center.fPhi - s1.fPhi) + (center.fTheta - s1.fTheta) * (center.fTheta - s1.fTheta)) ;
+ return true;
+ }
+ else
+ {
+ // points lie on a line
+ return false;
+ }
+}
+
+int GraphProcessor::getDelaunayNeighbour(CSphericalGraph* pGraph, CSGEdge* pEdge)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+ float fMinRadius = FLT_MAX;
+ float fRadius;
+ int nIndex = -1;
+
+ // go through all points
+ for (int v = 0 ; v < int(pNodes->size()) ; v++)
+ {
+ if ((v == pEdge->nIndex1) || (v == pEdge->nIndex2))
+ {
+ continue;
+ }
+
+ // check if v is already connected to index1, index2
+ if (GraphProcessor::isEdgePresent(pGraph, pEdge->nIndex1, v))
+ {
+ continue;
+ }
+
+ if (GraphProcessor::isEdgePresent(pGraph, pEdge->nIndex2, v))
+ {
+ continue;
+ }
+
+ // check if v is an unconnected node
+ if (pGraph->getNodeAdjacency(v)->size() != 0)
+ {
+ continue;
+ }
+
+ TSphereCoord center;
+
+ getCircumcircle(pGraph, pEdge->nIndex1, pEdge->nIndex2, v, center, fRadius);
+
+ if (fRadius < fMinRadius)
+ {
+ fMinRadius = fRadius;
+ nIndex = v;
+ }
+ }
+
+ return nIndex;
+}
+
+bool GraphProcessor::isEdgePresent(CSphericalGraph* pGraph, int nIndex1, int nIndex2)
+{
+
+ bool bPresent = false;
+
+ vector<int>::iterator iter = pGraph->getNodeAdjacency(nIndex1)->begin();
+
+ while (iter != pGraph->getNodeAdjacency(nIndex1)->end())
+ {
+ if ((*iter) == nIndex2)
+ {
+ bPresent = true;
+ }
+
+ iter++;
+ }
+
+ return bPresent;
+}
+
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.h b/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.h
new file mode 100644
index 000000000..2a609783b
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.h
@@ -0,0 +1,50 @@
+// *****************************************************************
+// Filename: GraphProcessor.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+#ifndef _GRAPH_PROCESSOR_H_
+#define _GRAPH_PROCESSOR_H_
+
+// *****************************************************************
+// forward declarations
+// *****************************************************************
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "SphericalGraph.h"
+
+// *****************************************************************
+// namespace GraphProcessor
+// *****************************************************************
+namespace GraphProcessor
+{
+ // graph operations
+ void copyGraphNodesAndEdges(CSphericalGraph* pSrcGraph, CSphericalGraph* pDstGraph);
+
+ // graph coordinate transformations
+ void transform(CSphericalGraph* pGraph, TTransform transform);
+ void inverseTransform(CSphericalGraph* pGraph, TTransform transform);
+ void invertPhi(CSphericalGraph* pGraph);
+ void invertTheta(CSphericalGraph* pGraph);
+
+ // edge operations
+ int findEdge(CSphericalGraph* pGraph, int nIndex1, int nIndex2);
+ bool isEdgePresent(CSphericalGraph* pGraph, int nIndex1, int nIndex2);
+
+ // neighbour and node operations
+ int findClosestNode(CSphericalGraph* pGraph, TSphereCoord coord);
+ void findClosestNeighbours(CSphericalGraph* pGraph, int& s, int& t);
+ int getDelaunayNeighbour(CSphericalGraph* pGraph, CSGEdge* pEdge);
+
+ // circumcircle operations
+ bool insideCircumcircle(CSphericalGraph* pGraph, int nIndex1, int nIndex2, int nIndex3, int nPointIndex, bool& bInside);
+ bool getCircumcircle(CSphericalGraph* pGraph, int nIndex1, int nIndex2, int nIndex3, TSphereCoord& center, float& fRadius);
+};
+
+#endif /* _GRAPH_PROCESSOR_H_ */
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.cpp
new file mode 100644
index 000000000..babc001fb
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.cpp
@@ -0,0 +1,108 @@
+// *****************************************************************
+// Filename: GraphLookupTable.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 10.10.2008
+// *****************************************************************
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include <cstdlib>
+#include "GraphPyramidLookupTable.h"
+#include "MathTools.h"
+
+// *****************************************************************
+// implementation of CGraphLookupTable
+// *****************************************************************
+// construction / destruction
+CGraphPyramidLookupTable::CGraphPyramidLookupTable(int nMaxZenithBins, int nMaxAzimuthBins)
+{
+ m_nMaxZenithBins = nMaxZenithBins;
+ m_nMaxAzimuthBins = nMaxAzimuthBins;
+
+ m_nSubDivision = 2;
+ m_nLevels = 0;
+
+ CGraphLookupTable* pTable;
+
+ while (nMaxZenithBins != 0)
+ {
+ pTable = new CGraphLookupTable(nMaxZenithBins, nMaxAzimuthBins);
+
+ m_Tables.push_back(pTable);
+
+ // printf("Level %d: zenithbins: %d, azimuithbins: %d\n", m_nLevels, nMaxZenithBins, nMaxAzimuthBins);
+
+ m_nLevels++;
+ nMaxZenithBins /= m_nSubDivision;
+ nMaxAzimuthBins /= m_nSubDivision;
+ }
+
+}
+
+CGraphPyramidLookupTable::~CGraphPyramidLookupTable()
+{
+ list<CGraphLookupTable*>::iterator iter = m_Tables.begin();
+
+ while (iter != m_Tables.end())
+ {
+ delete *iter;
+
+ iter++;
+ }
+}
+
+void CGraphPyramidLookupTable::buildLookupTable(CSphericalGraph* pGraph)
+{
+ list<CGraphLookupTable*>::iterator iter = m_Tables.begin();
+
+ while (iter != m_Tables.end())
+ {
+ (*iter)->buildLookupTable(pGraph);
+
+ iter++;
+ }
+}
+
+int CGraphPyramidLookupTable::getClosestNode(Eigen::Vector3d position)
+{
+ TSphereCoord coords;
+ MathTools::convert(position, coords);
+
+ return getClosestNode(coords);
+}
+
+int CGraphPyramidLookupTable::getClosestNode(TSphereCoord position)
+{
+ bool bFinished = false;
+ list<CGraphLookupTable*>::iterator iter = m_Tables.begin();
+ int nIndex = -1;
+
+ int L = 0;
+
+ while (iter != m_Tables.end())
+ {
+ nIndex = (*iter)->getClosestNode(position, bFinished);
+ // printf("Result in level %d: index %d, finished: %d\n",L,nIndex,bFinished);
+
+ if (bFinished)
+ {
+ break;
+ }
+
+ L++;
+ iter++;
+ }
+
+ if (!bFinished)
+ {
+ printf("CGraphPyramidLookupTable:: no closest node found\n");
+ exit(1);
+ }
+
+ return nIndex;
+}
+
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h b/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h
new file mode 100644
index 000000000..b871f1d0a
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h
@@ -0,0 +1,52 @@
+// *****************************************************************
+// Filename: GraphPyramidLookupTable.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 10.10.2008
+// *****************************************************************
+
+#ifndef _GRAPH_PYRAMID_LOOKUP_TABLE_H_
+#define _GRAPH_PYRAMID_LOOKUP_TABLE_H_
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "SphericalGraph.h"
+#include "GraphLookupTable.h"
+#include <list>
+
+// *****************************************************************
+// namespace
+// *****************************************************************
+using namespace std;
+
+// *****************************************************************
+// decleration of CGraphPyramidLookupTable
+// *****************************************************************
+class CGraphPyramidLookupTable
+{
+public:
+ // construction / destruction
+ CGraphPyramidLookupTable(int nMaxZenithBins, int nMaxAzimuthBins);
+ ~CGraphPyramidLookupTable();
+
+ // build table
+ void buildLookupTable(CSphericalGraph* pGraph);
+
+ // operation
+ int getClosestNode(Eigen::Vector3d position);
+ int getClosestNode(TSphereCoord position);
+
+private:
+ list<CGraphLookupTable*> m_Tables;
+ int m_nSubDivision;
+ int m_nLevels;
+
+ int m_nMaxZenithBins;
+ int m_nMaxAzimuthBins;
+};
+
+
+#endif /* _GRAPH_PYRAMID_LOOKUP_TABLE_H_ */
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.cpp
new file mode 100644
index 000000000..495ebc843
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.cpp
@@ -0,0 +1,423 @@
+// *****************************************************************
+// Filename: GraphProcessor.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+
+// *****************************************************************
+// includes
+// *****************************************************************
+// local includes
+#include "GraphTriangulation.h"
+#include "GraphProcessor.h"
+#include "Base/Math/MathTools.h"
+#include "Base/Tools/DebugMemory.h"
+
+void GraphTriangulation::delaunayTriangulationQuadratic(CSphericalGraph* pGraph)
+{
+ TC3DNodeList* pC3DNodeList = new TC3DNodeList();
+ int nNodes = (int) pGraph->getNodes()->size();
+
+ for (int i = 0; i < nNodes; i++)
+ {
+ // project point to polar coordinates
+ Vec3d cartesian_point;
+ MathTools::convert(pGraph->getNode(i)->getPosition(), cartesian_point);
+
+ CC3DNode* pC3DNode = new CC3DNode(cartesian_point);
+ pC3DNodeList->push_back(pC3DNode);
+ }
+
+ // generate the delaunay triangulation by computing the
+ // convex hull of the sphere
+ CCHGiftWrap giftWrapping = CCHGiftWrap(pC3DNodeList);
+ CSphericalGraph* pCopy = new CSphericalGraph();
+ giftWrapping.generateConvexHull(pCopy);
+
+ // add edges from triangulated graph to pGraph
+ TEdgeList* pEdges = pCopy->getEdges();
+
+ for (int e = 0 ; e < pEdges->size() ; e++)
+ {
+ pGraph->addEdge(pEdges->at(e)->nIndex1, pEdges->at(e)->nIndex2);
+ }
+
+ delete pCopy;
+}
+
+
+void GraphTriangulation::triangulationQuadraticSpherical(CSphericalGraph* pGraph)
+{
+ int nNumberViews = (int) pGraph->getNodes()->size();
+
+ // create graph with nodes and nodes + 360
+ CSphericalGraph* pDoubleGraph = doubleNodes(pGraph);
+
+ // do triangulation on the double size graph
+ //triangulationQuadratic(pDoubleGraph);
+ triangulationQuartic(pDoubleGraph);
+
+ // insert detrmined edges in original graph
+ int nIndex1, nIndex2;
+ CSGEdge* pCurrentEdge;
+
+ for (int e = 0 ; e < int(pDoubleGraph->getEdges()->size()) ; e++)
+ {
+ pCurrentEdge = pDoubleGraph->getEdges()->at(e);
+ nIndex1 = pCurrentEdge->nIndex1;
+ nIndex2 = pCurrentEdge->nIndex2;
+
+ // HACK: delete outer edges
+ if ((pDoubleGraph->getNode(nIndex1)->getPosition().fTheta < 30.0) || (pDoubleGraph->getNode(nIndex1)->getPosition().fTheta > 690.0))
+ {
+ continue;
+ }
+
+ if ((pDoubleGraph->getNode(nIndex2)->getPosition().fTheta < 30.0) || (pDoubleGraph->getNode(nIndex2)->getPosition().fTheta > 690.0))
+ {
+ continue;
+ }
+
+ /* if( (pDoubleGraph->getNode(nIndex1)->getPosition().fPhi < 30.0) || (pDoubleGraph->getNode(nIndex1)->getPosition().fPhi > 330.0))
+ continue;
+ if( (pDoubleGraph->getNode(nIndex2)->getPosition().fPhi < 30.0) || (pDoubleGraph->getNode(nIndex2)->getPosition().fPhi > 330.0))
+ continue;
+ if( (pDoubleGraph->getNode(nIndex1)->getPosition().fPhi < 180.0) && (pDoubleGraph->getNode(nIndex2)->getPosition().fPhi > 180.0))
+ continue;
+ if( (pDoubleGraph->getNode(nIndex2)->getPosition().fPhi < 180.0) && (pDoubleGraph->getNode(nIndex1)->getPosition().fPhi > 180.0))
+ continue;
+ */
+ // eliminate virtual nodes
+ if (nIndex1 >= nNumberViews)
+ {
+ nIndex1 %= nNumberViews;
+ }
+
+ if (nIndex2 >= nNumberViews)
+ {
+ nIndex2 %= nNumberViews;
+ }
+
+ // update graph
+ if (nIndex1 != nIndex2)
+ if (!GraphProcessor::isEdgePresent(pGraph, nIndex1, nIndex2))
+ {
+ pGraph->addEdge(nIndex1, nIndex2);
+ }
+ }
+
+ delete pDoubleGraph;
+}
+
+void GraphTriangulation::triangulationQuartic(CSphericalGraph* pGraph, float fThreshold)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+
+ int nNumberPoints = int(pNodes->size());
+ bool bPointFree;
+ int i, j, k, l;
+
+ for (i = 0 ; i < nNumberPoints - 2 ; i++)
+ {
+ printf("Round %d / %d\n", i, nNumberPoints - 2);
+
+ for (j = i + 1 ; j < nNumberPoints - 1 ; j++)
+ {
+ if (j != i)
+ {
+ for (k = j + 1; k < nNumberPoints; k++)
+ {
+ if (k != i && k != j)
+ {
+
+ // check if this rectangle is point free
+ bPointFree = true;
+
+ for (l = 0; l < nNumberPoints ; l++)
+ {
+ // only if not one of the triangle points
+ if (l != i && l != j && l != k)
+ {
+ bool bInside;
+
+ if (!GraphProcessor::insideCircumcircle(pGraph, i, j, k, l, bInside))
+ {
+ bPointFree = false;
+ break;
+ }
+
+ if (bInside)
+ {
+ bPointFree = false;
+ break;
+ }
+ }
+ }
+
+ if (bPointFree)
+ {
+ TSphereCoord p_i, p_j, p_k;
+ p_i = pNodes->at(i)->getPosition();
+ p_j = pNodes->at(j)->getPosition();
+ p_k = pNodes->at(k)->getPosition();
+
+ // add triangle
+ if (MathTools::getDistanceOnArc(p_i, p_j) < fThreshold)
+ {
+ pGraph->addEdge(i, j);
+ }
+
+ if (MathTools::getDistanceOnArc(p_i, p_k) < fThreshold)
+ {
+ pGraph->addEdge(i, k);
+ }
+
+ if (MathTools::getDistanceOnArc(p_k, p_j) < fThreshold)
+ {
+ pGraph->addEdge(k, j);
+ }
+ }
+
+ }
+ }
+ }
+ }
+ }
+}
+
+
+void GraphTriangulation::triangulationQuadratic(CSphericalGraph* pGraph)
+{
+ TEdgeList* pEdges = pGraph->getEdges();
+
+ int nCurrentEdge;
+ int nFaces = 0;
+ int s = 0, t = 0;
+
+ // find two closest neighbours and add edge to triangulation.
+ GraphProcessor::findClosestNeighbours(pGraph, s, t);
+
+ // Create seed edge
+ int nEdgeIndex = pGraph->addEdge(s , t , -1, -1);
+
+ nCurrentEdge = 0;
+
+ while (nCurrentEdge < int(pEdges->size()))
+ {
+ if (pEdges->at(nCurrentEdge)->nLeftFace == -1)
+ {
+ completeFacet(pGraph, nCurrentEdge, nFaces);
+ }
+
+ if (pEdges->at(nCurrentEdge)->nRightFace == -1)
+ {
+ completeFacet(pGraph, nCurrentEdge, nFaces);
+ }
+
+ nCurrentEdge++;
+ }
+}
+
+void GraphTriangulation::completeFacet(CSphericalGraph* pGraph, int nEdgeIndex, int nFaces)
+{
+ TNodeList* pNodes = pGraph->getNodes();
+ TEdgeList* pEdges = pGraph->getEdges();
+ int s, t, u;
+
+ // Cache s and t.
+ if (pEdges->at(nEdgeIndex)->nLeftFace == -1)
+ {
+ s = pEdges->at(nEdgeIndex)->nIndex1;
+ t = pEdges->at(nEdgeIndex)->nIndex2;
+ }
+ else if (pEdges->at(nEdgeIndex)->nRightFace == -1)
+ {
+ s = pEdges->at(nEdgeIndex)->nIndex2;
+ t = pEdges->at(nEdgeIndex)->nIndex1;
+ }
+ else
+ {
+ // Edge already completed.
+ return;
+ }
+
+
+ // Find a point on left of edge.
+ for (u = 0; u < int(pNodes->size()); u++)
+ {
+ if (u == s || u == t)
+ {
+ continue;
+ }
+
+ // check side
+ float fAngle = MathTools::CalculateCrossProductFromDifference(pNodes->at(s)->getPosition(), pNodes->at(t)->getPosition(), pNodes->at(u)->getPosition());
+
+ if (fAngle > 0.0)
+ {
+ break;
+ }
+ }
+
+ // Find best point on left of edge.
+ int nBestPoint = u;
+ float fCp;
+
+ if (nBestPoint < int(pNodes->size()))
+ {
+ TSphereCoord center;
+ float fRadius;
+ GraphProcessor::getCircumcircle(pGraph, s, t, nBestPoint, center, fRadius);
+
+ for (u = nBestPoint + 1; u < int(pNodes->size()); u++)
+ {
+ if (u == s || u == t)
+ {
+ continue;
+ }
+
+ fCp = MathTools::CalculateCrossProductFromDifference(pNodes->at(s)->getPosition(), pNodes->at(t)->getPosition(), pNodes->at(u)->getPosition());
+
+ if (fCp > 0.0)
+ {
+ bool bInside;
+
+ if (GraphProcessor::insideCircumcircle(pGraph, s, t, nBestPoint, u, bInside))
+ {
+ if (bInside)
+ {
+ nBestPoint = u;
+ }
+ }
+ }
+ }
+ }
+
+ // Add new triangle or update edge info if s-t is on hull.
+ if (nBestPoint < int(pNodes->size()))
+ {
+ // Update face information of edge being completed.
+ updateLeftFace(pGraph, nEdgeIndex, s, t, nFaces);
+ nFaces++;
+
+ // Add new edge or update face info of old edge.
+ nEdgeIndex = GraphProcessor::findEdge(pGraph, nBestPoint, s);
+
+ if (nEdgeIndex == -1)
+ {
+ // New edge.
+ nEdgeIndex = pGraph->addEdge(nBestPoint, s, nFaces, -1);
+ }
+ else
+ {
+ // Old edge.
+ updateLeftFace(pGraph, nEdgeIndex, nBestPoint, s, nFaces);
+ }
+
+ // Add new edge or update face info of old edge.
+ nEdgeIndex = GraphProcessor::findEdge(pGraph, t, nBestPoint);
+
+ if (nEdgeIndex == -1)
+ {
+ // New edge.
+ nEdgeIndex = pGraph->addEdge(t, nBestPoint, nFaces, -1);
+ }
+ else
+ {
+ // Old edge.
+ updateLeftFace(pGraph, nEdgeIndex, t, nBestPoint, nFaces);
+ }
+ }
+ else
+ {
+ updateLeftFace(pGraph, nEdgeIndex, s, t, -2);
+ }
+}
+
+void GraphTriangulation::updateLeftFace(CSphericalGraph* pGraph, int nEdgeIndex, int nIndex1, int nIndex2, int nFace)
+{
+ // vector<TView*>* pViews = &pGraph->getAspectPool()->m_Views;
+ vector<CSGEdge*>* pEdges = pGraph->getEdges();
+
+ bool bValid = false;
+
+ if ((pEdges->at(nEdgeIndex)->nIndex1 == nIndex1) && (pEdges->at(nEdgeIndex)->nIndex2 == nIndex2))
+ {
+ bValid = true;
+ }
+
+ if ((pEdges->at(nEdgeIndex)->nIndex1 == nIndex2) && (pEdges->at(nEdgeIndex)->nIndex2 == nIndex1))
+ {
+ bValid = true;
+ }
+
+ if (!bValid)
+ {
+ printf("updateLeftFace: adj. matrix and edge table mismatch\n");
+ }
+
+ if ((pEdges->at(nEdgeIndex)->nIndex1 == nIndex1) && (pEdges->at(nEdgeIndex)->nLeftFace == -1))
+ {
+ pEdges->at(nEdgeIndex)->nLeftFace = nFace;
+ }
+ else if ((pEdges->at(nEdgeIndex)->nIndex2 == nIndex1) && (pEdges->at(nEdgeIndex)->nRightFace == -1))
+ {
+ pEdges->at(nEdgeIndex)->nRightFace = nFace;
+ }
+
+}
+
+CSphericalGraph* GraphTriangulation::doubleNodes(CSphericalGraph* pSourceGraph)
+{
+ // list to source views
+ TNodeList* pNodes = pSourceGraph->getNodes();
+ int nNumberSourceNodes = (int) pNodes->size();
+
+ // add views to aspectpool
+ vector<TSphereCoord> coordinates;
+
+
+ for (int i = 0 ; i < nNumberSourceNodes ; i++)
+ {
+ coordinates.push_back(pNodes->at(i)->getPosition());
+ }
+
+ TSphereCoord shiftedCoord;
+
+ for (int i = 0 ; i < nNumberSourceNodes ; i++)
+ {
+ shiftedCoord = pNodes->at(i)->getPosition();
+ shiftedCoord.fTheta += 360.0f;
+ coordinates.push_back(shiftedCoord);
+ }
+
+ /* TSphereCoord shifted2Coord, shifted3Coord;
+ for(int i = 0 ; i < nNumberSourceNodes ; i++)
+ {
+ shifted2Coord = pNodes->at(i)->getPosition();
+ shifted2Coord.fPhi += 180.0f;
+ coordinates.push_back(shifted2Coord);
+ }
+
+ for(int i = 0 ; i < nNumberSourceNodes ; i++)
+ {
+ shifted3Coord = pNodes->at(i)->getPosition();
+ shifted3Coord.fPhi += 180.0f;
+ shifted3Coord.fTheta += 360.0f;
+ coordinates.push_back(shifted3Coord);
+ }
+ */
+ CSphericalGraph* pTmpGraph = new CSphericalGraph();
+
+ for (int i = 0 ; i < 2 * nNumberSourceNodes ; i++)
+ {
+ CSGNode* pNode = new CSGNode(coordinates.at(i));
+ pTmpGraph->addNode(pNode);
+ }
+
+ return pTmpGraph;
+}
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.h b/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.h
new file mode 100644
index 000000000..62e3dadc5
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.h
@@ -0,0 +1,52 @@
+// *****************************************************************
+// Filename: AspectGraphProcessor.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+#ifndef _GRAPH_TRIANGULATION_H_
+#define _GRAPH_TRIANGULATION_H_
+
+// *****************************************************************
+// forward declarations
+// *****************************************************************
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "SphericalGraph.h"
+#include "Base/DataStructures/Graph/Convexhull/C3DNode.h"
+#include "Base/DataStructures/Graph/Convexhull/HalfSpace.h"
+#include "Base/DataStructures/Graph/Convexhull/CHGiftWrap.h"
+#include <float.h>
+
+// *****************************************************************
+// namespaces
+// *****************************************************************
+
+// *****************************************************************
+// namespace GraphTriangulation
+// *****************************************************************
+class GraphTriangulation
+{
+public:
+ static void delaunayTriangulationQuadratic(CSphericalGraph* pGraph);
+ // triangulate with O(n^2) spherical
+ static void triangulationQuadraticSpherical(CSphericalGraph* pGraph);
+ // triangulate with O(n^4)
+ static void triangulationQuartic(CSphericalGraph* pGraph, float fThreshold = FLT_MAX);
+ // triangulate with O(n^2)
+ static void triangulationQuadratic(CSphericalGraph* pGraph);
+
+private:
+ static void updateLeftFace(CSphericalGraph* pGraph, int nEdgeIndex, int nIndex1, int nIndex2, int nFace);
+ static void completeFacet(CSphericalGraph* pGraph, int nEdgeIndex, int nFaces);
+
+ static CSphericalGraph* doubleNodes(CSphericalGraph* pSourceGraph);
+
+};
+
+#endif /* _GRAPH_TRIANGULATION_H_ */
diff --git a/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.cpp b/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.cpp
new file mode 100644
index 000000000..6d48757d1
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.cpp
@@ -0,0 +1,276 @@
+// *****************************************************************
+// Filename: SensoryEgoSphere.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 21.07.2008
+// *****************************************************************
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "IntensityGraph.h"
+//#include "Base/Math/DistributePoints.h"
+//#include "MathTools.h"
+//#include "Base/DataStructures/Graph/GraphProcessor.h"
+
+#include <float.h>
+#include <fstream>
+
+// *****************************************************************
+// implementation of CIntensityNode
+// *****************************************************************
+CIntensityNode::CIntensityNode()
+{
+ setIntensity(0.0f);
+}
+
+CIntensityGraph::CIntensityGraph(const CIntensityGraph& prototype)
+ : CSphericalGraph(prototype)
+{
+}
+
+CIntensityNode::CIntensityNode(TSphereCoord coord)
+ : CSGNode(coord)
+{
+ setIntensity(0.0f);
+}
+
+CIntensityNode::~CIntensityNode()
+{
+}
+
+void CIntensityNode::setIntensity(float fIntensity)
+{
+ m_fIntensity = fIntensity;
+}
+
+float CIntensityNode::getIntensity()
+{
+ return m_fIntensity;
+}
+
+// *****************************************************************
+// implementation of CIntensityGraph
+// *****************************************************************
+// *****************************************************************
+// construction / destruction
+// *****************************************************************
+CIntensityGraph::CIntensityGraph()
+{
+ reset();
+}
+
+//CIntensityGraph::CIntensityGraph(int nNumberNodes)
+//{
+// // generate points on sphere
+// points P = generatePoints(nNumberNodes,1000);
+
+// for(int i = 0 ; i < int(P.V.size()) ; i++)
+// {
+// // project point to polar coordinates
+// Vec3d point;
+// Math3d::SetVec(point,P.V.at(i).array()[0],P.V.at(i).array()[1],P.V.at(i).array()[2]);
+// TSphereCoord point_on_sphere;
+// MathTools::convert(point, point_on_sphere);
+
+// CIntensityNode* pNode = new CIntensityNode(point_on_sphere);
+
+// addNode(pNode);
+// }
+
+// // initialize
+// reset();
+//}
+
+CIntensityGraph::CIntensityGraph(string sFilename)
+{
+ printf("Reading intensity graph from: %s\n", sFilename.c_str());
+
+ ifstream infile;
+ infile.open(sFilename.c_str());
+
+ read(infile);
+
+ //printf("Read number nodes: %d\n", getNodes()->size());
+ //printf("Read number edges: %d\n", getEdges()->size());
+}
+
+CIntensityGraph::~CIntensityGraph()
+{
+}
+
+CIntensityGraph& CIntensityGraph::operator= (CIntensityGraph const& rhs)
+{
+ *((CSphericalGraph*) this) = (const CSphericalGraph) rhs;
+ return *this;
+}
+
+// *****************************************************************
+// setting
+// *****************************************************************
+void CIntensityGraph::reset()
+{
+ set(0.0f);
+}
+
+void CIntensityGraph::set(float fIntensity)
+{
+ TNodeList* pNodes = getNodes();
+
+ for (size_t i = 0 ; i < pNodes->size() ; i++)
+ {
+ ((CIntensityNode*) pNodes->at(i))->setIntensity(fIntensity);
+ }
+}
+
+
+// *****************************************************************
+// file io
+// *****************************************************************
+bool CIntensityGraph::read(istream& infile)
+{
+ try
+ {
+ int nNumberNodes;
+
+ // read number of nodes
+ infile >> nNumberNodes;
+
+ // read all nodes
+ for (int n = 0 ; n < nNumberNodes ; n++)
+ {
+ readNode(infile);
+ }
+
+ // read number of edges
+ int nNumberEdges;
+ infile >> nNumberEdges;
+
+ for (int e = 0 ; e < nNumberEdges ; e++)
+ {
+ readEdge(infile);
+ }
+
+ }
+ catch (istream::failure e)
+ {
+ printf("ERROR: failed to write FeatureGraph to file\n");
+ return false;
+ }
+
+ return true;
+}
+
+bool CIntensityGraph::readNode(istream& infile)
+{
+ CIntensityNode* pNewNode = getNewNode();
+
+ int nIndex;
+ TSphereCoord pos;
+ float fIntensity;
+
+ infile >> nIndex;
+ infile >> pos.fPhi;
+ infile >> pos.fTheta;
+ infile >> fIntensity;
+
+ pNewNode->setPosition(pos);
+ pNewNode->setIntensity(fIntensity);
+
+ int nTest = addNode(pNewNode);
+
+ if (nTest != nIndex)
+ {
+ printf("Error input file inconsistent: %d %d\n", nTest, nIndex);
+ return false;
+ }
+
+ return true;
+}
+
+bool CIntensityGraph::readEdge(istream& infile)
+{
+ int nIndex1, nIndex2, nLeftFace, nRightFace;
+
+ infile >> nIndex1;
+ infile >> nIndex2;
+
+ infile >> nLeftFace;
+ infile >> nRightFace;
+
+ addEdge(nIndex1, nIndex2, nLeftFace, nRightFace);
+ return true;
+}
+
+
+bool CIntensityGraph::write(ostream& outfile)
+{
+ try
+ {
+ int nNumberNodes = int(m_Nodes.size());
+ // write number of nodes
+ outfile << nNumberNodes << endl;
+
+ // write all nodes
+ for (int n = 0 ; n < nNumberNodes ; n++)
+ {
+ writeNode(outfile, n);
+ }
+
+ // write number of edges
+ int nNumberEdges = int(m_Edges.size());
+ outfile << nNumberEdges << endl;
+
+ for (int e = 0 ; e < nNumberEdges ; e++)
+ {
+ writeEdge(outfile, e);
+ }
+
+ }
+ catch (ostream::failure e)
+ {
+ printf("ERROR: failed to write FeatureGraph to file\n");
+ return false;
+ }
+
+ return true;
+}
+
+bool CIntensityGraph::writeNode(ostream& outfile, int n)
+{
+ CIntensityNode* pCurrentNode = (CIntensityNode*) m_Nodes.at(n);
+
+ outfile << pCurrentNode->getIndex() << endl;
+ outfile << pCurrentNode->getPosition().fPhi << endl;
+ outfile << pCurrentNode->getPosition().fTheta << endl;
+ outfile << pCurrentNode->getIntensity() << endl;
+
+ outfile << endl;
+ return true;
+}
+
+bool CIntensityGraph::writeEdge(ostream& outfile, int e)
+{
+ CSGEdge* pCurrentEdge = m_Edges.at(e);
+ outfile << pCurrentEdge->nIndex1 << " ";
+ outfile << pCurrentEdge->nIndex2 << " ";
+
+ outfile << pCurrentEdge->nLeftFace << " ";
+ outfile << pCurrentEdge->nRightFace << " ";
+
+ outfile << endl;
+ return true;
+}
+
+void CIntensityGraph::graphToVec(std::vector<float>& vec)
+{
+ TNodeList* nodes = this->getNodes();
+
+ for (size_t i = 0; i < nodes->size(); i++)
+ {
+ CIntensityNode* node = (CIntensityNode*) nodes->at(i);
+ vec.push_back(node->getIntensity());
+ }
+}
diff --git a/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.h b/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.h
new file mode 100644
index 000000000..4436704a4
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.h
@@ -0,0 +1,97 @@
+// *****************************************************************
+// Filename: IntensityGraph.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2008
+// *****************************************************************
+
+#ifndef _INTENSITY_GRAPH_H_
+#define _INTENSITY_GRAPH_H_
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "SphericalGraph.h"
+#include <string>
+
+using namespace std;
+
+// *****************************************************************
+// namespaces
+// *****************************************************************
+
+// *****************************************************************
+// structures
+// *****************************************************************
+// *****************************************************************
+// definition of nodes
+// *****************************************************************
+class CIntensityNode : public CSGNode
+{
+public:
+ CIntensityNode();
+ CIntensityNode(TSphereCoord coord);
+ ~CIntensityNode();
+
+ void setIntensity(float fIntensity);
+ float getIntensity();
+
+ virtual CSGNode* clone()
+ {
+ CIntensityNode* copy = new CIntensityNode(m_Position);
+ copy->setIndex(m_nIndex);
+ copy->setIntensity(m_fIntensity);
+ return copy;
+ }
+
+private:
+ float m_fIntensity;
+};
+
+
+// *****************************************************************
+// definition of sensory egosphere
+// *****************************************************************
+class CIntensityGraph : public CSphericalGraph
+{
+public:
+ CIntensityGraph();
+ CIntensityGraph(const CIntensityGraph& prototype);
+ //CIntensityGraph(int nNumberNodes);
+ CIntensityGraph(string sFileName);
+ ~CIntensityGraph();
+
+ void reset();
+ void set(float fIntensity);
+
+
+ void graphToVec(std::vector<float>& vec);
+
+ ///////////////////////////////
+ // file io
+ ///////////////////////////////
+ virtual bool read(istream& infile);
+ virtual bool readNode(istream& infile);
+ virtual bool readEdge(istream& infile);
+
+ virtual bool write(ostream& outfile);
+ virtual bool writeNode(ostream& outfile, int n);
+ virtual bool writeEdge(ostream& outfile, int e);
+
+
+ // inherited
+ virtual CIntensityNode* getNewNode()
+ {
+ return new CIntensityNode();
+ }
+
+ ///////////////////////////////
+ // operators
+ ///////////////////////////////
+ CIntensityGraph& operator= (CIntensityGraph const& rhs);
+
+};
+
+#endif // _INTENSITY_GRAPH_H_
diff --git a/source/RobotAPI/components/EarlyVisionGraph/MathTools.cpp b/source/RobotAPI/components/EarlyVisionGraph/MathTools.cpp
new file mode 100644
index 000000000..f43021463
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/MathTools.cpp
@@ -0,0 +1,488 @@
+// *****************************************************************
+// Filename: AspectGraphProcessor.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "MathTools.h"
+#include <math.h>
+//#include "Base/Tools/DebugMemory.h"
+
+
+// **********************************************************
+// transformation on sphere coordinates and paths
+// **********************************************************
+// returns transformation from p1 -> p2
+TTransform MathTools::determinePathTransformation(TPath* p1, TPath* p2)
+{
+ TSphereCoord start1, start2, end1, end2;
+ start1 = p1->at(0).Position;
+ end1 = p1->at(p1->size() - 1).Position;
+ start2 = p2->at(0).Position;
+ end2 = p2->at(p2->size() - 1).Position;
+
+ return determineTransformation(start1, start2, end1, end2);
+}
+
+TPath* MathTools::transformPath(TTransform transform, TPath* pPath)
+{
+ TPath* pResulting = new TPath();
+
+ if (pPath->size() < 2)
+ {
+ printf("ERROR: path is too short\n");
+ delete pResulting;
+ return NULL;
+ }
+
+ // copy path and transform
+ for (int e = 0 ; e < int(pPath->size()) ; e++)
+ {
+ TPathElement element = pPath->at(e);
+ TSphereCoord res = MathTools::transform(element.Position, transform);
+ element.Position = res;
+
+ pResulting->push_back(element);
+ }
+
+ return pResulting;
+
+}
+
+TPath* MathTools::inverseTransformPath(TTransform transform, TPath* pPath)
+{
+ TPath* pResulting = new TPath();
+
+ if (pPath->size() < 2)
+ {
+ printf("ERROR: path is too short\n");
+ delete pResulting;
+ return NULL;
+ }
+
+ // copy path and transform
+ for (int e = 0 ; e < int(pPath->size()) ; e++)
+ {
+ TPathElement element = pPath->at(e);
+ TSphereCoord res = MathTools::inverseTransform(element.Position, transform);
+ element.Position = res;
+
+ pResulting->push_back(element);
+ }
+
+ return pResulting;
+
+}
+
+TTransform MathTools::determineTransformation(TSphereCoord start1, TSphereCoord start2, TSphereCoord end1, TSphereCoord end2)
+{
+ TTransform result;
+
+ // first determine alpha and beta and beta axis
+ float fAlpha, fBeta;
+ Eigen::Vector3d betaAxis;
+ determineTransformationAlphaBeta(start1, start2, fAlpha, fBeta, betaAxis);
+ result.fAlpha = fAlpha;
+ result.fBeta = fBeta;
+ result.betaAxis = betaAxis;
+
+ // calculate angular for psi rotation
+
+ // apply transform to first path end
+ TSphereCoord end1_transformed = transformAlphaBeta(end1, fAlpha, fBeta, betaAxis);
+
+ Eigen::Vector3d end1_transformed_v, start2_v, end2_v;
+ convert(end1_transformed, end1_transformed_v);
+ convert(start2, start2_v);
+ convert(end2, end2_v);
+
+ // calculate angle between start2 <-> end1_tranformed ; start2 <-> end2
+ Eigen::Vector3d zero = Eigen::Vector3d::Zero();
+
+ Eigen::Vector3d line1 = MathTools::dropAPerpendicular(end1_transformed_v, zero, start2_v);
+ Eigen::Vector3d line2 = MathTools::dropAPerpendicular(end2_v, zero, start2_v);
+ line1 -= end1_transformed_v;
+ line2 -= end2_v;
+
+ float fPsi = MathTools::AngleAndDirection(line1, line2, start2_v) * 180.0 / M_PI;
+
+ result.fPsi = fPsi;
+ result.psiAxis = start2_v;
+
+ return result;
+}
+
+TSphereCoord MathTools::transform(TSphereCoord sc, TTransform transform)
+{
+ // first apply alpha , beta
+ TSphereCoord rotated_ab = transformAlphaBeta(sc, transform.fAlpha, transform.fBeta, transform.betaAxis);
+
+ Eigen::Vector3d rotated_ab_v;
+
+ convert(rotated_ab, rotated_ab_v);
+
+ // now apply final rotation
+ Eigen::AngleAxisd rot(-transform.fPsi * M_PI / 180.0, transform.psiAxis);
+ rotated_ab_v = rot.matrix() * rotated_ab_v;
+
+ TSphereCoord rotated;
+ convert(rotated_ab_v, rotated);
+
+ return rotated;
+}
+
+TSphereCoord MathTools::inverseTransform(TSphereCoord sc, TTransform transform)
+{
+ Eigen::Vector3d sc_v;
+ convert(sc, sc_v);
+
+ // first rotate
+ Eigen::AngleAxisd rot(transform.fPsi * M_PI / 180.0, transform.psiAxis);
+ sc_v = rot.matrix() * sc_v;
+
+ TSphereCoord rotated;
+ convert(sc_v, rotated);
+
+ return inverseTransformAlphaBeta(rotated, transform.fAlpha, transform.fBeta, transform.betaAxis);
+}
+
+// determine alpha and beta of a transform
+// fAlpha: rotation around the z axis in counterclockwise direction considering the positive z-axis
+// fBeta: rotation around the rotated y axis in counterclockwise direction considering the positive y-axis
+void MathTools::determineTransformationAlphaBeta(TSphereCoord sc1, TSphereCoord sc2, float& fAlpha, float& fBeta, Eigen::Vector3d& betaAxis)
+{
+ Eigen::Vector3d vector1, vector2;
+ convert(sc1, vector1);
+ convert(sc2, vector2);
+
+ // project both vectors to xy plane
+ Eigen::Vector3d vector1_xy, vector2_xy;
+
+ vector1_xy = vector1;
+ vector2_xy = vector2;
+
+ vector1_xy(2) = 0.0;
+ vector2_xy(2) = 0.0;
+
+ // calculate first angle from vec1_projected to vec2_projected with normale z
+ Eigen::Vector3d normal = Eigen::Vector3d::UnitZ();
+
+ if (vector1_xy.norm() == 0.0 || vector2_xy.norm() == 0)
+ {
+ fAlpha = ((sc2.fTheta - sc1.fTheta) / 180.0 * M_PI);
+ }
+ else
+ {
+ fAlpha = MathTools::AngleAndDirection(vector1_xy, vector2_xy, normal);
+ }
+
+ // now rotate first vector with alpha
+ Eigen::Vector3d rotated1, rotated_y;
+
+ // the vector to rotate
+
+ rotated1 = vector1;
+ // set rotation: alpha around z-axis to transform v1 in same plane as v2
+
+ Eigen::AngleAxisd rotation(fAlpha, Eigen::Vector3d::UnitZ());;
+ Eigen::AngleAxisd rotation_yaxis(sc2.fTheta, Eigen::Vector3d::UnitZ());
+
+ // we need normal for angle calculateion, so also rotate x-axis
+ rotated_y << 0.0, 1.0, 0.0;
+
+ // rotate vector and x-axis
+
+
+ rotated1 = rotation.matrix() * rotated1;
+
+ rotated_y = rotation_yaxis.matrix() * rotated_y;
+
+ betaAxis = rotated_y;
+
+ // calculate angle between both vectors
+ fBeta = MathTools::AngleAndDirection(rotated1, vector2, rotated_y);
+
+ fAlpha *= 180.0 / M_PI;
+ fBeta *= 180.0 / M_PI;
+}
+
+TSphereCoord MathTools::transformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis)
+{
+ TSphereCoord result;
+ Eigen::Vector3d vector1, vector1_rotated;
+ convert(sc, vector1);
+
+ // rotate around positive z-axis
+ Eigen::AngleAxisd rotation_alpha(fAlpha / 180.0 * M_PI, Eigen::Vector3d::UnitZ());
+
+ // rotate vector aroun z axis
+ vector1_rotated = rotation_alpha.matrix() * vector1;
+ // rotate vector with beta around betaAxis
+ Eigen::AngleAxisd rot(-fBeta / 180.0 * M_PI, betaAxis);
+
+ vector1_rotated = rot.matrix() * vector1_rotated;
+
+ convert(vector1_rotated, result);
+
+ return result;
+}
+
+TSphereCoord MathTools::inverseTransformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis)
+{
+ Eigen::Vector3d vector1;
+ convert(sc, vector1);
+
+ // do inverse of beta axis rotation
+ Eigen::AngleAxisd rot(fBeta / 180.0 * M_PI, betaAxis);
+ vector1 = rot.matrix() * vector1;
+
+ // do inverse of zaxis rotation
+ Eigen::AngleAxisd rotation_alpha(-fAlpha / 180.0 * M_PI, Eigen::Vector3d::UnitZ());
+
+ // rotate vector aroun z axis
+ vector1 = rotation_alpha * vector1;
+
+ TSphereCoord result;
+ convert(vector1, result);
+
+ return result;
+}
+
+// **********************************************************
+// calculations on sphere coordinates
+// **********************************************************
+float MathTools::CalculateCrossProductFromDifference(TSphereCoord p1, TSphereCoord p2, TSphereCoord p3)
+{
+ float u1, v1, u2, v2;
+
+ u1 = p2.fPhi - p1.fPhi;
+ v1 = p2.fTheta - p1.fTheta;
+ u2 = p3.fPhi - p1.fPhi;
+ v2 = p3.fTheta - p1.fTheta;
+
+ return u1 * v2 - v1 * u2;
+}
+
+double MathTools::Angle(Eigen::Vector3d vec1, Eigen::Vector3d vec2)
+{
+ double r = vec1.dot(vec2) / (vec1.norm() * vec2.norm());
+ r = std::min(1.0, std::max(-1.0, r));
+ return std::acos(r);
+}
+
+float MathTools::getDistanceOnArc(TSphereCoord p1, TSphereCoord p2)
+{
+ Eigen::Vector3d vec1, vec2;
+ p1.fDistance = 1.0f;
+ p2.fDistance = 1.0f;
+
+ convert(p1, vec1);
+ convert(p2, vec2);
+
+ float fAngle = (float) MathTools::Angle(vec1, vec2);
+
+ // length of circular arc
+ // not necessary because Math3d::Angle returns a value in radians
+ //float fDist = float( (2 * M_PI * fAngle ) / 360.0f);
+
+ return fAngle;
+}
+
+float MathTools::getDistance(TSphereCoord p1, TSphereCoord p2, float fRadius)
+{
+ // never use this!!!!
+ TSphereCoord diff;
+ diff.fPhi = p1.fPhi - p2.fPhi;
+ diff.fTheta = p1.fTheta - p2.fTheta;
+
+ float fAngle = sqrt(diff.fPhi * diff.fPhi + diff.fTheta * diff.fTheta);
+
+ return fAngle;
+}
+
+// **********************************************************
+// extended 2d Math
+// **********************************************************
+bool MathTools::IntersectLines(Eigen::Vector2d p1, Eigen::Vector2d m1, Eigen::Vector2d p2, Eigen::Vector2d m2, Eigen::Vector2d& res)
+{
+ // calculate perpendicular bisector of sides
+ float dx = float(p1(0) - p2(0));
+ float dy = float(p1(1) - p2(1));
+
+ // now from
+ // g1: g1(x,y) = (p1x,p1y) + (m1x,m1y) * c1
+ // g2: g2(x,y) = (p2x,p2y) + (m2x,m2y) * c2
+ // -->
+ // g1(x,y) = g2(x,y)
+ // -->
+ // (p1x,p1y) + (m1x,m1y) * c1 = (p2x,p2y) + (m2x,m2y) * c2
+ // (p1x,p1y) - (p2x,p2y) = (dx,dy) = (m2x,m2y) * c2 - (m1x,m1y) * c1
+ // -->
+ // dx = m2x * c2 - m1x * c1, dy = m2y * c2 - m1y * c1
+ // c1 = (m2y * c2 - dy) / m1y
+ // dx = m2x * c2 - m1x * (m2y * c2 - dy) / m1y
+
+ float z = float(dx - (m1(0) * dy) / m1(1));
+ float n = float(m2(0) - (m1(0) * m2(1)) / m1(1));
+
+ float c2;
+
+ if (n == 0)
+ {
+ return false;
+ }
+ else
+ {
+ c2 = z / n;
+ }
+
+ res(0) = p2(0) + c2 * m2(0);
+ res(1) = p2(1) + c2 * m2(1);
+
+ return true;
+}
+
+// **********************************************************
+// extended 3d Math
+// **********************************************************
+// checked and working
+float MathTools::AngleAndDirection(Eigen::Vector3d vector1, Eigen::Vector3d vector2, Eigen::Vector3d normal)
+{
+ double fAngle = MathTools::Angle(vector1, vector2);
+
+ Eigen::Vector3d axis = vector1.cross(vector2);
+
+ // compare axis and normal
+ normal.normalize();
+
+ double c = normal.dot(vector1);
+
+ axis += vector1;
+ double d = normal.dot(axis) - c;
+
+
+ if (d < 0)
+ {
+ return 2 * M_PI - fAngle;
+ }
+
+ return (float) fAngle;
+}
+
+// checked and working
+Eigen::Vector3d MathTools::dropAPerpendicular(Eigen::Vector3d point, Eigen::Vector3d linepoint, Eigen::Vector3d linevector)
+{
+ // helping plane normal (ax + by + cz + d = 0)
+ Eigen::Vector3d plane_normal = linevector; // (a,b,c)
+ float d = (float) - plane_normal.dot(point);
+
+ // calculate k from plane and line intersection
+ float z = float(- plane_normal(0) * linepoint(0) - plane_normal(1) * linepoint(1) - plane_normal(2) * linepoint(2) - d);
+ float n = float(plane_normal(0) * linevector(0) + plane_normal(1) * linevector(1) + plane_normal(2) * linevector(2));
+
+ float k = z / n;
+
+ // calculate line from line equation
+ linevector = k * linevector;
+ linevector += linepoint;
+
+ return linevector;
+
+}
+
+// **********************************************************
+// conversions
+// **********************************************************
+// checked and working
+void MathTools::convert(TSphereCoord in, Eigen::Vector3d& out)
+{
+ // alpha is azimuth, beta is polar angle
+ in.fPhi *= M_PI / 180.0f;
+ in.fTheta *= M_PI / 180.0f;
+
+ // OLD and wrong??
+ // out.x = in.fDistance * sin(in.fPhi) * cos(in.fTheta);
+ // out.y = in.fDistance * sin(in.fPhi) * sin(in.fTheta);
+ // out.z = in.fDistance * cos(in.fPhi);
+
+ Eigen::Vector3d vector = Eigen::Vector3d::UnitZ();
+ Eigen::AngleAxisd rotation_y(in.fPhi, Eigen::Vector3d::UnitY());
+ Eigen::AngleAxisd rotation_z(in.fTheta, Eigen::Vector3d::UnitZ());
+
+
+ // first rotate around y axis with phi
+ vector = rotation_y.matrix() * vector;
+
+ // second rotate around z with theta
+ vector = rotation_z.matrix() * vector;
+
+ out = vector;
+}
+
+
+void MathTools::convertWithDistance(TSphereCoord in, Eigen::Vector3d& out)
+{
+ convert(in, out);
+ out *= in.fDistance;
+}
+
+void MathTools::convert(TSphereCoord in, Eigen::Vector3f& out)
+{
+ Eigen::Vector3d vec;
+ convert(in, vec);
+ out = vec.cast<float>();
+}
+
+
+// checked and working
+void MathTools::convert(Eigen::Vector3d in, TSphereCoord& out)
+{
+ // alpha is azimuth, beta is polar angle
+ out.fDistance = in.norm();
+
+ float fAngle = in(2) / out.fDistance;
+
+ if (fAngle < -1.0f)
+ {
+ fAngle = -1.0f;
+ }
+
+ if (fAngle > 1.0f)
+ {
+ fAngle = 1.0f;
+ }
+
+ out.fPhi = (float) acos(fAngle);
+
+ out.fTheta = (float) atan2(in(1), in(0));
+
+ // convert to angles and correct interval
+ out.fPhi /= M_PI / 180.0f;
+ out.fTheta /= M_PI / 180.0f;
+
+ // only positive rotations
+ if (out.fTheta < 0.0)
+ {
+ out.fTheta = 360.0f + out.fTheta;
+ }
+
+ // be aware of limit
+ if (out.fPhi > 180.0)
+ {
+ out.fPhi = out.fPhi - 180.0f;
+ }
+}
+
+
+void MathTools::convert(Eigen::Vector3f in, TSphereCoord& out)
+{
+ Eigen::Vector3d vec(in(0), in(1), in(2));
+ convert(vec, out);
+}
+
diff --git a/source/RobotAPI/components/EarlyVisionGraph/MathTools.h b/source/RobotAPI/components/EarlyVisionGraph/MathTools.h
new file mode 100644
index 000000000..336629401
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/MathTools.h
@@ -0,0 +1,72 @@
+// *****************************************************************
+// Filename: MathTools.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+#ifndef _MATH_TOOLS_H_
+#define _MATH_TOOLS_H_
+
+// *****************************************************************
+// defines
+// *****************************************************************
+#include <math.h>
+#ifndef M_PI
+#define M_PI 3.141592653589793
+#endif
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "Structs.h"
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+// *****************************************************************
+// class MathTools
+// *****************************************************************
+class MathTools
+{
+public:
+ // transformation of coordinates and paths
+ static TTransform determinePathTransformation(TPath* p1, TPath* p2);
+ static TPath* transformPath(TTransform transform, TPath* pPath);
+ static TPath* inverseTransformPath(TTransform transform, TPath* pPath);
+
+ static TTransform determineTransformation(TSphereCoord start1, TSphereCoord start2, TSphereCoord end1, TSphereCoord end2);
+ static TSphereCoord transform(TSphereCoord sc, TTransform transform);
+ static TSphereCoord inverseTransform(TSphereCoord sc, TTransform transform);
+
+ static void determineTransformationAlphaBeta(TSphereCoord sc1, TSphereCoord sc2, float& fAlpha, float& fBeta, Eigen::Vector3d& betaAxis);
+ static TSphereCoord transformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis);
+ static TSphereCoord inverseTransformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis);
+
+ // calculations on sphere coordimates
+ static float CalculateCrossProductFromDifference(TSphereCoord p1, TSphereCoord p2, TSphereCoord p3);
+ static float getDistanceOnArc(TSphereCoord p1, TSphereCoord p2);
+ static float getDistance(TSphereCoord p1, TSphereCoord p2, float fRadius);
+
+ // extended 2d math
+ static bool IntersectLines(Eigen::Vector2d p1, Eigen::Vector2d m1, Eigen::Vector2d p2, Eigen::Vector2d m2, Eigen::Vector2d& res);
+
+ // extended 3d math
+ static Eigen::Vector3d dropAPerpendicular(Eigen::Vector3d point, Eigen::Vector3d linepoint, Eigen::Vector3d linevector);
+ static float AngleAndDirection(Eigen::Vector3d vector1, Eigen::Vector3d vector2, Eigen::Vector3d normal);
+
+ // coorrdinate conversion
+ static void convert(TSphereCoord in, Eigen::Vector3d& out);
+ static void convert(TSphereCoord in, Eigen::Vector3f& out);
+ static void convert(Eigen::Vector3d in, TSphereCoord& out);
+ static void convert(Eigen::Vector3f in, TSphereCoord& out);
+ static void convertWithDistance(TSphereCoord in, Eigen::Vector3d& out);
+
+ static double Angle(Eigen::Vector3d vec1, Eigen::Vector3d vec2);
+
+
+};
+
+#endif /* _MATH_TOOLS_H_ */
diff --git a/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.cpp b/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.cpp
new file mode 100644
index 000000000..31b5b39a4
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.cpp
@@ -0,0 +1,314 @@
+// *****************************************************************
+// Filename: AspectGraph.cpp
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+
+// *****************************************************************
+// includes
+// *****************************************************************
+#include "SphericalGraph.h"
+//#include "Base/Tools/DebugMemory.h"
+
+// *****************************************************************
+// construction / destruction
+// *****************************************************************
+CSphericalGraph::CSphericalGraph()
+{
+ clear();
+}
+
+CSphericalGraph::CSphericalGraph(const CSphericalGraph& prototype)
+{
+ *this = prototype;
+}
+
+CSphericalGraph::~CSphericalGraph()
+{
+ clear();
+}
+
+CSphericalGraph& CSphericalGraph::operator= (CSphericalGraph const& rhs)
+{
+ clear();
+
+ // copy all nodes
+ const TNodeList* nodes = &rhs.m_Nodes;
+
+ int nNumberNodes = int(nodes->size());
+
+ for (int n = 0 ; n < nNumberNodes ; n++)
+ {
+ addNode(nodes->at(n)->clone());
+ }
+
+ // copy all edges
+ const TEdgeList* edges = &rhs.m_Edges;
+
+ int nNumberEdges = int(edges->size());
+
+ for (int e = 0 ; e < nNumberEdges ; e++)
+ {
+ CSGEdge* edge = edges->at(e);
+ addEdge(edge->nIndex1, edge->nIndex2, edge->nLeftFace, edge->nRightFace);
+ }
+
+ return *this;
+}
+
+
+// *****************************************************************
+// control
+// *****************************************************************
+void CSphericalGraph::clear()
+{
+ // clear edges
+ vector<CSGEdge*>::iterator iter = m_Edges.begin();
+
+ while (iter != m_Edges.end())
+ {
+ delete *iter;
+ iter++;
+ }
+
+ m_Edges.clear();
+
+ // clear nodes
+ vector<CSGNode*>::iterator iter_node = m_Nodes.begin();
+
+ while (iter_node != m_Nodes.end())
+ {
+ delete *iter_node;
+ iter_node++;
+ }
+
+ m_Nodes.clear();
+}
+
+// *****************************************************************
+// graph building
+// *****************************************************************
+// nodes
+int CSphericalGraph::addNode(CSGNode* pNode)
+{
+ m_Nodes.push_back(pNode);
+ pNode->setIndex(int(m_Nodes.size() - 1));
+
+ return pNode->getIndex();
+}
+
+// edges
+int CSphericalGraph::addEdge(int nIndex1, int nIndex2)
+{
+ return addEdge(nIndex1, nIndex2, -1, -1);
+}
+
+int CSphericalGraph::addEdge(int nIndex1, int nIndex2, int nLeftFace, int nRightFace)
+{
+ CSGEdge* pEdge = new CSGEdge;
+ pEdge->nIndex1 = nIndex1;
+ pEdge->nIndex2 = nIndex2;
+ pEdge->nLeftFace = nLeftFace;
+ pEdge->nRightFace = nRightFace;
+
+ // printf("Adding edge %d --> %d\n",nIndex1,nIndex2);
+
+ // add to list of edges
+ m_Edges.push_back(pEdge);
+
+ // update node adjacency list
+ addNodeAdjacency(nIndex1, nIndex2);
+ addNodeAdjacency(nIndex2, nIndex1);
+
+ return int(m_Edges.size()) - 1;
+}
+
+void CSphericalGraph::addNodeAdjacency(int nNode, int nAdjacency)
+{
+ if (nNode == nAdjacency)
+ {
+ printf("Error: adding node as adjacenca\n");
+ }
+
+ // check if present
+ bool bPresent = false;
+
+ for (int n = 0 ; n < int(m_NodeAdjacency[nNode].size()) ; n++)
+ {
+ if (m_NodeAdjacency[nNode].at(n) == nAdjacency)
+ {
+ bPresent = true;
+ }
+ }
+
+ if (!bPresent)
+ {
+ m_NodeAdjacency[nNode].push_back(nAdjacency);
+ }
+}
+
+// *****************************************************************
+// member access
+// *****************************************************************
+TEdgeList* CSphericalGraph::getEdges()
+{
+ return &m_Edges;
+}
+
+TNodeList* CSphericalGraph::getNodes()
+{
+ return &m_Nodes;
+}
+
+CSGEdge* CSphericalGraph::getEdge(int nEdgeIndex)
+{
+ return m_Edges.at(nEdgeIndex);
+}
+
+CSGNode* CSphericalGraph::getNode(int nIndex)
+{
+ return m_Nodes.at(nIndex);
+}
+
+vector<int>* CSphericalGraph::getNodeAdjacency(int nIndex)
+{
+ return &m_NodeAdjacency[nIndex];
+}
+
+// *****************************************************************
+// file io
+// *****************************************************************
+bool CSphericalGraph::read(istream& infile)
+{
+ try
+ {
+ int nNumberNodes;
+
+ // read number of nodes
+ infile >> nNumberNodes;
+
+ // read all nodes
+ for (int n = 0 ; n < nNumberNodes ; n++)
+ {
+ readNode(infile);
+ }
+
+ // read number of edges
+ int nNumberEdges;
+ infile >> nNumberEdges;
+
+ for (int e = 0 ; e < nNumberEdges ; e++)
+ {
+ readEdge(infile);
+ }
+
+ }
+ catch (istream::failure e)
+ {
+ printf("ERROR: failed to write FeatureGraph to file\n");
+ return false;
+ }
+
+ return true;
+}
+
+bool CSphericalGraph::readNode(istream& infile)
+{
+ CSGNode* pNewNode = (CSGNode*) getNewNode();
+
+ int nIndex;
+ TSphereCoord pos;
+
+ infile >> nIndex;
+ infile >> pos.fPhi;
+ infile >> pos.fTheta;
+
+ pNewNode->setPosition(pos);
+
+ if (addNode(pNewNode) != nIndex)
+ {
+ printf("Error input file inconsistent\n");
+ return false;
+ }
+
+ return true;
+}
+
+
+
+bool CSphericalGraph::readEdge(istream& infile)
+{
+ int nIndex1, nIndex2, nLeftFace, nRightFace;
+
+ infile >> nIndex1;
+ infile >> nIndex2;
+
+ infile >> nLeftFace;
+ infile >> nRightFace;
+
+ addEdge(nIndex1, nIndex2, nLeftFace, nRightFace);
+ return true;
+}
+
+
+bool CSphericalGraph::write(ostream& outfile)
+{
+ try
+ {
+ int nNumberNodes = int(m_Nodes.size());
+ // write number of nodes
+ outfile << nNumberNodes << endl;
+
+ // write all nodes
+ for (int n = 0 ; n < nNumberNodes ; n++)
+ {
+ writeNode(outfile, n);
+ }
+
+ // write number of edges
+ int nNumberEdges = int(m_Edges.size());
+ outfile << nNumberEdges << endl;
+
+ for (int e = 0 ; e < nNumberEdges ; e++)
+ {
+ writeEdge(outfile, e);
+ }
+
+ }
+ catch (ostream::failure e)
+ {
+ printf("ERROR: failed to write FeatureGraph to file\n");
+ return false;
+ }
+
+ return true;
+}
+
+bool CSphericalGraph::writeNode(ostream& outfile, int n)
+{
+ CSGNode* pCurrentNode = m_Nodes.at(n);
+
+ outfile << pCurrentNode->getIndex() << endl;
+ outfile << pCurrentNode->getPosition().fPhi << endl;
+ outfile << pCurrentNode->getPosition().fTheta << endl;
+
+ outfile << endl;
+ return true;
+}
+
+bool CSphericalGraph::writeEdge(ostream& outfile, int e)
+{
+ CSGEdge* pCurrentEdge = m_Edges.at(e);
+ outfile << pCurrentEdge->nIndex1 << " ";
+ outfile << pCurrentEdge->nIndex2 << " ";
+
+ outfile << pCurrentEdge->nLeftFace << " ";
+ outfile << pCurrentEdge->nRightFace << " ";
+
+ outfile << endl;
+ return true;
+}
diff --git a/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.h b/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.h
new file mode 100644
index 000000000..fc8ffdbe8
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.h
@@ -0,0 +1,165 @@
+// *****************************************************************
+// Filename: AspectGraph.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+#ifndef _SPHERICAL_GRAPH_H_
+#define _SPHERICAL_GRAPH_H_
+
+// *****************************************************************
+// includes
+// *****************************************************************
+// stl includes
+#include <vector>
+#include <istream>
+#include <ostream>
+
+// local includes
+#include "Structs.h"
+
+// *****************************************************************
+// namespaces
+// *****************************************************************
+using namespace std;
+
+// *****************************************************************
+// defines
+// *****************************************************************
+#define MAX_NODES 100000
+
+// *****************************************************************
+// nodes and edges
+// *****************************************************************
+class CSGEdge
+{
+public:
+ // indices of nodes
+ int nIndex1;
+ int nIndex2;
+
+ // indices of faces (needed in triangulation
+ int nLeftFace;
+ int nRightFace;
+};
+
+class CSGNode
+{
+public:
+ CSGNode() {}
+ CSGNode(TSphereCoord position)
+ {
+ m_Position = position;
+ }
+ virtual ~CSGNode() {}
+
+ TSphereCoord getPosition()
+ {
+ return m_Position;
+ }
+ void setPosition(TSphereCoord position)
+ {
+ m_Position = position;
+ }
+
+ int getIndex()
+ {
+ return m_nIndex;
+ }
+ void setIndex(int nIndex)
+ {
+ m_nIndex = nIndex;
+ }
+
+ virtual CSGNode* clone()
+ {
+ CSGNode* copy = new CSGNode(m_Position);
+ copy->setIndex(m_nIndex);
+ return copy;
+ }
+
+protected:
+ TSphereCoord m_Position;
+ int m_nIndex;
+
+};
+
+// *****************************************************************
+// types
+// *****************************************************************
+typedef vector<CSGEdge*> TEdgeList;
+typedef vector<CSGNode*> TNodeList;
+
+// *****************************************************************
+// definition of class CSphericalGraph
+// *****************************************************************
+class CSphericalGraph
+{
+public:
+ ///////////////////////////////
+ // construction / destruction
+ ///////////////////////////////
+ CSphericalGraph();
+ CSphericalGraph(const CSphericalGraph& prototype);
+
+ virtual ~CSphericalGraph();
+ virtual CSGNode* getNewNode()
+ {
+ return new CSGNode();
+ }
+
+ ///////////////////////////////
+ // aspect graph manipulation
+ ///////////////////////////////
+ // clear graph
+ void clear();
+
+ // add nodes
+ int addNode(CSGNode* pNode);
+
+ // add edges
+ int addEdge(int nIndex1, int nIndex2);
+ int addEdge(int nIndex1, int nIndex2, int nLeftFace, int nRightFace);
+
+ // node adjacency (usually only required by addEdge)
+ void addNodeAdjacency(int nNode, int nAdjacency);
+
+ ///////////////////////////////
+ // member access
+ ///////////////////////////////
+ TEdgeList* getEdges();
+ TNodeList* getNodes();
+
+ CSGEdge* getEdge(int nEdgeIndex);
+ CSGNode* getNode(int nIndex);
+ vector<int>* getNodeAdjacency(int nIndex);
+
+ ///////////////////////////////
+ // file io
+ ///////////////////////////////
+ virtual bool read(istream& infile);
+ virtual bool readNode(istream& infile);
+ virtual bool readEdge(istream& infile);
+
+ virtual bool write(ostream& outfile);
+ virtual bool writeNode(ostream& outfile, int n);
+ virtual bool writeEdge(ostream& outfile, int e);
+
+
+
+ ///////////////////////////////
+ // operators
+ ///////////////////////////////
+ CSphericalGraph& operator= (CSphericalGraph const& rhs);
+
+protected:
+ TEdgeList m_Edges;
+ TNodeList m_Nodes;
+ vector<int> m_NodeAdjacency[MAX_NODES];
+
+};
+
+#endif /* _SPHERICAL_GRAPH_H_ */
diff --git a/source/RobotAPI/components/EarlyVisionGraph/Structs.h b/source/RobotAPI/components/EarlyVisionGraph/Structs.h
new file mode 100644
index 000000000..64e071bcd
--- /dev/null
+++ b/source/RobotAPI/components/EarlyVisionGraph/Structs.h
@@ -0,0 +1,85 @@
+// *****************************************************************
+// Filename: Structs.h
+// Copyright: Kai Welke, Chair Prof. Dillmann (IAIM),
+// Institute for Computer Science and Engineering (CSE),
+// University of Karlsruhe. All rights reserved.
+// Author: Kai Welke
+// Date: 12.06.2007
+// *****************************************************************
+
+#ifndef _STRUCTS_H_
+#define _STRUCTS_H_
+
+#include <vector>
+#include <Eigen/Core>
+
+using namespace std;
+
+// *****************************************************************
+// structs
+// *****************************************************************
+
+// sphere coordinates with the following convention:
+// phi: rotation from positive z-axis (pointing up) to the vector (p-o) (around the rotated y-axis)
+// theta: rotation from the positive x-axis to the projection of the point in xy plane (around z-axis)
+struct TSphereCoord
+{
+ float fDistance;
+ float fPhi; // zenith
+ float fTheta; // azimuth
+};
+
+// transformation from one sphere to another
+// both spheres wiht same radius
+// psi: orientation of the view = rotation around the rotated (by spherical coords) x-axis
+struct TSphereTransform
+{
+ TSphereCoord fPosition;
+ float fPsi; // orientation
+};
+
+// transformation from one sphere to another
+// both spheres wiht same radius
+// fAlpha: rotation around the z axis in counterclockwise direction considering the positive z-axis
+// fBeta: rotation around the rotated y axis in counterclockwise direction considering the positive y-axis
+struct TTransform
+{
+ float fAlpha;
+ float fBeta;
+ Eigen::Vector3d betaAxis;
+ float fPsi;
+ Eigen::Vector3d psiAxis;
+};
+
+// *****************************************************************
+// structs
+// *****************************************************************
+struct TPathElement
+{
+ TSphereCoord Position;
+ int nNodeIndex;
+ int nHits;
+ float fBestSimilarity;
+};
+
+typedef vector<TPathElement> TPath;
+
+struct THypothesis
+{
+ int nID;
+ TPath Path;
+ // path look-a-like rating
+ float fRating;
+ // visual similarity of last comparison
+ float fSimilarity;
+ // overall visual similarity
+ float fAccSimilarity;
+ // overall rating
+ float fOverallRating;
+ TTransform Transform;
+ bool bValidTransform;
+};
+
+
+
+#endif /* _STRUCTS_H_ */
--
GitLab