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()) {
22  }
23  } else {
25  }
26 }
27 
28 /* ************************************************************************* */
30  const std::optional<Values>& linearizationPoint)
31 : NonlinearFactor(factor->keys()), factor_(factor), linearizationPoint_(linearizationPoint) {
32 }
33 
34 /* ************************************************************************* */
36  const JacobianFactor& factor, const Values& linearizationPoint)
37 : NonlinearFactor(factor.keys()), factor_(factor.clone()) {
39 }
40 
41 /* ************************************************************************* */
43  const HessianFactor& factor, const Values& linearizationPoint)
44 : NonlinearFactor(factor.keys()), factor_(factor.clone()) {
46 }
47 
48 /* ************************************************************************* */
50  const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint)
51 : NonlinearFactor(factor->keys()), factor_(factor->clone()) {
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 
gtsam::LinearContainerFactor::rekey
NonlinearFactor::shared_ptr rekey(const std::map< Key, Key > &rekey_mapping) const override
Definition: LinearContainerFactor.cpp:168
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
s
RealScalar s
Definition: level1_cplx_impl.h:126
LinearContainerFactor.h
Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph.
gtsam::LinearContainerFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: LinearContainerFactor.cpp:65
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::NonlinearFactor::equals
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Definition: NonlinearFactor.cpp:45
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
gtsam::LinearContainerFactor::linearizationPoint
const std::optional< Values > & linearizationPoint() const
Definition: LinearContainerFactor.h:86
gtsam::HessianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition: HessianFactor.h:109
gtsam::NonlinearFactor::rekey
virtual shared_ptr rekey(const std::map< Key, Key > &rekey_mapping) const
Definition: NonlinearFactor.cpp:50
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
result
Values result
Definition: OdometryOptimize.cpp:8
view
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
Definition: gnuplot_common_settings.hh:27
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::LinearContainerFactor::negateToNonlinear
NonlinearFactor::shared_ptr negateToNonlinear() const
Definition: LinearContainerFactor.cpp:162
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::KeyFormatter
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
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::LinearContainerFactor::initializeLinearizationPoint
void initializeLinearizationPoint(const Values &linearizationPoint)
Definition: LinearContainerFactor.cpp:17
gtsam::LinearContainerFactor::isHessian
bool isHessian() const
Definition: LinearContainerFactor.cpp:141
VectorValues.h
Factor Graph Values.
gtsam::LinearContainerFactor::isJacobian
bool isJacobian() const
Definition: LinearContainerFactor.cpp:136
gtsam::LinearContainerFactor::factor_
GaussianFactor::shared_ptr factor_
Definition: LinearContainerFactor.h:32
gtsam::NonlinearFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:35
gtsam::LinearContainerFactor::toJacobian
std::shared_ptr< JacobianFactor > toJacobian() const
Definition: LinearContainerFactor.cpp:146
key
const gtsam::Symbol key('X', 0)
JacobianFactor.h
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
gtsam::LinearContainerFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=gtsam::DefaultKeyFormatter) const override
Definition: LinearContainerFactor.cpp:56
gtsam::LinearContainerFactor::dim
size_t dim() const override
Definition: LinearContainerFactor.cpp:96
Values
std::vector< float > Values
Definition: sparse_setter.cpp:45
gtsam
traits
Definition: SFMdata.h:40
gtsam::Values
Definition: Values.h:65
gtsam::LinearContainerFactor::error
double error(const Values &c) const override
Definition: LinearContainerFactor.cpp:77
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:143
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::LinearContainerFactor::negateToGaussian
GaussianFactor::shared_ptr negateToGaussian() const
Definition: LinearContainerFactor.cpp:156
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::JacobianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:97
gtsam::LinearContainerFactor
Definition: LinearContainerFactor.h:29
gtsam::LinearContainerFactor::hasLinearizationPoint
bool hasLinearizationPoint() const
Casting syntactic sugar.
Definition: LinearContainerFactor.h:141
gtsam::LinearContainerFactor::LinearContainerFactor
LinearContainerFactor()
Definition: LinearContainerFactor.h:47
gtsam::LinearContainerFactor::linearize
GaussianFactor::shared_ptr linearize(const Values &c) const override
Definition: LinearContainerFactor.cpp:104
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::LinearContainerFactor::linearizationPoint_
std::optional< Values > linearizationPoint_
Definition: LinearContainerFactor.h:33
gtsam::LinearContainerFactor::toHessian
std::shared_ptr< HessianFactor > toHessian() const
Definition: LinearContainerFactor.cpp:151
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::Values::find
deref_iterator find(Key j) const
Definition: Values.h:210
gtsam::LinearContainerFactor::ConvertLinearGraph
static NonlinearFactorGraph ConvertLinearGraph(const GaussianFactorGraph &linear_graph, const Values &linearizationPoint=Values())
Definition: LinearContainerFactor.cpp:210


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:07