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/hyper_graph.h>
26 
27 #include <corbo-core/console.h>
28 #include <corbo-core/types.h>
29 
30 #include <algorithm>
31 
32 namespace corbo {
33 
34 // generic lambdas are c++14, hence we create a local function here
35 template <typename EdgeContainer>
36 bool check_edge_vertices(const std::vector<VertexInterface*>& vertices, EdgeContainer& edges)
37 {
38  for (int i = 0; i < edges.size(); ++i)
39  {
40  EdgeInterface* edge = edges[i].get();
41  for (int i = 0; i < edge->getNumVertices(); ++i)
42  {
43  auto it = std::find(vertices.begin(), vertices.end(), edge->getVertexRaw(i));
44  if (it == vertices.end()) return false;
45  }
46  }
47  return true;
48 }
49 
50 bool HyperGraph::checkGraphConsistency()
51 {
52  if (!hasVertexSet() || !hasEdgeSet()) return false;
53 
54  std::vector<VertexInterface*> vertices;
55  getVertexSet()->getVertices(vertices);
56 
57  if (!check_edge_vertices(vertices, getEdgeSet()->getObjectiveEdgesRef())) return false;
58  if (!check_edge_vertices(vertices, getEdgeSet()->getEqualityEdgesRef())) return false;
59  if (!check_edge_vertices(vertices, getEdgeSet()->getInequalityEdgesRef())) return false;
60  if (!check_edge_vertices(vertices, getEdgeSet()->getMixedEdgesRef())) return false;
61  return true;
62 }
63 
64 // void HyperGraph2::addVertex(VertexInterface* vtx)
65 //{
66 // _vertices.push_back(vtx);
67 // _vertex_idx_precomputed = false;
68 // _finite_bounds_precomputed = false;
69 //}
70 // void HyperGraph2::addObjectiveEdge(EdgeInterface::UPtr edge)
71 //{
72 // // register edge at adjacent vertices
73 // for (int i = 0; i < edge->getNumVertices(); ++i)
74 // {
75 // edge->getVertexRaw(i)->registerObjectiveEdge(edge.get());
76 // }
77 // _edges_objective.push_back(std::move(edge));
78 // _edge_obj_idx_precomputed = false;
79 //}
80 // void HyperGraph2::addEqualityConstraintEdge(EdgeInterface::UPtr edge)
81 //{
82 // // register edge at adjacent vertices
83 // for (int i = 0; i < edge->getNumVertices(); ++i) edge->getVertexRaw(i)->registerEqualityEdge(edge.get());
84 // _edges_equalities.push_back(std::move(edge));
85 // _edge_eq_idx_precomputed = false;
86 //}
87 // void HyperGraph2::addInequalityConstraintEdge(EdgeInterface::UPtr edge)
88 //{
89 // // register edge at adjacent vertices
90 // for (int i = 0; i < edge->getNumVertices(); ++i) edge->getVertexRaw(i)->registerInequalityEdge(edge.get());
91 // _edges_inequalities.push_back(std::move(edge));
92 // _edge_ineq_idx_precomputed = false;
93 //}
94 
95 // void HyperGraph2::computeDenseJacobianActiveConstraints(VertexContainer& vertices, EdgeContainer& edges, Eigen::Ref<Eigen::MatrixXd> jacobian,
96 // double weight)
97 //{
98 // if (edges.empty() || vertices.empty()) return;
99 
100 // constexpr const double delta = 1e-9;
101 // constexpr const double neg2delta = -2 * delta;
102 // constexpr const double scalar = 1.0 / (2 * delta);
103 
104 // // just iterate edges
105 // for (EdgeInterface::UPtr& edge : edges)
106 // {
107 // assert(!edge->isLeastSquaresForm() && "computeDenseJacobianActive() currently does not support least-squares forms");
108 
109 // // compute values and check which values are active
110 // edge->computeValues();
111 // Eigen::Array<bool, -1, 1> active = Eigen::Map<Eigen::ArrayXd>(edge->getValuesRaw(), edge->getDimension()) > 0.0;
112 
113 // if (!active.any()) continue; // all parts are inactive -> zero jacobian
114 
115 // for (int i = 0; i < edge->getNumVertices(); ++i)
116 // {
117 // if (edge->getVertexRaw(i)->getDimensionUnfixed() == 0) continue;
118 
119 // edge->computeJacobian(i, jacobian.block(edge->getEdgeIdx(), edge->getVertexRaw(i)->getVertexIdx(), edge->getDimension(),
120 // edge->getVertexRaw(i)->getDimensionUnfixed()),
121 // nullptr);
122 
123 // // now set values to zero if inactive or multiply with weight otherwise
124 // for (int j = 0; j < edge->getDimension(); ++j)
125 // {
126 // if (active[j])
127 // jacobian.block(edge->getEdgeIdx() + j, edge->getVertexRaw(i)->getVertexIdx(), 1, edge->getVertexRaw(i)->getDimensionUnfixed())
128 // *=
129 // weight;
130 // else
131 // jacobian.block(edge->getEdgeIdx() + j, edge->getVertexRaw(i)->getVertexIdx(), 1, edge->getVertexRaw(i)->getDimensionUnfixed())
132 // .setZero();
133 // }
134 // }
135 // }
136 //}
137 
138 // void HyperGraph2::computeValuesObjective(Eigen::Ref<Eigen::VectorXd> values)
139 //{
140 // if (_edges_objective.empty()) return;
141 
142 // assert(values.size() == objectiveDimension());
143 
144 // // we require that edge indices are valid
145 // if (!_edge_obj_idx_precomputed) computeObjectiveEdgeIndices();
146 
147 // for (EdgeInterface::UPtr& edge : _edges_objective)
148 // {
149 // // TODO(roesmann) avoid copy by changing edge interface to directly accept external value vector in computeValues
150 // edge->computeValues();
151 
152 // if (!_retain_lsq_form && edge->isLeastSquaresForm())
153 // {
154 // values(edge->getEdgeIdx()) = Eigen::Map<const Eigen::VectorXd>(edge->getValues(), edge->getDimension()).squaredNorm();
155 // }
156 // else
157 // {
158 // values.segment(edge->getEdgeIdx(), edge->getDimension()) = Eigen::Map<const Eigen::VectorXd>(edge->getValues(), edge->getDimension());
159 // }
160 // }
161 //}
162 
163 // void HyperGraph2::computeValuesEquality(Eigen::Ref<Eigen::VectorXd> values)
164 //{
165 // if (_edges_equalities.empty()) return;
166 
167 // assert(values.size() == equalityDimension());
168 
169 // // we require that edge indices are valid
170 // if (!_edge_eq_idx_precomputed) computeEqualityEdgeIndices();
171 
172 // for (EdgeInterface::UPtr& edge : _edges_equalities)
173 // {
174 // // TODO(roesmann) avoid copy by changing edge interface to directly accept external value vector in computeValues
175 // edge->computeValues();
176 
177 // if (!_retain_lsq_form && edge->isLeastSquaresForm())
178 // {
179 // values(edge->getEdgeIdx()) = Eigen::Map<const Eigen::VectorXd>(edge->getValues(), edge->getDimension()).squaredNorm();
180 // }
181 // else
182 // {
183 // values.segment(edge->getEdgeIdx(), edge->getDimension()) = Eigen::Map<const Eigen::VectorXd>(edge->getValues(), edge->getDimension());
184 // }
185 // }
186 //}
187 
188 // void HyperGraph2::computeValuesInequality(Eigen::Ref<Eigen::VectorXd> values)
189 //{
190 // if (_edges_inequalities.empty()) return;
191 
192 // assert(values.size() == inequalityDimension());
193 
194 // // we require that edge indices are valid
195 // if (!_edge_ineq_idx_precomputed) computeInequalityEdgeIndices();
196 
197 // for (EdgeInterface::UPtr& edge : _edges_inequalities)
198 // {
199 // // TODO(roesmann) avoid copy by changing edge interface to directly accept external value vector in computeValues
200 // edge->computeValues();
201 
202 // if (!_retain_lsq_form && edge->isLeastSquaresForm())
203 // {
204 // values(edge->getEdgeIdx()) = Eigen::Map<const Eigen::VectorXd>(edge->getValues(), edge->getDimension()).squaredNorm();
205 // }
206 // else
207 // {
208 // values.segment(edge->getEdgeIdx(), edge->getDimension()) = Eigen::Map<const Eigen::VectorXd>(edge->getValues(), edge->getDimension());
209 // }
210 // }
211 //}
212 
213 // void HyperGraph2::computeValuesActiveInequality(Eigen::Ref<Eigen::VectorXd> values, double weight)
214 //{
215 // if (_edges_inequalities.empty()) return;
216 
217 // assert(values.size() == inequalityDimension());
218 
219 // // we require that edge indices are valid
220 // if (!_edge_ineq_idx_precomputed) computeInequalityEdgeIndices();
221 
222 // for (EdgeInterface::UPtr& edge : _edges_inequalities)
223 // {
224 // // TODO(roesmann) avoid copy by changing edge interface to directly accept external value vector in computeValues
225 // edge->computeValues();
226 
227 // if (!_retain_lsq_form && edge->isLeastSquaresForm())
228 // {
229 // values(edge->getEdgeIdx()) = Eigen::Map<const Eigen::VectorXd>(edge->getValues(), edge->getDimension()).squaredNorm();
230 // if (values(edge->getEdgeIdx()) < 0)
231 // values(edge->getEdgeIdx()) = 0;
232 // else
233 // values(edge->getEdgeIdx()) *= weight;
234 // }
235 // else
236 // {
237 // Eigen::Map<const Eigen::VectorXd> edge_val(edge->getValues(), edge->getDimension());
238 // values.segment(edge->getEdgeIdx(), edge->getDimension()) = (edge_val.array() < 0).select(0.0, edge_val * weight);
239 // }
240 // }
241 //}
242 
243 // void HyperGraph2::computeValuesBounds(Eigen::Ref<Eigen::VectorXd> lb, Eigen::Ref<Eigen::VectorXd> ub)
244 //{
245 // if (_vertices.empty()) return;
246 
247 // assert(lb.size() == parameterDimension());
248 // assert(ub.size() == parameterDimension());
249 
250 // if (!_vertex_idx_precomputed) computeVertexIndices();
251 
252 // for (const VertexInterface* vertex : _vertices)
253 // {
254 // if (vertex->getDimensionUnfixed() == vertex->getDimension())
255 // {
256 // lb.segment(vertex->getVertexIdx(), vertex->getDimension()) = vertex->getLowerBoundsMap();
257 // ub.segment(vertex->getVertexIdx(), vertex->getDimension()) = vertex->getUpperBoundsMap();
258 // }
259 // else
260 // {
261 // int param_idx = 0;
262 // for (int i = 0; i < vertex->getDimension(); ++i)
263 // {
264 // if (!vertex->isFixedComponent(i))
265 // {
266 // lb(vertex->getVertexIdx() + param_idx) = vertex->getLowerBounds()[i];
267 // ub(vertex->getVertexIdx() + param_idx) = vertex->getUpperBounds()[i];
268 // ++param_idx;
269 // }
270 // }
271 // }
272 // }
273 //}
274 
275 // void HyperGraph2::computeValuesLowerBounds(Eigen::Ref<Eigen::VectorXd> values)
276 //{
277 // if (_vertices.empty()) return;
278 
279 // assert(values.size() == parameterDimension());
280 
281 // if (!_vertex_idx_precomputed) computeVertexIndices();
282 
283 // for (const VertexInterface* vertex : _vertices)
284 // {
285 // if (vertex->getDimensionUnfixed() == vertex->getDimension())
286 // {
287 // values.segment(vertex->getVertexIdx(), vertex->getDimension()) = vertex->getLowerBoundsMap();
288 // }
289 // else
290 // {
291 // int param_idx = 0;
292 // for (int i = 0; i < vertex->getDimension(); ++i)
293 // {
294 // if (!vertex->isFixedComponent(i))
295 // {
296 // values(vertex->getVertexIdx() + param_idx) = vertex->getLowerBounds()[i];
297 // ++param_idx;
298 // }
299 // }
300 // }
301 // }
302 //}
303 
304 // void HyperGraph2::computeValuesUpperBounds(Eigen::Ref<Eigen::VectorXd> values)
305 //{
306 // if (_vertices.empty()) return;
307 
308 // assert(values.size() == parameterDimension());
309 
310 // if (!_vertex_idx_precomputed) computeVertexIndices();
311 
312 // for (const VertexInterface* vertex : _vertices)
313 // {
314 // if (vertex->getDimensionUnfixed() == vertex->getDimension())
315 // {
316 // values.segment(vertex->getVertexIdx(), vertex->getDimension()) = vertex->getUpperBoundsMap();
317 // }
318 
319 // else
320 // {
321 // int param_idx = 0;
322 // for (int i = 0; i < vertex->getDimension(); ++i)
323 // {
324 // if (!vertex->isFixedComponent(i))
325 // {
326 // values(vertex->getVertexIdx() + param_idx) = vertex->getUpperBounds()[i];
327 // ++param_idx;
328 // }
329 // }
330 // }
331 // }
332 //}
333 
334 // void HyperGraph2::computeDistanceFiniteCombinedBounds(Eigen::Ref<Eigen::VectorXd> values)
335 //{
336 // assert(values.size() == finiteCombinedBoundsDimension());
337 
338 // int idx = 0;
339 // for (const VertexInterface* vertex : _vertices)
340 // {
341 // if (!vertex->hasFiniteBounds()) continue;
342 
343 // for (int i = 0; i < vertex->getDimension(); ++i)
344 // {
345 // if (vertex->isFixedComponent(i)) continue;
346 
347 // if (vertex->getLowerBounds()[i] > -CORBO_INF_DBL || vertex->getUpperBounds()[i] < CORBO_INF_DBL)
348 // {
349 // if (vertex->getData()[i] < vertex->getLowerBounds()[i])
350 // values(idx) = vertex->getLowerBounds()[i] - vertex->getData()[i];
351 // else if (vertex->getData()[i] > vertex->getUpperBounds()[i])
352 // values(idx) = vertex->getData()[i] - vertex->getUpperBounds()[i];
353 // else
354 // values(idx) = 0;
355 
356 // ++idx; // only count finite bounds
357 // }
358 // }
359 // }
360 //}
361 
362 // void HyperGraph2::computeDenseJacobianObjective(Eigen::Ref<Eigen::MatrixXd> jacobian, const double* multipliers)
363 //{
364 // // we require that edge indices are valid
365 // if (_edges_objective.empty() || _vertices.empty()) return;
366 
367 // assert(jacobian.rows() == objectiveDimension());
368 // assert(jacobian.cols() == parameterDimension());
369 
370 // // we require that edge indices are valid
371 // if (!_edge_obj_idx_precomputed) computeObjectiveEdgeIndices();
372 // // if custom jacobians are specified, we also require valid vertex indices
373 // if ((_num_custom_jacobians_obj > 0 || _edge_based) && !_vertex_idx_precomputed) computeVertexIndices();
374 
375 // computeDenseJacobian<EdgeType::Objective>(_vertices, _edges_objective, jacobian, _num_custom_jacobians_obj, _edge_based, multipliers);
376 //}
377 
378 // void HyperGraph2::computeDenseJacobianEqualities(Eigen::Ref<Eigen::MatrixXd> jacobian, const double* multipliers)
379 //{
380 // if (_edges_equalities.empty() || _vertices.empty()) return;
381 
382 // assert(jacobian.rows() == equalityDimension());
383 // assert(jacobian.cols() == parameterDimension());
384 
385 // // we require that edge indices are valid
386 // if (!_edge_eq_idx_precomputed) computeEqualityEdgeIndices();
387 // // if custom jacobians are specified, we also require valid vertex indices
388 // if ((_num_custom_jacobians_eq > 0 || _edge_based) && !_vertex_idx_precomputed) computeVertexIndices();
389 
390 // computeDenseJacobian<EdgeType::Equality>(_vertices, _edges_equalities, jacobian, _num_custom_jacobians_eq, _edge_based, multipliers);
391 //}
392 
393 // void HyperGraph2::computeDenseJacobianInequalities(Eigen::Ref<Eigen::MatrixXd> jacobian, const double* multipliers)
394 //{
395 // if (_edges_inequalities.empty() || _vertices.empty()) return;
396 
397 // assert(jacobian.rows() == inequalityDimension());
398 // assert(jacobian.cols() == parameterDimension());
399 
400 // // we require that edge indices are valid
401 // if (!_edge_ineq_idx_precomputed) computeInequalityEdgeIndices();
402 // // if custom jacobians are specified, we also require valid vertex indices
403 // if ((_num_custom_jacobians_ineq > 0 || _edge_based) && !_vertex_idx_precomputed) computeVertexIndices();
404 
405 // computeDenseJacobian<EdgeType::Inequality>(_vertices, _edges_inequalities, jacobian, _num_custom_jacobians_ineq, _edge_based, multipliers);
406 //}
407 
409 // void HyperGraph2::computeDenseJacobianActiveInequalities(Eigen::Ref<Eigen::MatrixXd> jacobian, double weight)
410 //{
411 // if (_edges_inequalities.empty() || _vertices.empty()) return;
412 
413 // assert(jacobian.rows() == inequalityDimension());
414 // assert(jacobian.cols() == parameterDimension());
415 
416 // // we require that edge indices are valid
417 // if (!_edge_ineq_idx_precomputed) computeInequalityEdgeIndices();
418 // // if custom jacobians are specified, we also require valid vertex indices
419 // if (!_vertex_idx_precomputed) computeVertexIndices();
420 
421 // computeDenseJacobianActiveConstraints(_vertices, _edges_inequalities, jacobian, weight);
422 //}
423 
424 // void HyperGraph2::computeDenseJacobianFiniteCombinedBounds(Eigen::Ref<Eigen::MatrixXd> jacobian, double weight)
425 //{
426 // if (_vertices.empty()) return;
427 
428 // assert(jacobian.rows() == finiteCombinedBoundsDimension());
429 // assert(jacobian.cols() == parameterDimension());
430 
431 // // if custom jacobians are specified, we also require valid vertex indices
432 // if (!_vertex_idx_precomputed) computeVertexIndices();
433 
434 // int idx = 0;
435 // for (const VertexInterface* vertex : _vertices)
436 // {
437 // if (!vertex->hasFiniteBounds()) continue;
438 
439 // for (int i = 0; i < vertex->getDimension(); ++i)
440 // {
441 // if (vertex->isFixedComponent(i)) continue;
442 
443 // if (vertex->getLowerBounds()[i] > -CORBO_INF_DBL || vertex->getUpperBounds()[i] < CORBO_INF_DBL)
444 // {
445 // if (vertex->getData()[i] < vertex->getLowerBounds()[i])
446 // jacobian(idx, vertex->getVertexIdx() + i) = -weight; // -1 * weight
447 // else if (vertex->getData()[i] > vertex->getUpperBounds()[i])
448 // jacobian(idx, vertex->getVertexIdx() + i) = weight; // +1 * weight
449 // // else
450 // // jacobian(idx, vertex->vertexIdx() + i) = 0.0; // jacobian is already initialized with zeros!
451 
452 // ++idx; // only count finite bounds
453 // }
454 // }
455 // }
456 //}
457 
458 // void HyperGraph2::computeDenseHessian(VertexContainer& vertices, EdgeContainer& edges, Eigen::Ref<Eigen::MatrixXd> hessian,
459 // const Eigen::Ref<const Eigen::MatrixXd> jacobian, const double* multipliers, bool jacob_scaled)
460 //{
461 // if (edges.empty() || vertices.empty()) return;
462 
463 // for (EdgeInterface::UPtr& edge : edges)
464 // {
465 // // Hessian for linear equations is zero!
466 // if (edge->isLinear()) continue;
467 
468 // assert((!edge->isLeastSquaresForm() || _retain_lsq_form) &&
469 // "Hessian computation for automatic least-square form resolution not yet implemented.");
470 
471 // for (int i = 0; i < edge->getNumVertices(); ++i)
472 // {
473 // VertexInterface* vertex_i = edge->getVertexRaw(i);
474 
475 // if (vertex_i->getDimensionUnfixed() == 0) continue;
476 
477 // for (int j = i; j < edge->getNumVertices(); ++j)
478 // {
479 // VertexInterface* vertex_j = edge->getVertexRaw(j);
480 
481 // if (vertex_j->getDimensionUnfixed() == 0) continue;
482 
483 // if (multipliers)
484 // {
485 // edge->computeHessian(
486 // i, j, jacobian.block(edge->getEdgeIdx(), vertex_i->getVertexIdx(), edge->getDimension(), vertex_i->getDimensionUnfixed()),
487 // hessian.block(vertex_i->getVertexIdx(), vertex_j->getVertexIdx(), vertex_i->getDimensionUnfixed(),
488 // vertex_j->getDimensionUnfixed()),
489 // multipliers + edge->getEdgeIdx(), jacob_scaled);
490 // }
491 // else
492 // {
493 // edge->computeHessian(
494 // i, j, jacobian.block(edge->getEdgeIdx(), vertex_i->getVertexIdx(), edge->getDimension(), vertex_i->getDimensionUnfixed()),
495 // hessian.block(vertex_i->getVertexIdx(), vertex_j->getVertexIdx(), vertex_i->getDimensionUnfixed(),
496 // vertex_j->getDimensionUnfixed()),
497 // nullptr, false);
498 // }
499 
500 // if (i != j)
501 // {
502 // // assume symmetry:
503 // hessian
504 // .block(vertex_j->getVertexIdx(), vertex_i->getVertexIdx(), vertex_j->getDimensionUnfixed(), vertex_i->getDimensionUnfixed())
505 // .noalias() += hessian
506 // .block(vertex_i->getVertexIdx(), vertex_j->getVertexIdx(), vertex_i->getDimensionUnfixed(),
507 // vertex_j->getDimensionUnfixed())
508 // .transpose();
509 // }
510 // }
511 // }
512 // }
513 //}
514 
515 // void HyperGraph2::computeDenseHessianObjective(const Eigen::Ref<const Eigen::MatrixXd>& jacobian, Eigen::Ref<Eigen::MatrixXd> hessian,
516 // const double* multiplier, bool jacob_scaled)
517 //{
518 // if (_edges_objective.empty() || _vertices.empty()) return;
519 
520 // assert(hessian.rows() == parameterDimension());
521 // assert(hessian.cols() == parameterDimension());
522 
523 // // we require that edge indices are valid
524 // if (!_edge_obj_idx_precomputed) computeObjectiveEdgeIndices();
525 // // if custom jacobians are specified, we also require valid vertex indices
526 // // if ((_num_custom_jacobians_obj > 0 || edge_based) && !_vertex_idx_precomputed) computeVertexIndices();
527 // if (!_vertex_idx_precomputed) computeVertexIndices();
528 
529 // computeDenseHessian(_vertices, _edges_objective, hessian, jacobian, multiplier, jacob_scaled);
530 //}
531 
532 // void HyperGraph2::computeEdgeIndices(EdgeContainer& edges, int* dim, int* num_custom_jacobians)
533 //{
534 // int aux_num_custom_jacobians = 0;
535 // if (edges.empty())
536 // {
537 // if (dim) *dim = 0;
538 // if (num_custom_jacobians) *num_custom_jacobians = aux_num_custom_jacobians;
539 // return;
540 // }
541 
542 // edges.front()->_edge_jacobi_idx = 0;
543 // if (edges.front()->providesJacobian()) ++aux_num_custom_jacobians; // useful byproduct
544 // for (int i = 1; i < (int)edges.size(); ++i)
545 // {
546 // if (!_retain_lsq_form && edges[i]->isLeastSquaresForm())
547 // {
548 // edges[i]->_edge_jacobi_idx = edges[i - 1]->_edge_jacobi_idx + 1; // resulting dimension is always 1 -> f(x)^T * f(x)
549 // }
550 // else
551 // {
552 // edges[i]->_edge_jacobi_idx = edges[i - 1]->_edge_jacobi_idx + edges[i - 1]->getDimension();
553 // }
554 // if (edges[i]->providesJacobian()) ++aux_num_custom_jacobians; // useful byproduct
555 // }
556 
557 // if (dim) *dim = edges.back()->_edge_jacobi_idx + (!_retain_lsq_form && edges.back()->isLeastSquaresForm() ? 1 : edges.back()->getDimension());
558 // if (num_custom_jacobians) *num_custom_jacobians = aux_num_custom_jacobians;
559 //}
560 
561 // void HyperGraph2::computeVertexIndices()
562 //{
563 // if (_vertices.empty()) return;
564 
565 // _vertices.front()->_vertex_idx = 0;
566 // for (int i = 1; i < (int)_vertices.size(); ++i)
567 // {
568 // _vertices[i]->_vertex_idx = _vertices[i - 1]->_vertex_idx + _vertices[i - 1]->getDimensionUnfixed();
569 // }
570 
571 // _vertex_idx_precomputed = true;
572 //}
573 
574 // void HyperGraph2::computeFiniteBoundDimensions()
575 //{
576 // _dim_finite_bounds = 0;
577 // _dim_finite_combined_bounds = 0;
578 // for (const VertexInterface* vertex : _vertices)
579 // {
580 // if (!vertex->hasFiniteBounds()) continue;
581 // if (vertex->getDimensionUnfixed() == 0) continue;
582 
583 // for (int i = 0; i < vertex->getDimension(); ++i)
584 // {
585 // if (vertex->isFixedComponent(i)) continue;
586 
587 // if (vertex->getLowerBounds()[i] > -CORBO_INF_DBL)
588 // {
589 // ++_dim_finite_bounds;
590 // ++_dim_finite_combined_bounds;
591 // if (vertex->getUpperBounds()[i] < CORBO_INF_DBL) ++_dim_finite_bounds;
592 // }
593 // else if (vertex->getUpperBounds()[i] < CORBO_INF_DBL)
594 // {
595 // ++_dim_finite_bounds;
596 // ++_dim_finite_combined_bounds;
597 // }
598 // }
599 // }
600 // _finite_bounds_precomputed = true;
601 //}
602 
603 // int HyperGraph2::finiteBoundsDimension()
604 //{
605 // if (_finite_bounds_precomputed) return _dim_finite_bounds;
606 // computeFiniteBoundDimensions();
607 // return _dim_finite_bounds;
608 //}
609 
610 // int HyperGraph2::finiteCombinedBoundsDimension()
611 //{
612 // if (_finite_bounds_precomputed) return _dim_finite_combined_bounds;
613 // computeFiniteBoundDimensions();
614 // return _dim_finite_combined_bounds;
615 //}
616 
617 // bool HyperGraph2::isLeastSquaresProblem() const
618 //{
619 // for (const EdgeInterface::UPtr& edge : _edges_objective)
620 // {
621 // if (!edge->isLeastSquaresForm()) return false;
622 // }
623 // return _retain_lsq_form;
624 //}
625 
626 // void HyperGraph2::backupParameters()
627 //{
628 // for (VertexInterface* vertex : _vertices) vertex->push();
629 //}
630 
631 // void HyperGraph2::restoreBackupParameters(bool keep_backup)
632 //{
633 // if (keep_backup)
634 // for (VertexInterface* vertex : _vertices) vertex->top();
635 // else
636 // for (VertexInterface* vertex : _vertices) vertex->pop();
637 //}
638 
639 // void HyperGraph2::discardBackupParameters()
640 //{
641 // for (VertexInterface* vertex : _vertices) vertex->discardTop();
642 //}
643 
644 // void HyperGraph2::applyIncrement(const Eigen::Ref<const Eigen::VectorXd>& increment)
645 //{
646 // assert(increment.size() == parameterDimension());
647 
648 // if (!_vertex_idx_precomputed) computeVertexIndices();
649 
650 // for (VertexInterface* vertex : _vertices)
651 // {
652 // if (vertex->getDimensionUnfixed() != 0) vertex->plusUnfixed(increment.segment(vertex->getVertexIdx(),
653 // vertex->getDimensionUnfixed()).data());
654 // }
655 //}
656 
657 // double HyperGraph2::getLowerBound(int idx)
658 //{
659 // if (_vertices.empty()) return -CORBO_INF_DBL;
660 
661 // if (!_vertex_idx_precomputed) computeVertexIndices();
662 
663 // for (const VertexInterface* vertex : _vertices)
664 // {
665 // int vtx_idx = vertex->getVertexIdx();
666 // if (vertex->getDimensionUnfixed() == vertex->getDimension())
667 // {
668 // if (vtx_idx + vertex->getDimension() > idx)
669 // {
670 // return vertex->getLowerBounds()[idx - vtx_idx];
671 // }
672 // }
673 // else
674 // {
675 // int param_idx = 0;
676 // for (int i = 0; i < vertex->getDimension(); ++i)
677 // {
678 // if (!vertex->isFixedComponent(i))
679 // {
680 // if (vtx_idx + param_idx == idx) return vertex->getLowerBounds()[i];
681 // ++param_idx;
682 // }
683 // }
684 // }
685 // }
686 // return -CORBO_INF_DBL;
687 //}
688 
689 // double HyperGraph2::getUpperBound(int idx)
690 //{
691 // if (_vertices.empty()) return CORBO_INF_DBL;
692 
693 // if (!_vertex_idx_precomputed) computeVertexIndices();
694 
695 // for (const VertexInterface* vertex : _vertices)
696 // {
697 // int vtx_idx = vertex->getVertexIdx();
698 // if (vertex->getDimensionUnfixed() == vertex->getDimension())
699 // {
700 // if (vtx_idx + vertex->getDimension() > idx)
701 // {
702 // return vertex->getUpperBounds()[idx - vtx_idx];
703 // }
704 // }
705 // else
706 // {
707 // int param_idx = 0;
708 // for (int i = 0; i < vertex->getDimension(); ++i)
709 // {
710 // if (!vertex->isFixedComponent(i))
711 // {
712 // if (vtx_idx + param_idx == idx) return vertex->getUpperBounds()[i];
713 // ++param_idx;
714 // }
715 // }
716 // }
717 // }
718 // return CORBO_INF_DBL;
719 //}
720 // void HyperGraph2::setLowerBound(int idx, double lb)
721 //{
722 // if (_vertices.empty()) return;
723 
724 // if (!_vertex_idx_precomputed) computeVertexIndices();
725 
726 // for (VertexInterface* vertex : _vertices)
727 // {
728 // int vtx_idx = vertex->getVertexIdx();
729 // if (vertex->getDimensionUnfixed() == vertex->getDimension())
730 // {
731 // if (vtx_idx + vertex->getDimension() > idx)
732 // {
733 // vertex->setLowerBound(idx - vtx_idx, lb);
734 // return;
735 // }
736 // }
737 // else
738 // {
739 // int param_idx = 0;
740 // for (int i = 0; i < vertex->getDimension(); ++i)
741 // {
742 // if (!vertex->isFixedComponent(i))
743 // {
744 // if (vtx_idx + param_idx == idx)
745 // {
746 // vertex->setLowerBound(i, lb);
747 // return;
748 // }
749 // ++param_idx;
750 // }
751 // }
752 // }
753 // }
754 //}
755 
756 // void HyperGraph2::setUpperBound(int idx, double ub)
757 //{
758 // if (_vertices.empty()) return;
759 
760 // if (!_vertex_idx_precomputed) computeVertexIndices();
761 
762 // for (VertexInterface* vertex : _vertices)
763 // {
764 // int vtx_idx = vertex->getVertexIdx();
765 // if (vertex->getDimensionUnfixed() == vertex->getDimension())
766 // {
767 // if (vtx_idx + vertex->getDimension() > idx)
768 // {
769 // vertex->setUpperBound(idx - vtx_idx, ub);
770 // return;
771 // }
772 // }
773 // else
774 // {
775 // int param_idx = 0;
776 // for (int i = 0; i < vertex->getDimension(); ++i)
777 // {
778 // if (!vertex->isFixedComponent(i))
779 // {
780 // if (vtx_idx + param_idx == idx)
781 // {
782 // vertex->setUpperBound(i, ub);
783 // return;
784 // }
785 // ++param_idx;
786 // }
787 // }
788 // }
789 // }
790 //}
791 
792 // double HyperGraph2::getParameterValue(int idx)
793 //{
794 // if (_vertices.empty()) return CORBO_MAX_DBL;
795 
796 // if (!_vertex_idx_precomputed) computeVertexIndices();
797 
798 // for (const VertexInterface* vertex : _vertices)
799 // {
800 // int vtx_idx = vertex->getVertexIdx();
801 // if (vertex->getDimensionUnfixed() == vertex->getDimension())
802 // {
803 // if (vtx_idx + vertex->getDimension() > idx)
804 // {
805 // return vertex->getData()[idx - vtx_idx];
806 // }
807 // }
808 // else
809 // {
810 // int param_idx = 0;
811 // for (int i = 0; i < vertex->getDimension(); ++i)
812 // {
813 // if (!vertex->isFixedComponent(i))
814 // {
815 // if (vtx_idx + param_idx == idx) return vertex->getData()[i];
816 // ++param_idx;
817 // }
818 // }
819 // }
820 // }
821 // return CORBO_MAX_DBL;
822 //}
823 
824 // void HyperGraph2::setParameterValue(int idx, double x)
825 //{
826 // if (_vertices.empty()) return;
827 
828 // if (!_vertex_idx_precomputed) computeVertexIndices();
829 
830 // for (VertexInterface* vertex : _vertices)
831 // {
832 // int vtx_idx = vertex->getVertexIdx();
833 // if (vertex->getDimensionUnfixed() == vertex->getDimension())
834 // {
835 // if (vtx_idx + vertex->getDimension() > idx)
836 // {
837 // vertex->setData(idx - vtx_idx, x);
838 // return;
839 // }
840 // }
841 // else
842 // {
843 // int param_idx = 0;
844 // for (int i = 0; i < vertex->getDimension(); ++i)
845 // {
846 // if (!vertex->isFixedComponent(i))
847 // {
848 // if (vtx_idx + param_idx == idx)
849 // {
850 // vertex->setData(i, x);
851 // return;
852 // }
853 // ++param_idx;
854 // }
855 // }
856 // }
857 // }
858 //}
859 
860 // void HyperGraph2::resetStructure()
861 //{
862 // _edge_obj_idx_precomputed = false;
863 // _edge_eq_idx_precomputed = false;
864 // _edge_ineq_idx_precomputed = false;
865 // _vertex_idx_precomputed = false;
866 //}
867 
868 // void HyperGraph2::clearConnectedEdges()
869 //{
870 // for (VertexInterface* vertex : _vertices) vertex->clearConnectedEdges();
871 //}
872 
873 // void HyperGraph2::clear()
874 //{
875 // clearConnectedEdges();
876 // _edges_objective.clear();
877 // _edges_equalities.clear();
878 // _edges_inequalities.clear();
879 // _vertices.clear();
880 // resetStructure();
881 //}
882 
883 } // namespace corbo
corbo::HyperGraph::getEdgeSet
OptimizationEdgeSet::Ptr getEdgeSet() const
Definition: hyper_graph.h:73
hyper_graph.h
corbo::HyperGraph::checkGraphConsistency
bool checkGraphConsistency()
Return number of objective edges.
Definition: hyper_graph.cpp:50
corbo::EdgeInterface::getNumVertices
virtual int getNumVertices() const =0
Return number of attached vertices.
corbo::EdgeInterface::getVertexRaw
virtual VertexInterface * getVertexRaw(int idx)=0
Get access to vertex with index idx (0 <= idx < numVertices)
corbo::check_edge_vertices
bool check_edge_vertices(const std::vector< VertexInterface *> &vertices, EdgeContainer &edges)
Definition: hyper_graph.cpp:36
corbo::HyperGraph::hasVertexSet
bool hasVertexSet() const
Definition: hyper_graph.h:71
corbo::HyperGraph::getVertexSet
VertexSetInterface::Ptr getVertexSet() const
Definition: hyper_graph.h:74
corbo::EdgeInterface
Generic interface class for edges.
Definition: edge_interface.h:54
corbo::HyperGraph::hasEdgeSet
bool hasEdgeSet() const
Definition: hyper_graph.h:70

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