Loading...
Searching...
No Matches
RegularHessianFactor.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/RegularJacobianFactor.h>
#include <vector>
namespace gtsam {
template<size_t D>
class RegularHessianFactor: public HessianFactor {
public:
typedef Eigen::Matrix<double, D, 1> VectorD;
typedef Eigen::Matrix<double, D, D> MatrixD;
RegularHessianFactor(const KeyVector& js,
const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
HessianFactor(js, Gs, gs, f) {
checkInvariants();
}
RegularHessianFactor(Key j1, Key j2, const MatrixD& G11, const MatrixD& G12,
const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) :
HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) {
}
RegularHessianFactor(Key j1, Key j2, Key j3,
const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& g1,
const MatrixD& G22, const MatrixD& G23, const VectorD& g2,
const MatrixD& G33, const VectorD& g3, double f) :
HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) {
}
template<typename KEYS>
RegularHessianFactor(const KEYS& keys,
const SymmetricBlockMatrix& augmentedInformation) :
HessianFactor(keys, augmentedInformation) {
checkInvariants();
}
RegularHessianFactor(const RegularJacobianFactor<D>& jf)
: HessianFactor(jf) {}
RegularHessianFactor(const GaussianFactorGraph& factors,
const Scatter& scatter)
: HessianFactor(factors, scatter) {
checkInvariants();
}
RegularHessianFactor(const GaussianFactorGraph& factors)
: HessianFactor(factors) {
checkInvariants();
}
private:
void checkInvariants() {
if (info_.cols() != 1 + (info_.nBlocks()-1) * (DenseIndex)D)
throw std::invalid_argument(
"RegularHessianFactor constructor was given non-regular factors");
}
// Use Eigen magic to access raw memory
typedef Eigen::Map<VectorD> DMap;
typedef Eigen::Map<const VectorD> ConstDMap;
// Scratch space for multiplyHessianAdd
// According to link below this is thread-safe.
// http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe
mutable std::vector<VectorD> y_;
public:
void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const override {
HessianFactor::multiplyHessianAdd(alpha, x, y);
}
void multiplyHessianAdd(double alpha, const double* x,
double* yvalues) const {
// Create a vector of temporary y_ values, corresponding to rows i
y_.resize(size());
for(VectorD & yi: y_)
yi.setZero();
// Accessing the VectorValues one by one is expensive
// So we will loop over columns to access x only once per column
// And fill the above temporary y_ values, to be added into yvalues after
VectorD xj(D);
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
Key key = keys_[j];
const double* xj = x + key * D;
DenseIndex i = 0;
for (; i < j; ++i)
y_[i] += info_.aboveDiagonalBlock(i, j) * ConstDMap(xj);
// blocks on the diagonal are only half
y_[i] += info_.diagonalBlock(j) * ConstDMap(xj);
// for below diagonal, we take transpose block from upper triangular part
for (i = j + 1; i < (DenseIndex) size(); ++i)
y_[i] += info_.aboveDiagonalBlock(j, i).transpose() * ConstDMap(xj);
}
// copy to yvalues
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
Key key = keys_[i];
DMap(yvalues + key * D) += alpha * y_[i];
}
}
void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
std::vector<size_t> offsets) const {
// Create a vector of temporary y_ values, corresponding to rows i
y_.resize(size());
for(VectorD & yi: y_)
yi.setZero();
// Accessing the VectorValues one by one is expensive
// So we will loop over columns to access x only once per column
// And fill the above temporary y_ values, to be added into yvalues after
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
DenseIndex i = 0;
for (; i < j; ++i)
y_[i] += info_.aboveDiagonalBlock(i, j)
* ConstDMap(x + offsets[keys_[j]],
offsets[keys_[j] + 1] - offsets[keys_[j]]);
// blocks on the diagonal are only half
y_[i] += info_.diagonalBlock(j)
* ConstDMap(x + offsets[keys_[j]],
offsets[keys_[j] + 1] - offsets[keys_[j]]);
// for below diagonal, we take transpose block from upper triangular part
for (i = j + 1; i < (DenseIndex) size(); ++i)
y_[i] += info_.aboveDiagonalBlock(j, i).transpose()
* ConstDMap(x + offsets[keys_[j]],
offsets[keys_[j] + 1] - offsets[keys_[j]]);
}
// copy to yvalues
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i)
DMap(yvalues + offsets[keys_[i]],
offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y_[i];
}
void hessianDiagonal(double* d) const override {
// Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal
DMap(d + D * j) += info_.diagonal(pos);
}
}
void gradientAtZero(double* d) const override {
// Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal
DMap(d + D * j) -= info_.aboveDiagonalBlock(pos, size());;
}
}
/* ************************************************************************* */
};
// end class RegularHessianFactor
// traits
template<size_t D> struct traits<RegularHessianFactor<D> > : public Testable<
RegularHessianFactor<D> > {
};
}
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
RegularJacobianFactor.h
JacobianFactor class with fixed sized blcoks.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
gtsam::SymmetricBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition SymmetricBlockMatrix.h:117
gtsam::SymmetricBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition SymmetricBlockMatrix.h:120
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
gtsam::HessianFactor::augmentedInformation
Matrix augmentedInformation() const override
Return the augmented information matrix represented by this GaussianFactor.
Definition HessianFactor.cpp:282
gtsam::HessianFactor::info_
SymmetricBlockMatrix info_
The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'H[x -1].
Definition HessianFactor.h:104
gtsam::HessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
y += alpha * A'*A*x
Definition HessianFactor.cpp:391
gtsam::HessianFactor::HessianFactor
HessianFactor()
default constructor for I/O
Definition HessianFactor.cpp:77
gtsam::RegularHessianFactor
Definition RegularHessianFactor.h:28
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const GaussianFactorGraph &factors)
Construct from a GaussianFactorGraph.
Definition RegularHessianFactor.h:86
gtsam::RegularHessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
y += alpha * A'*A*x
Definition RegularHessianFactor.h:112
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(Key j1, Key j2, Key j3, const MatrixD &G11, const MatrixD &G12, const MatrixD &G13, const VectorD &g1, const MatrixD &G22, const MatrixD &G23, const VectorD &g2, const MatrixD &G33, const VectorD &g3, double f)
Construct a ternary factor.
Definition RegularHessianFactor.h:58
gtsam::RegularHessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const double *x, double *yvalues, std::vector< size_t > offsets) const
Raw memory version, with offsets TODO document reasoning.
Definition RegularHessianFactor.h:150
gtsam::RegularHessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const double *x, double *yvalues) const
y += alpha * A'*A*x
Definition RegularHessianFactor.h:118
gtsam::RegularHessianFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
Return the diagonal of the Hessian for this factor (raw memory version).
Definition RegularHessianFactor.h:185
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const KEYS &keys, const SymmetricBlockMatrix &augmentedInformation)
Constructor with an arbitrary number of keys and with the augmented information matrix specified as a...
Definition RegularHessianFactor.h:68
gtsam::RegularHessianFactor::gradientAtZero
void gradientAtZero(double *d) const override
Add gradient at zero to d TODO: is it really the goal to add ??
Definition RegularHessianFactor.h:196
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const GaussianFactorGraph &factors, const Scatter &scatter)
Construct from a GaussianFactorGraph.
Definition RegularHessianFactor.h:79
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const KeyVector &js, const std::vector< Matrix > &Gs, const std::vector< Vector > &gs, double f)
Construct an n-way factor.
Definition RegularHessianFactor.h:39
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(Key j1, Key j2, const MatrixD &G11, const MatrixD &G12, const VectorD &g1, const MatrixD &G22, const VectorD &g2, double f)
Construct a binary factor.
Definition RegularHessianFactor.h:49
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const RegularJacobianFactor< D > &jf)
Construct from RegularJacobianFactor.
Definition RegularHessianFactor.h:75
gtsam::RegularJacobianFactor
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition RegularJacobianFactor.h:32
gtsam::Scatter
Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
Definition Scatter.h:49
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74