Go to the documentation of this file.
30 const std::optional<Values>& linearizationPoint)
31 :
NonlinearFactor(factor->
keys()), factor_(factor), linearizationPoint_(linearizationPoint) {
59 factor_->print(
" Stored Factor", keyFormatter);
121 jacFactor->getb() = -jacFactor->unweighted_error(
delta);
125 const auto view = hesFactor->informationView();
128 hesFactor->constantTerm() += deltaVector.dot(G_delta) - 2.0 * deltaVector.dot(hesFactor->linearTerm().col(0));
129 hesFactor->linearTerm() -= G_delta;
147 return std::dynamic_pointer_cast<JacobianFactor>(
factor_);
152 return std::dynamic_pointer_cast<HessianFactor>(
factor_);
169 const std::map<Key, Key>& rekey_mapping)
const {
170 auto rekeyed_base_factor =
Base::rekey(rekey_mapping);
173 auto new_factor = std::static_pointer_cast<LinearContainerFactor>(rekeyed_base_factor);
175 Values newLinearizationPoint;
176 for (
size_t i = 0;
i <
factor_->size(); ++
i) {
178 if (mapping != rekey_mapping.end()) {
179 new_factor->factor_->keys()[
i] = mapping->second;
183 new_factor->linearizationPoint_ = newLinearizationPoint;
186 return std::static_pointer_cast<NonlinearFactor>(new_factor);
195 auto new_factor = std::static_pointer_cast<LinearContainerFactor>(rekeyed_base_factor);
196 new_factor->factor_->keys() = new_factor->keys();
198 Values newLinearizationPoint;
199 for(
size_t i=0;
i<new_keys.size(); ++
i) {
203 new_factor->linearizationPoint_ = newLinearizationPoint;
206 return std::static_pointer_cast<NonlinearFactor>(new_factor);
214 for (
const auto&
f : linear_graph)
NonlinearFactor::shared_ptr rekey(const std::map< Key, Key > &rekey_mapping) const override
std::shared_ptr< This > shared_ptr
Linear Factor Graph where all factors are Gaussians.
A Gaussian factor using the canonical parameters (information form)
Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph.
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Contains the HessianFactor class, a general quadratic factor.
const std::optional< Values > & linearizationPoint() const
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
virtual shared_ptr rekey(const std::map< Key, Key > &rekey_mapping) const
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
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 view
NonlinearFactor::shared_ptr negateToNonlinear() const
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
void initializeLinearizationPoint(const Values &linearizationPoint)
GaussianFactor::shared_ptr factor_
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
std::shared_ptr< JacobianFactor > toJacobian() const
const gtsam::Symbol key('X', 0)
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
void print(const std::string &s="", const KeyFormatter &keyFormatter=gtsam::DefaultKeyFormatter) const override
size_t dim() const override
std::vector< float > Values
double error(const Values &c) const override
const KeyVector & keys() const
Access the factor's involved variable keys.
void insert(Key j, const Value &val)
GaussianFactor::shared_ptr negateToGaussian() const
std::shared_ptr< This > shared_ptr
shared_ptr to this class
bool hasLinearizationPoint() const
Casting syntactic sugar.
GaussianFactor::shared_ptr linearize(const Values &c) const override
std::uint64_t Key
Integer nonlinear key type.
std::optional< Values > linearizationPoint_
std::shared_ptr< HessianFactor > toHessian() const
deref_iterator find(Key j) const
static NonlinearFactorGraph ConvertLinearGraph(const GaussianFactorGraph &linear_graph, const Values &linearizationPoint=Values())
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:07