Go to the documentation of this file.
43 using Dims = std::vector<Key>;
44 using Pairs = std::vector<std::pair<Key, Matrix>>;
47 JacobianFactor::JacobianFactor() :
60 throw std::invalid_argument(
61 "In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
66 Ab_(
Dims{1}, b_in.size()) {
101 if (success || maxrank == factor.
rows() - 1) {
104 Matrix::Zero(maxrank,
Ab_.
matrix().cols());
121 #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
129 for (
size_t jointVarpos = 0; jointVarpos < variableSlots.size();
131 const VariableSlots::const_iterator& slots = variableSlots[jointVarpos];
135 bool foundVariable =
false;
136 for (
size_t sourceFactorI = 0; sourceFactorI < slots->second.size();
138 const size_t sourceVarpos = slots->second[sourceFactorI];
141 if (sourceFactor.
cols() > 1) {
142 foundVariable =
true;
144 sourceFactor.
begin() + sourceVarpos);
146 #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
148 varDims[jointVarpos] = vardim;
151 if(!(varDims[jointVarpos] == vardim)) {
152 std::stringstream
ss;
154 " has different dimensionality of " << vardim <<
" instead of " << varDims[jointVarpos];
155 throw std::runtime_error(
ss.str());
160 varDims[jointVarpos] = vardim;
169 throw std::invalid_argument(
170 "Unable to determine dimensionality for all variables");
177 #if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
187 FastVector<JacobianFactor::shared_ptr> _convertOrCastToJacobians(
188 const GaussianFactorGraph&
factors) {
189 gttic(Convert_to_Jacobians);
190 FastVector<JacobianFactor::shared_ptr> jacobians;
196 jacobians.push_back(jf);
198 jacobians.push_back(std::make_shared<JacobianFactor>(*factor));
214 const auto [varDims,
m,
n] = _countDims(jacobians, orderedSlots);
223 [](
const VariableSlots::const_iterator& it) {return it->first;});
228 size_t combinedSlot = 0;
229 for(VariableSlots::const_iterator varslot: orderedSlots) {
233 for (
size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
235 const size_t sourceSlot = varslot->second[factorI];
236 const DenseIndex sourceRows = jacobians[factorI]->rows();
237 if (sourceRows > 0) {
239 destSlot.middleRows(nextRow, sourceRows));
242 destBlock = jacobians[factorI]->getA(
243 jacobians[factorI]->
begin() + sourceSlot);
246 nextRow += sourceRows;
255 bool anyConstrained =
false;
256 std::optional<Vector>
sigmas;
259 for (
size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
261 if (sourceRows > 0) {
262 this->
getb().segment(nextRow, sourceRows) = jacobians[factorI]->getb();
266 sigmas = Vector::Constant(
m, 1.0);
267 sigmas->segment(nextRow, sourceRows) =
268 jacobians[factorI]->get_model()->sigmas();
270 anyConstrained =
true;
272 nextRow += sourceRows;
291 orderedSlots.reserve(variableSlots.size());
295 size_t nOrderingSlotsUsed = 0;
296 orderedSlots.resize(
ordering.size());
298 for (VariableSlots::const_iterator item = variableSlots.begin();
299 item != variableSlots.end(); ++item) {
301 inverseOrdering.find(item->first);
302 if (orderingPosition == inverseOrdering.end()) {
303 unorderedSlots.push_back(item);
305 orderedSlots[orderingPosition->second] = item;
306 ++nOrderingSlotsUsed;
309 if (nOrderingSlotsUsed !=
ordering.size())
310 throw std::invalid_argument(
311 "The ordering provided to the JacobianFactor combine constructor\n"
312 "contained extra variables that did not appear in the factors to combine.");
314 for(VariableSlots::const_iterator item: unorderedSlots) {
315 orderedSlots.push_back(item);
325 gttic(JacobianFactor_combine_constructor);
336 orderedSlots.reserve(variableSlots.size());
340 for (VariableSlots::const_iterator item = variableSlots.begin();
341 item != variableSlots.end(); ++item)
342 orderedSlots.push_back(item);
351 gttic(JacobianFactor_combine_constructor);
361 orderedSlots.reserve(variableSlots.size());
365 for (VariableSlots::const_iterator item = variableSlots.begin();
366 item != variableSlots.end(); ++item)
367 orderedSlots.push_back(item);
376 gttic(JacobianFactor_combine_constructor);
393 gttic(JacobianFactor_combine_constructor);
413 model_->print(
" Noise model: ");
415 cout <<
" No noise model" << endl;
421 static const bool verbose =
false;
424 cout <<
"JacobianFactor::equals: Incorrect type" << endl;
430 if (
keys() !=
f.keys()) {
432 cout <<
"JacobianFactor::equals: keys do not match" << endl;
439 cout <<
"JacobianFactor::equals: noise model mismatch" << endl;
444 cout <<
"JacobianFactor::equals: noise modesl are not equal" << endl;
451 cout <<
"JacobianFactor::equals: augmented size mismatch" << endl;
462 cout <<
"JacobianFactor::equals: matrix mismatch at row " <<
row << endl;
489 if (
model_)
return 0.5 *
model_->squaredMahalanobisDistance(
e);
490 return 0.5 *
e.dot(
e);
497 model_->WhitenInPlace(AbWhitened);
498 return AbWhitened.transpose() * AbWhitened;
508 model_->WhitenInPlace(AWhitened);
509 return AWhitened.transpose() * AWhitened;
511 return this->
getA().transpose() * this->
getA();
524 for (
size_t k = 0; k < nj; ++k) {
527 Vector column_k_copy = column_k;
528 model_->whitenInPlace(column_k_copy);
530 dj(k) +=
dot(column_k_copy, column_k_copy);
532 dj(k) =
dot(column_k_copy, column_k_copy);
535 dj(k) +=
dot(column_k, column_k);
537 dj(k) =
dot(column_k, column_k);
546 throw std::runtime_error(
"JacobianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only");
551 map<Key, Matrix> blocks;
557 blocks.emplace(
j, Aj.transpose() * Aj);
565 gttic(updateHessian_JacobianFactor);
567 if (
rows() == 0)
return;
572 if (
model->isConstrained())
573 throw invalid_argument(
574 "JacobianFactor::updateHessian: cannot update information with "
575 "constrained noise model");
584 vector<DenseIndex> slots(
n+1);
592 info->updateOffDiagonalBlock(
I,
J,
Ab_(
i).transpose() * Ab_j);
595 info->diagonalBlock(
J).rankUpdate(Ab_j.transpose());
630 it->second.noalias() +=
Ab_(
pos).transpose() *
E;
654 const std::vector<size_t>& accumulatedDims)
const {
674 model_->whitenInPlace(Ax);
675 model_->whitenInPlace(Ax);
703 throw std::runtime_error(
"JacobianFactor::gradientAtZero raw memory access is allowed for Regular Factors only");
726 return make_pair(
A,
b);
778 std::pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr>
EliminateQR(
784 jointFactor = std::make_shared<JacobianFactor>(
factors,
keys);
785 }
catch (std::invalid_argument&) {
787 "EliminateQR was called with a request to eliminate variables that are not\n"
788 "involved in the provided factors.");
795 if (jointFactor->model_) {
799 jointFactor->model_ = jointFactor->model_->QR(
Ab.matrix());
806 size_t m =
Ab.rows(),
n =
Ab.cols() - 1;
807 size_t maxRank =
min(
m,
n);
813 jointFactor->splitConditional(
keys.size());
815 return make_pair(conditional, jointFactor);
820 gttic(JacobianFactor_splitConditional);
823 throw std::invalid_argument(
824 "JacobianFactor::splitConditional cannot be given a nullptr noise model");
827 if (nrFrontals >
size()) {
828 throw std::invalid_argument(
829 "JacobianFactor::splitConditional was requested to split off more variables than exist.");
841 const DenseIndex originalRowEnd =
Ab_.
rowEnd();
844 conditionalNoiseModel =
847 std::make_shared<GaussianConditional>(
Base::keys_, nrFrontals,
Ab_, conditionalNoiseModel);
859 if (
model_->isConstrained())
std::vector< Eigen::Index > Dims
const DenseIndex & rowEnd() const
static const GTSAM_EXPORT size_t Empty
Linear Factor Graph where all factors are Gaussians.
std::shared_ptr< GaussianConditional > splitConditional(size_t nrFrontals)
Conditional Gaussian Base class.
Vector unweighted_error(const VectorValues &c) const
Expression of a fixed-size or dynamic-size block.
Block< Derived, Dynamic, internal::traits< Derived >::ColsAtCompileTime, IsRowMajor > RowsBlockXpr
A Gaussian factor using the canonical parameters (information form)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double d[K][N]
static shared_ptr MixedSigmas(const Vector &mu, const Vector &sigmas)
const SharedDiagonal & get_model() const
bool isConstrained() const
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
const KeyFormatter & formatter
Block range(DenseIndex startBlock, DenseIndex endBlock)
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)
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).
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Variable ordering for the elimination algorithm.
const Eigen::IOFormat & matlabFormat()
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
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
GaussianFactor::shared_ptr negate() const override
void hessianDiagonalAdd(VectorValues &d) const override
Add the current diagonal to a VectorValues instance.
pair< size_t, bool > choleskyCareful(Matrix &ATA, int order)
const constBVector getb() const
Vector operator*(const VectorValues &x) const
std::pair< std::shared_ptr< GaussianConditional >, shared_ptr > eliminate(const Ordering &keys)
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
static std::stringstream ss
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
noiseModel::Diagonal::shared_ptr model_
const DenseIndex & firstBlock() const
Exceptions that may be thrown by linear solver components.
const DenseIndex & rowStart() const
void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
Internal function to fill blocks and set dimensions.
JacobiRotation< float > J
Vector gradient(Key key, const VectorValues &x) const override
Compute the gradient wrt a key at any values.
DenseIndex getDim(const_iterator variable) const override
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Matrix information() const override
std::pair< Matrix, Vector > jacobianUnweighted() const
Returns (dense) A,b pair associated with factor, does not bake in weights.
VectorValues gradientAtZero() const override
A'*b for Jacobian.
friend GTSAM_EXPORT std::pair< std::shared_ptr< GaussianConditional >, shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
std::shared_ptr< This > shared_ptr
shared_ptr to this class
static shared_ptr Create(size_t dim)
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT
noiseModel::Diagonal::shared_ptr SharedDiagonal
double dot(const V1 &a, const V2 &b)
Vector gradient(Key key, const VectorValues &x) const override
Matrix augmentedJacobian() const override
DenseIndex nBlocks() const
Block count.
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
const_iterator end() const
A matrix or vector expression mapping an existing array of data.
noiseModel::Diagonal::shared_ptr model
void g(const string &key, int i)
static enum @1096 ordering
Vector error_vector(const VectorValues &c) const
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 >)
std::vector< std::pair< Key, Matrix > > Pairs
VariableSlots describes the structure of a combined factor in terms of where each block comes from in...
void inplace_QR(Matrix &A)
KeyVector::const_iterator const_iterator
Const iterator over keys.
FastVector< VariableSlots::const_iterator > orderedSlotsHelper(const Ordering &ordering, const VariableSlots &variableSlots)
A matrix or vector expression mapping an existing expression.
DenseIndex cols() const
Column size.
KeyVector keys_
The keys involved in this factor.
Key front() const
First key.
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
assert equality up to a tolerance
const KeyVector & keys() const
Access the factor's involved variable keys.
std::map< Key, Matrix > hessianBlockDiagonal() const override
Return the block diagonal of the Hessian for this factor.
ptrdiff_t DenseIndex
The index type for Eigen objects.
const Matrix & matrix() const
void JacobianFactorHelper(const GaussianFactorGraph &graph, const FastVector< VariableSlots::const_iterator > &orderedSlots)
std::shared_ptr< This > shared_ptr
shared_ptr to this class
DenseIndex rows() const
Row size.
static DenseIndex Slot(const CONTAINER &keys, Key key)
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print with optional string
double error(const VectorValues &c) const override
void transposeMultiplyAdd(double alpha, const Vector &e, VectorValues &x) const
std::shared_ptr< This > shared_ptr
shared_ptr to this class
NonlinearFactorGraph graph
std::uint64_t Key
Integer nonlinear key type.
JacobianFactor whiten() const
void setModel(bool anyConstrained, const Vector &sigmas)
Matrix augmentedJacobianUnweighted() const
Maps global variable indices to slot indices.
Matrix augmentedInformation() const override
A thin wrapper around std::map that uses boost's fast_pool_allocator.
std::string formatMatrixIndented(const std::string &label, const Matrix &matrix, bool makeVectorHorizontal)
void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const override
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:55