From f4bfc435ff6e1a6425c62d26924e30b59f784d24 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 22 Aug 2011 15:35:24 +0000 Subject: [PATCH] Added first of the Kalman Filter examples --- examples/Makefile.am | 1 + examples/elaboratePoint2KalmanFilter.cpp | 304 +++++++++++++++++++++-- gtsam/nonlinear/LinearApproxFactor-inl.h | 93 +++++++ gtsam/nonlinear/LinearApproxFactor.h | 123 +++++++++ gtsam/nonlinear/LinearizedFactor.h | 83 +++++++ 5 files changed, 584 insertions(+), 20 deletions(-) create mode 100644 gtsam/nonlinear/LinearApproxFactor-inl.h create mode 100644 gtsam/nonlinear/LinearApproxFactor.h create mode 100644 gtsam/nonlinear/LinearizedFactor.h diff --git a/examples/Makefile.am b/examples/Makefile.am index c33a4abf7..87744e8c0 100644 --- a/examples/Makefile.am +++ b/examples/Makefile.am @@ -18,6 +18,7 @@ noinst_PROGRAMS += Pose2SLAMExample_easy # Solves SLAM example from tutorial noinst_PROGRAMS += Pose2SLAMExample_advanced # Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver +noinst_PROGRAMS += elaboratePoint2KalmanFilter # simple linear Kalman filter on a moving 2D point, but done using factor graphs SUBDIRS = vSLAMexample # visual SLAM examples with 3D point landmarks and 6D camera poses #---------------------------------------------------------------------------------------------------- # rules to build local programs diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 724fab387..672ee4f92 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -19,45 +19,309 @@ * @Author: Stephen Williams */ +#include +#include +#include +//#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + using namespace std; using namespace gtsam; +typedef TypedSymbol Key; /// Variable labels for a specific type +typedef LieValues Values; /// Class to store values - acts as a state for the system + int main() { // [code below basically does SRIF with LDL] + // Create a factor graph to perform the inference + NonlinearFactorGraph::shared_ptr nonlinearFactorGraph(new NonlinearFactorGraph); + // Ground truth example // Start at origin, move to the right (x-axis): 0,0 0,1 0,2 // Motion model is just moving to the right (x'-x)^2 // 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 - // Init state x1 (2D point) at origin, i.e., Bayes net P(x1) + // Create new state variable, x0 + Key x0(0); - // Update using z1, P(x1|z1) ~ P(x1)P(z1|x1) ~ f1(x1) f2(x1;z1) - // Hence, make small factor graph f1-(x1)-f2 - // Eliminate to get Bayes net P(x1|z1) + // 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 factor1(x0, x_initial, P_initial); + nonlinearFactorGraph->add(factor1); - // Predict x2 P(x2|z1) = \int P(x2|x1) P(x1|z1) - // Make a small factor graph f1-(x1)-f2-(x2) - // where factor f1 is just the posterior from time t1, P(x1|z1) - // where factor f2 is the motion model (x'-x)^2 - // Now, eliminate this in order x1, x2, to get Bayes net P(x1|x2)P(x2) - // so, we just keep the root of the Bayes net, P(x2) which is really P(x2|z1) + // 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} + // For the Kalman Filter, this requires a motion model, f(x_{t}) = x_{t+1|t) + // Assuming the system is linear, this will be of the form f(x_{t}) = F*x_{t} + B*u_{t} + w + // where F is the state transition model/matrix, B is the control input model, + // and w is zero-mean, Gaussian white noise with covariance Q + // Note, in some models, Q is actually derived as G*w*G^T where w models uncertainty of some + // physical property, such as velocity or acceleration, and G is derived from physics + // + // For the purposes of this example, let us assume we are using a constant-position model and + // the controls are driving the point to the right at 1 m/s. Then, F = [1 0 ; 0 1], B = [1 0 ; 0 1] + // and u = [1 ; 0]. Let us also assume that the process noise Q = [0.1 0 ; 0 0.1]; + // + // In the case of factor graphs, the factor related to the motion model would be defined as + // f2 = (f(x_{t}) - x_{t+1}) * Q^-1 * (f(x_{t}) - x_{t+1})^T + // Conveniently, there is a factor type, called a BetweenFactor, that can generate this factor + // given the expected difference, f(x_{t}) - x_{t+1}, and Q. + // so, difference = x_{t+1} - x_{t} = F*x_{t} + B*u_{t} - I*x_{t} + // = (F - I)*x_{t} + B*u_{t} + // = B*u_{t} (for our example) + Key x1(1); + Point2 difference(1,0); + SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + BetweenFactor factor2(x0, x1, difference, Q); + nonlinearFactorGraph->add(factor2); - // Update using z2, yielding Bayes net P(x2|z1,z2) - // repeat.... + // We have now made the small factor graph f1-(x0)-f2-(x1) + // where factor f1 is just the prior from time t0, P(x0) + // and factor f2 is from the motion model + // Eliminate this in order x0, x1, to get Bayes net P(x0|x1)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 + // + // Because of the way GTSAM works internally, we have used nonlinear class even though this example + // system is linear. We first convert the nonlinear factor graph into a linear one, using the specified + // ordering. Linear factors are simply numbered, and are not accessible via named key like the nonlinear + // variables. Also, the nonlinear factors are linearized around an initial estimate. For a true linear + // system, the initial estimate is not important. - // Combined predict-update is more efficient - // Predict x3, update using z3 - // P(x3|z1,z2,z3) ~ P(z3|x3) \int_{x2} P(x3|x2) P(x2|z1,z2) - // form factor graph f3-(x3)-f2-(x2)-f1 - // where f3 ~ P(z3|x3), f2 ~ P(x3|x2), and f1 ~ P(x2|z1,z2) - // Now, eliminate this in order x2, x3, to get Bayes net P(x2|x3)P(x3) - // and again, just keep the root of the Bayes net, P(x3) which is really P(x3|z1,z2,z3) + // 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(); + //->template dynamicCastFactors() + + // 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 + 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::KeyLookup lookup; + lookup[0] = x0; lookup[1] = x1; + LinearizedFactor factor3(linearBayesNet->back()->toFactor(), lookup, linearizationPoints); + + // Create a new, empty graph and add the prior from the previous step + nonlinearFactorGraph = NonlinearFactorGraph::shared_ptr(new NonlinearFactorGraph); + nonlinearFactorGraph->add(factor3); + + // 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} + // 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 + // + // For the purposes of this example, let us assume we have something like a GPS that returns + // the current position of the robot. For this simple example, we can use a PriorFactor to model the + // observation as it depends on only a single state variable, x1. To model real sensor observations + // generally requires the creation of a new factor type. For example, factors for range sensors, bearing + // sensors, and camera projections have already been added to GTSAM. + // + // 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. + Point2 z1(1.0, 0.0); + SharedDiagonal R1 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); + PriorFactor factor4(x1, z1, R1); + nonlinearFactorGraph->add(factor4); + + // We have now made the small factor graph f3-(x1)-f2 + // 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(); + // 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 + 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 factor5(linearBayesNet->back()->toFactor(), lookup, linearizationPoints); + + // Create a new, empty graph and add the prior from the previous step + nonlinearFactorGraph = NonlinearFactorGraph::shared_ptr(new NonlinearFactorGraph); + nonlinearFactorGraph->add(factor5); - // print out posterior on x3 + + // 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 factor6(x1, x2, difference, Q); + nonlinearFactorGraph->add(factor6); + + // 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(); + // 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 + 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 factor7(linearBayesNet->back()->toFactor(), lookup, linearizationPoints); + + // Create a new, empty graph and add the prior from the previous step + nonlinearFactorGraph = NonlinearFactorGraph::shared_ptr(new NonlinearFactorGraph); + nonlinearFactorGraph->add(factor7); + + // And update using z2 ... + Point2 z2(2.0, 0.0); + SharedDiagonal R2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); + PriorFactor factor8(x2, z2, R2); + nonlinearFactorGraph->add(factor8); + + // We have now made the small factor graph f7-(x2)-f8 + // where factor f7 is the prior from previous time ( P(x2) ) + // and factor f8 is from the measurement, z2 ( P(x2|z2) ) + // Eliminate this in order x2, to get Bayes net P(x2) + // 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(); + // 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 + 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 factor9(linearBayesNet->back()->toFactor(), lookup, linearizationPoints); + + // Create a new, empty graph and add the prior from the previous step + nonlinearFactorGraph = NonlinearFactorGraph::shared_ptr(new NonlinearFactorGraph); + nonlinearFactorGraph->add(factor9); + + // Wash, rinse, repeat for another time step + Key x3(3); + difference = Point2(1,0); + Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + BetweenFactor 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(); + // Solve the linear factor graph, converting it into a linear Bayes Network ( P(x2,x3) = P(x2|x3)*P(x3) ) + GaussianSequentialSolver solver4(*linearFactorGraph); + linearBayesNet = solver4.eliminate(); + + // If needed, the current estimate of x3 may be extracted 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 factor11(linearBayesNet->back()->toFactor(), lookup, linearizationPoints); + + // Create a new, empty graph and add the prior from the previous step + nonlinearFactorGraph = NonlinearFactorGraph::shared_ptr(new NonlinearFactorGraph); + nonlinearFactorGraph->add(factor11); + + // And update using z3 ... + Point2 z3(3.0, 0.0); + SharedDiagonal R3 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); + PriorFactor factor12(x3, z3, R3); + nonlinearFactorGraph->add(factor12); + + // We have now made the small factor graph f11-(x3)-f12 + // where factor f11 is the prior from previous time ( P(x3) ) + // and factor f12 is from the measurement, z3 ( P(x3|z3) ) + // Eliminate this in order x3, to get Bayes net P(x3) + // 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(); + // 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 + result = optimize(*linearBayesNet); + Point2 x3_update = linearizationPoints[x3].expmap(result[ordering->at(x3)]); + x3_update.print("X3 Update"); + + return 0; } diff --git a/gtsam/nonlinear/LinearApproxFactor-inl.h b/gtsam/nonlinear/LinearApproxFactor-inl.h new file mode 100644 index 000000000..5373665e7 --- /dev/null +++ b/gtsam/nonlinear/LinearApproxFactor-inl.h @@ -0,0 +1,93 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +template +Vector LinearApproxFactor::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 +boost::shared_ptr +LinearApproxFactor::linearize(const VALUES& c, const Ordering& ordering) const { + + // sort by varid - only known at linearization time + typedef std::map 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 > 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(new JacobianFactor(terms, b - b_, model_)); +} + +/* ************************************************************************* */ +template +IndexFactor::shared_ptr +LinearApproxFactor::symbolic(const Ordering& ordering) const { + std::vector 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(new IndexFactor(key_ids.begin(), key_ids.end())); +} + +/* ************************************************************************* */ +template +void LinearApproxFactor::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 diff --git a/gtsam/nonlinear/LinearApproxFactor.h b/gtsam/nonlinear/LinearApproxFactor.h new file mode 100644 index 000000000..0faa360da --- /dev/null +++ b/gtsam/nonlinear/LinearApproxFactor.h @@ -0,0 +1,123 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include +#include +#include +#include +#include + +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 LinearApproxFactor : public NonlinearFactor { + +public: + /** type for the variable */ + typedef typename KEY::Value X; + + /** base type */ + typedef NonlinearFactor Base; + + /** shared pointer for convenience */ + typedef boost::shared_ptr > shared_ptr; + + /** typedefs for key vectors */ + typedef std::vector KeyVector; + +protected: + /** hold onto the factor itself */ + // store components of a jacobian factor + typedef std::map 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 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 + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor", + boost::serialization::base_object(*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 diff --git a/gtsam/nonlinear/LinearizedFactor.h b/gtsam/nonlinear/LinearizedFactor.h new file mode 100644 index 000000000..11c1cd1ec --- /dev/null +++ b/gtsam/nonlinear/LinearizedFactor.h @@ -0,0 +1,83 @@ +/* + * @file LinearizedFactor.h + * @brief A dummy factor that allows a linear factor to act as a nonlinear factor + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace gtsam{ + +/** + * Wrapper around a LinearApproxFactor with some extra interfaces + */ +template +class LinearizedFactor : public LinearApproxFactor { + +public: + /** base type */ + typedef LinearApproxFactor Base; + + /** shared pointer for convenience */ + typedef boost::shared_ptr > shared_ptr; + + /** decoder for keys - avoids the use of a full ordering */ + typedef std::map 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 + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("LinearApproxFactor", + boost::serialization::base_object(*this)); + } +}; + + +} // \namespace gtsam