From 597d617808787858ec4990e31b96cd2d8c66636a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Dec 2013 05:52:38 +0000 Subject: [PATCH] removed checking const variable each time and created appropriate Eliminate function in constructor instead. --- gtsam/linear/KalmanFilter.cpp | 41 ++++++++++++++--------------------- gtsam/linear/KalmanFilter.h | 9 +++++--- 2 files changed, 22 insertions(+), 28 deletions(-) diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 8a42fe998..6e413f846 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -34,14 +34,15 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ // Auxiliary function to solve factor graph and return pointer to root conditional -static KalmanFilter::State solve(const GaussianFactorGraph& factorGraph, - const GaussianFactorGraph::Eliminate& function) { +KalmanFilter::State // +KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const { // Eliminate the graph using the provided Eliminate function Ordering ordering(factorGraph.keys()); GaussianBayesNet::shared_ptr bayesNet = // - factorGraph.eliminateSequential(ordering, function); + factorGraph.eliminateSequential(ordering, function_); // As this is a filter, all we need is the posterior P(x_t). // This is the last GaussianConditional in the resulting BayesNet @@ -50,22 +51,16 @@ static KalmanFilter::State solve(const GaussianFactorGraph& factorGraph, } /* ************************************************************************* */ -static KalmanFilter::State fuse(const KalmanFilter::State& p, - GaussianFactor::shared_ptr newFactor, - const GaussianFactorGraph::Eliminate& function) { +// Auxiliary function to create a small graph for predict or update and solve +KalmanFilter::State // +KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) { + // Create a factor graph GaussianFactorGraph factorGraph; factorGraph += p, newFactor; // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) - return solve(factorGraph, function); -} - -/* ************************************************************************* */ -GaussianFactorGraph::Eliminate KalmanFilter::factorization() const { - return - method_ == QR ? GaussianFactorGraph::Eliminate(EliminateQR) : - GaussianFactorGraph::Eliminate(EliminateCholesky); + return solve(factorGraph); } /* ************************************************************************* */ @@ -75,7 +70,7 @@ KalmanFilter::State KalmanFilter::init(const Vector& x0, // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; factorGraph += JacobianFactor(0, I_, x0, P0); // |x-x0|^2_diagSigma - return solve(factorGraph, factorization()); + return solve(factorGraph); } /* ************************************************************************* */ @@ -84,7 +79,7 @@ KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) { // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; factorGraph += HessianFactor(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0) - return solve(factorGraph, factorization()); + return solve(factorGraph); } /* ************************************************************************* */ @@ -100,8 +95,7 @@ KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F, // 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 Key k = step(p); return fuse(p, - boost::make_shared(k, -F, k + 1, I_, B * u, model), - factorization()); + boost::make_shared(k, -F, k + 1, I_, B * u, model)); } /* ************************************************************************* */ @@ -127,8 +121,7 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, double f = dot(b, g2); Key k = step(p); return fuse(p, - boost::make_shared(k, k + 1, G11, G12, g1, G22, g2, f), - factorization()); + boost::make_shared(k, k + 1, G11, G12, g1, G22, g2, f)); } /* ************************************************************************* */ @@ -137,8 +130,7 @@ KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0, // 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 Key k = step(p); - return fuse(p, boost::make_shared(k, A0, k + 1, A1, b, model), - factorization()); + return fuse(p, boost::make_shared(k, A0, k + 1, A1, b, model)); } /* ************************************************************************* */ @@ -148,8 +140,7 @@ KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H, // 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 Key k = step(p); - return fuse(p, boost::make_shared(k, H, z, model), - factorization()); + return fuse(p, boost::make_shared(k, H, z, model)); } /* ************************************************************************* */ @@ -160,7 +151,7 @@ KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, Matrix G = Ht * M * H; Vector g = Ht * M * z; double f = dot(z, M * z); - return fuse(p, boost::make_shared(k, G, g, f), factorization()); + return fuse(p, boost::make_shared(k, G, g, f)); } /* ************************************************************************* */ diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index ec10608ef..7c738d639 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -59,16 +59,19 @@ private: const size_t n_; /** dimensionality of state */ const Matrix I_; /** identity matrix of size n*n */ - const Factorization method_; /** algorithm */ + const GaussianFactorGraph::Eliminate function_; /** algorithm */ - GaussianFactorGraph::Eliminate factorization() const; + State solve(const GaussianFactorGraph& factorGraph) const; + State fuse(const State& p, GaussianFactor::shared_ptr newFactor); public: // Constructor KalmanFilter(size_t n, Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION) : - n_(n), I_(eye(n_, n_)), method_(method) { + n_(n), I_(eye(n_, n_)), function_( + method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) : + GaussianFactorGraph::Eliminate(EliminateCholesky)) { } /**