Loading...
Searching...
No Matches
RegularJacobianFactor.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/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam {
template<size_t D>
class RegularJacobianFactor: public JacobianFactor {
private:
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
public:
RegularJacobianFactor() {}
template<typename TERMS>
RegularJacobianFactor(const TERMS& terms, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) :
JacobianFactor(terms, b, model) {
}
template<typename KEYS>
RegularJacobianFactor(const KEYS& keys,
const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas =
SharedDiagonal()) :
JacobianFactor(keys, augmentedMatrix, sigmas) {
}
using JacobianFactor::multiplyHessianAdd;
void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const override {
JacobianFactor::multiplyHessianAdd(alpha, x, y);
}
void multiplyHessianAdd(double alpha, const double* x, double* y) const {
if (empty())
return;
Vector Ax = Vector::Zero(Ab_.rows());
// Just iterate over all A matrices and multiply in correct config part
for (size_t pos = 0; pos < size(); ++pos)
Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
// Deal with noise properly, need to Double* whiten as we are dividing by variance
if (model_) {
model_->whitenInPlace(Ax);
model_->whitenInPlace(Ax);
}
// multiply with alpha
Ax *= alpha;
// Again iterate over all A matrices and insert Ai^e into y
for (size_t pos = 0; pos < size(); ++pos)
DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
}
using GaussianFactor::hessianDiagonal;
void hessianDiagonal(double* d) const override {
// Loop over all variables in the factor
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
// Get the diagonal block, and insert its diagonal
DVector dj;
for (size_t k = 0; k < D; ++k) {
if (model_) {
Vector column_k = Ab_(j).col(k);
column_k = model_->whiten(column_k);
dj(k) = dot(column_k, column_k);
} else {
dj(k) = Ab_(j).col(k).squaredNorm();
}
}
DMap(d + D * j) += dj;
}
}
VectorValues gradientAtZero() const override {
return JacobianFactor::gradientAtZero();
}
void gradientAtZero(double* d) const override {
// Get vector b not weighted
Vector b = getb();
// Whitening b
if (model_) {
b = model_->whiten(b);
b = model_->whiten(b);
}
// Just iterate over all A matrices
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
DVector dj;
// gradient -= A'*b/sigma^2
// Computing with each column
for (size_t k = 0; k < D; ++k) {
Vector column_k = Ab_(j).col(k);
dj(k) = -1.0 * dot(b, column_k);
}
DMap(d + D * j) += dj;
}
}
void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const {
Vector E = alpha * (model_ ? model_->whiten(e) : e);
// Just iterate over all A matrices and insert Ai^e into y
for (size_t pos = 0; pos < size(); ++pos)
DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E;
}
Vector operator*(const double* x) const {
Vector Ax = Vector::Zero(Ab_.rows());
if (empty())
return Ax;
// Just iterate over all A matrices and multiply in correct config part
for (size_t pos = 0; pos < size(); ++pos)
Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
return model_ ? model_->whiten(Ax) : Ax;
}
};
// end class RegularJacobianFactor
}// end namespace gtsam
VectorValues.h
Factor Graph Values.
JacobianFactor.h
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::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
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::empty
bool empty() const
Whether the factor is empty (involves zero variables).
Definition Factor.h:128
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
gtsam::GaussianFactor::hessianDiagonal
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
Definition GaussianFactor.cpp:35
gtsam::JacobianFactor::getb
const constBVector getb() const
Get a view of the r.h.s.
Definition JacobianFactor.h:297
gtsam::JacobianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
y += alpha * A'*A*x
Definition JacobianFactor.cpp:649
gtsam::JacobianFactor::JacobianFactor
JacobianFactor(const GaussianFactor &gf)
Convert from other GaussianFactor.
Definition JacobianFactor.cpp:60
gtsam::JacobianFactor::gradientAtZero
VectorValues gradientAtZero() const override
A'*b for Jacobian.
Definition JacobianFactor.cpp:701
gtsam::RegularJacobianFactor::operator*
Vector operator*(const double *x) const
double* Matrix-vector multiply, i.e.
Definition RegularJacobianFactor.h:172
gtsam::RegularJacobianFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
Raw memory access version of hessianDiagonal.
Definition RegularJacobianFactor.h:109
gtsam::RegularJacobianFactor::gradientAtZero
void gradientAtZero(double *d) const override
Raw memory access version of gradientAtZero.
Definition RegularJacobianFactor.h:133
gtsam::RegularJacobianFactor::transposeMultiplyAdd
void transposeMultiplyAdd(double alpha, const Vector &e, double *x) const
double* Transpose Matrix-vector multiply, i.e.
Definition RegularJacobianFactor.h:161
gtsam::RegularJacobianFactor::gradientAtZero
VectorValues gradientAtZero() const override
Expose base class gradientAtZero.
Definition RegularJacobianFactor.h:128
gtsam::RegularJacobianFactor::RegularJacobianFactor
RegularJacobianFactor(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Construct an n-ary factor.
Definition RegularJacobianFactor.h:52
gtsam::RegularJacobianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
y += alpha * A'*A*x
Definition RegularJacobianFactor.h:73
gtsam::RegularJacobianFactor::RegularJacobianFactor
RegularJacobianFactor()
Default constructor.
Definition RegularJacobianFactor.h:44
gtsam::RegularJacobianFactor::RegularJacobianFactor
RegularJacobianFactor(const KEYS &keys, const VerticalBlockMatrix &augmentedMatrix, const SharedDiagonal &sigmas=SharedDiagonal())
Constructor with arbitrary number keys, and where the augmented matrix is given all together instead ...
Definition RegularJacobianFactor.h:64
gtsam::RegularJacobianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const double *x, double *y) const
double* Hessian-vector multiply, i.e.
Definition RegularJacobianFactor.h:82
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74