29 #include <boost/shared_ptr.hpp> 37 class GaussianFactorGraph;
38 class SymbolicFactorGraph;
42 class ExpressionFactor;
63 paperHorizontalAxis(
Y), paperVerticalAxis(
X),
64 figureWidthInches(5), figureHeightInches(5), scale(1),
65 mergeSimilarFactors(false), plotFactorPoints(true),
66 connectKeysToFactor(true), binaryEdges(true) {}
90 template<
typename ITERATOR>
94 template<
class CONTAINER>
98 template<
class DERIVEDFACTOR>
106 const std::string&
str =
"NonlinearFactorGraph: ",
110 void printErrors(
const Values&
values,
const std::string&
str =
"NonlinearFactorGraph: ",
112 const std::function<
bool(
const Factor* ,
double ,
size_t )>&
113 printCondition = [](
const Factor *,
double,
size_t) {
return true;})
const;
119 void saveGraph(std::ostream& stm,
const Values& values =
Values(),
137 double probPrime(
const Values& values)
const;
142 boost::shared_ptr<SymbolicFactorGraph> symbolic()
const;
160 boost::shared_ptr<GaussianFactorGraph> linearize(
const Values& linearizationPoint)
const;
163 typedef std::function<void(const boost::shared_ptr<HessianFactor>& hessianFactor)>
Dampen;
172 boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
173 const Values& values,
const Dampen& dampen =
nullptr)
const;
183 boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
189 const Dampen& dampen =
nullptr)
const;
194 const Dampen& dampen =
nullptr)
const;
246 emplace_shared<PriorFactor<T>>(
key,
prior, covariance);
255 boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
259 friend class boost::serialization::access;
260 template<
class ARCHIVE>
262 ar & boost::serialization::make_nvp(
"NonlinearFactorGraph",
263 boost::serialization::base_object<Base>(*
this));
270 const Values& values, boost::none_t,
const Dampen& dampen =
nullptr)
const 271 {
return linearizeToHessianFactor(values, dampen);}
275 const Dampen& dampen =
nullptr)
const 276 {
return updateCholesky(values, dampen);}
void print(const Matrix &A, const string &s, ostream &stream)
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&...args)
void addPrior(Key key, const T &prior, const Matrix &covariance)
Point2 prior(const Point2 &x)
Prior on a single pose.
noiseModel::Diagonal::shared_ptr model
virtual ~NonlinearFactorGraph()
Destructor.
static enum @843 ordering
Rot2 R(Rot2::fromAngle(0.1))
GaussianFactorGraph factors(list_of(factor1)(factor2)(factor3))
NonlinearFactorGraph graph
static const KeyFormatter DefaultKeyFormatter
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
boost::shared_ptr< This > shared_ptr
std::function< void(const boost::shared_ptr< HessianFactor > &hessianFactor)> Dampen
typdef for dampen functions used below
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
NonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
NonlinearFactorGraph This
std::vector< float > Values
Non-linear factor base classes.
FactorGraph< NonlinearFactor > Base
NonlinearFactorGraph(const CONTAINER &factors)
Values updateCholesky(const Values &values, boost::none_t, const Dampen &dampen=nullptr) const
NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
void serialize(ARCHIVE &ar, const unsigned int)
boost::shared_ptr< HessianFactor > linearizeToHessianFactor(const Values &values, boost::none_t, const Dampen &dampen=nullptr) const
std::uint64_t Key
Integer nonlinear key type.
noiseModel::Base::shared_ptr SharedNoiseModel