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
12/**
13 * @file LinearizedFactor.cpp
14 * @brief A dummy factor that allows a linear factor to act as a nonlinear factor
15 * @author Alex Cunningham
16 */
17
18#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
19#include <iostream>
20#include <cassert>
21
22namespace gtsam {
23
24/* ************************************************************************* */
25LinearizedGaussianFactor::LinearizedGaussianFactor(
26 const GaussianFactor::shared_ptr& gaussian, const Values& lin_points)
27: NonlinearFactor(gaussian->keys())
28{
29 // Extract the keys and linearization points
30 for(const Key& key: gaussian->keys()) {
31 // extract linearization point
32 assert(lin_points.exists(key));
33 this->lin_points_.insert(j: key, val: lin_points.at(j: key));
34 }
35}
36
37/* ************************************************************************* */
38// LinearizedJacobianFactor
39/* ************************************************************************* */
40LinearizedJacobianFactor::LinearizedJacobianFactor() {
41}
42
43/* ************************************************************************* */
44LinearizedJacobianFactor::LinearizedJacobianFactor(
45 const JacobianFactor::shared_ptr& jacobian, const Values& lin_points)
46: Base(jacobian, lin_points) {
47
48 // Create the dims array
49 size_t *dims = (size_t *)alloca(sizeof(size_t) * (jacobian->size() + 1));
50 size_t index = 0;
51 for(JacobianFactor::const_iterator iter = jacobian->begin(); iter != jacobian->end(); ++iter) {
52 dims[index++] = jacobian->getDim(variable: iter);
53 }
54 dims[index] = 1;
55
56 // Update the BlockInfo accessor
57 Ab_ = VerticalBlockMatrix(dims, dims+jacobian->size()+1, jacobian->rows());
58 // Get the Ab matrix from the Jacobian factor, with any covariance baked in
59 Ab_.matrix() = jacobian->augmentedJacobian();
60}
61
62/* ************************************************************************* */
63void LinearizedJacobianFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
64
65 std::cout << s << std::endl;
66
67 std::cout << "Nonlinear Keys: ";
68 for(const Key& key: this->keys())
69 std::cout << keyFormatter(key) << " ";
70 std::cout << std::endl;
71
72 for(const_iterator key=begin(); key!=end(); ++key) {
73 std::cout << "A[" << keyFormatter(*key) << "]=\n" << A(key: *key) << std::endl;
74 }
75 std::cout << "b=\n" << b() << std::endl;
76
77 lin_points_.print(str: "Linearization Point: ");
78}
79
80/* ************************************************************************* */
81bool LinearizedJacobianFactor::equals(const NonlinearFactor& expected, double tol) const {
82
83 const This *e = dynamic_cast<const This*> (&expected);
84 if (e) {
85
86 Matrix thisMatrix = this->Ab_.range(startBlock: 0, endBlock: Ab_.nBlocks());
87 Matrix rhsMatrix = e->Ab_.range(startBlock: 0, endBlock: Ab_.nBlocks());
88
89 return Base::equals(f: expected, tol)
90 && lin_points_.equals(other: e->lin_points_, tol)
91 && equal_with_abs_tol(A: thisMatrix, B: rhsMatrix, tol);
92 } else {
93 return false;
94 }
95}
96
97/* ************************************************************************* */
98double LinearizedJacobianFactor::error(const Values& c) const {
99 Vector errorVector = error_vector(c);
100 return 0.5 * errorVector.dot(other: errorVector);
101}
102
103/* ************************************************************************* */
104std::shared_ptr<GaussianFactor>
105LinearizedJacobianFactor::linearize(const Values& c) const {
106
107 // Create the 'terms' data structure for the Jacobian constructor
108 std::vector<std::pair<Key, Matrix> > terms;
109 for(Key key: keys()) {
110 terms.push_back(x: std::make_pair(x&: key, y: this->A(key)));
111 }
112
113 // compute rhs
114 Vector b = -error_vector(c);
115
116 return std::shared_ptr<GaussianFactor>(new JacobianFactor(terms, b, noiseModel::Unit::Create(dim: dim())));
117}
118
119/* ************************************************************************* */
120Vector LinearizedJacobianFactor::error_vector(const Values& c) const {
121
122 Vector errorVector = -b();
123
124 for(Key key: this->keys()) {
125 const Value& newPt = c.at(j: key);
126 const Value& linPt = lin_points_.at(j: key);
127 Vector d = linPt.localCoordinates_(value: newPt);
128 const constABlock A = this->A(key);
129 errorVector += A * d;
130 }
131
132 return errorVector;
133}
134
135/* ************************************************************************* */
136// LinearizedHessianFactor
137/* ************************************************************************* */
138LinearizedHessianFactor::LinearizedHessianFactor() {
139}
140
141/* ************************************************************************* */
142LinearizedHessianFactor::LinearizedHessianFactor(
143 const HessianFactor::shared_ptr& hessian, const Values& lin_points)
144 : Base(hessian, lin_points), info_(hessian->info()) {}
145
146/* ************************************************************************* */
147void LinearizedHessianFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
148
149 std::cout << s << std::endl;
150
151 std::cout << "Nonlinear Keys: ";
152 for(const Key& key: this->keys())
153 std::cout << keyFormatter(key) << " ";
154 std::cout << std::endl;
155
156 gtsam::print(A: Matrix(info_.selfadjointView()), s: "Ab^T * Ab: ");
157
158 lin_points_.print(str: "Linearization Point: ");
159}
160
161/* ************************************************************************* */
162bool LinearizedHessianFactor::equals(const NonlinearFactor& expected, double tol) const {
163
164 const This *e = dynamic_cast<const This*> (&expected);
165 if (e) {
166
167 Matrix thisMatrix = this->info_.selfadjointView();
168 thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
169 Matrix rhsMatrix = e->info_.selfadjointView();
170 rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0;
171
172 return Base::equals(f: expected, tol)
173 && lin_points_.equals(other: e->lin_points_, tol)
174 && equal_with_abs_tol(A: thisMatrix, B: rhsMatrix, tol);
175 } else {
176 return false;
177 }
178}
179
180/* ************************************************************************* */
181double LinearizedHessianFactor::error(const Values& c) const {
182
183 // Construct an error vector in key-order from the Values
184 Vector dx = Vector::Zero(size: dim());
185 size_t index = 0;
186 for(unsigned int i = 0; i < this->size(); ++i){
187 Key key = this->keys()[i];
188 const Value& newPt = c.at(j: key);
189 const Value& linPt = lin_points_.at(j: key);
190 dx.segment(start: index, n: linPt.dim()) = linPt.localCoordinates_(value: newPt);
191 index += linPt.dim();
192 }
193
194 // error 0.5*(f - 2*x'*g + x'*G*x)
195 double f = constantTerm();
196 double xtg = dx.dot(other: linearTerm());
197 double xGx = dx.transpose() * squaredTerm() * dx;
198
199 return 0.5 * (f - 2.0 * xtg + xGx);
200}
201
202/* ************************************************************************* */
203std::shared_ptr<GaussianFactor>
204LinearizedHessianFactor::linearize(const Values& c) const {
205
206 // Construct an error vector in key-order from the Values
207 Vector dx = Vector::Zero(size: dim());
208 size_t index = 0;
209 for(unsigned int i = 0; i < this->size(); ++i){
210 Key key = this->keys()[i];
211 const Value& newPt = c.at(j: key);
212 const Value& linPt = lin_points_.at(j: key);
213 dx.segment(start: index, n: linPt.dim()) = linPt.localCoordinates_(value: newPt);
214 index += linPt.dim();
215 }
216
217 // f2 = f1 - 2*dx'*g1 + dx'*G1*dx
218 //newInfo(this->size(), this->size())(0,0) += -2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm().selfadjointView<Eigen::Upper>() * dx;
219 double f = constantTerm() - 2*dx.dot(other: linearTerm()) + dx.transpose() * squaredTerm() * dx;
220
221 // g2 = g1 - G1*dx
222 //newInfo.rangeColumn(0, this->size(), this->size(), 0) -= squaredTerm().selfadjointView<Eigen::Upper>() * dx;
223 Vector g = linearTerm() - squaredTerm() * dx;
224 std::vector<Vector> gs;
225 std::size_t offset = 0;
226 for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) {
227 const std::size_t dim = info_.getDim(block: i);
228 gs.push_back(x: g.segment(start: offset, n: dim));
229 offset += dim;
230 }
231
232 // G2 = G1
233 // Do Nothing
234 std::vector<Matrix> Gs;
235 for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) {
236 Gs.push_back(x: info_.diagonalBlock(J: i));
237 for(DenseIndex j = i + 1; j < info_.nBlocks()-1; ++j) {
238 Gs.push_back(x: info_.aboveDiagonalBlock(I: i, J: j));
239 }
240 }
241
242 // Create a Hessian Factor from the modified info matrix
243 //return std::shared_ptr<GaussianFactor>(new HessianFactor(js, newInfo));
244 return std::shared_ptr<GaussianFactor>(new HessianFactor(keys(), Gs, gs, f));
245}
246
247} // \namespace aspn
248