32 const Values& linearizationPoint,
Key lastKey,
39 lastKeyAsOrdering += lastKey;
45 const T& current = linearizationPoint.
at<
T>(lastKey);
52 assert(marginal->nrFrontals() == 1);
53 assert(marginal->nrParents() == 0);
54 *newPrior = boost::make_shared<JacobianFactor>(
55 marginal->keys().front(),
56 marginal->getA(marginal->begin()),
57 marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey],
58 marginal->get_model());
64 template <
class VALUE>
82 const auto keys = motionFactor.
keys();
104 template<
class VALUE>
107 const auto keys = measurementFactor.
keys();
116 Values linearizationPoint;
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
ExtendedKalmanFilter(Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial)
void insert(Key j, const Value &val)
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
T update(const NoiseModelFactor &measurementFactor)
static shared_ptr Create(size_t dim)
Class to perform generic Kalman Filtering using nonlinear factor graphs.
static T solve_(const GaussianFactorGraph &linearFactorGraph, const Values &linearizationPoints, Key x, JacobianFactor::shared_ptr *newPrior)
const ValueType at(Key j) const
JacobianFactor::shared_ptr priorFactor_
T predict(const NoiseModelFactor &motionFactor)
Linear Factor Graph where all factors are Gaussians.
Non-linear factor base classes.
const KeyVector & keys() const
Access the factor's involved variable keys.
Chordal Bayes Net, the result of eliminating a factor graph.
boost::shared_ptr< Gaussian > shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
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
std::uint64_t Key
Integer nonlinear key type.
boost::shared_ptr< BayesNetType > marginalMultifrontalBayesNet(boost::variant< const Ordering &, const KeyVector & > variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const