hyper_graph.hpp
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/macros.h>
28 
29 namespace corbo {
30 
31 template <HyperGraph2::EdgeType type>
32 void HyperGraph2::computeDenseJacobian(VertexContainer& vertices, EdgeContainer& edges, Eigen::Ref<Eigen::MatrixXd> jacobian,
33  int num_custom_jacobians, bool edge_based, const double* multipliers)
34 {
35  if (edges.empty() || vertices.empty()) return;
36 
37  constexpr const double delta = 1e-9;
38  constexpr const double neg2delta = -2 * delta;
39  constexpr const double scalar = 1.0 / (2 * delta);
40 
41  // first store analytic jacobians if any
42  if (num_custom_jacobians > 0 || edge_based)
43  {
44  for (EdgeInterface::UPtr& edge : edges)
45  {
46  if (edge->providesJacobian() || edge_based)
47  {
48  for (int i = 0; i < edge->getNumVertices(); ++i)
49  {
50  if (edge->getVertexRaw(i)->getDimensionUnfixed() == 0) continue;
51 
52  // in least squares form, we must compute the jacobian of f(x)^2, hence -> 2 * f(x)^T * Jacobian(f(x)).
53  if (!_retain_lsq_form && edge->isLeastSquaresForm())
54  {
55  Eigen::MatrixXd jacobf(edge->getDimension(), edge->getVertexRaw(i)->getDimensionUnfixed());
56  // we require a temporary matrix since we need to apply the chain rule in order to get a
57  // dimension of 1
58  edge->computeJacobian(i, jacobf, nullptr);
59  edge->computeValues();
60  const Eigen::Map<const Eigen::VectorXd> values(edge->getValues(), edge->getDimension());
61  if (multipliers)
62  {
63  jacobian.block(edge->getEdgeIdx(), edge->getVertexRaw(i)->getVertexIdx(), 1, edge->getVertexRaw(i)->getDimensionUnfixed())
64  .noalias() = 2.0 * multipliers[edge->getEdgeIdx()] * values.transpose() * jacobf;
65  }
66  else
67  {
68  jacobian.block(edge->getEdgeIdx(), edge->getVertexRaw(i)->getVertexIdx(), 1, edge->getVertexRaw(i)->getDimensionUnfixed())
69  .noalias() = 2.0 * values.transpose() * jacobf;
70  }
71  }
72  else
73  {
74  if (multipliers)
75  edge->computeJacobian(i, jacobian.block(edge->getEdgeIdx(), edge->getVertexRaw(i)->getVertexIdx(), edge->getDimension(),
76  edge->getVertexRaw(i)->getDimensionUnfixed()),
77  multipliers + edge->getEdgeIdx());
78  else
79  edge->computeJacobian(i, jacobian.block(edge->getEdgeIdx(), edge->getVertexRaw(i)->getVertexIdx(), edge->getDimension(),
80  edge->getVertexRaw(i)->getDimensionUnfixed()),
81  nullptr);
82  }
83  }
84  }
85  }
86  if (num_custom_jacobians == edges.size() || edge_based) return; // we have everything completed already
87  }
88 
89  // now let's iterate vertices
90  int col_idx = 0;
91  for (VertexInterface* vertex : vertices)
92  {
93  if (vertex->getDimensionUnfixed() == 0) continue; // without increasing col_idx
94 
95  int num_connected_edges;
96  static_if(type == EdgeType::Objective)
97  {
98  num_connected_edges = (int)vertex->getConnectedObjectiveEdgesRef().size();
99  if (vertex->getNumObjectiveEdgesWithCustomJacobian() == num_connected_edges)
100  {
101  ++col_idx;
102  continue; // skip what we already have...
103  }
104  }
105  else static_if(type == EdgeType::Equality)
106  {
107  num_connected_edges = (int)vertex->getConnectedEqualityEdgesRef().size();
108  if (vertex->getNumEqualityEdgesWithCustomJacobian() == num_connected_edges)
109  {
110  ++col_idx;
111  continue; // skip what we already have...
112  }
113  }
114  else
115  {
116  num_connected_edges = (int)vertex->getConnectedInequalityEdgesRef().size();
117  if (vertex->getNumInequalityEdgesWithCustomJacobian() == num_connected_edges)
118  {
119  ++col_idx;
120  continue; // skip what we already have...
121  }
122  }
123 
124  std::vector<Eigen::VectorXd> values2(num_connected_edges); // if edges provide custom jacobians, we reserve too much.
125 
126  for (int i = 0; i < vertex->getDimension(); ++i)
127  {
128  if (vertex->isFixedComponent(i)) continue; // continue without increasing col_idx
129 
130  std::set<EdgeInterface*>* connected_edges;
131  static_if(type == EdgeType::Objective) connected_edges = &vertex->getConnectedObjectiveEdgesRef();
132  else static_if(type == EdgeType::Equality) connected_edges = &vertex->getConnectedEqualityEdgesRef();
133  else connected_edges = &vertex->getConnectedInequalityEdgesRef();
134 
135  vertex->plus(i, delta); // increment current value by delta
136  // compute values for all connected edges
137  int edge_idx = 0;
138  for (EdgeInterface* edge : *connected_edges)
139  {
140  if (!edge->providesJacobian())
141  {
142  edge->computeValues();
143  values2[edge_idx] = Eigen::Map<const Eigen::VectorXd>(edge->getValues(), edge->getDimension());
144  }
145  ++edge_idx;
146  }
147  vertex->plus(i, neg2delta); // subtract 2*delta
148  edge_idx = 0;
149  for (EdgeInterface* edge : *connected_edges)
150  {
151  if (!edge->providesJacobian())
152  {
153  edge->computeValues();
154  const Eigen::Map<const Eigen::VectorXd> values(edge->getValues(), edge->getDimension());
155  // in least squares form, we must compute the jacobian of f(x)^2, hence -> 2 * f(x)^T * Jacobian(f(x)).
156  if (!_retain_lsq_form && edge->isLeastSquaresForm())
157  {
158  // get [1 x 1] jacobi block in the jacobi matrix
159  if (multipliers)
160  jacobian(edge->getEdgeIdx(), col_idx) =
161  2.0 * values.transpose() * scalar * multipliers[edge->getEdgeIdx()] * (values2[edge_idx] - values);
162  else
163  jacobian(edge->getEdgeIdx(), col_idx) = 2.0 * values.transpose() * scalar * (values2[edge_idx] - values);
164  }
165  else
166  {
167  // get [edge_dim x 1] jacobi block in the jacobi matrix
168  if (multipliers)
169  {
170  const Eigen::Map<const Eigen::VectorXd> values_multp(multipliers + edge->getEdgeIdx(), edge->getDimension());
171  jacobian.block(edge->getEdgeIdx(), col_idx, edge->getDimension(), 1).noalias() =
172  scalar * (values2[edge_idx] - values).cwiseProduct(values_multp);
173  }
174  else
175  {
176  jacobian.block(edge->getEdgeIdx(), col_idx, edge->getDimension(), 1).noalias() = scalar * (values2[edge_idx] - values);
177  }
178  }
179  }
180  ++edge_idx;
181  }
182  vertex->plus(i, delta); // revert offset
183 
184  // increase column index
185  ++col_idx;
186  }
187  }
188 }
189 
190 } // namespace corbo
corbo::EdgeInterface::UPtr
std::unique_ptr< EdgeInterface > UPtr
Definition: edge_interface.h:60
static_if
#define static_if
Definition: macros.h:64
int
return int(ret)+1
hyper_graph.h
Eigen::Map
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
Eigen::Ref
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:192

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