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

Made KalmanFilter compile

release/4.3a0
Richard Roberts 2013-08-05 22:31:02 +00:00
parent 00c1036814
commit 114f389eeb
2 changed files with 51 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
gtsam/linear/KalmanFilter.cpp
View File

@ -20,83 +20,77 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/JacobianFactorOrdered.h>
#include <gtsam/linear/KalmanFilter.h> #include <gtsam/linear/KalmanFilter.h>
#include <gtsam/linear/HessianFactorOrdered.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/PermutationOrdered.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <boost/assign/list_of.hpp>
using namespace boost::assign;
using namespace std;
namespace gtsam { namespace gtsam {
using namespace std;
/// Auxiliary function to solve factor graph and return pointer to root conditional /// Auxiliary function to solve factor graph and return pointer to root conditional
KalmanFilter::State solve(const GaussianFactorGraphOrdered& factorGraph, bool useQR) KalmanFilter::State solve(const GaussianFactorGraph& factorGraph, const GaussianFactorGraph::Eliminate& function)
{ {
// Start indices at zero // Do a dense solve, e.g. don't use the multifrontal framework
Index nVars = 0; // Eliminate the keys in increasing order so that the last key is the one we want to keep.
internal::Reduction remapping; FastSet<Key> keysToEliminate = factorGraph.keys();
BOOST_FOREACH(const GaussianFactorGraphOrdered::sharedFactor& factor, factorGraph) FastSet<Key>::const_iterator lastKeyPos = keysToEliminate.end();
BOOST_FOREACH(Index j, *factor) -- lastKeyPos;
if(remapping.insert(make_pair(j, nVars)).second) Key remainingKey = *lastKeyPos;
++ nVars; keysToEliminate.erase(lastKeyPos);
Permutation inverseRemapping = remapping.inverse(); GaussianFactorGraph::EliminationResult result = function(factorGraph, Ordering(keysToEliminate));
GaussianFactorGraphOrdered factorGraphOrdered(factorGraph); // NOTE this shares the factors with the original!!
factorGraphOrdered.reduceWithInverse(remapping);
// Solve the factor graph // As this is a filter, all we need is the posterior P(x_t). Eliminate it to be
GaussianSequentialSolver solver(factorGraphOrdered, useQR); // upper-triangular.
GaussianBayesNetOrdered::shared_ptr bayesNet = solver.eliminate(); GaussianFactorGraph graphOfRemainingFactor;
graphOfRemainingFactor += result.second;
GaussianDensity::shared_ptr state = boost::make_shared<GaussianDensity>(
*function(graphOfRemainingFactor, Ordering(cref_list_of<1>(remainingKey))).first);
// As this is a filter, all we need is the posterior P(x_t), return state;
// so we just keep the root of the Bayes net
GaussianConditionalOrdered::shared_ptr conditional = bayesNet->back();
// Undo the remapping
factorGraphOrdered.permuteWithInverse(inverseRemapping);
conditional->permuteWithInverse(inverseRemapping);
// TODO: awful ! A copy constructor followed by ANOTHER copy constructor in make_shared?
return boost::make_shared<GaussianDensity>(*conditional);
} }
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State fuse(const KalmanFilter::State& p, KalmanFilter::State fuse(const KalmanFilter::State& p, GaussianFactor::shared_ptr newFactor,
GaussianFactorOrdered* newFactor, bool useQR) { const GaussianFactorGraph::Eliminate& function)
{
// Create a factor graph // Create a factor graph
GaussianFactorGraphOrdered factorGraph; GaussianFactorGraph factorGraph;
factorGraph += p, newFactor;
// push back previous solution and new factor
factorGraph.push_back(p->toFactor());
factorGraph.push_back(GaussianFactorOrdered::shared_ptr(newFactor));
// Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
return solve(factorGraph, useQR); return solve(factorGraph, function);
} }
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x0, GaussianFactorGraph::Eliminate KalmanFilter::factorization() const {
const SharedDiagonal& P0) { return method_ == QR ?
GaussianFactorGraph::Eliminate(EliminateQR) :
GaussianFactorGraph::Eliminate(EliminateCholesky);
}
/* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x0, const SharedDiagonal& P0) {
// Create a factor graph f(x0), eliminate it into P(x0) // Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraphOrdered factorGraph; GaussianFactorGraph factorGraph;
factorGraph.add(0, I_, x0, P0); // |x-x0|^2_diagSigma factorGraph += JacobianFactor(0, I_, x0, P0); // |x-x0|^2_diagSigma
return solve(factorGraph, useQR()); return solve(factorGraph, factorization());
} }
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) { KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) {
// Create a factor graph f(x0), eliminate it into P(x0) // Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraphOrdered factorGraph; GaussianFactorGraph factorGraph;
// 0.5*(x-x0)'*inv(Sigma)*(x-x0) factorGraph += HessianFactor(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0)
HessianFactorOrdered::shared_ptr factor(new HessianFactorOrdered(0, x, P0)); return solve(factorGraph, factorization());
factorGraph.push_back(factor);
return solve(factorGraph, useQR());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -111,7 +105,7 @@ namespace gtsam {
// The factor related to the motion model is defined as // The factor related to the motion model is defined as
// f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
Index k = step(p); Index k = step(p);
return fuse(p, new JacobianFactorOrdered(k, -F, k + 1, I_, B * u, model), useQR()); return fuse(p, boost::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model), factorization());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -119,7 +113,7 @@ namespace gtsam {
const Matrix& B, const Vector& u, const Matrix& Q) { const Matrix& B, const Vector& u, const Matrix& Q) {
#ifndef NDEBUG #ifndef NDEBUG
int n = F.cols(); DenseIndex n = F.cols();
assert(F.rows() == n); assert(F.rows() == n);
assert(B.rows() == n); assert(B.rows() == n);
assert(B.cols() == u.size()); assert(B.cols() == u.size());
@ -136,8 +130,7 @@ namespace gtsam {
Vector b = B * u, g2 = M * b, g1 = -Ft * g2; Vector b = B * u, g2 = M * b, g1 = -Ft * g2;
double f = dot(b, g2); double f = dot(b, g2);
Index k = step(p); Index k = step(p);
return fuse(p, new HessianFactorOrdered(k, k + 1, G11, G12, g1, G22, g2, f), return fuse(p, boost::make_shared<HessianFactor>(k, k + 1, G11, G12, g1, G22, g2, f), factorization());
useQR());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -146,7 +139,7 @@ namespace gtsam {
// Nhe factor related to the motion model is defined as // Nhe factor related to the motion model is defined as
// f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2 // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2
Index k = step(p); Index k = step(p);
return fuse(p, new JacobianFactorOrdered(k, A0, k + 1, A1, b, model), useQR()); return fuse(p, boost::make_shared<JacobianFactor>(k, A0, k + 1, A1, b, model), factorization());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -156,7 +149,7 @@ namespace gtsam {
// f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T
// = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T // = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T
Index k = step(p); Index k = step(p);
return fuse(p, new JacobianFactorOrdered(k, H, z, model), useQR()); return fuse(p, boost::make_shared<JacobianFactor>(k, H, z, model), factorization());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -167,7 +160,7 @@ namespace gtsam {
Matrix G = Ht * M * H; Matrix G = Ht * M * H;
Vector g = Ht * M * z; Vector g = Ht * M * z;
double f = dot(z, M * z); double f = dot(z, M * z);
return fuse(p, new HessianFactorOrdered(k, G, g, f), useQR()); return fuse(p, boost::make_shared<HessianFactor>(k, G, g, f), factorization());
} }
/* ************************************************************************* */ /* ************************************************************************* */

5
gtsam/linear/KalmanFilter.h
View File

@ -20,6 +20,7 @@
#pragma once #pragma once
#include <gtsam/linear/GaussianDensity.h> #include <gtsam/linear/GaussianDensity.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#ifndef KALMANFILTER_DEFAULT_FACTORIZATION #ifndef KALMANFILTER_DEFAULT_FACTORIZATION
@ -60,9 +61,7 @@ namespace gtsam {
const Matrix I_; /** identity matrix of size n*n */ const Matrix I_; /** identity matrix of size n*n */
const Factorization method_; /** algorithm */ const Factorization method_; /** algorithm */
bool useQR() const { GaussianFactorGraph::Eliminate factorization() const;
return method_ == QR;
}
public: public: