40 const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs,
double f) :
50 const VectorD&
g1,
const MatrixD& G22,
const VectorD&
g2,
double f) :
59 const MatrixD& G11,
const MatrixD& G12,
const MatrixD& G13,
const VectorD&
g1,
60 const MatrixD& G22,
const MatrixD& G23,
const VectorD&
g2,
61 const MatrixD& G33,
const VectorD& g3,
double f) :
62 HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) {
67 template<
typename KEYS>
96 throw std::invalid_argument(
97 "RegularHessianFactor constructor was given non-regular factors");
107 mutable std::vector<VectorD>
y_;
119 double* yvalues)
const {
122 for(VectorD & yi: y_)
131 const double* xj = x + key *
D;
145 DMap(yvalues + key *
D) += alpha * y_[
i];
151 std::vector<size_t>
offsets)
const {
155 for(VectorD & yi: y_)
181 offsets[
keys_[i] + 1] - offsets[
keys_[i]]) += alpha * y_[
i];
213 RegularHessianFactor<D> > {
Eigen::Map< VectorD > DMap
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
A matrix or vector expression mapping an existing array of data.
std::vector< VectorD > y_
KeyVector keys_
The keys involved in this factor.
GaussianFactorGraph factors(list_of(factor1)(factor2)(factor3))
void checkInvariants()
Check invariants after construction.
void hessianDiagonal(double *d) const override
ptrdiff_t DenseIndex
The index type for Eigen objects.
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)
Matrix augmentedInformation() const override
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
void multiplyHessianAdd(double alpha, const double *x, double *yvalues, std::vector< size_t > offsets) const
Raw memory version, with offsets TODO document reasoning.
RegularHessianFactor(const RegularJacobianFactor< D > &jf)
Construct from RegularJacobianFactor.
Eigen::Map< const VectorD > ConstDMap
Vector diagonal(DenseIndex J) const
Get the diagonal of the J'th diagonal block.
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Contains the HessianFactor class, a general quadratic factor.
JacobianFactor class with fixed sized blcoks.
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
SymmetricBlockMatrix info_
The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'H[x -1]...
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set offsets
RegularHessianFactor(const KEYS &keys, const SymmetricBlockMatrix &augmentedInformation)
RegularHessianFactor(const GaussianFactorGraph &factors, const Scatter &scatter)
Construct from a GaussianFactorGraph.
const KeyVector & keys() const
Access the factor's involved variable keys.
void multiplyHessianAdd(double alpha, const double *x, double *yvalues) const
DenseIndex cols() const
Column size.
Eigen::Matrix< double, D, 1 > VectorD
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
A Gaussian factor using the canonical parameters (information form)
RegularHessianFactor(const KeyVector &js, const std::vector< Matrix > &Gs, const std::vector< Vector > &gs, double f)
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
void gradientAtZero(double *d) const override
Add gradient at zero to d TODO: is it really the goal to add ??
RegularHessianFactor(const GaussianFactorGraph &factors)
Construct from a GaussianFactorGraph.
The matrix class, also used for vectors and row-vectors.
DenseIndex nBlocks() const
Block count.
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Pose3 g2(g1.expmap(h *V1_g1))
Eigen::Matrix< double, D, D > MatrixD
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
std::uint64_t Key
Integer nonlinear key type.
RegularHessianFactor(Key j1, Key j2, const MatrixD &G11, const MatrixD &G12, const VectorD &g1, const MatrixD &G22, const VectorD &g2, double f)