From 4815f9cf5b2f107fe7e0f171712f44b8734a2e13 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Dec 2013 05:32:13 +0000 Subject: [PATCH] Drastically simplified solve --- gtsam/linear/KalmanFilter.cpp | 23 ++++++----------------- 1 file changed, 6 insertions(+), 17 deletions(-) diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 2d5f06ef0..8a42fe998 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -38,26 +38,15 @@ namespace gtsam { static KalmanFilter::State solve(const GaussianFactorGraph& factorGraph, const GaussianFactorGraph::Eliminate& function) { - // We will eliminate the keys in increasing order, except the last key - Ordering ordering1(factorGraph.keys()); - Key lastKey = ordering1.back(); - ordering1.pop_back(); - // Eliminate the graph using the provided Eliminate function - GaussianConditional::shared_ptr conditional; - GaussianFactor::shared_ptr factor; - boost::tie(conditional, factor) = function(factorGraph, ordering1); + Ordering ordering(factorGraph.keys()); + GaussianBayesNet::shared_ptr bayesNet = // + factorGraph.eliminateSequential(ordering, function); // As this is a filter, all we need is the posterior P(x_t). - GaussianFactorGraph graphOfRemainingFactor; - graphOfRemainingFactor += factor; - - // Eliminate it to be upper-triangular. - Ordering ordering2; - ordering2 += lastKey; - boost::tie(conditional, factor) = function(graphOfRemainingFactor, ordering2); - - return boost::make_shared(*conditional); + // This is the last GaussianConditional in the resulting BayesNet + GaussianConditional::shared_ptr posterior = *(--bayesNet->end()); + return boost::make_shared(*posterior); } /* ************************************************************************* */