LinearContainerFactor.cpp
Go to the documentation of this file.
1 
13 
14 namespace gtsam {
15 
16 /* ************************************************************************* */
18  if (!linearizationPoint.empty()) {
20  for (Key key : keys()) {
21  linearizationPoint_->insert(key, linearizationPoint.at(key));
22  }
23  } else {
25  }
26 }
27 
28 /* ************************************************************************* */
30  const std::optional<Values>& linearizationPoint)
31 : NonlinearFactor(factor->keys()), factor_(factor), linearizationPoint_(linearizationPoint) {
32 }
33 
34 /* ************************************************************************* */
37 : NonlinearFactor(factor.keys()), factor_(factor.clone()) {
38  initializeLinearizationPoint(linearizationPoint);
39 }
40 
41 /* ************************************************************************* */
44 : NonlinearFactor(factor.keys()), factor_(factor.clone()) {
45  initializeLinearizationPoint(linearizationPoint);
46 }
47 
48 /* ************************************************************************* */
51 : NonlinearFactor(factor->keys()), factor_(factor->clone()) {
52  initializeLinearizationPoint(linearizationPoint);
53 }
54 
55 /* ************************************************************************* */
56 void LinearContainerFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
57  Base::print(s+"LinearContainerFactor", keyFormatter);
58  if (factor_)
59  factor_->print(" Stored Factor", keyFormatter);
61  linearizationPoint_->print(" LinearizationPoint", keyFormatter);
62 }
63 
64 /* ************************************************************************* */
65 bool LinearContainerFactor::equals(const NonlinearFactor& f, double tol) const {
66  const LinearContainerFactor* jcf = dynamic_cast<const LinearContainerFactor*>(&f);
67  if (!jcf || !factor_->equals(*jcf->factor_, tol) || !NonlinearFactor::equals(f))
68  return false;
70  return true;
72  return linearizationPoint_->equals(*jcf->linearizationPoint_, tol);
73  return false;
74 }
75 
76 /* ************************************************************************* */
77 double LinearContainerFactor::error(const Values& c) const {
79  return 0;
80 
81  // Extract subset of values for comparison
82  Values csub;
83  for (Key key : keys())
84  csub.insert(key, c.at(key));
85 
86  // create dummy ordering for evaluation
87  VectorValues delta = linearizationPoint_->localCoordinates(csub);
88 
89  // compute error
90  double error = factor_->error(delta);
91 
92  return error;
93 }
94 
95 /* ************************************************************************* */
97  if (isJacobian())
98  return toJacobian()->get_model()->dim();
99  else
100  return 1; // Hessians don't have true dimension
101 }
102 
103 /* ************************************************************************* */
105  // Clone factor and update as necessary
106  GaussianFactor::shared_ptr linFactor = factor_->clone();
107  if (!hasLinearizationPoint())
108  return linFactor;
109 
110  // Extract subset of values
111  Values subsetC;
112  for (Key key : keys())
113  subsetC.insert(key, c.at(key));
114 
115  // Determine delta between linearization points using new ordering
116  VectorValues delta = linearizationPoint_->localCoordinates(subsetC);
117 
118  // Apply changes due to relinearization
119  if (isJacobian()) {
120  JacobianFactor::shared_ptr jacFactor = std::dynamic_pointer_cast<JacobianFactor>(linFactor);
121  jacFactor->getb() = -jacFactor->unweighted_error(delta);
122  } else {
123  HessianFactor::shared_ptr hesFactor = std::dynamic_pointer_cast<HessianFactor>(linFactor);
124 
125  const auto view = hesFactor->informationView();
126  Vector deltaVector = delta.vector(keys());
127  Vector G_delta = view * deltaVector;
128  hesFactor->constantTerm() += deltaVector.dot(G_delta) - 2.0 * deltaVector.dot(hesFactor->linearTerm().col(0));
129  hesFactor->linearTerm() -= G_delta;
130  }
131 
132  return linFactor;
133 }
134 
135 /* ************************************************************************* */
137  return dynamic_cast<const JacobianFactor*>(factor_.get()) != 0;
138 }
139 
140 /* ************************************************************************* */
142  return dynamic_cast<const HessianFactor*>(factor_.get()) != 0;
143 }
144 
145 /* ************************************************************************* */
147  return std::dynamic_pointer_cast<JacobianFactor>(factor_);
148 }
149 
150 /* ************************************************************************* */
152  return std::dynamic_pointer_cast<HessianFactor>(factor_);
153 }
154 
155 /* ************************************************************************* */
158  return result;
159 }
160 
161 /* ************************************************************************* */
163  GaussianFactor::shared_ptr antifactor = factor_->negate(); // already has keys in place
165 }
166 
167 /* ************************************************************************* */
169  const std::map<Key, Key>& rekey_mapping) const {
170  auto rekeyed_base_factor = Base::rekey(rekey_mapping);
171  // Update the keys to the properties as well
172  // Downncast so we have access to members
173  auto new_factor = std::static_pointer_cast<LinearContainerFactor>(rekeyed_base_factor);
174  // Create a new Values to assign later
175  Values newLinearizationPoint;
176  for (size_t i = 0; i < factor_->size(); ++i) {
177  auto mapping = rekey_mapping.find(factor_->keys()[i]);
178  if (mapping != rekey_mapping.end()) {
179  new_factor->factor_->keys()[i] = mapping->second;
180  newLinearizationPoint.insert(mapping->second, linearizationPoint_->at(mapping->first));
181  }
182  }
183  new_factor->linearizationPoint_ = newLinearizationPoint;
184 
185  // upcast back and return
186  return std::static_pointer_cast<NonlinearFactor>(new_factor);
187 }
188 
189 /* ************************************************************************* */
191  const KeyVector& new_keys) const {
192  auto rekeyed_base_factor = Base::rekey(new_keys);
193  // Update the keys to the properties as well
194  // Downncast so we have access to members
195  auto new_factor = std::static_pointer_cast<LinearContainerFactor>(rekeyed_base_factor);
196  new_factor->factor_->keys() = new_factor->keys();
197  // Create a new Values to assign later
198  Values newLinearizationPoint;
199  for(size_t i=0; i<new_keys.size(); ++i) {
200  Key cur_key = linearizationPoint_->keys()[i];
201  newLinearizationPoint.insert(new_keys[i], linearizationPoint_->at(cur_key));
202  }
203  new_factor->linearizationPoint_ = newLinearizationPoint;
204 
205  // upcast back and return
206  return std::static_pointer_cast<NonlinearFactor>(new_factor);
207 }
208 
209 /* ************************************************************************* */
211  const GaussianFactorGraph& linear_graph, const Values& linearizationPoint) {
213  result.reserve(linear_graph.size());
214  for (const auto& f : linear_graph)
215  if (f)
217  return result;
218 }
219 
220 /* ************************************************************************* */
221 } // \namespace gtsam
222 
const gtsam::Symbol key('X', 0)
std::shared_ptr< HessianFactor > toHessian() const
const GaussianFactor::shared_ptr & factor() const
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
GaussianFactor::shared_ptr negateToGaussian() const
Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph...
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
const ValueType at(Key j) const
Definition: Values-inl.h:204
const std::optional< Values > & linearizationPoint() const
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
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
deref_iterator find(Key j) const
Definition: Values.h:210
NonlinearFactor::shared_ptr rekey(const std::map< Key, Key > &rekey_mapping) const override
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
GaussianFactor::shared_ptr factor_
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
size_t size() const
Definition: FactorGraph.h:334
Factor Graph Values.
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
std::optional< Values > linearizationPoint_
const constBVector getb() const
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Contains the HessianFactor class, a general quadratic factor.
RealScalar s
Linear Factor Graph where all factors are Gaussians.
std::shared_ptr< This > shared_ptr
shared_ptr to this class
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
std::shared_ptr< This > shared_ptr
shared_ptr to this class
void print(const std::string &s="", const KeyFormatter &keyFormatter=gtsam::DefaultKeyFormatter) const override
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
traits
Definition: chartTesting.h:28
std::vector< float > Values
bool hasLinearizationPoint() const
Casting syntactic sugar.
NonlinearFactor::shared_ptr clone() const override
std::shared_ptr< This > shared_ptr
virtual shared_ptr rekey(const std::map< Key, Key > &rekey_mapping) const
void initializeLinearizationPoint(const Values &linearizationPoint)
NonlinearFactor::shared_ptr negateToNonlinear() const
A Gaussian factor using the canonical parameters (information form)
Vector vector() const
const KeyVector & keys() const
Access the factor&#39;s involved variable keys.
Definition: Factor.h:142
void insert(Key j, const Value &val)
Definition: Values.cpp:155
void reserve(size_t size)
Definition: FactorGraph.h:186
std::shared_ptr< JacobianFactor > toJacobian() const
const G double tol
Definition: Group.h:86
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
static NonlinearFactorGraph ConvertLinearGraph(const GaussianFactorGraph &linear_graph, const Values &linearizationPoint=Values())
GaussianFactor::shared_ptr linearize(const Values &c) const override
double error(const Values &c) const override
bool empty() const
Definition: Values.h:181
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:33