Go to the documentation of this file.
27 #if GTSAM_ENABLE_BOOST_SERIALIZATION
28 #include <boost/serialization/version.hpp>
29 #include <boost/serialization/split_member.hpp>
37 class GaussianFactorGraph;
49 GTSAM_EXPORT std::pair<std::shared_ptr<GaussianConditional>, std::shared_ptr<JacobianFactor> >
145 template<
typename TERMS>
152 template<
typename KEYS>
193 return std::static_pointer_cast<GaussianFactor>(
194 std::make_shared<JacobianFactor>(*
this));
198 void print(
const std::string&
s =
"",
219 Matrix augmentedInformation()
const override;
224 Matrix information()
const override;
227 using Base::hessianDiagonal;
233 void hessianDiagonal(
double*
d)
const override;
236 std::map<Key,Matrix> hessianBlockDiagonal()
const override;
241 std::pair<Matrix, Vector> jacobian()
const override;
246 std::pair<Matrix, Vector> jacobianUnweighted()
const;
251 Matrix augmentedJacobian()
const override;
256 Matrix augmentedJacobianUnweighted()
const;
273 return model_ && model_->isConstrained();
280 return Ab_(variable - begin()).
cols();
335 void transposeMultiplyAdd(
double alpha,
const Vector&
e,
350 void multiplyHessianAdd(
double alpha,
const double*
x,
double*
y,
351 const std::vector<size_t>& accumulatedDims)
const;
357 void gradientAtZero(
double*
d)
const override;
366 std::pair<std::shared_ptr<GaussianConditional>, shared_ptr>
370 void setModel(
bool anyConstrained,
const Vector&
sigmas);
383 friend GTSAM_EXPORT std::pair<std::shared_ptr<GaussianConditional>, shared_ptr>
393 std::shared_ptr<GaussianConditional> splitConditional(
size_t nrFrontals);
398 template<
typename TERMS>
407 void JacobianFactorHelper(
417 template<
class KEYS,
class DIMENSIONS>
426 #if GTSAM_ENABLE_BOOST_SERIALIZATION
428 friend class boost::serialization::access;
429 template<
class ARCHIVE>
430 void save(ARCHIVE & ar,
const unsigned int version)
const {
435 ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
436 ar << BOOST_SERIALIZATION_NVP(Ab_);
437 bool model_null =
false;
438 if(model_.get() ==
nullptr) {
440 ar << boost::serialization::make_nvp(
"model_null", model_null);
442 ar << boost::serialization::make_nvp(
"model_null", model_null);
443 ar << BOOST_SERIALIZATION_NVP(model_);
447 template<
class ARCHIVE>
448 void load(ARCHIVE & ar,
const unsigned int version) {
450 ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
451 ar >> BOOST_SERIALIZATION_NVP(Ab_);
453 ar >> BOOST_SERIALIZATION_NVP(model_);
456 ar >> BOOST_SERIALIZATION_NVP(model_null);
458 ar >> BOOST_SERIALIZATION_NVP(model_);
463 BOOST_SERIALIZATION_SPLIT_MEMBER()
473 #if GTSAM_ENABLE_BOOST_SERIALIZATION
virtual double error(const VectorValues &c) const
Expression of a fixed-size or dynamic-size block.
A Gaussian factor using the canonical parameters (information form)
Included from all GTSAM files.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double d[K][N]
const SharedDiagonal & get_model() const
bool isConstrained() const
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
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
~JacobianFactor() override
GaussianFactor::shared_ptr clone() const override
const KeyFormatter & formatter
Block range(DenseIndex startBlock, DenseIndex endBlock)
VerticalBlockMatrix::constBlock constABlock
ABlock getA(const Key &key)
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
const constBVector getb() const
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Point2 operator*(double s, const Point2 &p)
multiply with scalar
noiseModel::Diagonal::shared_ptr model_
ABlock getA(iterator variable)
void print(const Matrix &A, const string &s, ostream &stream)
JacobianFactor(const KEYS &keys, const DIMENSIONS &dims, DenseIndex m, const SharedDiagonal &model=SharedDiagonal())
DenseIndex getDim(const_iterator variable) const override
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
A matrix with column blocks of pre-defined sizes. Used in JacobianFactor and GaussianConditional.
std::shared_ptr< This > shared_ptr
shared_ptr to this class
noiseModel::Diagonal::shared_ptr SharedDiagonal
SharedDiagonal & get_model()
JacobianFactor(const JacobianFactor &jf)
JacobianFactor This
Typedef to this class.
noiseModel::Diagonal::shared_ptr model
static enum @1096 ordering
const gtsam::Symbol key('X', 0)
Block< Derived, internal::traits< Derived >::RowsAtCompileTime, 1, !IsRowMajor > ColXpr
const typedef Block< const Derived, internal::traits< Derived >::RowsAtCompileTime, 1, !IsRowMajor > ConstColXpr
void save(const Matrix &A, const string &s, const string &filename)
VariableSlots describes the structure of a combined factor in terms of where each block comes from in...
KeyVector::const_iterator const_iterator
Const iterator over keys.
constABlock::ConstColXpr constBVector
DenseIndex cols() const
Column size.
constABlock getA(const_iterator variable) const
ptrdiff_t DenseIndex
The index type for Eigen objects.
GaussianFactor Base
Typedef to base class.
std::shared_ptr< Diagonal > shared_ptr
DenseIndex rows() const
Row size.
VerticalBlockMatrix::Block ABlock
std::shared_ptr< This > shared_ptr
shared_ptr to this class
const VerticalBlockMatrix & matrixObject() const
A factor with a quadratic error function - a Gaussian.
static const EIGEN_DEPRECATED end_t end
NonlinearFactorGraph graph
std::uint64_t Key
Integer nonlinear key type.
VerticalBlockMatrix & matrixObject()
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
gtsam
Author(s):
autogenerated on Sat Jan 4 2025 04:01:29