HessianFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 
25 #include <gtsam/base/cholesky.h>
26 #include <gtsam/base/debug.h>
27 #include <gtsam/base/FastMap.h>
28 #include <gtsam/base/Matrix.h>
30 #include <gtsam/base/timing.h>
31 
32 #include <sstream>
33 #include <limits>
34 #include "gtsam/base/Vector.h"
35 
36 using namespace std;
37 
38 namespace gtsam {
39 
40 // Typedefs used in constructors below.
41 using Dims = std::vector<Key>;
42 
43 /* ************************************************************************* */
44 void HessianFactor::Allocate(const Scatter& scatter) {
45  gttic(HessianFactor_Allocate);
46 
47  // Allocate with dimensions for each variable plus 1 at the end for the information vector
48  const size_t n = scatter.size();
49  keys_.resize(n);
50  FastVector<DenseIndex> dims(n + 1);
51  DenseIndex slot = 0;
52  for(const SlotEntry& slotentry: scatter) {
53  keys_[slot] = slotentry.key;
54  dims[slot] = slotentry.dimension;
55  ++slot;
56  }
57  dims.back() = 1;
58  info_ = SymmetricBlockMatrix(dims);
59 }
60 
61 /* ************************************************************************* */
62 HessianFactor::HessianFactor(const Scatter& scatter) {
63  Allocate(scatter);
64 }
65 
66 /* ************************************************************************* */
67 HessianFactor::HessianFactor() :
68  info_(Dims{1}) {
69  assert(info_.rows() == 1);
70  constantTerm() = 0.0;
71 }
72 
73 /* ************************************************************************* */
74 HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f)
75  : GaussianFactor(KeyVector{j}), info_(Dims{static_cast<Key>(G.cols()), 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);
80  linearTerm() = g;
81  constantTerm() = f;
82 }
83 
84 /* ************************************************************************* */
85 // error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu)
86 // where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g
88  : GaussianFactor(KeyVector{j}), info_(Dims{static_cast<Key>(Sigma.cols()), 1}) {
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()); // G
93  linearTerm() = info_.diagonalBlock(0) * mu; // g
94  constantTerm() = mu.dot(linearTerm().col(0)); // f
95 }
96 
97 /* ************************************************************************* */
99  const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2,
100  double f) :
101  GaussianFactor(KeyVector{j1,j2}), info_(
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;
107  constantTerm() = f;
108 }
109 
110 /* ************************************************************************* */
112  const Matrix& G12, const Matrix& G13, const Vector& g1, const Matrix& G22,
113  const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3,
114  double f) :
115  GaussianFactor(KeyVector{j1,j2,j3}), info_(
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;
130  constantTerm() = f;
131 }
132 
133 /* ************************************************************************* */
134 namespace {
135 static std::vector<DenseIndex> _getSizeHFVec(const std::vector<Vector>& m) {
136  std::vector<DenseIndex> dims;
137  for (const Vector& v : m) {
138  dims.push_back(v.size());
139  }
140  return dims;
141 }
142 } // namespace
143 
144 /* ************************************************************************* */
146  const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
147  GaussianFactor(js), info_(_getSizeHFVec(gs), true) {
148  // Get the number of variables
149  size_t variable_count = js.size();
150 
151  // Verify the provided number of entries in the vectors are consistent
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");
157 
158  // Verify the dimensions of each provided matrix are consistent
159  // Note: equations for calculating the indices derived from the "sum of an arithmetic sequence" formula
160  for (size_t i = 0; i < variable_count; ++i) {
161  DenseIndex block_size = gs[i].size();
162  // Check rows
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");
168  }
169  }
170  // Check cols
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");
176  }
177  }
178  }
179 
180  // Fill in the blocks
181  size_t index = 0;
182  for (size_t i = 0; i < variable_count; ++i) {
183  for (size_t j = i; j < variable_count; ++j) {
184  if (i == j) {
185  info_.setDiagonalBlock(i, Gs[index]);
186  } else {
187  info_.setOffDiagonalBlock(i, j, Gs[index]);
188  }
189  index++;
190  }
191  info_.setOffDiagonalBlock(i, variable_count, gs[i]);
192  }
193  constantTerm() = f;
194 }
195 
196 /* ************************************************************************* */
197 namespace {
198 void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) {
199  gttic(HessianFactor_fromJacobian);
200  const SharedDiagonal& jfModel = jf.get_model();
201  auto A = jf.matrixObject().full();
202  if (jfModel) {
203  if (jf.get_model()->isConstrained())
204  throw invalid_argument(
205  "Cannot construct HessianFactor from JacobianFactor with constrained noise model");
206 
207  auto D = (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal();
208 
209  info.setFullMatrix(A.transpose() * D * A);
210  } else {
211  info.setFullMatrix(A.transpose() * A);
212  }
213 }
214 }
215 
216 /* ************************************************************************* */
218  GaussianFactor(jf), info_(
219  SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) {
220  _FromJacobianHelper(jf, info_);
221 }
222 
223 /* ************************************************************************* */
225  GaussianFactor(gf) {
226  // Copy the matrix data depending on what type of factor we're copying from
227  if (const JacobianFactor* jf = dynamic_cast<const JacobianFactor*>(&gf)) {
229  _FromJacobianHelper(*jf, info_);
230  } else if (const HessianFactor* hf = dynamic_cast<const HessianFactor*>(&gf)) {
231  info_ = hf->info_;
232  } else {
233  throw std::invalid_argument(
234  "In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor");
235  }
236 }
237 
238 /* ************************************************************************* */
240  const Scatter& scatter) {
241  gttic(HessianFactor_MergeConstructor);
242 
243  Allocate(scatter);
244 
245  // Form A' * A
246  gttic(update);
247  info_.setZero();
248  for(const auto& factor: factors)
249  if (factor)
250  factor->updateHessian(keys_, &info_);
251  gttoc(update);
252 }
253 
254 /* ************************************************************************* */
255 void HessianFactor::print(const std::string& s,
256  const KeyFormatter& formatter) const {
257  cout << s << "\n";
258  cout << " keys: ";
259  for (const_iterator key = begin(); key != end(); ++key)
260  cout << formatter(*key) << "(" << getDim(key) << ") ";
261  cout << "\n";
263  "Augmented information matrix: ");
264 }
265 
266 /* ************************************************************************* */
267 bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
268  const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&lf);
269  if (!rhs || !Factor::equals(lf, tol))
270  return false;
272  tol);
273 }
274 
275 /* ************************************************************************* */
277  return info_.selfadjointView();
278 }
279 
280 /* ************************************************************************* */
283  return info_.selfadjointView(0, size());
284 }
285 
286 /* ************************************************************************* */
288  return informationView();
289 }
290 
291 /* ************************************************************************* */
293  for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
294  auto result = d.emplace(keys_[j], info_.diagonal(j));
295  if(!result.second) {
296  // if emplace fails, it returns an iterator to the existing element, which we add to:
297  result.first->second += info_.diagonal(j);
298  }
299  }
300 }
301 
302 /* ************************************************************************* */
303 // Raw memory access version should be called in Regular Factors only currently
304 void HessianFactor::hessianDiagonal(double* d) const {
305  throw std::runtime_error(
306  "HessianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only");
307 }
308 
309 /* ************************************************************************* */
310 map<Key, Matrix> HessianFactor::hessianBlockDiagonal() const {
311  map<Key, Matrix> blocks;
312  // Loop over all variables
313  for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
314  // Get the diagonal block, and insert it
315  blocks.emplace(keys_[j], info_.diagonalBlock(j));
316  }
317  return blocks;
318 }
319 
320 /* ************************************************************************* */
322  return JacobianFactor(*this).augmentedJacobian();
323 }
324 
325 /* ************************************************************************* */
326 std::pair<Matrix, Vector> HessianFactor::jacobian() const {
327  return JacobianFactor(*this).jacobian();
328 }
329 
330 /* ************************************************************************* */
331 double HessianFactor::error(const VectorValues& c) const {
332  // error 0.5*(f - 2*x'*g + x'*G*x)
333  const double f = constantTerm();
334  if (empty()) {
335  return 0.5 * f;
336  }
337  double xtg = 0, xGx = 0;
338  // extract the relevant subset of the VectorValues
339  // NOTE may not be as efficient
340  const Vector x = c.vector(keys());
341  xtg = x.dot(linearTerm().col(0));
342  auto AtA = informationView();
343  xGx = x.transpose() * AtA * x;
344  return 0.5 * (f - 2.0 * xtg + xGx);
345 }
346 
347 /* ************************************************************************* */
349  SymmetricBlockMatrix* info) const {
350  gttic(updateHessian_HessianFactor);
351  assert(info);
352  // Apply updates to the upper triangle
353  DenseIndex nrVariablesInThisFactor = size(), nrBlocksInInfo = info->nBlocks() - 1;
354  vector<DenseIndex> slots(nrVariablesInThisFactor + 1);
355  // Loop over this factor's blocks with indices (i,j)
356  // For every block (i,j), we determine the block (I,J) in info.
357  for (DenseIndex j = 0; j <= nrVariablesInThisFactor; ++j) {
358  const bool rhs = (j == nrVariablesInThisFactor);
359  const DenseIndex J = rhs ? nrBlocksInInfo : Slot(infoKeys, keys_[j]);
360  slots[j] = J;
361  for (DenseIndex i = 0; i <= j; ++i) {
362  const DenseIndex I = slots[i]; // because i<=j, slots[i] is valid.
363 
364  if (i == j) {
365  assert(I == J);
367  } else {
368  assert(i < j);
369  assert(I != J);
371  }
372  }
373  }
374 }
375 
376 /* ************************************************************************* */
378  shared_ptr result = std::make_shared<This>(*this);
379  // Negate the information matrix of the result
380  result->info_.negate();
381  return std::move(result);
382 }
383 
384 /* ************************************************************************* */
386  VectorValues& yvalues) const {
387 
388  // Create a vector of temporary y values, corresponding to rows i
389  vector<Vector> y;
390  y.reserve(size());
391  for (const_iterator it = begin(); it != end(); it++)
392  y.push_back(Vector::Zero(getDim(it)));
393 
394  // Accessing the VectorValues one by one is expensive
395  // So we will loop over columns to access x only once per column
396  // And fill the above temporary y values, to be added into yvalues after
397  for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
398  // xj is the input vector
399  Vector xj = x.at(keys_[j]);
400  DenseIndex i = 0;
401  for (; i < j; ++i)
402  y[i] += info_.aboveDiagonalBlock(i, j) * xj;
403 
404  // blocks on the diagonal are only half
405  y[i] += info_.diagonalBlock(j) * xj;
406  // for below diagonal, we take transpose block from upper triangular part
407  for (i = j + 1; i < (DenseIndex) size(); ++i)
408  y[i] += info_.aboveDiagonalBlock(j, i).transpose() * xj;
409  }
410 
411  // copy to yvalues
412  for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
413  const auto [it, didNotExist] = yvalues.tryInsert(keys_[i], Vector());
414  if (didNotExist)
415  it->second = alpha * y[i]; // init
416  else
417  it->second += alpha * y[i]; // add
418  }
419 }
420 
421 /* ************************************************************************* */
423  VectorValues g;
424  size_t n = size();
425  for (size_t j = 0; j < n; ++j)
426  g.emplace(keys_[j], -info_.aboveDiagonalBlock(j, n));
427  return g;
428 }
429 
430 /* ************************************************************************* */
431 // Raw memory access version should be called in Regular Factors only currently
432 void HessianFactor::gradientAtZero(double* d) const {
433  throw std::runtime_error(
434  "HessianFactor::gradientAtZero raw memory access is allowed for Regular Factors only");
435 }
436 
437 /* ************************************************************************* */
439  const Factor::const_iterator it_i = find(key);
440  const DenseIndex I = std::distance(begin(), it_i);
441 
442  // Sum over G_ij*xj for all xj connecting to xi
443  Vector b = Vector::Zero(x.at(key).size());
444  for (Factor::const_iterator it_j = begin(); it_j != end(); ++it_j) {
445  const DenseIndex J = std::distance(begin(), it_j);
446 
447  // Obtain Gij from the Hessian factor
448  // Hessian factor only stores an upper triangular matrix, so be careful when i>j
449  const Matrix Gij = info_.block(I, J);
450  // Accumulate Gij*xj to gradf
451  b += Gij * x.at(*it_j);
452  }
453  // Subtract the linear term gi
454  b += -linearTerm(it_i);
455  return b;
456 }
457 
458 /* ************************************************************************* */
459 std::shared_ptr<GaussianConditional> HessianFactor::eliminateCholesky(const Ordering& keys) {
460  gttic(HessianFactor_eliminateCholesky);
461 
463 
464  try {
465  // Do dense elimination
466  size_t nFrontals = keys.size();
467  assert(nFrontals <= size());
468  info_.choleskyPartial(nFrontals);
469 
470  // TODO(frank): pre-allocate GaussianConditional and write into it
471  const VerticalBlockMatrix Ab = info_.split(nFrontals);
472  conditional = std::make_shared<GaussianConditional>(keys_, nFrontals, Ab);
473 
474  // Erase the eliminated keys in this factor
475  keys_.erase(begin(), begin() + nFrontals);
476  } catch (const CholeskyFailed&) {
477 #ifndef NDEBUG
478  cout << "Partial Cholesky on HessianFactor failed." << endl;
479  keys.print("Frontal keys ");
480  print("HessianFactor:");
481 #endif
483  }
484 
485  // Return result
486  return conditional;
487 }
488 
489 /* ************************************************************************* */
491  gttic(HessianFactor_solve);
492 
493  // Do Cholesky Factorization
494  const size_t n = size();
495  assert(size_t(info_.nBlocks()) == n + 1);
497  auto R = info_.triangularView(0, n);
498  auto eta = linearTerm();
499 
500  // Solve
501  const Vector solution = R.solve(eta);
502 
503  // Parse into VectorValues
505  std::size_t offset = 0;
506  for (DenseIndex j = 0; j < (DenseIndex)n; ++j) {
507  const DenseIndex dim = info_.getDim(j);
508  delta.emplace(keys_[j], solution.segment(offset, dim));
509  offset += dim;
510  }
511 
512  return delta;
513 }
514 
515 /* ************************************************************************* */
516 std::pair<std::shared_ptr<GaussianConditional>, std::shared_ptr<HessianFactor> >
519 
520  // Build joint factor
521  HessianFactor::shared_ptr jointFactor;
522  try {
523  Scatter scatter(factors, keys);
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.");
529  }
530 
531  // Do dense elimination
532  auto conditional = jointFactor->eliminateCholesky(keys);
533 
534  // Return result
535  return make_pair(conditional, jointFactor);
536 }
537 
538 /* ************************************************************************* */
539 std::pair<std::shared_ptr<GaussianConditional>,
540  std::shared_ptr<GaussianFactor> > EliminatePreferCholesky(
541  const GaussianFactorGraph& factors, const Ordering& keys) {
543 
544  // If any JacobianFactors have constrained noise models, we have to convert
545  // all factors to JacobianFactors. Otherwise, we can convert all factors
546  // to HessianFactors. This is because QR can handle constrained noise
547  // models but Cholesky cannot.
548  if (hasConstraints(factors))
549  return EliminateQR(factors, keys);
550  else
551  return EliminateCholesky(factors, keys);
552 }
553 
554 } // gtsam
gttoc
#define gttoc(label)
Definition: timing.h:296
timing.h
Timing utilities.
relicense.update
def update(text)
Definition: relicense.py:46
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
GaussianConditional.h
Conditional Gaussian Base class.
col
m col(1)
D
MatrixXcd D
Definition: EigenSolver_EigenSolver_MatrixType.cpp:14
gtsam::HessianFactor::equals
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
Definition: HessianFactor.cpp:267
Vector.h
typedef and functions to augment Eigen's VectorXd
gtsam::HessianFactor::augmentedInformation
Matrix augmentedInformation() const override
Definition: HessianFactor.cpp:276
gtsam::SymmetricBlockMatrix::setOffDiagonalBlock
void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Set an off-diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition: SymmetricBlockMatrix.h:203
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
s
RealScalar s
Definition: level1_cplx_impl.h:126
d
static const double d[K][N]
Definition: igam.h:11
gtsam::JacobianFactor::get_model
const SharedDiagonal & get_model() const
Definition: JacobianFactor.h:292
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
g1
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
mu
double mu
Definition: testBoundingConstraint.cpp:37
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
Matrix.h
typedef and functions to augment Eigen's MatrixXd
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
x
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
Definition: gnuplot_common_settings.hh:12
gtsam::HessianFactor::gradientAtZero
VectorValues gradientAtZero() const override
eta for Hessian
Definition: HessianFactor.cpp:422
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::GaussianFactor
Definition: GaussianFactor.h:38
gtsam::HessianFactor::HessianFactor
HessianFactor()
Definition: HessianFactor.cpp:67
gtsam::Factor::find
const_iterator find(Key key) const
find
Definition: Factor.h:140
formatter
const KeyFormatter & formatter
Definition: treeTraversal-inst.h:204
cholesky.h
Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky.
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
gtsam::Factor::empty
bool empty() const
Whether the factor is empty (involves zero variables).
Definition: Factor.h:131
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition: SymmetricBlockMatrix.h:158
gtsam::VerticalBlockMatrix::full
Block full()
Definition: VerticalBlockMatrix.h:157
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::HessianFactor::informationView
Eigen::SelfAdjointView< SymmetricBlockMatrix::constBlock, Eigen::Upper > informationView() const
Return self-adjoint view onto the information matrix (NOT augmented).
Definition: HessianFactor.cpp:282
gtsam::HessianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition: HessianFactor.h:109
Eigen::SelfAdjointView
Expression of a selfadjoint matrix from a triangular part of a dense matrix.
Definition: SelfAdjointView.h:49
gtsam::SymmetricBlockMatrix::setDiagonalBlock
void setDiagonalBlock(DenseIndex I, const XprType &xpr)
Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition: SymmetricBlockMatrix.h:197
gtsam::HessianFactor::error
double error(const VectorValues &c) const override
Definition: HessianFactor.cpp:331
gtsam::HessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
Definition: HessianFactor.cpp:385
gtsam::HessianFactor::info
const SymmetricBlockMatrix & info() const
Return underlying information matrix.
Definition: HessianFactor.h:264
gtsam::HessianFactor::negate
GaussianFactor::shared_ptr negate() const override
Definition: HessianFactor.cpp:377
gtsam::JacobianFactor::jacobian
std::pair< Matrix, Vector > jacobian() const override
Returns (dense) A,b pair associated with factor, bakes in the weights.
Definition: JacobianFactor.cpp:714
gtsam::Factor::begin
const_iterator begin() const
Definition: Factor.h:146
gtsam::Factor::shared_ptr
std::shared_ptr< Factor > shared_ptr
A shared_ptr to this class.
Definition: Factor.h:76
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
result
Values result
Definition: OdometryOptimize.cpp:8
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
gtsam::GaussianFactor::hessianDiagonal
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
Definition: GaussianFactor.cpp:35
gtsam::CholeskyFailed
Indicate Cholesky factorization failure.
Definition: ThreadsafeException.h:115
gtsam::EliminatePreferCholesky
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: HessianFactor.cpp:540
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::Dims
std::vector< Key > Dims
Definition: HessianFactor.cpp:41
linearExceptions.h
Exceptions that may be thrown by linear solver components.
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:103
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
J
JacobiRotation< float > J
Definition: Jacobi_makeJacobi.cpp:3
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::VectorValues
Definition: VectorValues.h:74
ThreadsafeException.h
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::HessianFactor::solve
VectorValues solve()
Solve the system A'*A delta = A'*b in-place, return delta as VectorValues.
Definition: HessianFactor.cpp:490
gtsam::HessianFactor::augmentedJacobian
Matrix augmentedJacobian() const override
Definition: HessianFactor.cpp:321
I
#define I
Definition: main.h:112
gtsam::HessianFactor::linearTerm
SymmetricBlockMatrix::constBlock linearTerm() const
Definition: HessianFactor.h:250
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
g2
Pose3 g2(g1.expmap(h *V1_g1))
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::InvalidDenseElimination
Definition: linearExceptions.h:134
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
gtsam::SymmetricBlockMatrix::LikeActiveViewOf
static SymmetricBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix &other)
Definition: SymmetricBlockMatrix.cpp:34
gtsam::HessianFactor::gradient
Vector gradient(Key key, const VectorValues &x) const override
Definition: HessianFactor.cpp:438
gtsam::JacobianFactor::augmentedJacobian
Matrix augmentedJacobian() const override
Definition: JacobianFactor.cpp:730
info
else if n * info
Definition: 3rdparty/Eigen/lapack/cholesky.cpp:18
gtsam::Scatter
Definition: Scatter.h:49
gtsam::VectorValues::tryInsert
std::pair< iterator, bool > tryInsert(Key j, const Vector &value)
Definition: VectorValues.h:212
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::SymmetricBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition: SymmetricBlockMatrix.h:122
gtsam::HessianFactor::hessianDiagonalAdd
void hessianDiagonalAdd(VectorValues &d) const override
Add the current diagonal to a VectorValues instance.
Definition: HessianFactor.cpp:292
gtsam::Factor::end
const_iterator end() const
Definition: Factor.h:149
size_t
std::size_t size_t
Definition: wrap/pybind11/include/pybind11/detail/common.h:490
gtsam::HessianFactor::print
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
Definition: HessianFactor.cpp:255
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
y
Scalar * y
Definition: level1_cplx_impl.h:124
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
offset
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
Definition: gnuplot_common_settings.hh:64
key
const gtsam::Symbol key('X', 0)
JacobianFactor.h
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::HessianFactor::information
Matrix information() const override
Definition: HessianFactor.cpp:287
gtsam::SymmetricBlockMatrix::triangularView
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.
Definition: SymmetricBlockMatrix.h:165
gtsam::b
const G & b
Definition: Group.h:79
gtsam::SymmetricBlockMatrix::updateDiagonalBlock
void updateDiagonalBlock(DenseIndex I, const XprType &xpr)
Increment the diagonal block by the values in xpr. Only reads the upper triangular part of xpr.
Definition: SymmetricBlockMatrix.h:214
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:83
gtsam::HessianFactor::jacobian
std::pair< Matrix, Vector > jacobian() const override
Return (dense) matrix associated with factor.
Definition: HessianFactor.cpp:326
gtsam
traits
Definition: SFMdata.h:40
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:88
gtsam::SymmetricBlockMatrix::setZero
void setZero()
Set the entire active matrix zero.
Definition: SymmetricBlockMatrix.h:260
gtsam::SymmetricBlockMatrix::split
VerticalBlockMatrix split(DenseIndex nFrontals)
Definition: SymmetricBlockMatrix.cpp:92
gtsam::HessianFactor::Allocate
void Allocate(const Scatter &scatter)
Allocate for given scatter pattern.
Definition: HessianFactor.cpp:44
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:143
gtsam::HessianFactor::updateHessian
void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const override
Definition: HessianFactor.cpp:348
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:103
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42
gtsam::SymmetricBlockMatrix::updateOffDiagonalBlock
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Definition: SymmetricBlockMatrix.h:230
std
Definition: BFloat16.h:88
gtsam::HessianFactor::eliminateCholesky
std::shared_ptr< GaussianConditional > eliminateCholesky(const Ordering &keys)
Definition: HessianFactor.cpp:459
G
JacobiRotation< float > G
Definition: Jacobi_makeGivens.cpp:2
gtsam::SymmetricBlockMatrix::diagonal
Vector diagonal(DenseIndex J) const
Get the diagonal of the J'th diagonal block.
Definition: SymmetricBlockMatrix.h:147
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition: SymmetricBlockMatrix.h:137
gtsam::GaussianConditional::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianConditional.h:46
exampleQR::Ab
Matrix Ab
Definition: testNoiseModel.cpp:207
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::HessianFactor::rows
size_t rows() const
Definition: HessianFactor.h:217
gtsam::GaussianFactor::Slot
static DenseIndex Slot(const CONTAINER &keys, Key key)
Definition: GaussianFactor.h:175
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::HessianFactor::constantTerm
double constantTerm() const
Definition: HessianFactor.h:229
gtsam::SymmetricBlockMatrix::getDim
DenseIndex getDim(DenseIndex block) const
Number of dimensions for variable on this diagonal block.
Definition: SymmetricBlockMatrix.h:125
gtsam::JacobianFactor::matrixObject
const VerticalBlockMatrix & matrixObject() const
Definition: JacobianFactor.h:257
gtsam::distance
Double_ distance(const OrientedPlane3_ &p)
Definition: slam/expressions.h:117
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
gtsam::SymmetricBlockMatrix::block
Matrix block(DenseIndex I, DenseIndex J) const
Definition: SymmetricBlockMatrix.cpp:60
gtsam::IndeterminantLinearSystemException
Definition: linearExceptions.h:94
gtsam::SymmetricBlockMatrix::aboveDiagonalBlock
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition: SymmetricBlockMatrix.h:152
gtsam::EliminateCholesky
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: HessianFactor.cpp:517
cols
int cols
Definition: Tutorial_commainit_02.cpp:1
gtsam::hasConstraints
bool hasConstraints(const GaussianFactorGraph &factors)
Definition: GaussianFactorGraph.cpp:442
gtsam::SymmetricBlockMatrix::choleskyPartial
void choleskyPartial(DenseIndex nFrontals)
Definition: SymmetricBlockMatrix.cpp:83
gtsam::HessianFactor::hessianBlockDiagonal
std::map< Key, Matrix > hessianBlockDiagonal() const override
Return the block diagonal of the Hessian for this factor.
Definition: HessianFactor.cpp:310
gtsam::Factor::size
size_t size() const
Definition: Factor.h:160
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:53
gtsam::SlotEntry
One SlotEntry stores the slot index for a variable, as well its dim.
Definition: Scatter.h:32
gtsam::Ordering
Definition: inference/Ordering.h:33
j1
double j1(double x)
Definition: j1.c:174
gtsam::HessianFactor::getDim
DenseIndex getDim(const_iterator variable) const override
Definition: HessianFactor.h:212
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
R
Rot2 R(Rot2::fromAngle(0.1))
gttic
#define gttic(label)
Definition: timing.h:295
debug.h
Global debugging flags.
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: JacobianFactor.cpp:778


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:41