Generic edge for functions .
More...
#include <generic_edge.h>
Public Types | |
using | ExtFunT = std::function< double(const VertexContainer &)> |
using | ExtJacobT = std::function< void(const VertexContainer &, int, Eigen::Ref< Eigen::MatrixXd >, const double *)> |
using | Ptr = std::shared_ptr< EdgeGenericScalarFun > |
using | UPtr = std::unique_ptr< EdgeGenericScalarFun > |
![]() | |
using | ConstPtr = std::shared_ptr< const Edge > |
using | Ptr = std::shared_ptr< Edge > |
using | UPtr = std::unique_ptr< Edge > |
using | VertexContainer = std::array< VertexInterface *, numVerticesCompileTime > |
Typedef to represent the vertex container. More... | |
![]() | |
using | Ptr = std::shared_ptr< BaseEdge > |
using | UPtr = std::unique_ptr< BaseEdge > |
![]() | |
using | Ptr = std::shared_ptr< EdgeInterface > |
using | UPtr = std::unique_ptr< EdgeInterface > |
Public Member Functions | |
void | computeJacobian (int vtx_idx, Eigen::Ref< Eigen::MatrixXd > block_jacobian, const double *multiplier=nullptr) override |
Compute edge block jacobian for a given vertex. More... | |
void | computeValues (Eigen::Ref< Eigen::VectorXd > values) override |
Compute function values. More... | |
EdgeGenericScalarFun (const ExtFunT &fun, bool lsq_form, VerticesT &... vertices) | |
Construct generic scalar function edge by passing the function object and all vertices. More... | |
EdgeGenericScalarFun (const ExtFunT &fun, const ExtJacobT &jacobian, bool lsq_form, VerticesT &... vertices) | |
Construct generic scalar function edge by passing the function object, a jacobian, and all vertices. More... | |
int | getDimension () const override |
Get dimension of the edge (dimension of the cost-function/constraint value vector) More... | |
bool | isLeastSquaresForm () const override |
Defines if the edge is formulated as Least-Squares form. More... | |
bool | isLinear () const override |
Return true if the edge is linear (and hence its Hessian is always zero) More... | |
bool | providesJacobian () const override |
Return true if a custom Jacobian is provided (e.g. computeJacobian() is overwritten) More... | |
void | setLinear (bool linear) |
![]() | |
Edge ()=delete | |
Edge (VerticesT &... args) | |
Construct edge by providing connected vertices. More... | |
int | getNumVertices () const override |
Return number of attached vertices. More... | |
const VertexInterface * | getVertex (int idx) const override |
VertexInterface * | getVertexRaw (int idx) override |
Get access to vertex with index idx (0 <= idx < numVertices) More... | |
bool | providesJacobian () const override |
Return true if a custom Jacobian is provided (e.g. computeJacobian() is overwritten) More... | |
int | verticesDimension () const override |
Return the combined dimension of all attached vertices (excluding fixed vertex components) More... | |
![]() | |
virtual void | computeHessian (int vtx_idx_i, int vtx_idx_j, const Eigen::Ref< const Eigen::MatrixXd > &block_jacobian_i, Eigen::Ref< Eigen::MatrixXd > block_hessian_ij, const double *multipliers=nullptr, double weight=1.0) |
virtual void | computeHessianInc (int vtx_idx_i, int vtx_idx_j, const Eigen::Ref< const Eigen::MatrixXd > &block_jacobian_i, Eigen::Ref< Eigen::MatrixXd > block_hessian_ij, const double *multipliers=nullptr, double weight=1.0) |
Compute edge block Hessian for a given vertex pair. More... | |
virtual void | computeHessianInc (int vtx_idx_i, int vtx_idx_j, Eigen::Ref< Eigen::MatrixXd > block_hessian_ij, const double *multipliers=nullptr, double weight=1.0) |
int | getEdgeIdx () const |
Retrieve current edge index (warning, this value is determined within the related HyperGraph) More... | |
virtual bool | providesHessian () const |
Return true if a custom Hessian is provided (e.g. computeHessian() is overwritten) More... | |
void | reserveCacheMemory (int num_value_vectors, int num_jacobians) |
void | reserveValuesCacheMemory (int num_value_vectors) |
void | reserveJacobiansCacheMemory (int num_jacobians) |
void | computeValuesCached () |
Call computeValues() and store result to previously allocated internal cache (call allocateInteralValuesCache() first once) More... | |
void | computeSquaredNormOfValuesCached () |
compute the specialied squared-norm method for computing the values (note only the first element in the values cache is used) More... | |
EdgeCache & | getCache () |
Retreive values computed previously via computeValuesCached() More... | |
const EdgeCache & | getCache () const |
![]() | |
virtual double | computeSquaredNormOfValues () |
virtual double | computeSumOfValues () |
int | getNumFiniteVerticesLowerBounds () const |
int | getNumFiniteVerticesUpperBounds () const |
virtual | ~EdgeInterface () |
Virtual destructor. More... | |
Protected Attributes | |
ExtFunT | _fun |
Store the external function or functor. More... | |
ExtJacobT | _jacob |
bool | _linear = false |
bool | _lsq_form |
![]() | |
const VertexContainer | _vertices |
Vertex container. More... | |
![]() | |
EdgeCache | _cache |
int | _edge_idx = 0 |
Additional Inherited Members | |
![]() | |
static constexpr const int | numVerticesCompileTime |
Return number of vertices at compile-time. More... | |
Generic edge for functions .
This edge can be used for passing a generic function (e.g. a lambda) to the optimization problem. Jacobians and Hessians are calculated numerically if necessary.
This edge should be only used for Rapid-Prototyping puroposes, due to the computational overhead that rises from copying and calling the external function resp. functor.
You can pass a function that accepts a EdgeGenericScalarFun::VertexContainer as argument. Each vertex stored in that container corresponds to the vertex passed using the class constructor (in the same order). The corresponding Vertex types are specified as template arguments of this class as well.
Example usage for defining (x-1)^2 (Note, keep in mind the special definitions for squaring within different solvers):
Definition at line 68 of file generic_edge.h.
using corbo::EdgeGenericScalarFun< VerticesT >::ExtFunT = std::function<double(const VertexContainer&)> |
Definition at line 76 of file generic_edge.h.
using corbo::EdgeGenericScalarFun< VerticesT >::ExtJacobT = std::function<void(const VertexContainer&, int, Eigen::Ref<Eigen::MatrixXd>, const double*)> |
Definition at line 77 of file generic_edge.h.
using corbo::EdgeGenericScalarFun< VerticesT >::Ptr = std::shared_ptr<EdgeGenericScalarFun> |
Definition at line 79 of file generic_edge.h.
using corbo::EdgeGenericScalarFun< VerticesT >::UPtr = std::unique_ptr<EdgeGenericScalarFun> |
Definition at line 80 of file generic_edge.h.
|
inline |
Construct generic scalar function edge by passing the function object and all vertices.
Definition at line 83 of file generic_edge.h.
|
inline |
Construct generic scalar function edge by passing the function object, a jacobian, and all vertices.
Definition at line 88 of file generic_edge.h.
|
inlineoverridevirtual |
Compute edge block jacobian for a given vertex.
This interface class provides a numerical computation of the Jacobian using finite differences. However, a user-defined implementation might be specified by overwriting this method. In that case do not forget to also overwrite providesJacobian().
[in] | vtx_idx | Vertex number for which the block jacobian should be computed |
[out] | block_jacobian | Resulting block jacobian [dimension() x VertexInterface::dimensionUnfixed()] (must be preallocated) |
[in] | multipliers | Scale each value component by an optional multiplier if argument is not null null. |
Reimplemented from corbo::BaseEdge.
Definition at line 105 of file generic_edge.h.
|
inlineoverridevirtual |
Compute function values.
Here, the actual cost/constraint function values are computed:
[in] | values | values should be stored here according to getDimension(). |
Implements corbo::Edge< VerticesT... >.
Definition at line 103 of file generic_edge.h.
|
inlineoverridevirtual |
Get dimension of the edge (dimension of the cost-function/constraint value vector)
Implements corbo::Edge< VerticesT... >.
Definition at line 93 of file generic_edge.h.
|
inlineoverridevirtual |
Defines if the edge is formulated as Least-Squares form.
Least-squares cost terms are defined as and the function values and Jacobian are computed for
rather than for
. Specialiezed least-squares solvers require the optimization problem to be defined in this particular form. Other solvers can automatically compute the square of least-squares edges if required. However, the other way round is more restrictive: general solvers might not cope with non-least-squares forms.
Note, in the LS-form case computeValues() computes e(x) and otherwise f(x).
Reimplemented from corbo::BaseEdge.
Definition at line 98 of file generic_edge.h.
|
inlineoverridevirtual |
Return true if the edge is linear (and hence its Hessian is always zero)
Implements corbo::Edge< VerticesT... >.
Definition at line 96 of file generic_edge.h.
|
inlineoverridevirtual |
Return true if a custom Jacobian is provided (e.g. computeJacobian() is overwritten)
Reimplemented from corbo::BaseEdge.
Definition at line 100 of file generic_edge.h.
|
inline |
Definition at line 114 of file generic_edge.h.
|
protected |
Store the external function or functor.
Definition at line 117 of file generic_edge.h.
|
protected |
Definition at line 118 of file generic_edge.h.
|
protected |
Definition at line 120 of file generic_edge.h.
|
protected |
Definition at line 119 of file generic_edge.h.