test_hyper_graph.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement
4  *
5  * Copyright (c) 2020,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * This program is free software: you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation, either version 3 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program. If not, see <https://www.gnu.org/licenses/>.
21  *
22  * Authors: Christoph Rösmann
23  *********************************************************************/
24 
25 #include <corbo-optimization/hyper_graph/edge.h>
26 #include <corbo-optimization/hyper_graph/generic_edge.h>
27 #include <corbo-optimization/hyper_graph/hyper_graph.h>
28 
29 #include <corbo-core/console.h>
30 #include <corbo-core/macros.h>
31 #include <corbo-core/types.h>
32 #include <corbo-core/utilities.h>
33 #include <corbo-core/value_comparison.h>
34 
35 #include <array>
36 #include <functional>
37 
38 #include "gtest/gtest.h"
39 
40 using corbo::HyperGraph;
41 using corbo::VertexSet;
42 using corbo::VertexInterface;
43 using corbo::OptimizationEdgeSet;
44 using pFVectorVertex = corbo::PartiallyFixedVectorVertex;
45 using corbo::VectorVertex;
46 using corbo::EdgeInterface;
47 using corbo::EdgeGenericScalarFun;
48 using corbo::EdgeGenericVectorFun;
49 using corbo::BaseEdge;
50 using corbo::BaseMixedEdge;
51 using corbo::CORBO_INF_DBL;
52 
53 class TestHyperGraph : public testing::Test
54 {
55  protected:
56  // You can do set-up work for each test here.
57  TestHyperGraph()
58  {
59  vertices = std::make_shared<VertexSet>();
60  edges = std::make_shared<OptimizationEdgeSet>();
61  graph.setVertexSet(vertices);
62  graph.setEdgeSet(edges);
63 
64  v1 = std::make_shared<pFVectorVertex>(1);
65  v1->values().setOnes();
66  v2 = std::make_shared<pFVectorVertex>(Eigen::Vector2d(1, 1));
67  }
68 
69  // You can do clean-up work that doesn't throw exceptions here.
70  virtual ~TestHyperGraph() {}
71  // If the constructor and destructor are not enough for setting up
72  // and cleaning up each test, you can define the following methods:
73 
74  // Code here will be called immediately after the constructor (right
75  // before each test).
76  void SetUp() override {}
77 
78  // Code here will be called immediately after each test (right
79  // before the destructor).
80  // virtual void TearDown()
81 
82  using Edge1T = EdgeGenericScalarFun<pFVectorVertex, pFVectorVertex>;
83 
84  // create some edges types
85  static double edge1_fun(const Edge1T::VertexContainer& vertices)
86  {
87  return vertices[0]->getData()[0] + 2 * vertices[1]->getData()[0] + 3 * vertices[1]->getData()[1];
88  }
89 
90  using Edge2T = EdgeGenericVectorFun<2, pFVectorVertex>;
91 
92  static void edge2_fun(const Edge2T::VertexContainer& vertices, Eigen::Ref<Edge2T::ErrorVector> values)
93  {
94  values[0] = 4 * vertices[0]->getData()[0];
95  values[1] = 5 * vertices[0]->getData()[0] + 6 * vertices[0]->getData()[1];
96  }
97 
98  using Edge3T = EdgeGenericVectorFun<2, pFVectorVertex, pFVectorVertex>;
99 
100  static void edge3_fun(const Edge1T::VertexContainer& vertices, Eigen::Ref<Edge2T::ErrorVector> values)
101  {
102  // x^2 + 2*y^2 + 3*z^2
103  values[0] = vertices[0]->getData()[0] * vertices[0]->getData()[0] + 2 * vertices[1]->getData()[0] * vertices[1]->getData()[0] +
104  3 * vertices[1]->getData()[1] * vertices[1]->getData()[1];
105  // x*y + 2*x*z
106  values[1] = vertices[0]->getData()[0] * vertices[1]->getData()[0] + 2 * vertices[0]->getData()[0] * vertices[1]->getData()[1];
107  }
108 
109  using Edge4T = EdgeGenericVectorFun<3, pFVectorVertex, pFVectorVertex>;
110 
111  static void edge4_fun(const Edge4T::VertexContainer& vertices, Eigen::Ref<Edge4T::ErrorVector> values)
112  {
113  values[0] = vertices[0]->getData()[0] - 5;
114  values[1] = vertices[1]->getData()[0] + 3;
115  values[2] = vertices[1]->getData()[1];
116  }
117 
118  HyperGraph graph;
119  VertexSet::Ptr vertices;
120  OptimizationEdgeSet::Ptr edges;
121 
122  pFVectorVertex::Ptr v1;
123  pFVectorVertex::Ptr v2;
124 };
125 
126 TEST_F(TestHyperGraph, vertex_set_active_vertices)
127 {
128  // Vertices
129  pFVectorVertex::Ptr p1 = std::make_shared<pFVectorVertex>(Eigen::Vector2d(2, 3));
130  p1->setFixed(1, true);
131  VectorVertex::Ptr p2 = std::make_shared<VectorVertex>(Eigen::Vector3d(4, 5, 3));
132  p2->setFixed(true);
133  pFVectorVertex::Ptr p3 = std::make_shared<pFVectorVertex>(1);
134  p3->setData(0, -1);
135 
136  // Create Vertex Set
137  VertexSet::Ptr vertices = VertexSet::Ptr(new VertexSet({p1, p2, p3}));
138 
139  std::vector<VertexInterface*>& active_vertices = vertices->getActiveVertices();
140 
141  EXPECT_EQ(active_vertices.size(), 2);
142  EXPECT_EQ(active_vertices[0], p1.get());
143  EXPECT_EQ(active_vertices[1], p3.get());
144 }
145 
146 TEST_F(TestHyperGraph, vertex_set_increment)
147 {
148  // Vertices
149  pFVectorVertex::Ptr p1 = std::make_shared<pFVectorVertex>(1);
150  p1->setData(0, 1);
151  pFVectorVertex::Ptr p2 = std::make_shared<pFVectorVertex>(Eigen::Vector2d(1, 1));
152 
153  // Create Vertex Set
154  VertexSet::Ptr vertices = VertexSet::Ptr(new VertexSet({p1, p2}));
155 
156  EXPECT_EQ(vertices->getParameterDimension(), 3);
157 
158  Eigen::Vector3d inc(5, -1, 7);
159  vertices->applyIncrementNonFixed(inc);
160 
161  EXPECT_DOUBLE_EQ(p1->values()[0], 6);
162  EXPECT_DOUBLE_EQ(p2->values()[0], 0);
163  EXPECT_DOUBLE_EQ(p2->values()[1], 8);
164 }
165 
166 TEST_F(TestHyperGraph, vertex_set_increment_fixed)
167 {
168  // Vertices
169  pFVectorVertex::Ptr p1 = std::make_shared<pFVectorVertex>(1);
170  p1->setData(0, 1);
171  p1->setFixed(0, true);
172  pFVectorVertex::Ptr p2 = std::make_shared<pFVectorVertex>(Eigen::Vector2d(1, 1));
173  p2->setFixed(1, true);
174 
175  // Create Vertex Set
176  VertexSet::Ptr vertices = VertexSet::Ptr(new VertexSet({p1, p2}));
177 
178  EXPECT_EQ(vertices->getParameterDimension(), 1);
179 
180  Eigen::Matrix<double, 1, 1> inc;
181  inc[0] = -1;
182  vertices->applyIncrementNonFixed(inc);
183 
184  EXPECT_DOUBLE_EQ(p1->values()[0], 1);
185  EXPECT_DOUBLE_EQ(p2->values()[0], 0);
186  EXPECT_DOUBLE_EQ(p2->values()[1], 1);
187 }
188 
189 TEST_F(TestHyperGraph, vertex_set_increment_fixed2)
190 {
191  // Vertices
192  pFVectorVertex::Ptr p1 = std::make_shared<pFVectorVertex>(1);
193  p1->setData(0, 1);
194  p1->setFixed(0, true);
195  pFVectorVertex::Ptr p2 = std::make_shared<pFVectorVertex>(Eigen::Vector2d(1, 1));
196  p2->setFixed(0, true);
197 
198  // Create Vertex Set
199  VertexSet::Ptr vertices = VertexSet::Ptr(new VertexSet({p1, p2}));
200 
201  EXPECT_EQ(vertices->getParameterDimension(), 1);
202 
203  Eigen::Matrix<double, 1, 1> inc;
204  inc[0] = -1;
205  vertices->applyIncrementNonFixed(inc);
206 
207  EXPECT_DOUBLE_EQ(p1->values()[0], 1);
208  EXPECT_DOUBLE_EQ(p2->values()[0], 1);
209  EXPECT_DOUBLE_EQ(p2->values()[1], 0);
210 }
211 
212 TEST_F(TestHyperGraph, graph_consistency)
213 {
214  vertices->addVertex(v1);
215 
216  // Edges
217  BaseEdge::Ptr edge1(new Edge1T(edge1_fun, false, *v1, *v2));
218  BaseEdge::Ptr edge2(new Edge2T(edge2_fun, false, *v2));
219  edges->addEdges({edge1, edge2}, {}, {}, {}, {});
220 
221  EXPECT_FALSE(graph.checkGraphConsistency());
222 
223  vertices->addVertex(v2);
224 
225  EXPECT_TRUE(graph.checkGraphConsistency());
226 }
227 
228 TEST_F(TestHyperGraph, numerical_jacobian1)
229 {
230  vertices->addVertex(v1);
231  vertices->addVertex(v2);
232 
233  // Edge1
234  BaseEdge::Ptr edge1(new Edge1T(edge1_fun, false, *v1, *v2));
235 
236  Eigen::MatrixXd jac1_v1(edge1->getDimension(), v1->getDimensionUnfixed());
237  edge1->computeJacobian(0, jac1_v1);
238  EXPECT_NEAR(jac1_v1(0, 0), 1, 1e-6);
239 
240  Eigen::MatrixXd jac1_v2(edge1->getDimension(), v2->getDimensionUnfixed());
241  edge1->computeJacobian(1, jac1_v2);
242  EXPECT_NEAR(jac1_v2(0, 0), 2, 1e-6);
243  EXPECT_NEAR(jac1_v2(0, 1), 3, 1e-6);
244 }
245 
246 TEST_F(TestHyperGraph, numerical_jacobian2)
247 {
248  vertices->addVertex(v1);
249  vertices->addVertex(v2);
250 
251  // Edge3
252  BaseEdge::Ptr edge3(new Edge3T(edge3_fun, false, *v1, *v2));
253 
254  Eigen::MatrixXd jac3_v1(edge3->getDimension(), v1->getDimensionUnfixed());
255  edge3->computeJacobian(0, jac3_v1);
256 
257  Eigen::MatrixXd jac3_v1_sol(2, 1);
258  jac3_v1_sol << 2, 3;
259  EXPECT_EQ_MATRIX(jac3_v1, jac3_v1_sol, 1e-6);
260 
261  Eigen::MatrixXd jac3_v2(edge3->getDimension(), v2->getDimensionUnfixed());
262  edge3->computeJacobian(1, jac3_v2);
263 
264  Eigen::MatrixXd jac3_v2_sol(2, 2);
265  jac3_v2_sol << 4, 6, 1, 2;
266  EXPECT_EQ_MATRIX(jac3_v2, jac3_v2_sol, 1e-6);
267 }
268 
269 TEST_F(TestHyperGraph, numerical_hessian)
270 {
271  vertices->addVertex(v1);
272  vertices->addVertex(v2);
273 
274  // Edge1
275  BaseEdge::Ptr edge1(new Edge1T(edge1_fun, false, *v1, *v2));
276 
277  Eigen::MatrixXd hes_v1_v1(v1->getDimensionUnfixed(), v1->getDimensionUnfixed());
278  hes_v1_v1.setZero();
279  edge1->computeHessianInc(0, 0, hes_v1_v1);
280  EXPECT_TRUE(hes_v1_v1.isZero(1e-3)) << "hes_v1_v1: " << hes_v1_v1;
281 
282  Eigen::MatrixXd hes_v1_v2(v1->getDimensionUnfixed(), v2->getDimensionUnfixed());
283  hes_v1_v2.setZero();
284  edge1->computeHessianInc(0, 1, hes_v1_v2);
285  EXPECT_TRUE(hes_v1_v2.isZero(1e-3)) << "hes_v1_v2: " << hes_v1_v2;
286 
287  Eigen::MatrixXd hes_v2_v1(v2->getDimensionUnfixed(), v1->getDimensionUnfixed());
288  hes_v2_v1.setZero();
289  edge1->computeHessianInc(1, 0, hes_v2_v1);
290  EXPECT_TRUE(hes_v2_v1.isZero(1e-3)) << "hes_v2_v1: " << hes_v2_v1;
291 
292  Eigen::MatrixXd hes_v2_v2(v2->getDimensionUnfixed(), v2->getDimensionUnfixed());
293  hes_v2_v2.setZero();
294  edge1->computeHessianInc(1, 1, hes_v2_v2);
295  EXPECT_TRUE(hes_v2_v2.isZero(1e-3)) << "hes_v2_v2: " << hes_v2_v2;
296 }
297 
298 TEST_F(TestHyperGraph, numerical_hessian2)
299 {
300  vertices->addVertex(v1);
301  vertices->addVertex(v2);
302 
303  // Edge1
304  BaseEdge::Ptr edge3(new Edge3T(edge3_fun, false, *v1, *v2));
305 
306  Eigen::MatrixXd hes_v1_v1(v1->getDimensionUnfixed(), v1->getDimensionUnfixed());
307  hes_v1_v1.setZero();
308  edge3->computeHessianInc(0, 0, hes_v1_v1);
309  Eigen::MatrixXd hes_v1_v1_sol(v1->getDimensionUnfixed(), v1->getDimensionUnfixed());
310  hes_v1_v1_sol << 2;
311  EXPECT_EQ_MATRIX(hes_v1_v1, hes_v1_v1_sol, 1e-3);
312 
313  Eigen::MatrixXd hes_v1_v2(v1->getDimensionUnfixed(), v2->getDimensionUnfixed());
314  hes_v1_v2.setZero();
315  edge3->computeHessianInc(0, 1, hes_v1_v2);
316  Eigen::MatrixXd hes_v1_v2_sol(v1->getDimensionUnfixed(), v2->getDimensionUnfixed());
317  hes_v1_v2_sol << 1, 2;
318  EXPECT_EQ_MATRIX(hes_v1_v2, hes_v1_v2_sol, 1e-3);
319 
320  Eigen::MatrixXd hes_v2_v1(v2->getDimensionUnfixed(), v1->getDimensionUnfixed());
321  hes_v2_v1.setZero();
322  edge3->computeHessianInc(1, 0, hes_v2_v1);
323  Eigen::MatrixXd hes_v2_v1_sol(v2->getDimensionUnfixed(), v1->getDimensionUnfixed());
324  hes_v2_v1_sol << 1, 2;
325  EXPECT_EQ_MATRIX(hes_v2_v1, hes_v2_v1_sol, 1e-3);
326 
327  Eigen::MatrixXd hes_v2_v2(v2->getDimensionUnfixed(), v2->getDimensionUnfixed());
328  hes_v2_v2.setZero();
329  edge3->computeHessianInc(1, 1, hes_v2_v2);
330  Eigen::MatrixXd hes_v2_v2_sol(v2->getDimensionUnfixed(), v2->getDimensionUnfixed());
331  hes_v2_v2_sol << 4, 0, 0, 6;
332  EXPECT_EQ_MATRIX(hes_v2_v2, hes_v2_v2_sol, 1e-3);
333 }
334 // TEST_F(TestHyperGraph, optim_edge_set)
335 //{
336 // Vertices
337 // pFVectorVertex::Ptr p1 = std::make_shared<pFVectorVertex>(Eigen::Vector2d(2, 3));
338 // p1->setFixed(1, true);
339 // VectorVertex::Ptr p2 = std::make_shared<VectorVertex>(Eigen::Vector3d(4, 5, 3));
340 // p2->setFixed(true);
341 // pFVectorVertex::Ptr p3 = std::make_shared<pFVectorVertex>(1);
342 // p3->setData(0, -1);
343 
344 // // Edge
345 // using Edge1T = EdgeGenericScalarFun<pFVectorVertex, pFVectorVertex>;
346 // auto edge1_fun = [](const Edge1T::VertexContainer& vertices) {
347 // return vertices[0]->getData()[0] + 2 * vertices[1]->getData()[0] + 3 * vertices[1]->getData()[1];
348 // };
349 
350 // // Create Edge Set
351 // OptimizationEdgeSet::Ptr vertices = VertexSet::Ptr(new VertexSet({p1, p2, p3}));
352 //}
TestHyperGraph::Edge2T
EdgeGenericVectorFun< 2, pFVectorVertex > Edge2T
Definition: test_hyper_graph.cpp:90
TestHyperGraph::SetUp
void SetUp() override
Definition: test_hyper_graph.cpp:76
corbo::HyperGraph
hyper-graph representation
Definition: hyper_graph.h:46
corbo::PartiallyFixedVectorVertex::Ptr
std::shared_ptr< PartiallyFixedVectorVertex > Ptr
Definition: vector_vertex.h:279
hyper_graph.h
TEST_F
TEST_F(TestHyperGraph, vertex_set_active_vertices)
Definition: test_hyper_graph.cpp:126
corbo::HyperGraph::checkGraphConsistency
bool checkGraphConsistency()
Return number of objective edges.
Definition: hyper_graph.cpp:50
TestHyperGraph::edge4_fun
static void edge4_fun(const Edge4T::VertexContainer &vertices, Eigen::Ref< Edge4T::ErrorVector > values)
Definition: test_hyper_graph.cpp:111
TestHyperGraph::v2
pFVectorVertex::Ptr v2
Definition: test_hyper_graph.cpp:123
TestHyperGraph::Edge3T
EdgeGenericVectorFun< 2, pFVectorVertex, pFVectorVertex > Edge3T
Definition: test_hyper_graph.cpp:98
TestHyperGraph::edges
OptimizationEdgeSet::Ptr edges
Definition: test_hyper_graph.cpp:120
corbo::VertexInterface
Generic interface class for vertices.
Definition: vertex_interface.h:53
TestHyperGraph::v1
pFVectorVertex::Ptr v1
Definition: test_hyper_graph.cpp:122
TestHyperGraph::edge2_fun
static void edge2_fun(const Edge2T::VertexContainer &vertices, Eigen::Ref< Edge2T::ErrorVector > values)
Definition: test_hyper_graph.cpp:92
TestHyperGraph::Edge1T
EdgeGenericScalarFun< pFVectorVertex, pFVectorVertex > Edge1T
Definition: test_hyper_graph.cpp:82
corbo::PartiallyFixedVectorVertex
Vector based vertex with support for partially fixed components.
Definition: vector_vertex.h:276
generic_edge.h
TestHyperGraph::~TestHyperGraph
virtual ~TestHyperGraph()
Definition: test_hyper_graph.cpp:70
value_comparison.h
corbo::HyperGraph::setVertexSet
void setVertexSet(VertexSetInterface::Ptr vertices)
Definition: hyper_graph.h:68
corbo::Edge< VerticesT... >::VertexContainer
std::array< VertexInterface *, numVerticesCompileTime > VertexContainer
Typedef to represent the vertex container.
Definition: edge.h:159
corbo::EdgeGenericScalarFun
Generic edge for functions .
Definition: generic_edge.h:68
corbo::CORBO_INF_DBL
constexpr const double CORBO_INF_DBL
Representation for infinity (double version)
Definition: core/include/corbo-core/types.h:53
corbo::EdgeGenericVectorFun
Generic edge for functions .
Definition: generic_edge.h:150
Eigen::Ref
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:192
TestHyperGraph::edge1_fun
static double edge1_fun(const Edge1T::VertexContainer &vertices)
Definition: test_hyper_graph.cpp:85
corbo::VectorVertex
Vertex implementation that stores an Eigen::VectorXd (dynamic dimension)
Definition: vector_vertex.h:51
TestHyperGraph::vertices
VertexSet::Ptr vertices
Definition: test_hyper_graph.cpp:119
EXPECT_EQ_MATRIX
#define EXPECT_EQ_MATRIX(A, B, tol)
Definition: macros.h:39
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: Matrix.h:178
corbo::HyperGraph::setEdgeSet
void setEdgeSet(OptimizationEdgeSet::Ptr edges)
Definition: hyper_graph.h:67
corbo::EdgeInterface
Generic interface class for edges.
Definition: edge_interface.h:54
TestHyperGraph::edge3_fun
static void edge3_fun(const Edge1T::VertexContainer &vertices, Eigen::Ref< Edge2T::ErrorVector > values)
Definition: test_hyper_graph.cpp:100

control_box_rst
Author(s): Christoph Rösmann
autogenerated on Mon Feb 28 2022 22:07:52