LinearizedFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #include <iostream>
20 
21 namespace gtsam {
22 
23 /* ************************************************************************* */
25  const GaussianFactor::shared_ptr& gaussian, const Values& lin_points)
26 : NonlinearFactor(gaussian->keys())
27 {
28  // Extract the keys and linearization points
29  for(const Key& key: gaussian->keys()) {
30  // extract linearization point
31  assert(lin_points.exists(key));
32  this->lin_points_.insert(key, lin_points.at(key));
33  }
34 }
35 
36 /* ************************************************************************* */
37 // LinearizedJacobianFactor
38 /* ************************************************************************* */
40 }
41 
42 /* ************************************************************************* */
44  const JacobianFactor::shared_ptr& jacobian, const Values& lin_points)
45 : Base(jacobian, lin_points) {
46 
47  // Create the dims array
48  size_t *dims = (size_t *)alloca(sizeof(size_t) * (jacobian->size() + 1));
49  size_t index = 0;
50  for(JacobianFactor::const_iterator iter = jacobian->begin(); iter != jacobian->end(); ++iter) {
51  dims[index++] = jacobian->getDim(iter);
52  }
53  dims[index] = 1;
54 
55  // Update the BlockInfo accessor
56  Ab_ = VerticalBlockMatrix(dims, dims+jacobian->size()+1, jacobian->rows());
57  // Get the Ab matrix from the Jacobian factor, with any covariance baked in
58  Ab_.matrix() = jacobian->augmentedJacobian();
59 }
60 
61 /* ************************************************************************* */
62 void LinearizedJacobianFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
63 
64  std::cout << s << std::endl;
65 
66  std::cout << "Nonlinear Keys: ";
67  for(const Key& key: this->keys())
68  std::cout << keyFormatter(key) << " ";
69  std::cout << std::endl;
70 
71  for(const_iterator key=begin(); key!=end(); ++key) {
72  std::cout << "A[" << keyFormatter(*key) << "]=\n" << A(*key) << std::endl;
73  }
74  std::cout << "b=\n" << b() << std::endl;
75 
76  lin_points_.print("Linearization Point: ");
77 }
78 
79 /* ************************************************************************* */
81 
82  const This *e = dynamic_cast<const This*> (&expected);
83  if (e) {
84 
85  Matrix thisMatrix = this->Ab_.range(0, Ab_.nBlocks());
86  Matrix rhsMatrix = e->Ab_.range(0, Ab_.nBlocks());
87 
88  return Base::equals(expected, tol)
89  && lin_points_.equals(e->lin_points_, tol)
90  && equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
91  } else {
92  return false;
93  }
94 }
95 
96 /* ************************************************************************* */
98  Vector errorVector = error_vector(c);
99  return 0.5 * errorVector.dot(errorVector);
100 }
101 
102 /* ************************************************************************* */
103 std::shared_ptr<GaussianFactor>
105 
106  // Create the 'terms' data structure for the Jacobian constructor
107  std::vector<std::pair<Key, Matrix> > terms;
108  for(Key key: keys()) {
109  terms.push_back(std::make_pair(key, this->A(key)));
110  }
111 
112  // compute rhs
113  Vector b = -error_vector(c);
114 
115  return std::shared_ptr<GaussianFactor>(new JacobianFactor(terms, b, noiseModel::Unit::Create(dim())));
116 }
117 
118 /* ************************************************************************* */
120 
121  Vector errorVector = -b();
122 
123  for(Key key: this->keys()) {
124  const Value& newPt = c.at(key);
125  const Value& linPt = lin_points_.at(key);
126  Vector d = linPt.localCoordinates_(newPt);
127  const constABlock A = this->A(key);
128  errorVector += A * d;
129  }
130 
131  return errorVector;
132 }
133 
134 /* ************************************************************************* */
135 // LinearizedHessianFactor
136 /* ************************************************************************* */
138 }
139 
140 /* ************************************************************************* */
142  const HessianFactor::shared_ptr& hessian, const Values& lin_points)
143  : Base(hessian, lin_points), info_(hessian->info()) {}
144 
145 /* ************************************************************************* */
146 void LinearizedHessianFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
147 
148  std::cout << s << std::endl;
149 
150  std::cout << "Nonlinear Keys: ";
151  for(const Key& key: this->keys())
152  std::cout << keyFormatter(key) << " ";
153  std::cout << std::endl;
154 
155  gtsam::print(Matrix(info_.selfadjointView()), "Ab^T * Ab: ");
156 
157  lin_points_.print("Linearization Point: ");
158 }
159 
160 /* ************************************************************************* */
162 
163  const This *e = dynamic_cast<const This*> (&expected);
164  if (e) {
165 
166  Matrix thisMatrix = this->info_.selfadjointView();
167  thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
168  Matrix rhsMatrix = e->info_.selfadjointView();
169  rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0;
170 
171  return Base::equals(expected, tol)
172  && lin_points_.equals(e->lin_points_, tol)
173  && equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
174  } else {
175  return false;
176  }
177 }
178 
179 /* ************************************************************************* */
181 
182  // Construct an error vector in key-order from the Values
183  Vector dx = Vector::Zero(dim());
184  size_t index = 0;
185  for(unsigned int i = 0; i < this->size(); ++i){
186  Key key = this->keys()[i];
187  const Value& newPt = c.at(key);
188  const Value& linPt = lin_points_.at(key);
189  dx.segment(index, linPt.dim()) = linPt.localCoordinates_(newPt);
190  index += linPt.dim();
191  }
192 
193  // error 0.5*(f - 2*x'*g + x'*G*x)
194  double f = constantTerm();
195  double xtg = dx.dot(linearTerm());
196  double xGx = dx.transpose() * squaredTerm() * dx;
197 
198  return 0.5 * (f - 2.0 * xtg + xGx);
199 }
200 
201 /* ************************************************************************* */
202 std::shared_ptr<GaussianFactor>
204 
205  // Construct an error vector in key-order from the Values
206  Vector dx = Vector::Zero(dim());
207  size_t index = 0;
208  for(unsigned int i = 0; i < this->size(); ++i){
209  Key key = this->keys()[i];
210  const Value& newPt = c.at(key);
211  const Value& linPt = lin_points_.at(key);
212  dx.segment(index, linPt.dim()) = linPt.localCoordinates_(newPt);
213  index += linPt.dim();
214  }
215 
216  // f2 = f1 - 2*dx'*g1 + dx'*G1*dx
217  //newInfo(this->size(), this->size())(0,0) += -2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm().selfadjointView<Eigen::Upper>() * dx;
218  double f = constantTerm() - 2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm() * dx;
219 
220  // g2 = g1 - G1*dx
221  //newInfo.rangeColumn(0, this->size(), this->size(), 0) -= squaredTerm().selfadjointView<Eigen::Upper>() * dx;
222  Vector g = linearTerm() - squaredTerm() * dx;
223  std::vector<Vector> gs;
224  std::size_t offset = 0;
225  for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) {
226  const std::size_t dim = info_.getDim(i);
227  gs.push_back(g.segment(offset, dim));
228  offset += dim;
229  }
230 
231  // G2 = G1
232  // Do Nothing
233  std::vector<Matrix> Gs;
234  for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) {
235  Gs.push_back(info_.diagonalBlock(i));
236  for(DenseIndex j = i + 1; j < info_.nBlocks()-1; ++j) {
237  Gs.push_back(info_.aboveDiagonalBlock(i, j));
238  }
239  }
240 
241  // Create a Hessian Factor from the modified info matrix
242  //return std::shared_ptr<GaussianFactor>(new HessianFactor(js, newInfo));
243  return std::shared_ptr<GaussianFactor>(new HessianFactor(keys(), Gs, gs, f));
244 }
245 
246 } // \namespace aspn
gtsam::Values::exists
bool exists(Key j) const
Definition: Values.cpp:93
Eigen::Block
Expression of a fixed-size or dynamic-size block.
Definition: Block.h:103
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
gtsam::LinearizedHessianFactor::info_
SymmetricBlockMatrix info_
Definition: LinearizedFactor.h:191
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
gtsam::LinearizedHessianFactor::dim
size_t dim() const override
Definition: LinearizedFactor.h:268
gtsam::VerticalBlockMatrix::range
Block range(DenseIndex startBlock, DenseIndex endBlock)
Definition: VerticalBlockMatrix.h:130
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition: SymmetricBlockMatrix.h:158
gtsam::Value::dim
virtual size_t dim() const =0
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::HessianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition: HessianFactor.h:109
gtsam::Factor
Definition: Factor.h:70
gtsam::LinearizedJacobianFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: LinearizedFactor.cpp:62
gtsam::Factor::begin
const_iterator begin() const
Definition: Factor.h:146
gtsam::LinearizedJacobianFactor::dim
size_t dim() const override
Definition: LinearizedFactor.h:135
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::LinearizedHessianFactor::error
double error(const Values &c) const override
Definition: LinearizedFactor.cpp:180
gtsam::Value::localCoordinates_
virtual Vector localCoordinates_(const Value &value) const =0
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
A
Definition: test_numpy_dtypes.cpp:298
gtsam::LinearizedJacobianFactor::Ab_
VerticalBlockMatrix Ab_
Definition: LinearizedFactor.h:101
gtsam::LinearizedGaussianFactor::LinearizedGaussianFactor
LinearizedGaussianFactor()=default
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::LinearizedHessianFactor::LinearizedHessianFactor
LinearizedHessianFactor()
Definition: LinearizedFactor.cpp:137
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::LinearizedHessianFactor::squaredTerm
Eigen::SelfAdjointView< constBlock, Eigen::Upper > squaredTerm() const
Definition: LinearizedFactor.h:263
gtsam::LinearizedJacobianFactor::error_vector
Vector error_vector(const Values &c) const
Definition: LinearizedFactor.cpp:119
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::LinearizedJacobianFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &c) const override
Definition: LinearizedFactor.cpp:104
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:631
gtsam::Value
Definition: Value.h:39
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::LinearizedJacobianFactor::LinearizedJacobianFactor
LinearizedJacobianFactor()
Definition: LinearizedFactor.cpp:39
gtsam::Values::equals
bool equals(const Values &other, double tol=1e-9) const
Definition: Values.cpp:77
info
else if n * info
Definition: 3rdparty/Eigen/lapack/cholesky.cpp:18
gtsam::VerticalBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition: VerticalBlockMatrix.h:121
gtsam::SymmetricBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition: SymmetricBlockMatrix.h:122
gtsam::LinearizedHessianFactor::linearTerm
constColumn linearTerm() const
Definition: LinearizedFactor.h:239
gtsam::Factor::end
const_iterator end() const
Definition: Factor.h:149
size_t
std::size_t size_t
Definition: wrap/pybind11/include/pybind11/detail/common.h:490
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
offset
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 set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate offset
Definition: gnuplot_common_settings.hh:64
key
const gtsam::Symbol key('X', 0)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::LinearizedJacobianFactor::error
double error(const Values &c) const override
Definition: LinearizedFactor.cpp:97
gtsam::LinearizedJacobianFactor::b
const constBVector b() const
Definition: LinearizedFactor.h:130
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:83
gtsam::Values::print
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
gtsam
traits
Definition: SFMdata.h:40
gtsam::LinearizedHessianFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: LinearizedFactor.cpp:146
gtsam::Values
Definition: Values.h:65
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:143
iter
iterator iter(handle obj)
Definition: pytypes.h:2475
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:103
gtsam::VerticalBlockMatrix::matrix
const Matrix & matrix() const
Definition: VerticalBlockMatrix.h:188
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition: SymmetricBlockMatrix.h:137
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::LinearizedHessianFactor::constantTerm
double constantTerm() const
Definition: LinearizedFactor.h:224
gtsam::LinearizedJacobianFactor::A
const constABlock A() const
Definition: LinearizedFactor.h:131
gtsam::LinearizedHessianFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
Definition: LinearizedFactor.cpp:161
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::SymmetricBlockMatrix::getDim
DenseIndex getDim(DenseIndex block) const
Number of dimensions for variable on this diagonal block.
Definition: SymmetricBlockMatrix.h:125
gtsam::LinearizedHessianFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &c) const override
Definition: LinearizedFactor.cpp:203
gtsam::LinearizedGaussianFactor::lin_points_
Values lin_points_
Definition: LinearizedFactor.h:43
gtsam::SymmetricBlockMatrix::aboveDiagonalBlock
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition: SymmetricBlockMatrix.h:152
gtsam::Factor::size
size_t size() const
Definition: Factor.h:160
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::LinearizedJacobianFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
Definition: LinearizedFactor.cpp:80
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
LinearizedFactor.h
A dummy factor that allows a linear factor to act as a nonlinear factor.


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