12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Removed dependency of eloboratePoint2KalmnFIlter example on the LinearizedFactor class

release/4.3a0
Stephen Williams 2011-08-25 13:34:06 +00:00
parent c503ed45ed
commit b39970ad9a
4 changed files with 154 additions and 405 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

260
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -21,9 +21,6 @@
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LinearizedFactor.h>
//#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Key.h>
@ -44,7 +41,13 @@ int main() {
// [code below basically does SRIF with LDL]
// Create a factor graph to perform the inference
NonlinearFactorGraph<Values>::shared_ptr nonlinearFactorGraph(new NonlinearFactorGraph<Values>);
GaussianFactorGraph::shared_ptr linearFactorGraph(new GaussianFactorGraph);
// Create the desired ordering
Ordering::shared_ptr ordering(new Ordering);
// Create a structure to hold the linearization points
Values linearizationPoints;
// Ground truth example
// Start at origin, move to the right (x-axis): 0,0 0,1 0,2
@ -52,15 +55,19 @@ int main() {
// Measurements are GPS like, (x-z)^2, where z is a 2D measurement
// i.e., we should get 0,0 0,1 0,2 if there is no noise
// Create new state variable, x0
// Create new state variable
Key x0(0);
ordering->insert(x0, 0);
// Initialize state x0 (2D point) at origin by adding a prior factor, i.e., Bayes net P(x0)
// This is equivalent to x_0 and P_0
Point2 x_initial(0,0);
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
PriorFactor<Values, Key> factor1(x0, x_initial, P_initial);
nonlinearFactorGraph->add(factor1);
// Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x0, x_initial);
linearFactorGraph->push_back(factor1.linearize(linearizationPoints, *ordering));
// Now predict the state at t=1, i.e. argmax_{x1} P(x1) = P(x1|x0) P(x0)
// In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}
@ -83,10 +90,14 @@ int main() {
// = (F - I)*x_{t} + B*u_{t}
// = B*u_{t} (for our example)
Key x1(1);
ordering->insert(x1, 1);
Point2 difference(1,0);
SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
BetweenFactor<Values, Key> factor2(x0, x1, difference, Q);
nonlinearFactorGraph->add(factor2);
// Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x1, x_initial);
linearFactorGraph->push_back(factor2.linearize(linearizationPoints, *ordering));
// We have now made the small factor graph f1-(x0)-f2-(x1)
// where factor f1 is just the prior from time t0, P(x0)
@ -100,40 +111,52 @@ int main() {
// variables. Also, the nonlinear factors are linearized around an initial estimate. For a true linear
// system, the initial estimate is not important.
// Create the desired ordering
Ordering::shared_ptr ordering(new Ordering);
ordering->insert(x0, 0);
ordering->insert(x1, 1);
// Create a set of linearization points at (0,0). Since this is a linear system, the actual linearization point doesn't matter
Values linearizationPoints;
linearizationPoints.insert(x0, Point2(0,0));
linearizationPoints.insert(x1, Point2(0,0));
// Convert the nonlinear factor graph into an "ordered" linear factor graph
GaussianFactorGraph::shared_ptr linearFactorGraph = nonlinearFactorGraph->linearize(linearizationPoints, *ordering)->dynamicCastFactors<GaussianFactorGraph>();
//->template dynamicCastFactors<GaussianFactorGraph>()
// Solve the linear factor graph, converting it into a linear Bayes Network ( P(x0,x1) = P(x0|x1)*P(x1) )
GaussianSequentialSolver solver0(*linearFactorGraph);
GaussianBayesNet::shared_ptr linearBayesNet = solver0.eliminate();
// If needed, the current estimate of x1 may be extracted from the Bayes Network
// Extract the current estimate of x1,P1 from the Bayes Network
VectorValues result = optimize(*linearBayesNet);
Point2 x1_predict = linearizationPoints[x1].expmap(result[ordering->at(x1)]);
x1_predict.print("X1 Predict");
// Convert the root conditional, P(x1) in this case, into a Prior for the next step
LinearizedFactor<Values,Key>::KeyLookup lookup;
lookup[0] = x0; lookup[1] = x1;
LinearizedFactor<Values,Key> factor3(linearBayesNet->back()->toFactor(), lookup, linearizationPoints);
// Update the new linearization point to the new estimate
linearizationPoints.update(x1, x1_predict);
// Create a new, empty graph and add the prior from the previous step
nonlinearFactorGraph = NonlinearFactorGraph<Values>::shared_ptr(new NonlinearFactorGraph<Values>);
nonlinearFactorGraph->add(factor3);
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step
// Some care must be done here, as the linearization point in future steps will be different
// than what was used when the factor was created.
// f = || F*dx1' - (F*x0 - x1) ||^2, originally linearized at x1 = x0
// After this step, the factor needs to be linearized around x1 = x1_predict
// This changes the factor to f = || F*dx1'' - b'' ||^2
// = || F*(dx1' - (dx1' - dx1'')) - b'' ||^2
// = || F*dx1' - F*(dx1' - dx1'') - b'' ||^2
// = || F*dx1' - (b'' + F(dx1' - dx1'')) ||^2
// -> b' = b'' + F(dx1' - dx1'')
// -> b'' = b' - F(dx1' - dx1'')
// = || F*dx1'' - (b' - F(dx1' - dx1'')) ||^2
// = || F*dx1'' - (b' - F(x_predict - x_inital)) ||^2
const GaussianConditional::shared_ptr& cg0 = linearBayesNet->back();
assert(cg0->nrFrontals() == 1);
assert(cg0->nrParents() == 0);
linearFactorGraph->add(0, cg0->get_R(), cg0->get_d() - cg0->get_R()*result[ordering->at(x1)], noiseModel::Diagonal::Sigmas(cg0->get_sigmas(), true));
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
ordering->insert(x1, 0);
// Now, a measurement, z1, has been received, and the Kalman Filter should be "Updated"/"Corrected"
// This is equivalent to saying P(x1|z1) ~ P(z1|x1)*P(x1) ~ f3(x1)*f4(x1;z1)
// So, we need to create the measurement factor, f4
// For the Kalman Filter, this the the measurement function, h(x_{t}) = z_{t}
// where f3 is the prior from the previous step, and
// where f4 is a measurement factor
//
// So, now we need to create the measurement factor, f4
// For the Kalman Filter, this is the measurement function, h(x_{t}) = z_{t}
// Assuming the system is linear, this will be of the form h(x_{t}) = H*x_{t} + v
// where H is the observation model/matrix, and v is zero-mean, Gaussian white noise with covariance R
//
@ -146,85 +169,99 @@ int main() {
// In the case of factor graphs, the factor related to the measurements would be defined as
// f4 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T
// = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T
// This can again be modeled using the PriorFactor, where the mean is z_{t} and the covariance is R.
// This can be modeled using the PriorFactor, where the mean is z_{t} and the covariance is R.
Point2 z1(1.0, 0.0);
SharedDiagonal R1 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
PriorFactor<Values, Key> factor4(x1, z1, R1);
nonlinearFactorGraph->add(factor4);
// Linearize the factor and add it to the linear factor graph
linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering));
// We have now made the small factor graph f3-(x1)-f2
// We have now made the small factor graph f3-(x1)-f4
// where factor f3 is the prior from previous time ( P(x1) )
// and factor f4 is from the measurement, z1 ( P(x1|z1) )
// Eliminate this in order x1, to get Bayes net P(x1)
// As this is a filter, all we need is the posterior P(x1), so we just keep the root of the Bayes net
// We solve as before...
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
ordering->insert(x1, 0);
// Create a set of linearization points at (0,0). Since this is a linear system, the actual linearization point doesn't matter
linearizationPoints.insert(x1, Point2(0,0));
// Convert the nonlinear factor graph into an "ordered" linear factor graph
linearFactorGraph = nonlinearFactorGraph->linearize(linearizationPoints, *ordering)->dynamicCastFactors<gtsam::GaussianFactorGraph>();
// Solve the linear factor graph, converting it into a linear Bayes Network ( P(x0,x1) = P(x0|x1)*P(x1) )
GaussianSequentialSolver solver1(*linearFactorGraph);
linearBayesNet = solver1.eliminate();
// If needed, the current estimate of x1 may be extracted from the Bayes Network
// Extract the current estimate of x1 from the Bayes Network
result = optimize(*linearBayesNet);
Point2 x1_update = linearizationPoints[x1].expmap(result[ordering->at(x1)]);
x1_update.print("X1 Update");
// Convert the root conditional, P(x1) in this case, into a Prior for the next step
lookup[0] = x1;
LinearizedFactor<Values,Key> factor5(linearBayesNet->back()->toFactor(), lookup, linearizationPoints);
// Update the linearization point to the new estimate
linearizationPoints.update(x1, x1_update);
// Create a new, empty graph and add the prior from the previous step
nonlinearFactorGraph = NonlinearFactorGraph<Values>::shared_ptr(new NonlinearFactorGraph<Values>);
nonlinearFactorGraph->add(factor5);
// Wash, rinse, repeat for another time step
Key x2(2);
difference = Point2(1,0);
Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
BetweenFactor<Values, Key> factor6(x1, x2, difference, Q);
nonlinearFactorGraph->add(factor6);
// Create a new, empty graph and add the prior from the previous step
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step
const GaussianConditional::shared_ptr& cg1 = linearBayesNet->back();
assert(cg1->nrFrontals() == 1);
assert(cg1->nrParents() == 0);
linearFactorGraph->add(0, cg1->get_R(), cg1->get_d() - cg1->get_R()*result[ordering->at(x1)], noiseModel::Diagonal::Sigmas(cg1->get_sigmas(), true));
// Create a key for the new state
Key x2(2);
// We have now made the small factor graph f5-(x1)-f6-(x2)
// Eliminate this in order x1, x2, to get Bayes net P(x1|x2)P(x2)
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
ordering->insert(x1, 0);
ordering->insert(x2, 1);
// Create a set of linearization points at (0,0). Since this is a linear system, the actual linearization point doesn't matter
linearizationPoints.insert(x1, Point2(0,0));
linearizationPoints.insert(x2, Point2(0,0));
// Convert the nonlinear factor graph into an "ordered" linear factor graph
linearFactorGraph = nonlinearFactorGraph->linearize(linearizationPoints, *ordering)->dynamicCastFactors<gtsam::GaussianFactorGraph>();
// Create a nonlinear factor describing the motion model
difference = Point2(1,0);
Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
BetweenFactor<Values, Key> factor6(x1, x2, difference, Q);
// Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x2, x1_update);
linearFactorGraph->push_back(factor6.linearize(linearizationPoints, *ordering));
// Solve the linear factor graph, converting it into a linear Bayes Network ( P(x1,x2) = P(x1|x2)*P(x2) )
GaussianSequentialSolver solver2(*linearFactorGraph);
linearBayesNet = solver2.eliminate();
// If needed, the current estimate of x2 may be extracted from the Bayes Network
// Extract the current estimate of x2 from the Bayes Network
result = optimize(*linearBayesNet);
Point2 x2_predict = linearizationPoints[x2].expmap(result[ordering->at(x2)]);
x2_predict.print("X2 Predict");
// Convert the root conditional, P(x2) in this case, into a Prior for the next step
lookup[0] = x1; lookup[1] = x2;
LinearizedFactor<Values,Key> factor7(linearBayesNet->back()->toFactor(), lookup, linearizationPoints);
// Update the linearization point to the new estimate
linearizationPoints.update(x2, x2_predict);
// Now add the next measurement
// Create a new, empty graph and add the prior from the previous step
nonlinearFactorGraph = NonlinearFactorGraph<Values>::shared_ptr(new NonlinearFactorGraph<Values>);
nonlinearFactorGraph->add(factor7);
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step
const GaussianConditional::shared_ptr& cg2 = linearBayesNet->back();
assert(cg2->nrFrontals() == 1);
assert(cg2->nrParents() == 0);
linearFactorGraph->add(0, cg2->get_R(), cg2->get_d() - cg2->get_R()*result[ordering->at(x2)], noiseModel::Diagonal::Sigmas(cg2->get_sigmas(), true));
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
ordering->insert(x2, 0);
// And update using z2 ...
Point2 z2(2.0, 0.0);
SharedDiagonal R2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
PriorFactor<Values, Key> factor8(x2, z2, R2);
nonlinearFactorGraph->add(factor8);
// Linearize the factor and add it to the linear factor graph
linearFactorGraph->push_back(factor8.linearize(linearizationPoints, *ordering));
// We have now made the small factor graph f7-(x2)-f8
// where factor f7 is the prior from previous time ( P(x2) )
@ -233,72 +270,85 @@ int main() {
// As this is a filter, all we need is the posterior P(x2), so we just keep the root of the Bayes net
// We solve as before...
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
ordering->insert(x2, 0);
// Create a set of linearization points at (0,0). Since this is a linear system, the actual linearization point doesn't matter
linearizationPoints.insert(x2, Point2(0,0));
// Convert the nonlinear factor graph into an "ordered" linear factor graph
linearFactorGraph = nonlinearFactorGraph->linearize(linearizationPoints, *ordering)->dynamicCastFactors<gtsam::GaussianFactorGraph>();
// Solve the linear factor graph, converting it into a linear Bayes Network ( P(x0,x1) = P(x0|x1)*P(x1) )
GaussianSequentialSolver solver3(*linearFactorGraph);
linearBayesNet = solver3.eliminate();
// If needed, the current estimate of x2 may be extracted from the Bayes Network
// Extract the current estimate of x2 from the Bayes Network
result = optimize(*linearBayesNet);
Point2 x2_update = linearizationPoints[x2].expmap(result[ordering->at(x2)]);
x2_update.print("X2 Update");
// Convert the root conditional, P(x1) in this case, into a Prior for the next step
lookup[0] = x2;
LinearizedFactor<Values,Key> factor9(linearBayesNet->back()->toFactor(), lookup, linearizationPoints);
// Update the linearization point to the new estimate
linearizationPoints.update(x2, x2_update);
// Wash, rinse, repeat for a third time step
// Create a new, empty graph and add the prior from the previous step
nonlinearFactorGraph = NonlinearFactorGraph<Values>::shared_ptr(new NonlinearFactorGraph<Values>);
nonlinearFactorGraph->add(factor9);
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step
const GaussianConditional::shared_ptr& cg3 = linearBayesNet->back();
assert(cg3->nrFrontals() == 1);
assert(cg3->nrParents() == 0);
linearFactorGraph->add(0, cg3->get_R(), cg3->get_d() - cg3->get_R()*result[ordering->at(x2)], noiseModel::Diagonal::Sigmas(cg3->get_sigmas(), true));
// Wash, rinse, repeat for another time step
// Create a key for the new state
Key x3(3);
difference = Point2(1,0);
Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
BetweenFactor<Values, Key> factor10(x2, x3, difference, Q);
nonlinearFactorGraph->add(factor10);
// We have now made the small factor graph f9-(x2)-f10-(x3)
// Eliminate this in order x2, x3, to get Bayes net P(x2|x3)P(x3)
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
ordering->insert(x2, 0);
ordering->insert(x3, 1);
// Create a set of linearization points at (0,0). Since this is a linear system, the actual linearization point doesn't matter
linearizationPoints.insert(x2, Point2(0,0));
linearizationPoints.insert(x3, Point2(0,0));
// Convert the nonlinear factor graph into an "ordered" linear factor graph
linearFactorGraph = nonlinearFactorGraph->linearize(linearizationPoints, *ordering)->dynamicCastFactors<gtsam::GaussianFactorGraph>();
// Solve the linear factor graph, converting it into a linear Bayes Network ( P(x2,x3) = P(x2|x3)*P(x3) )
// Create a nonlinear factor describing the motion model
difference = Point2(1,0);
Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
BetweenFactor<Values, Key> factor10(x2, x3, difference, Q);
// Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x3, x2_update);
linearFactorGraph->push_back(factor10.linearize(linearizationPoints, *ordering));
// Solve the linear factor graph, converting it into a linear Bayes Network ( P(x1,x2) = P(x1|x2)*P(x2) )
GaussianSequentialSolver solver4(*linearFactorGraph);
linearBayesNet = solver4.eliminate();
// If needed, the current estimate of x3 may be extracted from the Bayes Network
// Extract the current estimate of x3 from the Bayes Network
result = optimize(*linearBayesNet);
Point2 x3_predict = linearizationPoints[x3].expmap(result[ordering->at(x3)]);
x3_predict.print("X3 Predict");
// Convert the root conditional, P(x3) in this case, into a Prior for the next step
lookup[0] = x2; lookup[1] = x3;
LinearizedFactor<Values,Key> factor11(linearBayesNet->back()->toFactor(), lookup, linearizationPoints);
// Update the linearization point to the new estimate
linearizationPoints.update(x3, x3_predict);
// Now add the next measurement
// Create a new, empty graph and add the prior from the previous step
nonlinearFactorGraph = NonlinearFactorGraph<Values>::shared_ptr(new NonlinearFactorGraph<Values>);
nonlinearFactorGraph->add(factor11);
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step
const GaussianConditional::shared_ptr& cg4 = linearBayesNet->back();
assert(cg4->nrFrontals() == 1);
assert(cg4->nrParents() == 0);
linearFactorGraph->add(0, cg4->get_R(), cg4->get_d() - cg4->get_R()*result[ordering->at(x3)], noiseModel::Diagonal::Sigmas(cg4->get_sigmas(), true));
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
ordering->insert(x3, 0);
// And update using z3 ...
Point2 z3(3.0, 0.0);
SharedDiagonal R3 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
PriorFactor<Values, Key> factor12(x3, z3, R3);
nonlinearFactorGraph->add(factor12);
// Linearize the factor and add it to the linear factor graph
linearFactorGraph->push_back(factor12.linearize(linearizationPoints, *ordering));
// We have now made the small factor graph f11-(x3)-f12
// where factor f11 is the prior from previous time ( P(x3) )
@ -307,21 +357,19 @@ int main() {
// As this is a filter, all we need is the posterior P(x3), so we just keep the root of the Bayes net
// We solve as before...
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
ordering->insert(x3, 0);
// Create a set of linearization points at (0,0). Since this is a linear system, the actual linearization point doesn't matter
linearizationPoints.insert(x3, Point2(0,0));
// Convert the nonlinear factor graph into an "ordered" linear factor graph
linearFactorGraph = nonlinearFactorGraph->linearize(linearizationPoints, *ordering)->dynamicCastFactors<gtsam::GaussianFactorGraph>();
// Solve the linear factor graph, converting it into a linear Bayes Network ( P(x0,x1) = P(x0|x1)*P(x1) )
GaussianSequentialSolver solver5(*linearFactorGraph);
linearBayesNet = solver5.eliminate();
// If needed, the current estimate of x1 may be extracted from the Bayes Network
// Extract the current estimate of x2 from the Bayes Network
result = optimize(*linearBayesNet);
Point2 x3_update = linearizationPoints[x3].expmap(result[ordering->at(x3)]);
x3_update.print("X3 Update");
// Update the linearization point to the new estimate
linearizationPoints.update(x3, x3_update);
return 0;
}

93
gtsam/nonlinear/LinearApproxFactor-inl.h
View File

@ -1,93 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file LinearApproxFactor.h
* @brief A dummy factor that allows a linear factor to act as a nonlinear factor
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/nonlinear/LinearApproxFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <iostream>
#include <boost/foreach.hpp>
namespace gtsam {
/* ************************************************************************* */
template <class VALUES, class KEY>
Vector LinearApproxFactor<VALUES,KEY>::unwhitenedError(const VALUES& c) const {
// extract the points in the new values
Vector ret = b_;
BOOST_FOREACH(const KEY& key, nonlinearKeys_) {
X newPt = c[key], linPt = lin_points_[key];
Vector d = linPt.logmap(newPt);
const Matrix& A = matrices_.at(key);
ret -= A * d;
}
return ret;
}
/* ************************************************************************* */
template <class VALUES, class KEY>
boost::shared_ptr<GaussianFactor>
LinearApproxFactor<VALUES,KEY>::linearize(const VALUES& c, const Ordering& ordering) const {
// sort by varid - only known at linearization time
typedef std::map<Index, Matrix> VarMatrixMap;
VarMatrixMap sorting_terms;
BOOST_FOREACH(const typename KeyMatrixMap::value_type& p, matrices_)
sorting_terms.insert(std::make_pair(ordering[p.first], p.second));
// move into terms
std::vector<std::pair<Index, Matrix> > terms;
BOOST_FOREACH(const VarMatrixMap::value_type& p, sorting_terms)
terms.push_back(p);
// compute rhs: adjust current by whitened update
Vector b = unwhitenedError(c) + b_; // remove original b
this->noiseModel_->whitenInPlace(b);
return boost::shared_ptr<GaussianFactor>(new JacobianFactor(terms, b - b_, model_));
}
/* ************************************************************************* */
template <class VALUES, class KEY>
IndexFactor::shared_ptr
LinearApproxFactor<VALUES,KEY>::symbolic(const Ordering& ordering) const {
std::vector<Index> key_ids(this->keys_.size());
size_t i=0;
BOOST_FOREACH(const Symbol& key, this->keys_)
key_ids[i++] = ordering[key];
std::sort(key_ids.begin(), key_ids.end());
return boost::shared_ptr<IndexFactor>(new IndexFactor(key_ids.begin(), key_ids.end()));
}
/* ************************************************************************* */
template <class VALUES, class KEY>
void LinearApproxFactor<VALUES,KEY>::print(const std::string& s) const {
this->noiseModel_->print(s + std::string(" model"));
BOOST_FOREACH(const typename KeyMatrixMap::value_type& p, matrices_) {
gtsam::print(p.second, (std::string) p.first);
}
gtsam::print(b_, std::string("b"));
std::cout << " nonlinear keys: ";
BOOST_FOREACH(const KEY& key, nonlinearKeys_)
key.print(" ");
lin_points_.print("Linearization Point");
}
} // \namespace gtsam

123
gtsam/nonlinear/LinearApproxFactor.h
View File

@ -1,123 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file LinearApproxFactor.h
* @brief A dummy factor that allows a linear factor to act as a nonlinear factor
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <vector>
namespace gtsam{
/**
* A dummy factor that takes a linearized factor and inserts it into
* a nonlinear graph. This version uses exactly one type of variable.
*
* IMPORTANT: Don't use this factor - used LinearizedFactor instead
*/
template <class VALUES, class KEY>
class LinearApproxFactor : public NonlinearFactor<VALUES> {
public:
/** type for the variable */
typedef typename KEY::Value X;
/** base type */
typedef NonlinearFactor<VALUES> Base;
/** shared pointer for convenience */
typedef boost::shared_ptr<LinearApproxFactor<VALUES,KEY> > shared_ptr;
/** typedefs for key vectors */
typedef std::vector<KEY> KeyVector;
protected:
/** hold onto the factor itself */
// store components of a jacobian factor
typedef std::map<KEY, Matrix> KeyMatrixMap;
KeyMatrixMap matrices_;
Vector b_;
SharedDiagonal model_; /// separate from the noisemodel in NonlinearFactor due to Diagonal/Gaussian
/** linearization points for error calculation */
VALUES lin_points_;
/** keep keys for the factor */
KeyVector nonlinearKeys_;
/** default constructor for serialization */
LinearApproxFactor() {}
/**
* use this for derived classes with keys that don't copy easily
*/
LinearApproxFactor(size_t dim, const VALUES& lin_points)
: Base(noiseModel::Unit::Create(dim)), lin_points_(lin_points) {}
LinearApproxFactor(SharedDiagonal model)
: Base(model), model_(model) {}
LinearApproxFactor(SharedDiagonal model, const VALUES& lin_points)
: Base(model), model_(model), lin_points_(lin_points) {}
public:
virtual ~LinearApproxFactor() {}
/** Vector of errors, unwhitened ! */
virtual Vector unwhitenedError(const VALUES& c) const;
/**
* linearize to a GaussianFactor
* Reconstructs the linear factor from components to ensure that
* the ordering is correct
*/
virtual boost::shared_ptr<GaussianFactor> linearize(
const VALUES& c, const Ordering& ordering) const;
/**
* Create a symbolic factor using the given ordering to determine the
* variable indices.
*/
IndexFactor::shared_ptr symbolic(const Ordering& ordering) const;
/** get access to nonlinear keys */
KeyVector nonlinearKeys() const { return nonlinearKeys_; }
/** override print function */
virtual void print(const std::string& s="") const;
/** access to b vector of gaussian */
Vector get_b() const { return b_; }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(matrices_);
ar & BOOST_SERIALIZATION_NVP(b_);
ar & BOOST_SERIALIZATION_NVP(model_);
ar & BOOST_SERIALIZATION_NVP(lin_points_);
ar & BOOST_SERIALIZATION_NVP(nonlinearKeys_);
}
};
} // \namespace gtsam

83
gtsam/nonlinear/LinearizedFactor.h
View File

@ -1,83 +0,0 @@
/*
* @file LinearizedFactor.h
* @brief A dummy factor that allows a linear factor to act as a nonlinear factor
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/nonlinear/LinearApproxFactor-inl.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/Matrix.h>
#include <vector>
#include <iostream>
#include <boost/foreach.hpp>
namespace gtsam{
/**
* Wrapper around a LinearApproxFactor with some extra interfaces
*/
template <class VALUES, class KEY>
class LinearizedFactor : public LinearApproxFactor<VALUES, KEY> {
public:
/** base type */
typedef LinearApproxFactor<VALUES, KEY> Base;
/** shared pointer for convenience */
typedef boost::shared_ptr<LinearizedFactor<VALUES,KEY> > shared_ptr;
/** decoder for keys - avoids the use of a full ordering */
typedef std::map<Index, KEY> KeyLookup;
protected:
/** default constructor for serialization */
LinearizedFactor() {}
public:
/**
* Use this constructor when starting with linear keys and adding in a label
* @param label is a label to add to the keys
* @param lin_factor is a gaussian factor with linear keys (no labels baked in)
* @param values is assumed to have the correct key structure with labels
*/
LinearizedFactor(JacobianFactor::shared_ptr lin_factor,
const KeyLookup& decoder, const VALUES& lin_points)
: Base(lin_factor->get_model()) {
this->b_ = lin_factor->getb();
BOOST_FOREACH(const Index& idx, *lin_factor) {
// find nonlinear multirobot key
typename KeyLookup::const_iterator decode_it = decoder.find(idx);
assert(decode_it != decoder.end());
KEY key = decode_it->second;
// extract linearization point
assert(lin_points.exists(key));
typename KEY::Value value = lin_points[key];
this->lin_points_.insert(key, value); // NOTE: will not overwrite
// extract Jacobian
Matrix A = lin_factor->getA(lin_factor->find(idx));
this->matrices_.insert(std::make_pair(key, A));
// store keys
this->nonlinearKeys_.push_back(key);
this->keys_.push_back(key);
}
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("LinearApproxFactor",
boost::serialization::base_object<Base>(*this));
}
};
} // \namespace gtsam