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<Eigen::Index>;
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{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");
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{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{G11.cols(),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{G11.cols(),G22.cols(),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)) {
228  info_ = SymmetricBlockMatrix::LikeActiveViewOf(jf->matrixObject());
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
482  throw IndeterminantLinearSystemException(keys.front());
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
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
const gtsam::Symbol key('X', 0)
Matrix3f m
bool empty() const
Whether the factor is empty (involves zero variables).
Definition: Factor.h:130
One SlotEntry stores the slot index for a variable, as well its dim.
Definition: Scatter.h:32
std::shared_ptr< This > shared_ptr
shared_ptr to this class
VectorValues gradientAtZero() const override
eta for Hessian
#define I
Definition: main.h:112
VectorValues solve()
Solve the system A&#39;*A delta = A&#39;*b in-place, return delta as VectorValues.
Scalar * y
def update(text)
Definition: relicense.py:46
Global debugging flags.
void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Set an off-diagonal block. Only the upper triangular portion of xpr is evaluated. ...
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
const VerticalBlockMatrix & matrixObject() const
JacobiRotation< float > G
size_t size() const
Definition: Factor.h:159
double mu
std::vector< Eigen::Index > Dims
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
int n
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
A factor with a quadratic error function - a Gaussian.
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
Rot2 R(Rot2::fromAngle(0.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 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 GaussianFactorGraph factors
Double_ distance(const OrientedPlane3_ &p)
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:87
Definition: BFloat16.h:88
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
Vector & at(Key j)
Definition: VectorValues.h:139
Matrix augmentedJacobian() const override
static SymmetricBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix &other)
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:108
void g(const string &key, int i)
Definition: testBTree.cpp:41
const KeyFormatter & formatter
std::pair< iterator, bool > tryInsert(Key j, const Vector &value)
Definition: VectorValues.h:209
const_iterator find(Key key) const
find
Definition: Factor.h:139
Matrix augmentedInformation() const override
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Definition: VectorValues.h:185
double error(const VectorValues &c) const override
void Allocate(const Scatter &scatter)
Allocate for given scatter pattern.
const_iterator end() const
Definition: Factor.h:148
std::pair< Matrix, Vector > jacobian() const override
Returns (dense) A,b pair associated with factor, bakes in the weights.
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
#define gttic(label)
Definition: timing.h:295
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
DenseIndex nBlocks() const
Block count.
size_t rows() const
DenseIndex rows() const
Row size.
void setDiagonalBlock(DenseIndex I, const XprType &xpr)
Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Array< int, Dynamic, 1 > v
Matrix information() const override
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
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. ...
RealScalar alpha
Contains the HessianFactor class, a general quadratic factor.
RealScalar s
void updateDiagonalBlock(DenseIndex I, const XprType &xpr)
Increment the diagonal block by the values in xpr. Only reads the upper triangular part of xpr...
Expression of a selfadjoint matrix from a triangular part of a dense matrix.
double constantTerm() const
Conditional Gaussian Base class.
Linear Factor Graph where all factors are Gaussians.
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
SymmetricBlockMatrix::constBlock linearTerm() const
std::shared_ptr< Factor > shared_ptr
A shared_ptr to this class.
Definition: Factor.h:75
JacobiRotation< float > J
Eigen::SelfAdjointView< SymmetricBlockMatrix::constBlock, Eigen::Upper > informationView() const
Return self-adjoint view onto the information matrix (NOT augmented).
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
const G & b
Definition: Group.h:86
SymmetricBlockMatrix info_
The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]&#39;H[x -1]...
GaussianFactor::shared_ptr negate() const override
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Exceptions that may be thrown by linear solver components.
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
void setFullMatrix(const XprType &xpr)
Set the entire active matrix. Only reads the upper triangular part of xpr.
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
traits
Definition: chartTesting.h:28
typedef and functions to augment Eigen&#39;s VectorXd
VerticalBlockMatrix split(DenseIndex nFrontals)
A thin wrapper around std::map that uses boost&#39;s fast_pool_allocator.
GTSAM_EXPORT void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Ordering.cpp:258
std::pair< Matrix, Vector > jacobian() const override
Return (dense) matrix associated with factor.
#define gttoc(label)
Definition: timing.h:296
void choleskyPartial(DenseIndex nFrontals)
void setZero()
Set the entire active matrix zero.
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
std::shared_ptr< GaussianConditional > eliminateCholesky(const Ordering &keys)
Matrix augmentedJacobian() const override
A Gaussian factor using the canonical parameters (information form)
Vector vector() const
m col(1)
const KeyVector & keys() const
Access the factor&#39;s involved variable keys.
Definition: Factor.h:142
DenseIndex getDim(const_iterator variable) const override
Vector diagonal(DenseIndex J) const
Get the diagonal of the J&#39;th diagonal block.
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Indicate Cholesky factorization failure.
void hessianDiagonalAdd(VectorValues &d) const override
Add the current diagonal to a VectorValues instance.
const G double tol
Definition: Group.h:86
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J&#39;th diagonal block as a self adjoint view.
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:82
void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const override
Vector gradient(Key key, const VectorValues &x) const override
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
DenseIndex getDim(DenseIndex block) const
Number of dimensions for variable on this diagonal block.
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
const_iterator begin() const
Definition: Factor.h:145
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))
const SymmetricBlockMatrix & info() const
Return underlying information matrix.
bool hasConstraints(const GaussianFactorGraph &factors)
static DenseIndex Slot(const CONTAINER &keys, Key key)
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.
Definition: types.h:102
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Matrix block(DenseIndex I, DenseIndex J) const
std::ptrdiff_t j
const SharedDiagonal & get_model() const
Timing utilities.
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42
Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:19