From c868056c228d8d1279d93849078f8df8a276ea76 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 1 Aug 2013 21:57:33 +0000 Subject: [PATCH] Updating nonlinear factors, nonlinear factor graph, nonlinear optimizers, and LinearContainerFactor to work with unordered linear. Does not compile - needs more work. --- gtsam/nonlinear/DoglegOptimizer.cpp | 28 +++-- gtsam/nonlinear/DoglegOptimizer.h | 6 +- gtsam/nonlinear/DoglegOptimizerImpl.cpp | 10 +- gtsam/nonlinear/DoglegOptimizerImpl.h | 47 +++---- gtsam/nonlinear/GaussNewtonOptimizer.cpp | 17 ++- gtsam/nonlinear/GaussNewtonOptimizer.h | 8 +- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 24 ++-- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 6 +- gtsam/nonlinear/LinearContainerFactor.cpp | 103 +++++---------- gtsam/nonlinear/LinearContainerFactor.h | 41 +++--- gtsam/nonlinear/NonlinearFactor.h | 110 +++++++--------- gtsam/nonlinear/NonlinearFactorGraph.cpp | 118 ++++++------------ gtsam/nonlinear/NonlinearFactorGraph.h | 55 ++++---- .../SuccessiveLinearizationOptimizer.cpp | 17 ++- .../SuccessiveLinearizationOptimizer.h | 7 +- gtsam/nonlinear/Values.cpp | 46 +------ gtsam/nonlinear/Values.h | 24 +--- 17 files changed, 253 insertions(+), 414 deletions(-) diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index 383e77a6e..1e76886bd 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -17,10 +17,11 @@ */ #include - -#include -#include #include +#include +#include +#include +#include #include @@ -53,8 +54,7 @@ std::string DoglegParams::verbosityDLTranslator(VerbosityDL verbosityDL) const { void DoglegOptimizer::iterate(void) { // Linearize graph - const OrderingOrdered& ordering = *params_.ordering; - GaussianFactorGraphOrdered::shared_ptr linear = graph_.linearize(state_.values, ordering); + GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values); // Pull out parameters we'll use const bool dlVerbose = (params_.verbosityDL > DoglegParams::SILENT); @@ -63,13 +63,12 @@ void DoglegOptimizer::iterate(void) { DoglegOptimizerImpl::IterationResult result; if ( params_.isMultifrontal() ) { - GaussianBayesTreeOrdered bt; - bt.insert(GaussianJunctionTreeOrdered(*linear).eliminate(params_.getEliminationFunction())); - result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, bt, graph_, state_.values, ordering, state_.error, dlVerbose); + GaussianBayesTree bt = *linear->eliminateMultifrontal(*params_.ordering, params_.getEliminationFunction()); + result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, bt, graph_, state_.values, state_.error, dlVerbose); } else if ( params_.isSequential() ) { - GaussianBayesNetOrdered::shared_ptr bn = EliminationTreeOrdered::Create(*linear)->eliminate(params_.getEliminationFunction()); - result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *bn, graph_, state_.values, ordering, state_.error, dlVerbose); + GaussianBayesNet bn = *linear->eliminateSequential(*params_.ordering, params_.getEliminationFunction()); + result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, bn, graph_, state_.values, state_.error, dlVerbose); } else if ( params_.isCG() ) { throw runtime_error("todo: "); @@ -82,10 +81,17 @@ void DoglegOptimizer::iterate(void) { if(params_.verbosity >= NonlinearOptimizerParams::DELTA) result.dx_d.print("delta"); // Create new state with new values and new error - state_.values = state_.values.retract(result.dx_d, ordering); + state_.values = state_.values.retract(result.dx_d); state_.error = result.f_error; state_.Delta = result.Delta; ++state_.iterations; } +/* ************************************************************************* */ +DoglegParams DoglegOptimizer::ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const { + if(!params.ordering) + params.ordering = Ordering::COLAMD(graph); + return params; +} + } /* namespace gtsam */ diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h index ffd0b0d8a..2e9dd81c3 100644 --- a/gtsam/nonlinear/DoglegOptimizer.h +++ b/gtsam/nonlinear/DoglegOptimizer.h @@ -158,11 +158,7 @@ protected: virtual const NonlinearOptimizerState& _state() const { return state_; } /** Internal function for computing a COLAMD ordering if no ordering is specified */ - DoglegParams ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph, const Values& values) const { - if(!params.ordering) - params.ordering = *graph.orderingCOLAMD(values); - return params; - } + DoglegParams ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const; }; } diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp index 894c41ffa..671dfe8f8 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp +++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp @@ -22,8 +22,8 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -VectorValuesOrdered DoglegOptimizerImpl::ComputeDoglegPoint( - double Delta, const VectorValuesOrdered& dx_u, const VectorValuesOrdered& dx_n, const bool verbose) { +VectorValues DoglegOptimizerImpl::ComputeDoglegPoint( + double Delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose) { // Get magnitude of each update and find out which segment Delta falls in assert(Delta >= 0.0); @@ -33,7 +33,7 @@ VectorValuesOrdered DoglegOptimizerImpl::ComputeDoglegPoint( if(verbose) cout << "Steepest descent magnitude " << std::sqrt(x_u_norm_sq) << ", Newton's method magnitude " << std::sqrt(x_n_norm_sq) << endl; if(DeltaSq < x_u_norm_sq) { // Trust region is smaller than steepest descent update - VectorValuesOrdered x_d = std::sqrt(DeltaSq / x_u_norm_sq) * dx_u; + VectorValues x_d = std::sqrt(DeltaSq / x_u_norm_sq) * dx_u; if(verbose) cout << "In steepest descent region with fraction " << std::sqrt(DeltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl; return x_d; } else if(DeltaSq < x_n_norm_sq) { @@ -48,7 +48,7 @@ VectorValuesOrdered DoglegOptimizerImpl::ComputeDoglegPoint( } /* ************************************************************************* */ -VectorValuesOrdered DoglegOptimizerImpl::ComputeBlend(double Delta, const VectorValuesOrdered& x_u, const VectorValuesOrdered& x_n, const bool verbose) { +VectorValues DoglegOptimizerImpl::ComputeBlend(double Delta, const VectorValues& x_u, const VectorValues& x_n, const bool verbose) { // See doc/trustregion.lyx or doc/trustregion.pdf @@ -78,7 +78,7 @@ VectorValuesOrdered DoglegOptimizerImpl::ComputeBlend(double Delta, const Vector // Compute blended point if(verbose) cout << "In blend region with fraction " << tau << " of Newton's method point" << endl; - VectorValuesOrdered blend = (1. - tau) * x_u; axpy(tau, x_n, blend); + VectorValues blend = (1. - tau) * x_u; axpy(tau, x_n, blend); return blend; } diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index e992776a1..8b76629cb 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -18,10 +18,8 @@ #include -#include -#include // To get optimize(BayesTree) -//#include -#include +#include +#include namespace gtsam { @@ -42,7 +40,7 @@ struct GTSAM_EXPORT DoglegOptimizerImpl { struct GTSAM_EXPORT IterationResult { double Delta; - VectorValuesOrdered dx_d; + VectorValues dx_d; double f_error; }; @@ -103,7 +101,7 @@ struct GTSAM_EXPORT DoglegOptimizerImpl { template static IterationResult Iterate( double Delta, TrustRegionAdaptationMode mode, const M& Rd, - const F& f, const VALUES& x0, const OrderingOrdered& ordering, const double f_error, const bool verbose=false); + const F& f, const VALUES& x0, const double f_error, const bool verbose=false); /** * Compute the dogleg point given a trust region radius \f$ \Delta \f$. The @@ -127,7 +125,7 @@ struct GTSAM_EXPORT DoglegOptimizerImpl { * @param dx_n The Gauss-Newton point * @return The dogleg point \f$ \delta x_d \f$ */ - static VectorValuesOrdered ComputeDoglegPoint(double Delta, const VectorValuesOrdered& dx_u, const VectorValuesOrdered& dx_n, const bool verbose=false); + static VectorValues ComputeDoglegPoint(double Delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose=false); /** Compute the point on the line between the steepest descent point and the * Newton's method point intersecting the trust region boundary. @@ -138,7 +136,7 @@ struct GTSAM_EXPORT DoglegOptimizerImpl { * @param x_u Steepest descent minimizer * @param x_n Newton's method minimizer */ - static VectorValuesOrdered ComputeBlend(double Delta, const VectorValuesOrdered& x_u, const VectorValuesOrdered& x_n, const bool verbose=false); + static VectorValues ComputeBlend(double Delta, const VectorValues& x_u, const VectorValues& x_n, const bool verbose=false); }; @@ -146,25 +144,18 @@ struct GTSAM_EXPORT DoglegOptimizerImpl { template typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( double Delta, TrustRegionAdaptationMode mode, const M& Rd, - const F& f, const VALUES& x0, const OrderingOrdered& ordering, const double f_error, const bool verbose) { - + const F& f, const VALUES& x0, const double f_error, const bool verbose) +{ // Compute steepest descent and Newton's method points gttic(optimizeGradientSearch); - gttic(allocateVectorValues); - VectorValuesOrdered dx_u = *allocateVectorValues(Rd); - gttoc(allocateVectorValues); - gttic(optimizeGradientSearchInPlace); - optimizeGradientSearchInPlace(Rd, dx_u); - gttoc(optimizeGradientSearchInPlace); + VectorValuesOrdered dx_u = Rd.optimizeGradientSearch(); gttoc(optimizeGradientSearch); - gttic(optimizeInPlace); - VectorValuesOrdered dx_n(VectorValuesOrdered::SameStructure(dx_u)); - optimizeInPlace(Rd, dx_n); - gttoc(optimizeInPlace); - gttic(jfg_error); - const GaussianFactorGraphOrdered jfg(Rd); - const double M_error = jfg.error(VectorValuesOrdered::Zero(dx_u)); - gttoc(jfg_error); + gttic(optimize); + VectorValuesOrdered dx_n = Rd.optimize(); + gttoc(optimize); + gttic(M_error); + const double M_error = Rd.error(VectorValuesOrdered::Zero(dx_u)); + gttoc(M_error); // Result to return IterationResult result; @@ -181,7 +172,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( gttic(retract); // Compute expmapped solution - const VALUES x_d(x0.retract(result.dx_d, ordering)); + const VALUES x_d(x0.retract(result.dx_d)); gttoc(retract); gttic(decrease_in_f); @@ -189,10 +180,10 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( result.f_error = f.error(x_d); gttoc(decrease_in_f); - gttic(decrease_in_M); + gttic(new_M_error); // Compute decrease in M - const double new_M_error = jfg.error(result.dx_d); - gttoc(decrease_in_M); + const double new_M_error = Rd.error(result.dx_d); + gttoc(new_M_error); if(verbose) std::cout << std::setprecision(15) << "f error: " << f_error << " -> " << result.f_error << std::endl; if(verbose) std::cout << std::setprecision(15) << "M error: " << M_error << " -> " << new_M_error << std::endl; diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.cpp b/gtsam/nonlinear/GaussNewtonOptimizer.cpp index e33e0b8be..e0e66adab 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.cpp +++ b/gtsam/nonlinear/GaussNewtonOptimizer.cpp @@ -17,6 +17,8 @@ */ #include +#include +#include using namespace std; @@ -28,18 +30,27 @@ void GaussNewtonOptimizer::iterate() { const NonlinearOptimizerState& current = state_; // Linearize graph - GaussianFactorGraphOrdered::shared_ptr linear = graph_.linearize(current.values, *params_.ordering); + GaussianFactorGraph::shared_ptr linear = graph_.linearize(current.values); // Solve Factor Graph - const VectorValuesOrdered delta = solveGaussianFactorGraph(*linear, params_); + const VectorValues delta = solveGaussianFactorGraph(*linear, params_); // Maybe show output if(params_.verbosity >= NonlinearOptimizerParams::DELTA) delta.print("delta"); // Create new state with new values and new error - state_.values = current.values.retract(delta, *params_.ordering); + state_.values = current.values.retract(delta); state_.error = graph_.error(state_.values); ++ state_.iterations; } +/* ************************************************************************* */ +GaussNewtonParams GaussNewtonOptimizer::ensureHasOrdering( + GaussNewtonParams params, const NonlinearFactorGraph& graph) const +{ + if(!params.ordering) + params.ordering = Ordering::COLAMD(graph); + return params; +} + } /* namespace gtsam */ diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.h b/gtsam/nonlinear/GaussNewtonOptimizer.h index 091a4be89..0c1da8a58 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.h +++ b/gtsam/nonlinear/GaussNewtonOptimizer.h @@ -70,7 +70,7 @@ public: * @param graph The nonlinear factor graph to optimize * @param initialValues The initial variable assignments */ - GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const OrderingOrdered& ordering) : + GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Ordering& ordering) : NonlinearOptimizer(graph), state_(graph, initialValues) { params_.ordering = ordering; } @@ -110,11 +110,7 @@ protected: virtual const NonlinearOptimizerState& _state() const { return state_; } /** Internal function for computing a COLAMD ordering if no ordering is specified */ - GaussNewtonParams ensureHasOrdering(GaussNewtonParams params, const NonlinearFactorGraph& graph, const Values& values) const { - if(!params.ordering) - params.ordering = *graph.orderingCOLAMD(values); - return params; - } + GaussNewtonParams ensureHasOrdering(GaussNewtonParams params, const NonlinearFactorGraph& graph) const; }; diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index a502b694d..1b1f2e93e 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -19,6 +19,8 @@ #include #include +#include +#include #include #include @@ -74,7 +76,7 @@ void LevenbergMarquardtOptimizer::iterate() { gttic(LM_iterate); // Linearize graph - GaussianFactorGraphOrdered::shared_ptr linear = graph_.linearize(state_.values, *params_.ordering); + GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values); // Pull out parameters we'll use const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity; @@ -88,7 +90,7 @@ void LevenbergMarquardtOptimizer::iterate() { // Add prior-factors // TODO: replace this dampening with a backsubstitution approach gttic(damp); - GaussianFactorGraphOrdered dampedSystem(*linear); + GaussianFactorGraph dampedSystem = *linear; { double sigma = 1.0 / std::sqrt(state_.lambda); dampedSystem.reserve(dampedSystem.size() + dimensions_.size()); @@ -97,9 +99,8 @@ void LevenbergMarquardtOptimizer::iterate() { size_t dim = (dimensions_)[j]; Matrix A = eye(dim); Vector b = zero(dim); - SharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma); - GaussianFactorOrdered::shared_ptr prior(new JacobianFactorOrdered(j, A, b, model)); - dampedSystem.push_back(prior); + SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); + dampedSystem += boost::make_shared(j, A, b, model); } } gttoc(damp); @@ -108,14 +109,14 @@ void LevenbergMarquardtOptimizer::iterate() { // Try solving try { // Solve Damped Gaussian Factor Graph - const VectorValuesOrdered delta = solveGaussianFactorGraph(dampedSystem, params_); + const VectorValues delta = solveGaussianFactorGraph(dampedSystem, params_); if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.norm() << endl; if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta"); // update values gttic(retract); - Values newValues = state_.values.retract(delta, *params_.ordering); + Values newValues = state_.values.retract(delta); gttoc(retract); // create new optimization state with more adventurous lambda @@ -168,4 +169,13 @@ void LevenbergMarquardtOptimizer::iterate() { ++state_.iterations; } +/* ************************************************************************* */ +LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( + LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const +{ + if(!params.ordering) + params.ordering = Ordering::COLAMD(graph); + return params; +} + } /* namespace gtsam */ diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 9bbc9f1d3..ba6f994dd 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -172,11 +172,7 @@ protected: virtual const NonlinearOptimizerState& _state() const { return state_; } /** Internal function for computing a COLAMD ordering if no ordering is specified */ - LevenbergMarquardtParams ensureHasOrdering(LevenbergMarquardtParams params, const NonlinearFactorGraph& graph, const Values& values) const { - if(!params.ordering) - params.ordering = *graph.orderingCOLAMD(values); - return params; - } + LevenbergMarquardtParams ensureHasOrdering(LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const; }; } diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index 9748049b6..8c511277b 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -6,20 +6,13 @@ */ #include +#include +#include #include namespace gtsam { -/* ************************************************************************* */ -void LinearContainerFactor::rekeyFactor(const OrderingOrdered& ordering) { - BOOST_FOREACH(Index& idx, factor_->keys()) { - Key fullKey = ordering.key(idx); - idx = fullKey; - keys_.push_back(fullKey); - } -} - /* ************************************************************************* */ void LinearContainerFactor::initializeLinearizationPoint(const Values& linearizationPoint) { if (!linearizationPoint.empty()) { @@ -33,7 +26,7 @@ void LinearContainerFactor::initializeLinearizationPoint(const Values& lineariza } /* ************************************************************************* */ -LinearContainerFactor::LinearContainerFactor(const GaussianFactorOrdered::shared_ptr& factor, +LinearContainerFactor::LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional& linearizationPoint) : factor_(factor), linearizationPoint_(linearizationPoint) { // Extract keys stashed in linear factor @@ -43,28 +36,22 @@ LinearContainerFactor::LinearContainerFactor(const GaussianFactorOrdered::shared /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor( - const JacobianFactorOrdered& factor, const OrderingOrdered& ordering, - const Values& linearizationPoint) + const JacobianFactor& factor, const Values& linearizationPoint) : factor_(factor.clone()) { - rekeyFactor(ordering); initializeLinearizationPoint(linearizationPoint); } /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor( - const HessianFactorOrdered& factor, const OrderingOrdered& ordering, - const Values& linearizationPoint) + const HessianFactor& factor, const Values& linearizationPoint) : factor_(factor.clone()) { - rekeyFactor(ordering); initializeLinearizationPoint(linearizationPoint); } /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor( - const GaussianFactorOrdered::shared_ptr& factor, const OrderingOrdered& ordering, - const Values& linearizationPoint) + const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint) : factor_(factor->clone()) { - rekeyFactor(ordering); initializeLinearizationPoint(linearizationPoint); } @@ -112,19 +99,11 @@ double LinearContainerFactor::error(const Values& c) const { csub.insert(key, c.at(key)); // create dummy ordering for evaluation - OrderingOrdered ordering = *csub.orderingArbitrary(); - VectorValuesOrdered delta = linearizationPoint_->localCoordinates(csub, ordering); - - // Change keys on stored factor - BOOST_FOREACH(gtsam::Index& index, factor_->keys()) - index = ordering[index]; + VectorValues delta = linearizationPoint_->localCoordinates(csub); // compute error double error = factor_->error(delta); - // change keys back - factor_->keys() = keys(); - return error; } @@ -137,103 +116,79 @@ size_t LinearContainerFactor::dim() const { } /* ************************************************************************* */ -GaussianFactorOrdered::shared_ptr LinearContainerFactor::order(const OrderingOrdered& ordering) const { - // clone factor - boost::shared_ptr result = factor_->clone(); - - // rekey - BOOST_FOREACH(Index& key, result->keys()) - key = ordering[key]; - - return result; -} - -/* ************************************************************************* */ -GaussianFactorOrdered::shared_ptr LinearContainerFactor::linearize( - const Values& c, const OrderingOrdered& ordering) const { +GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) const { if (!hasLinearizationPoint()) - return order(ordering); + return factor_; // Extract subset of values Values subsetC; BOOST_FOREACH(const gtsam::Key& key, this->keys()) subsetC.insert(key, c.at(key)); - // Create a temp ordering for this factor - OrderingOrdered localOrdering = *subsetC.orderingArbitrary(); - // Determine delta between linearization points using new ordering - VectorValuesOrdered delta = linearizationPoint_->localCoordinates(subsetC, localOrdering); + VectorValues delta = linearizationPoint_->localCoordinates(subsetC); // clone and reorder linear factor to final ordering - GaussianFactorOrdered::shared_ptr linFactor = order(localOrdering); + GaussianFactor::shared_ptr linFactor = factor_->clone(); if (isJacobian()) { - JacobianFactorOrdered::shared_ptr jacFactor = boost::dynamic_pointer_cast(linFactor); + JacobianFactor::shared_ptr jacFactor = boost::dynamic_pointer_cast(linFactor); jacFactor->getb() = -jacFactor->unweighted_error(delta); } else { - HessianFactorOrdered::shared_ptr hesFactor = boost::dynamic_pointer_cast(linFactor); + HessianFactor::shared_ptr hesFactor = boost::dynamic_pointer_cast(linFactor); size_t dim = hesFactor->linearTerm().size(); - Eigen::Block Gview = hesFactor->info().block(0, 0, dim, dim); + Eigen::Block Gview = hesFactor->info().block(0, 0, dim, dim); Vector deltaVector = delta.asVector(); Vector G_delta = Gview.selfadjointView() * deltaVector; hesFactor->constantTerm() += deltaVector.dot(G_delta) - 2.0 * deltaVector.dot(hesFactor->linearTerm()); hesFactor->linearTerm() -= G_delta; } - // reset ordering - BOOST_FOREACH(Index& idx, linFactor->keys()) - idx = ordering[localOrdering.key(idx)]; return linFactor; } /* ************************************************************************* */ bool LinearContainerFactor::isJacobian() const { - return boost::dynamic_pointer_cast(factor_); + return boost::dynamic_pointer_cast(factor_); } /* ************************************************************************* */ bool LinearContainerFactor::isHessian() const { - return boost::dynamic_pointer_cast(factor_); + return boost::dynamic_pointer_cast(factor_); } /* ************************************************************************* */ -JacobianFactorOrdered::shared_ptr LinearContainerFactor::toJacobian() const { - return boost::dynamic_pointer_cast(factor_); +JacobianFactor::shared_ptr LinearContainerFactor::toJacobian() const { + return boost::dynamic_pointer_cast(factor_); } /* ************************************************************************* */ -HessianFactorOrdered::shared_ptr LinearContainerFactor::toHessian() const { - return boost::dynamic_pointer_cast(factor_); +HessianFactor::shared_ptr LinearContainerFactor::toHessian() const { + return boost::dynamic_pointer_cast(factor_); } /* ************************************************************************* */ -GaussianFactorOrdered::shared_ptr LinearContainerFactor::negate(const OrderingOrdered& ordering) const { - GaussianFactorOrdered::shared_ptr result = factor_->negate(); - BOOST_FOREACH(Key& key, result->keys()) - key = ordering[key]; - return result; +GaussianFactor::shared_ptr LinearContainerFactor::negateToGaussian() const { + GaussianFactor::shared_ptr result = factor_->negate(); } /* ************************************************************************* */ -NonlinearFactor::shared_ptr LinearContainerFactor::negate() const { - GaussianFactorOrdered::shared_ptr antifactor = factor_->negate(); // already has keys in place - return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor,linearizationPoint_)); +NonlinearFactor::shared_ptr LinearContainerFactor::negateToNonlinear() const { + GaussianFactor::shared_ptr antifactor = factor_->negate(); // already has keys in place + return boost::make_shared(antifactor, linearizationPoint_); } /* ************************************************************************* */ NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( - const GaussianFactorGraphOrdered& linear_graph, const OrderingOrdered& ordering, - const Values& linearizationPoint) { + const GaussianFactorGraph& linear_graph, const Values& linearizationPoint) +{ NonlinearFactorGraph result; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& f, linear_graph) + BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph) if (f) result.push_back(NonlinearFactorGraph::sharedFactor( - new LinearContainerFactor(f, ordering, linearizationPoint))); + new LinearContainerFactor(f, linearizationPoint))); return result; } /* ************************************************************************* */ } // \namespace gtsam - - diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index a3ab53cec..25020a378 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -9,11 +9,13 @@ #pragma once -#include #include namespace gtsam { + // Forward declarations + class HessianFactor; + /** * Dummy version of a generic linear factor to be injected into a nonlinear factor graph * @@ -23,37 +25,32 @@ namespace gtsam { class GTSAM_EXPORT LinearContainerFactor : public NonlinearFactor { protected: - GaussianFactorOrdered::shared_ptr factor_; + GaussianFactor::shared_ptr factor_; boost::optional linearizationPoint_; /** Default constructor - necessary for serialization */ LinearContainerFactor() {} /** direct copy constructor */ - LinearContainerFactor(const GaussianFactorOrdered::shared_ptr& factor, - const boost::optional& linearizationPoint); + LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional& linearizationPoint); public: /** Primary constructor: store a linear factor and decode the ordering */ - LinearContainerFactor(const JacobianFactorOrdered& factor, const OrderingOrdered& ordering, - const Values& linearizationPoint = Values()); + LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values()); /** Primary constructor: store a linear factor and decode the ordering */ - LinearContainerFactor(const HessianFactorOrdered& factor, const OrderingOrdered& ordering, - const Values& linearizationPoint = Values()); + LinearContainerFactor(const HessianFactor& factor, const Values& linearizationPoint = Values()); /** Constructor from shared_ptr */ - LinearContainerFactor(const GaussianFactorOrdered::shared_ptr& factor, const OrderingOrdered& ordering, - const Values& linearizationPoint = Values()); + LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values()); /** Constructor from re-keyed factor: all indices assumed replaced with Key */ - LinearContainerFactor(const GaussianFactorOrdered::shared_ptr& factor, - const Values& linearizationPoint = Values()); + LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values()); // Access - const GaussianFactorOrdered::shared_ptr& factor() const { return factor_; } + const GaussianFactor::shared_ptr& factor() const { return factor_; } // Testable @@ -81,9 +78,6 @@ public: /** Extract the linearization point used in recalculating error */ const boost::optional& linearizationPoint() const { return linearizationPoint_; } - /** Apply the ordering to a graph - same as linearize(), but without needing a linearization point */ - GaussianFactorOrdered::shared_ptr order(const OrderingOrdered& ordering) const; - /** * Linearize to a GaussianFactor, with method depending on the presence of a linearizationPoint * - With no linearization point, returns a reordered, but numerically identical, @@ -101,18 +95,18 @@ public: * TODO: better approximation of relinearization * TODO: switchable modes for approximation technique */ - GaussianFactorOrdered::shared_ptr linearize(const Values& c, const OrderingOrdered& ordering) const; + GaussianFactor::shared_ptr linearize(const Values& c) const; /** * Creates an anti-factor directly and performs rekeying due to ordering */ - GaussianFactorOrdered::shared_ptr negate(const OrderingOrdered& ordering) const; + GaussianFactor::shared_ptr negateToGaussian() const; /** * Creates the equivalent anti-factor as another LinearContainerFactor, * so it remains independent of ordering. */ - NonlinearFactor::shared_ptr negate() const; + NonlinearFactor::shared_ptr negateToNonlinear() const; /** * Creates a shared_ptr clone of the factor - needs to be specialized to allow @@ -135,20 +129,19 @@ public: bool isHessian() const; /** Casts to JacobianFactor */ - JacobianFactorOrdered::shared_ptr toJacobian() const; + JacobianFactor::shared_ptr toJacobian() const; /** Casts to HessianFactor */ - HessianFactorOrdered::shared_ptr toHessian() const; + HessianFactor::shared_ptr toHessian() const; /** * Utility function for converting linear graphs to nonlinear graphs * consisting of LinearContainerFactors. */ - static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraphOrdered& linear_graph, - const OrderingOrdered& ordering, const Values& linearizationPoint = Values()); + static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, + const Values& linearizationPoint = Values()); protected: - void rekeyFactor(const OrderingOrdered& ordering); void initializeLinearizationPoint(const Values& linearizationPoint); private: diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 1f181a5a9..ff3fe1649 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -20,19 +20,14 @@ #pragma once -#include -#include - #include -#include - -#include -#include -#include -#include +#include #include -#include +#include +#include +#include + /** * Macro to add a standard clone function to a derived factor @@ -54,51 +49,39 @@ namespace gtsam { * which are objects in non-linear manifolds (Lie groups). * \nosubgrouping */ -class NonlinearFactor: public FactorOrdered { +class NonlinearFactor: public Factor { protected: // Some handy typedefs - typedef FactorOrdered Base; + typedef Factor Base; typedef NonlinearFactor This; public: - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ /** Default constructor for I/O only */ - NonlinearFactor() { - } + NonlinearFactor() {} /** - * Constructor from a vector of the keys involved in this factor + * Constructor from a collection of the keys involved in this factor */ - NonlinearFactor(const std::vector& keys) : + template + NonlinearFactor(const CONTAINER& keys) : Base(keys) {} - /** - * Constructor from iterators over the keys involved in this factor - */ - template - NonlinearFactor(ITERATOR beginKeys, ITERATOR endKeys) : - Base(beginKeys, endKeys) {} - - NonlinearFactor(Key key) : Base(key) {} ///< Convenience constructor for 1 key - NonlinearFactor(Key key1, Key key2) : Base(key1, key2) {} ///< Convenience constructor for 2 keys - NonlinearFactor(Key key1, Key key2, Key key3) : Base(key1, key2, key3) {} ///< Convenience constructor for 3 keys - NonlinearFactor(Key key1, Key key2, Key key3, Key key4) : Base(key1, key2, key3, key4) {} ///< Convenience constructor for 4 keys - NonlinearFactor(Key key1, Key key2, Key key3, Key key4, Key key5) : Base(key1, key2, key3, key4, key5) {} ///< Convenience constructor for 5 keys - NonlinearFactor(Key key1, Key key2, Key key3, Key key4, Key key5, Key key6) : Base(key1, key2, key3, key4, key5, key6) {} ///< Convenience constructor for 6 keys - /// @} /// @name Testable /// @{ /** print */ - virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const + { std::cout << s << " keys = { "; BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; } std::cout << "}" << std::endl; @@ -140,19 +123,19 @@ public: virtual bool active(const Values& c) const { return true; } /** linearize to a GaussianFactor */ - virtual boost::shared_ptr - linearize(const Values& c, const OrderingOrdered& ordering) const = 0; + virtual boost::shared_ptr + linearize(const Values& c) const = 0; /** * Create a symbolic factor using the given ordering to determine the * variable indices. */ - virtual IndexFactorOrdered::shared_ptr symbolic(const OrderingOrdered& ordering) const { - std::vector indices(this->size()); - for(size_t j=0; jsize(); ++j) - indices[j] = ordering[this->keys()[j]]; - return IndexFactorOrdered::shared_ptr(new IndexFactorOrdered(indices)); - } + //virtual IndexFactorOrdered::shared_ptr symbolic(const OrderingOrdered& ordering) const { + // std::vector indices(this->size()); + // for(size_t j=0; jsize(); ++j) + // indices[j] = ordering[this->keys()[j]]; + // return IndexFactorOrdered::shared_ptr(new IndexFactorOrdered(indices)); + //} /** * Creates a shared_ptr clone of the factor - needs to be specialized to allow @@ -218,11 +201,10 @@ protected: public: - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; /** Default constructor for I/O only */ - NoiseModelFactor() { - } + NoiseModelFactor() {} /** Destructor */ virtual ~NoiseModelFactor() {} @@ -230,17 +212,9 @@ public: /** * Constructor */ - template - NoiseModelFactor(const SharedNoiseModel& noiseModel, ITERATOR beginKeys, ITERATOR endKeys) - : Base(beginKeys, endKeys), noiseModel_(noiseModel) { - } - - NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key) : Base(key), noiseModel_(noiseModel) {} ///< Convenience constructor for 1 key - NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key1, Key key2) : Base(key1, key2), noiseModel_(noiseModel) {} ///< Convenience constructor for 2 keys - NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key1, Key key2, Key key3) : Base(key1, key2, key3), noiseModel_(noiseModel) {} ///< Convenience constructor for 3 keys - NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key1, Key key2, Key key3, Key key4) : Base(key1, key2, key3, key4), noiseModel_(noiseModel) {} ///< Convenience constructor for 4 keys - NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key1, Key key2, Key key3, Key key4, Key key5) : Base(key1, key2, key3, key4, key5), noiseModel_(noiseModel) {} ///< Convenience constructor for 5 keys - NoiseModelFactor(const SharedNoiseModel& noiseModel, Key key1, Key key2, Key key3, Key key4, Key key5, Key key6) : Base(key1, key2, key3, key4, key5, key6), noiseModel_(noiseModel) {} ///< Convenience constructor for 6 keys + template + NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys) : + Base(keys), noiseModel_(noiseModel) {} protected: @@ -252,7 +226,9 @@ protected: public: /** Print */ - virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const + { Base::print(s, keyFormatter); this->noiseModel_->print(" noise model: "); } @@ -314,10 +290,10 @@ public: * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ - boost::shared_ptr linearize(const Values& x, const OrderingOrdered& ordering) const { + boost::shared_ptr linearize(const Values& x) const { // Only linearize if the factor is active if (!this->active(x)) - return boost::shared_ptr(); + return boost::shared_ptr(); // Create the set of terms - Jacobians for each index Vector b; @@ -332,7 +308,7 @@ public: std::vector > terms(this->size()); // Fill in terms for(size_t j=0; jsize(); ++j) { - terms[j].first = ordering[this->keys()[j]]; + terms[j].first = this->keys()[j]; terms[j].second.swap(A[j]); } @@ -340,11 +316,9 @@ public: noiseModel::Constrained::shared_ptr constrained = boost::dynamic_pointer_cast(this->noiseModel_); if(constrained) - return GaussianFactorOrdered::shared_ptr( - new JacobianFactorOrdered(terms, b, constrained->unit())); + return GaussianFactor::shared_ptr(new JacobianFactor(terms, b, constrained->unit())); else - return GaussianFactorOrdered::shared_ptr( - new JacobianFactorOrdered(terms, b, noiseModel::Unit::Create(b.size()))); + return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); } private: @@ -392,7 +366,7 @@ public: * @param key1 by which to look up X value in Values */ NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) : - Base(noiseModel, key1) {} + Base(noiseModel, boost::assign::cref_list_of<1>(key1)) {} /** Calls the 1-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. @@ -461,7 +435,7 @@ public: * @param j2 key of the second variable */ NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) : - Base(noiseModel, j1, j2) {} + Base(noiseModel, boost::assign::cref_list_of<2>(j1)(j2)) {} virtual ~NoiseModelFactor2() {} @@ -538,7 +512,7 @@ public: * @param j3 key of the third variable */ NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) : - Base(noiseModel, j1, j2, j3) {} + Base(noiseModel, boost::assign::cref_list_of<1>(j1)(j2)(j3)) {} virtual ~NoiseModelFactor3() {} @@ -617,7 +591,7 @@ public: * @param j4 key of the fourth variable */ NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) : - Base(noiseModel, j1, j2, j3, j4) {} + Base(noiseModel, boost::assign::cref_list_of<1>(j1)(j2)(j3)(j4)) {} virtual ~NoiseModelFactor4() {} @@ -700,7 +674,7 @@ public: * @param j5 key of the fifth variable */ NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) : - Base(noiseModel, j1, j2, j3, j4, j5) {} + Base(noiseModel, boost::assign::cref_list_of<1>(j1)(j2)(j3)(j4)(j5)) {} virtual ~NoiseModelFactor5() {} @@ -787,7 +761,7 @@ public: * @param j6 key of the fifth variable */ NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) : - Base(noiseModel, j1, j2, j3, j4, j5, j6) {} + Base(noiseModel, boost::assign::cref_list_of<1>(j1)(j2)(j3)(j4)(j5)(j6)) {} virtual ~NoiseModelFactor6() {} diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index b85ebe5f6..0cc330e5e 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -20,10 +20,12 @@ #include #include #include -#include -#include +#include #include #include +#include +#include +#include #include using namespace std; @@ -207,107 +209,61 @@ FastSet NonlinearFactorGraph::keys() const { } /* ************************************************************************* */ -OrderingOrdered::shared_ptr NonlinearFactorGraph::orderingCOLAMD( - const Values& config) const +Ordering NonlinearFactorGraph::orderingCOLAMD() const { - gttic(NonlinearFactorGraph_orderingCOLAMD); - - // Create symbolic graph and initial (iterator) ordering - SymbolicFactorGraphOrdered::shared_ptr symbolic; - OrderingOrdered::shared_ptr ordering; - boost::tie(symbolic, ordering) = this->symbolic(config); - - // Compute the VariableIndex (column-wise index) - VariableIndexOrdered variableIndex(*symbolic, ordering->size()); - if (config.size() != variableIndex.size()) throw std::runtime_error( - "orderingCOLAMD: some variables in the graph are not constrained!"); - - // Compute a fill-reducing ordering with COLAMD - Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMD( - variableIndex)); - - // Permute the Ordering with the COLAMD ordering - ordering->permuteInPlace(*colamdPerm); - return ordering; + return Ordering::COLAMD(*this); } /* ************************************************************************* */ -OrderingOrdered::shared_ptr NonlinearFactorGraph::orderingCOLAMDConstrained(const Values& config, - const std::map& constraints) const +Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap& constraints) const { - gttic(NonlinearFactorGraph_orderingCOLAMDConstrained); - - // Create symbolic graph and initial (iterator) ordering - SymbolicFactorGraphOrdered::shared_ptr symbolic; - OrderingOrdered::shared_ptr ordering; - boost::tie(symbolic, ordering) = this->symbolic(config); - - // Compute the VariableIndex (column-wise index) - VariableIndexOrdered variableIndex(*symbolic, ordering->size()); - if (config.size() != variableIndex.size()) throw std::runtime_error( - "orderingCOLAMD: some variables in the graph are not constrained!"); - - // Create a constraint list with correct indices - typedef std::map::value_type constraint_type; - std::map index_constraints; - BOOST_FOREACH(const constraint_type& p, constraints) - index_constraints[ordering->at(p.first)] = p.second; - - // Compute a constrained fill-reducing ordering with COLAMD - Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMDGrouped( - variableIndex, index_constraints)); - - // Permute the Ordering with the COLAMD ordering - ordering->permuteInPlace(*colamdPerm); - return ordering; + return Ordering::COLAMDConstrained(*this, constraints); } /* ************************************************************************* */ -SymbolicFactorGraphOrdered::shared_ptr NonlinearFactorGraph::symbolic(const OrderingOrdered& ordering) const { - gttic(NonlinearFactorGraph_symbolic_from_Ordering); - - // Generate the symbolic factor graph - SymbolicFactorGraphOrdered::shared_ptr symbolicfg(new SymbolicFactorGraphOrdered); - symbolicfg->reserve(this->size()); - - BOOST_FOREACH(const sharedFactor& factor, this->factors_) { - if(factor) - symbolicfg->push_back(factor->symbolic(ordering)); - else - symbolicfg->push_back(SymbolicFactorGraphOrdered::sharedFactor()); - } - - return symbolicfg; -} +//SymbolicFactorGraphOrdered::shared_ptr NonlinearFactorGraph::symbolic(const OrderingOrdered& ordering) const { +// gttic(NonlinearFactorGraph_symbolic_from_Ordering); +// +// // Generate the symbolic factor graph +// SymbolicFactorGraphOrdered::shared_ptr symbolicfg(new SymbolicFactorGraphOrdered); +// symbolicfg->reserve(this->size()); +// +// BOOST_FOREACH(const sharedFactor& factor, this->factors_) { +// if(factor) +// symbolicfg->push_back(factor->symbolic(ordering)); +// else +// symbolicfg->push_back(SymbolicFactorGraphOrdered::sharedFactor()); +// } +// +// return symbolicfg; +//} /* ************************************************************************* */ -pair NonlinearFactorGraph::symbolic( - const Values& config) const -{ - gttic(NonlinearFactorGraph_symbolic_from_Values); - - // Generate an initial key ordering in iterator order - OrderingOrdered::shared_ptr ordering(config.orderingArbitrary()); - return make_pair(symbolic(*ordering), ordering); -} +//pair NonlinearFactorGraph::symbolic( +// const Values& config) const +//{ +// gttic(NonlinearFactorGraph_symbolic_from_Values); +// +// // Generate an initial key ordering in iterator order +// OrderingOrdered::shared_ptr ordering(config.orderingArbitrary()); +// return make_pair(symbolic(*ordering), ordering); +//} /* ************************************************************************* */ -GaussianFactorGraphOrdered::shared_ptr NonlinearFactorGraph::linearize( - const Values& config, const OrderingOrdered& ordering) const +GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& linearizationPoint) const { gttic(NonlinearFactorGraph_linearize); // create an empty linear FG - GaussianFactorGraphOrdered::shared_ptr linearFG(new GaussianFactorGraphOrdered); + GaussianFactorGraph::shared_ptr linearFG = boost::make_shared(); linearFG->reserve(this->size()); // linearize all factors BOOST_FOREACH(const sharedFactor& factor, this->factors_) { if(factor) { - GaussianFactorOrdered::shared_ptr lf = factor->linearize(config, ordering); - if (lf) linearFG->push_back(lf); + (*linearFG) += factor->linearize(linearizationPoint); } else - linearFG->push_back(GaussianFactorOrdered::shared_ptr()); + (*linearFG) += GaussianFactor::shared_ptr(); } return linearFG; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 8af5ff91b..bbb58b866 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -22,12 +22,16 @@ #pragma once #include -#include -#include #include +#include namespace gtsam { + // Forward declarations + class Values; + class Ordering; + class GaussianFactorGraph; + /** * Formatting options when saving in GraphViz format using * NonlinearFactorGraph::saveGraph. @@ -58,13 +62,28 @@ namespace gtsam { * tangent vector space at the linearization point. Because the tangent space is a true * vector space, the config type will be an VectorValues in that linearized factor graph. */ - class NonlinearFactorGraph: public FactorGraphOrdered { + class NonlinearFactorGraph: public FactorGraph { public: - typedef FactorGraphOrdered Base; - typedef boost::shared_ptr shared_ptr; - typedef boost::shared_ptr sharedFactor; + typedef FactorGraph Base; + typedef NonlinearFactorGraph This; + typedef boost::shared_ptr shared_ptr; + + /** Default constructor */ + NonlinearFactorGraph() {} + + /** Construct from iterator over factors */ + template + NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {} + + /** Construct from container of factors (shared_ptr or plain objects) */ + template + explicit NonlinearFactorGraph(const CONTAINER& factors) : Base(factors) {} + + /** Implicit copy/downcast constructor to override explicit template container constructor */ + template + NonlinearFactorGraph(const FactorGraph& graph) : Base(graph) {} /** print just calls base class */ GTSAM_EXPORT void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; @@ -83,20 +102,10 @@ namespace gtsam { /** Unnormalized probability. O(n) */ GTSAM_EXPORT double probPrime(const Values& c) const; - /// Add a factor by value - copies the factor object - void add(const NonlinearFactor& factor) { - this->push_back(factor.clone()); - } - - /// Add a factor by pointer - stores pointer without copying factor object - void add(const sharedFactor& factor) { - this->push_back(factor); - } - /** * Create a symbolic factor graph using an existing ordering */ - GTSAM_EXPORT SymbolicFactorGraphOrdered::shared_ptr symbolic(const OrderingOrdered& ordering) const; + //GTSAM_EXPORT SymbolicFactorGraph::shared_ptr symbolic() const; /** * Create a symbolic factor graph and initial variable ordering that can @@ -104,13 +113,13 @@ namespace gtsam { * The graph and ordering should be permuted after such a fill-reducing * ordering is found. */ - GTSAM_EXPORT std::pair - symbolic(const Values& config) const; + //GTSAM_EXPORT std::pair + // symbolic(const Values& config) const; /** * Compute a fill-reducing ordering using COLAMD. */ - GTSAM_EXPORT OrderingOrdered::shared_ptr orderingCOLAMD(const Values& config) const; + GTSAM_EXPORT Ordering orderingCOLAMD() const; /** * Compute a fill-reducing ordering with constraints using CCOLAMD @@ -120,14 +129,12 @@ namespace gtsam { * indices need to appear in the constraints, unconstrained is assumed for all * other variables */ - GTSAM_EXPORT OrderingOrdered::shared_ptr orderingCOLAMDConstrained(const Values& config, - const std::map& constraints) const; + GTSAM_EXPORT Ordering orderingCOLAMDConstrained(const FastMap& constraints) const; /** * linearize a nonlinear factor graph */ - GTSAM_EXPORT boost::shared_ptr - linearize(const Values& config, const OrderingOrdered& ordering) const; + GTSAM_EXPORT boost::shared_ptr linearize(const Values& linearizationPoint) const; /** * Clone() performs a deep-copy of the graph, including all of the factors diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp index 1a08068f3..b4f099b0a 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp @@ -6,10 +6,10 @@ */ #include -#include -#include +#include +#include #include -#include +#include #include #include @@ -53,15 +53,14 @@ void SuccessiveLinearizationParams::print(const std::string& str) const { std::cout.flush(); } -VectorValuesOrdered solveGaussianFactorGraph(const GaussianFactorGraphOrdered &gfg, const SuccessiveLinearizationParams ¶ms) { +VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const SuccessiveLinearizationParams ¶ms) +{ gttic(solveGaussianFactorGraph); - VectorValuesOrdered delta; + VectorValues delta; if (params.isMultifrontal()) { - delta = GaussianJunctionTreeOrdered(gfg).optimize(params.getEliminationFunction()); + delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); } else if(params.isSequential()) { - const boost::shared_ptr gbn = - EliminationTreeOrdered::Create(gfg)->eliminate(params.getEliminationFunction()); - delta = gtsam::optimize(*gbn); + delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize(); } else if ( params.isCG() ) { if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ..."); diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h index a4e5c2e72..cb4bdc1b6 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h @@ -20,6 +20,7 @@ #include #include +#include namespace gtsam { @@ -36,7 +37,7 @@ public: }; LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer - boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) + boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. SuccessiveLinearizationParams() : linearSolverType(MULTIFRONTAL_CHOLESKY) {} @@ -75,7 +76,7 @@ public: void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); } void setIterativeParams(const SubgraphSolverParameters ¶ms); - void setOrdering(const OrderingOrdered& ordering) { this->ordering = ordering; } + void setOrdering(const Ordering& ordering) { this->ordering = ordering; } private: std::string linearSolverTranslator(LinearSolverType linearSolverType) const { @@ -101,6 +102,6 @@ private: }; /* a wrapper for solving a GaussianFactorGraph according to the parameters */ -GTSAM_EXPORT VectorValuesOrdered solveGaussianFactorGraph(const GaussianFactorGraphOrdered &gfg, const SuccessiveLinearizationParams ¶ms) ; +GTSAM_EXPORT VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const SuccessiveLinearizationParams ¶ms); } /* namespace gtsam */ diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index f962d24bb..f694a9fad 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -23,6 +23,7 @@ */ #include +#include #include @@ -76,12 +77,8 @@ namespace gtsam { } /* ************************************************************************* */ - VectorValuesOrdered Values::zeroVectors(const OrderingOrdered& ordering) const { - return VectorValuesOrdered::Zero(this->dims(ordering)); - } - - /* ************************************************************************* */ - Values Values::retract(const VectorValuesOrdered& delta, const OrderingOrdered& ordering) const { + Values Values::retract(const VectorValues& delta) const + { Values result; for(const_iterator key_value = begin(); key_value != end(); ++key_value) { @@ -95,32 +92,20 @@ namespace gtsam { } /* ************************************************************************* */ - VectorValuesOrdered Values::localCoordinates(const Values& cp, const OrderingOrdered& ordering) const { - VectorValuesOrdered result(this->dims(ordering)); + VectorValues Values::localCoordinates(const Values& cp) const { if(this->size() != cp.size()) throw DynamicValuesMismatched(); + VectorValues result; for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) { if(it1->key != it2->key) throw DynamicValuesMismatched(); // If keys do not match // Will throw a dynamic_cast exception if types do not match // NOTE: this is separate from localCoordinates(cp, ordering, result) due to at() vs. insert - result.at(ordering[it1->key]) = it1->value.localCoordinates_(it2->value); + result.insert(it1->key, it1->value.localCoordinates_(it2->value)); } return result; } - /* ************************************************************************* */ - void Values::localCoordinates(const Values& cp, const OrderingOrdered& ordering, VectorValuesOrdered& result) const { - if(this->size() != cp.size()) - throw DynamicValuesMismatched(); - for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) { - if(it1->key != it2->key) - throw DynamicValuesMismatched(); // If keys do not match - // Will throw a dynamic_cast exception if types do not match - result.insert(ordering[it1->key], it1->value.localCoordinates_(it2->value)); - } - } - /* ************************************************************************* */ const Value& Values::at(Key j) const { // Find the item @@ -192,16 +177,6 @@ namespace gtsam { return *this; } - /* ************************************************************************* */ - vector Values::dims(const OrderingOrdered& ordering) const { - assert(ordering.size() == this->size()); // reads off of end of array if difference in size - vector result(values_.size()); - BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) { - result[ordering[key_value.key]] = key_value.value.dim(); - } - return result; - } - /* ************************************************************************* */ size_t Values::dim() const { size_t result = 0; @@ -211,15 +186,6 @@ namespace gtsam { return result; } - /* ************************************************************************* */ - OrderingOrdered::shared_ptr Values::orderingArbitrary(Index firstVar) const { - OrderingOrdered::shared_ptr ordering(new OrderingOrdered); - for(const_iterator key_value = begin(); key_value != end(); ++key_value) { - ordering->insert(key_value->key, firstVar++); - } - return ordering; - } - /* ************************************************************************* */ const char* ValuesKeyAlreadyExists::what() const throw() { if(message_.empty()) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index af8c8b9d6..593f946a8 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -45,13 +45,12 @@ #include #include -#include #include -#include namespace gtsam { // Forward declarations / utilities + class VectorValues; class ValueCloneAllocator; class ValueAutomaticCasting; template static bool _truePredicate(const T&) { return true; } @@ -206,9 +205,6 @@ namespace gtsam { /** whether the config is empty */ bool empty() const { return values_.empty(); } - /** Get a zero VectorValues of the correct structure */ - VectorValuesOrdered zeroVectors(const OrderingOrdered& ordering) const; - const_iterator begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); } const_iterator end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); } iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); } @@ -222,13 +218,10 @@ namespace gtsam { /// @{ /** Add a delta config to current config and returns a new config */ - Values retract(const VectorValuesOrdered& delta, const OrderingOrdered& ordering) const; + Values retract(const VectorValues& delta) const; /** Get a delta config about a linearization point c0 (*this) */ - VectorValuesOrdered localCoordinates(const Values& cp, const OrderingOrdered& ordering) const; - - /** Get a delta config about a linearization point c0 (*this) - assumes uninitialized delta */ - void localCoordinates(const Values& cp, const OrderingOrdered& ordering, VectorValuesOrdered& delta) const; + VectorValues localCoordinates(const Values& cp) const; ///@} @@ -262,20 +255,9 @@ namespace gtsam { /** Remove all variables from the config */ void clear() { values_.clear(); } - /** Create an array of variable dimensions using the given ordering (\f$ O(n) \f$) */ - std::vector dims(const OrderingOrdered& ordering) const; - /** Compute the total dimensionality of all values (\f$ O(n) \f$) */ size_t dim() const; - /** - * Generate a default ordering, simply in key sort order. To instead - * create a fill-reducing ordering, use - * NonlinearFactorGraph::orderingCOLAMD(). Alternatively, you may permute - * this ordering yourself (as orderingCOLAMD() does internally). - */ - OrderingOrdered::shared_ptr orderingArbitrary(Index firstVar = 0) const; - /** * Return a filtered view of this Values class, without copying any data. * When iterating over the filtered view, only the key-value pairs