Go to the documentation of this file.
41 using Dims = std::vector<Key>;
44 void HessianFactor::Allocate(
const Scatter& scatter) {
45 gttic(HessianFactor_Allocate);
48 const size_t n = scatter.size();
52 for(
const SlotEntry& slotentry: scatter) {
53 keys_[slot] = slotentry.key;
54 dims[slot] = slotentry.dimension;
62 HessianFactor::HessianFactor(
const Scatter& scatter) {
67 HessianFactor::HessianFactor() :
69 assert(info_.rows() == 1);
76 if (
G.rows() !=
G.cols() ||
G.rows() !=
g.size())
77 throw invalid_argument(
78 "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
79 info_.setDiagonalBlock(0,
G);
89 if (Sigma.rows() != Sigma.cols() || Sigma.rows() !=
mu.size())
90 throw invalid_argument(
91 "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
92 info_.setDiagonalBlock(0, Sigma.inverse());
93 linearTerm() = info_.diagonalBlock(0) *
mu;
94 constantTerm() =
mu.dot(linearTerm().
col(0));
102 Dims{
static_cast<Key>(G11.cols()),
static_cast<Key>(G22.cols()),1}) {
103 info_.setDiagonalBlock(0, G11);
104 info_.setOffDiagonalBlock(0, 1, G12);
105 info_.setDiagonalBlock(1, G22);
106 linearTerm() <<
g1,
g2;
116 Dims{
static_cast<Key>(G11.cols()),
static_cast<Key>(G22.cols()),
static_cast<Key>(G33.cols()),1}) {
117 if (G11.rows() != G11.cols() || G11.rows() != G12.rows()
118 || G11.rows() != G13.rows() || G11.rows() !=
g1.size()
119 || G22.cols() != G12.cols() || G33.cols() != G13.cols()
120 || G22.cols() !=
g2.size() || G33.cols() != g3.size())
121 throw invalid_argument(
122 "Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
123 info_.setDiagonalBlock(0, G11);
124 info_.setOffDiagonalBlock(0, 1, G12);
125 info_.setOffDiagonalBlock(0, 2, G13);
126 info_.setDiagonalBlock(1, G22);
127 info_.setOffDiagonalBlock(1, 2, G23);
128 info_.setDiagonalBlock(2, G33);
129 linearTerm() <<
g1,
g2, g3;
135 static std::vector<DenseIndex> _getSizeHFVec(
const std::vector<Vector>&
m) {
136 std::vector<DenseIndex> dims;
138 dims.push_back(
v.size());
146 const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs,
double f) :
149 size_t variable_count = js.
size();
152 if (gs.size() != variable_count
153 || Gs.size() != (variable_count * (variable_count + 1)) / 2)
154 throw invalid_argument(
155 "Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \
156 in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2");
160 for (
size_t i = 0;
i < variable_count; ++
i) {
163 for (
size_t j = 0;
j < variable_count -
i; ++
j) {
164 size_t index =
i * (2 * variable_count -
i + 1) / 2 +
j;
165 if (Gs[index].
rows() != block_size) {
166 throw invalid_argument(
167 "Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
171 for (
size_t j = 0;
j <=
i; ++
j) {
172 size_t index =
j * (2 * variable_count -
j + 1) / 2 + (
i -
j);
173 if (Gs[index].
cols() != block_size) {
174 throw invalid_argument(
175 "Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
182 for (
size_t i = 0;
i < variable_count; ++
i) {
183 for (
size_t j =
i;
j < variable_count; ++
j) {
199 gttic(HessianFactor_fromJacobian);
204 throw invalid_argument(
205 "Cannot construct HessianFactor from JacobianFactor with constrained noise model");
207 auto D = (jfModel->invsigmas().array() * jfModel->invsigmas().array()).
matrix().asDiagonal();
209 info.setFullMatrix(
A.transpose() *
D *
A);
211 info.setFullMatrix(
A.transpose() *
A);
220 _FromJacobianHelper(jf,
info_);
229 _FromJacobianHelper(*jf,
info_);
233 throw std::invalid_argument(
234 "In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor");
241 gttic(HessianFactor_MergeConstructor);
248 for(
const auto& factor:
factors)
263 "Augmented information matrix: ");
305 throw std::runtime_error(
306 "HessianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only");
311 map<Key, Matrix> blocks;
337 double xtg = 0, xGx = 0;
343 xGx =
x.transpose() * AtA *
x;
344 return 0.5 * (
f - 2.0 * xtg + xGx);
350 gttic(updateHessian_HessianFactor);
354 vector<DenseIndex> slots(nrVariablesInThisFactor + 1);
358 const bool rhs = (
j == nrVariablesInThisFactor);
392 y.push_back(Vector::Zero(
getDim(it)));
425 for (
size_t j = 0;
j <
n; ++
j)
433 throw std::runtime_error(
434 "HessianFactor::gradientAtZero raw memory access is allowed for Regular Factors only");
451 b += Gij *
x.at(*it_j);
460 gttic(HessianFactor_eliminateCholesky);
466 size_t nFrontals =
keys.size();
467 assert(nFrontals <=
size());
472 conditional = std::make_shared<GaussianConditional>(
keys_, nFrontals,
Ab);
478 cout <<
"Partial Cholesky on HessianFactor failed." << endl;
479 keys.print(
"Frontal keys ");
480 print(
"HessianFactor:");
491 gttic(HessianFactor_solve);
494 const size_t n =
size();
501 const Vector solution =
R.solve(eta);
516 std::pair<std::shared_ptr<GaussianConditional>, std::shared_ptr<HessianFactor> >
524 jointFactor = std::make_shared<HessianFactor>(
factors, scatter);
525 }
catch (std::invalid_argument&) {
527 "EliminateCholesky was called with a request to eliminate variables that are not\n"
528 "involved in the provided factors.");
532 auto conditional = jointFactor->eliminateCholesky(
keys);
535 return make_pair(conditional, jointFactor);
539 std::pair<std::shared_ptr<GaussianConditional>,
Linear Factor Graph where all factors are Gaussians.
Conditional Gaussian Base class.
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
typedef and functions to augment Eigen's VectorXd
Matrix augmentedInformation() const override
void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Set an off-diagonal block. Only the upper triangular portion of xpr is evaluated.
A Gaussian factor using the canonical parameters (information form)
static const double d[K][N]
const SharedDiagonal & get_model() const
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
typedef and functions to augment Eigen's MatrixXd
const GaussianFactorGraph factors
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
VectorValues gradientAtZero() const override
eta for Hessian
const_iterator find(Key key) const
find
const KeyFormatter & formatter
Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky.
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Contains the HessianFactor class, a general quadratic factor.
bool empty() const
Whether the factor is empty (involves zero variables).
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Eigen::SelfAdjointView< SymmetricBlockMatrix::constBlock, Eigen::Upper > informationView() const
Return self-adjoint view onto the information matrix (NOT augmented).
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Expression of a selfadjoint matrix from a triangular part of a dense matrix.
void setDiagonalBlock(DenseIndex I, const XprType &xpr)
Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
double error(const VectorValues &c) const override
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
const SymmetricBlockMatrix & info() const
Return underlying information matrix.
GaussianFactor::shared_ptr negate() const override
std::pair< Matrix, Vector > jacobian() const override
Returns (dense) A,b pair associated with factor, bakes in the weights.
const_iterator begin() const
std::shared_ptr< Factor > shared_ptr
A shared_ptr to this class.
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
Indicate Cholesky factorization failure.
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Exceptions that may be thrown by linear solver components.
SymmetricBlockMatrix info_
The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'H[x -1].
void print(const Matrix &A, const string &s, ostream &stream)
JacobiRotation< float > J
VectorValues solve()
Solve the system A'*A delta = A'*b in-place, return delta as VectorValues.
Matrix augmentedJacobian() const override
SymmetricBlockMatrix::constBlock linearTerm() const
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Pose3 g2(g1.expmap(h *V1_g1))
std::shared_ptr< This > shared_ptr
shared_ptr to this class
noiseModel::Diagonal::shared_ptr SharedDiagonal
static SymmetricBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix &other)
Vector gradient(Key key, const VectorValues &x) const override
Matrix augmentedJacobian() const override
std::pair< iterator, bool > tryInsert(Key j, const Vector &value)
DenseIndex nBlocks() const
Block count.
void hessianDiagonalAdd(VectorValues &d) const override
Add the current diagonal to a VectorValues instance.
const_iterator end() const
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
void g(const string &key, int i)
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
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 set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate offset
const gtsam::Symbol key('X', 0)
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Matrix information() const override
Eigen::TriangularView< constBlock, Eigen::Upper > triangularView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j) as a triangular view.
void updateDiagonalBlock(DenseIndex I, const XprType &xpr)
Increment the diagonal block by the values in xpr. Only reads the upper triangular part of xpr.
KeyVector::const_iterator const_iterator
Const iterator over keys.
std::pair< Matrix, Vector > jacobian() const override
Return (dense) matrix associated with factor.
KeyVector keys_
The keys involved in this factor.
void setZero()
Set the entire active matrix zero.
VerticalBlockMatrix split(DenseIndex nFrontals)
void Allocate(const Scatter &scatter)
Allocate for given scatter pattern.
const KeyVector & keys() const
Access the factor's involved variable keys.
void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const override
ptrdiff_t DenseIndex
The index type for Eigen objects.
bool equals(const This &other, double tol=1e-9) const
check equality
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
std::shared_ptr< GaussianConditional > eliminateCholesky(const Ordering &keys)
JacobiRotation< float > G
Vector diagonal(DenseIndex J) const
Get the diagonal of the J'th diagonal block.
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Array< int, Dynamic, 1 > v
static DenseIndex Slot(const CONTAINER &keys, Key key)
double constantTerm() const
DenseIndex getDim(DenseIndex block) const
Number of dimensions for variable on this diagonal block.
const VerticalBlockMatrix & matrixObject() const
Double_ distance(const OrientedPlane3_ &p)
A factor with a quadratic error function - a Gaussian.
Matrix block(DenseIndex I, DenseIndex J) const
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
bool hasConstraints(const GaussianFactorGraph &factors)
void choleskyPartial(DenseIndex nFrontals)
std::map< Key, Matrix > hessianBlockDiagonal() const override
Return the block diagonal of the Hessian for this factor.
std::uint64_t Key
Integer nonlinear key type.
One SlotEntry stores the slot index for a variable, as well its dim.
DenseIndex getDim(const_iterator variable) const override
Rot2 R(Rot2::fromAngle(0.1))
A thin wrapper around std::map that uses boost's fast_pool_allocator.
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:41