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

Updating nonlinear factors, nonlinear factor graph, nonlinear optimizers, and LinearContainerFactor to work with unordered linear. Does not compile - needs more work.

release/4.3a0
Richard Roberts 2013-08-01 21:57:33 +00:00
parent 5e3b4bf477
commit c868056c22
17 changed files with 253 additions and 414 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/nonlinear/DoglegOptimizer.cpp
View File

@ -17,10 +17,11 @@
*/ */
#include <gtsam/nonlinear/DoglegOptimizer.h> #include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/inference/EliminationTreeOrdered.h>
#include <gtsam/linear/GaussianJunctionTreeOrdered.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h> #include <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/algorithm/string.hpp> #include <boost/algorithm/string.hpp>
@ -53,8 +54,7 @@ std::string DoglegParams::verbosityDLTranslator(VerbosityDL verbosityDL) const {
void DoglegOptimizer::iterate(void) { void DoglegOptimizer::iterate(void) {
// Linearize graph // Linearize graph
const OrderingOrdered& ordering = *params_.ordering; GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values);
GaussianFactorGraphOrdered::shared_ptr linear = graph_.linearize(state_.values, ordering);
// Pull out parameters we'll use // Pull out parameters we'll use
const bool dlVerbose = (params_.verbosityDL > DoglegParams::SILENT); const bool dlVerbose = (params_.verbosityDL > DoglegParams::SILENT);
@ -63,13 +63,12 @@ void DoglegOptimizer::iterate(void) {
DoglegOptimizerImpl::IterationResult result; DoglegOptimizerImpl::IterationResult result;
if ( params_.isMultifrontal() ) { if ( params_.isMultifrontal() ) {
GaussianBayesTreeOrdered bt; GaussianBayesTree bt = *linear->eliminateMultifrontal(*params_.ordering, params_.getEliminationFunction());
bt.insert(GaussianJunctionTreeOrdered(*linear).eliminate(params_.getEliminationFunction())); result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, bt, graph_, state_.values, state_.error, dlVerbose);
result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, bt, graph_, state_.values, ordering, state_.error, dlVerbose);
} }
else if ( params_.isSequential() ) { else if ( params_.isSequential() ) {
GaussianBayesNetOrdered::shared_ptr bn = EliminationTreeOrdered<GaussianFactorOrdered>::Create(*linear)->eliminate(params_.getEliminationFunction()); GaussianBayesNet bn = *linear->eliminateSequential(*params_.ordering, params_.getEliminationFunction());
result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *bn, graph_, state_.values, ordering, state_.error, dlVerbose); result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, bn, graph_, state_.values, state_.error, dlVerbose);
} }
else if ( params_.isCG() ) { else if ( params_.isCG() ) {
throw runtime_error("todo: "); throw runtime_error("todo: ");
@ -82,10 +81,17 @@ void DoglegOptimizer::iterate(void) {
if(params_.verbosity >= NonlinearOptimizerParams::DELTA) result.dx_d.print("delta"); if(params_.verbosity >= NonlinearOptimizerParams::DELTA) result.dx_d.print("delta");
// Create new state with new values and new error // 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_.error = result.f_error;
state_.Delta = result.Delta; state_.Delta = result.Delta;
++state_.iterations; ++state_.iterations;
} }
/* ************************************************************************* */
DoglegParams DoglegOptimizer::ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const {
if(!params.ordering)
params.ordering = Ordering::COLAMD(graph);
return params;
}
} /* namespace gtsam */ } /* namespace gtsam */

6
gtsam/nonlinear/DoglegOptimizer.h
View File

@ -158,11 +158,7 @@ protected:
virtual const NonlinearOptimizerState& _state() const { return state_; } virtual const NonlinearOptimizerState& _state() const { return state_; }
/** Internal function for computing a COLAMD ordering if no ordering is specified */ /** Internal function for computing a COLAMD ordering if no ordering is specified */
DoglegParams ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph, const Values& values) const { DoglegParams ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const;
if(!params.ordering)
params.ordering = *graph.orderingCOLAMD(values);
return params;
}
}; };
} }

10
gtsam/nonlinear/DoglegOptimizerImpl.cpp
View File

@ -22,8 +22,8 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
VectorValuesOrdered DoglegOptimizerImpl::ComputeDoglegPoint( VectorValues DoglegOptimizerImpl::ComputeDoglegPoint(
double Delta, const VectorValuesOrdered& dx_u, const VectorValuesOrdered& dx_n, const bool verbose) { 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 // Get magnitude of each update and find out which segment Delta falls in
assert(Delta >= 0.0); 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(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) { if(DeltaSq < x_u_norm_sq) {
// Trust region is smaller than steepest descent update // 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; if(verbose) cout << "In steepest descent region with fraction " << std::sqrt(DeltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl;
return x_d; return x_d;
} else if(DeltaSq < x_n_norm_sq) { } 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 // See doc/trustregion.lyx or doc/trustregion.pdf
@ -78,7 +78,7 @@ VectorValuesOrdered DoglegOptimizerImpl::ComputeBlend(double Delta, const Vector
// Compute blended point // Compute blended point
if(verbose) cout << "In blend region with fraction " << tau << " of Newton's method point" << endl; 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; return blend;
} }

47
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -18,10 +18,8 @@
#include <iomanip> #include <iomanip>
#include <gtsam/linear/GaussianBayesNetOrdered.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianISAMOrdered.h> // To get optimize(BayesTree<GaussianConditional>) #include <gtsam/inference/Ordering.h>
//#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/OrderingOrdered.h>
namespace gtsam { namespace gtsam {
@ -42,7 +40,7 @@ struct GTSAM_EXPORT DoglegOptimizerImpl {
struct GTSAM_EXPORT IterationResult { struct GTSAM_EXPORT IterationResult {
double Delta; double Delta;
VectorValuesOrdered dx_d; VectorValues dx_d;
double f_error; double f_error;
}; };
@ -103,7 +101,7 @@ struct GTSAM_EXPORT DoglegOptimizerImpl {
template<class M, class F, class VALUES> template<class M, class F, class VALUES>
static IterationResult Iterate( static IterationResult Iterate(
double Delta, TrustRegionAdaptationMode mode, const M& Rd, 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 * 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 * @param dx_n The Gauss-Newton point
* @return The dogleg point \f$ \delta x_d \f$ * @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 /** Compute the point on the line between the steepest descent point and the
* Newton's method point intersecting the trust region boundary. * 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_u Steepest descent minimizer
* @param x_n Newton's method 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<class M, class F, class VALUES> template<class M, class F, class VALUES>
typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
double Delta, TrustRegionAdaptationMode mode, const M& Rd, 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 // Compute steepest descent and Newton's method points
gttic(optimizeGradientSearch); gttic(optimizeGradientSearch);
gttic(allocateVectorValues); VectorValuesOrdered dx_u = Rd.optimizeGradientSearch();
VectorValuesOrdered dx_u = *allocateVectorValues(Rd);
gttoc(allocateVectorValues);
gttic(optimizeGradientSearchInPlace);
optimizeGradientSearchInPlace(Rd, dx_u);
gttoc(optimizeGradientSearchInPlace);
gttoc(optimizeGradientSearch); gttoc(optimizeGradientSearch);
gttic(optimizeInPlace); gttic(optimize);
VectorValuesOrdered dx_n(VectorValuesOrdered::SameStructure(dx_u)); VectorValuesOrdered dx_n = Rd.optimize();
optimizeInPlace(Rd, dx_n); gttoc(optimize);
gttoc(optimizeInPlace); gttic(M_error);
gttic(jfg_error); const double M_error = Rd.error(VectorValuesOrdered::Zero(dx_u));
const GaussianFactorGraphOrdered jfg(Rd); gttoc(M_error);
const double M_error = jfg.error(VectorValuesOrdered::Zero(dx_u));
gttoc(jfg_error);
// Result to return // Result to return
IterationResult result; IterationResult result;
@ -181,7 +172,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
gttic(retract); gttic(retract);
// Compute expmapped solution // Compute expmapped solution
const VALUES x_d(x0.retract(result.dx_d, ordering)); const VALUES x_d(x0.retract(result.dx_d));
gttoc(retract); gttoc(retract);
gttic(decrease_in_f); gttic(decrease_in_f);
@ -189,10 +180,10 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
result.f_error = f.error(x_d); result.f_error = f.error(x_d);
gttoc(decrease_in_f); gttoc(decrease_in_f);
gttic(decrease_in_M); gttic(new_M_error);
// Compute decrease in M // Compute decrease in M
const double new_M_error = jfg.error(result.dx_d); const double new_M_error = Rd.error(result.dx_d);
gttoc(decrease_in_M); 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) << "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; if(verbose) std::cout << std::setprecision(15) << "M error: " << M_error << " -> " << new_M_error << std::endl;

17
gtsam/nonlinear/GaussNewtonOptimizer.cpp
View File

@ -17,6 +17,8 @@
*/ */
#include <gtsam/nonlinear/GaussNewtonOptimizer.h> #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
using namespace std; using namespace std;
@ -28,18 +30,27 @@ void GaussNewtonOptimizer::iterate() {
const NonlinearOptimizerState& current = state_; const NonlinearOptimizerState& current = state_;
// Linearize graph // Linearize graph
GaussianFactorGraphOrdered::shared_ptr linear = graph_.linearize(current.values, *params_.ordering); GaussianFactorGraph::shared_ptr linear = graph_.linearize(current.values);
// Solve Factor Graph // Solve Factor Graph
const VectorValuesOrdered delta = solveGaussianFactorGraph(*linear, params_); const VectorValues delta = solveGaussianFactorGraph(*linear, params_);
// Maybe show output // Maybe show output
if(params_.verbosity >= NonlinearOptimizerParams::DELTA) delta.print("delta"); if(params_.verbosity >= NonlinearOptimizerParams::DELTA) delta.print("delta");
// Create new state with new values and new error // 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_.error = graph_.error(state_.values);
++ state_.iterations; ++ state_.iterations;
} }
/* ************************************************************************* */
GaussNewtonParams GaussNewtonOptimizer::ensureHasOrdering(
GaussNewtonParams params, const NonlinearFactorGraph& graph) const
{
if(!params.ordering)
params.ordering = Ordering::COLAMD(graph);
return params;
}
} /* namespace gtsam */ } /* namespace gtsam */

8
gtsam/nonlinear/GaussNewtonOptimizer.h
View File

@ -70,7 +70,7 @@ public:
* @param graph The nonlinear factor graph to optimize * @param graph The nonlinear factor graph to optimize
* @param initialValues The initial variable assignments * @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) { NonlinearOptimizer(graph), state_(graph, initialValues) {
params_.ordering = ordering; } params_.ordering = ordering; }
@ -110,11 +110,7 @@ protected:
virtual const NonlinearOptimizerState& _state() const { return state_; } virtual const NonlinearOptimizerState& _state() const { return state_; }
/** Internal function for computing a COLAMD ordering if no ordering is specified */ /** Internal function for computing a COLAMD ordering if no ordering is specified */
GaussNewtonParams ensureHasOrdering(GaussNewtonParams params, const NonlinearFactorGraph& graph, const Values& values) const { GaussNewtonParams ensureHasOrdering(GaussNewtonParams params, const NonlinearFactorGraph& graph) const;
if(!params.ordering)
params.ordering = *graph.orderingCOLAMD(values);
return params;
}
}; };

24
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -19,6 +19,8 @@
#include <cmath> #include <cmath>
#include <gtsam/linear/linearExceptions.h> #include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h> #include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h>
@ -74,7 +76,7 @@ void LevenbergMarquardtOptimizer::iterate() {
gttic(LM_iterate); gttic(LM_iterate);
// Linearize graph // 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 // Pull out parameters we'll use
const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity; const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity;
@ -88,7 +90,7 @@ void LevenbergMarquardtOptimizer::iterate() {
// Add prior-factors // Add prior-factors
// TODO: replace this dampening with a backsubstitution approach // TODO: replace this dampening with a backsubstitution approach
gttic(damp); gttic(damp);
GaussianFactorGraphOrdered dampedSystem(*linear); GaussianFactorGraph dampedSystem = *linear;
{ {
double sigma = 1.0 / std::sqrt(state_.lambda); double sigma = 1.0 / std::sqrt(state_.lambda);
dampedSystem.reserve(dampedSystem.size() + dimensions_.size()); dampedSystem.reserve(dampedSystem.size() + dimensions_.size());
@ -97,9 +99,8 @@ void LevenbergMarquardtOptimizer::iterate() {
size_t dim = (dimensions_)[j]; size_t dim = (dimensions_)[j];
Matrix A = eye(dim); Matrix A = eye(dim);
Vector b = zero(dim); Vector b = zero(dim);
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma); SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
GaussianFactorOrdered::shared_ptr prior(new JacobianFactorOrdered(j, A, b, model)); dampedSystem += boost::make_shared<JacobianFactor>(j, A, b, model);
dampedSystem.push_back(prior);
} }
} }
gttoc(damp); gttoc(damp);
@ -108,14 +109,14 @@ void LevenbergMarquardtOptimizer::iterate() {
// Try solving // Try solving
try { try {
// Solve Damped Gaussian Factor Graph // 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::TRYLAMBDA) cout << "linear delta norm = " << delta.norm() << endl;
if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta"); if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta");
// update values // update values
gttic(retract); gttic(retract);
Values newValues = state_.values.retract(delta, *params_.ordering); Values newValues = state_.values.retract(delta);
gttoc(retract); gttoc(retract);
// create new optimization state with more adventurous lambda // create new optimization state with more adventurous lambda
@ -168,4 +169,13 @@ void LevenbergMarquardtOptimizer::iterate() {
++state_.iterations; ++state_.iterations;
} }
/* ************************************************************************* */
LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering(
LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const
{
if(!params.ordering)
params.ordering = Ordering::COLAMD(graph);
return params;
}
} /* namespace gtsam */ } /* namespace gtsam */

6
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -172,11 +172,7 @@ protected:
virtual const NonlinearOptimizerState& _state() const { return state_; } virtual const NonlinearOptimizerState& _state() const { return state_; }
/** Internal function for computing a COLAMD ordering if no ordering is specified */ /** Internal function for computing a COLAMD ordering if no ordering is specified */
LevenbergMarquardtParams ensureHasOrdering(LevenbergMarquardtParams params, const NonlinearFactorGraph& graph, const Values& values) const { LevenbergMarquardtParams ensureHasOrdering(LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const;
if(!params.ordering)
params.ordering = *graph.orderingCOLAMD(values);
return params;
}
}; };
} }

103
gtsam/nonlinear/LinearContainerFactor.cpp
View File

@ -6,20 +6,13 @@
*/ */
#include <gtsam/nonlinear/LinearContainerFactor.h> #include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
namespace gtsam { 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) { void LinearContainerFactor::initializeLinearizationPoint(const Values& linearizationPoint) {
if (!linearizationPoint.empty()) { 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<Values>& linearizationPoint) const boost::optional<Values>& linearizationPoint)
: factor_(factor), linearizationPoint_(linearizationPoint) { : factor_(factor), linearizationPoint_(linearizationPoint) {
// Extract keys stashed in linear factor // Extract keys stashed in linear factor
@ -43,28 +36,22 @@ LinearContainerFactor::LinearContainerFactor(const GaussianFactorOrdered::shared
/* ************************************************************************* */ /* ************************************************************************* */
LinearContainerFactor::LinearContainerFactor( LinearContainerFactor::LinearContainerFactor(
const JacobianFactorOrdered& factor, const OrderingOrdered& ordering, const JacobianFactor& factor, const Values& linearizationPoint)
const Values& linearizationPoint)
: factor_(factor.clone()) { : factor_(factor.clone()) {
rekeyFactor(ordering);
initializeLinearizationPoint(linearizationPoint); initializeLinearizationPoint(linearizationPoint);
} }
/* ************************************************************************* */ /* ************************************************************************* */
LinearContainerFactor::LinearContainerFactor( LinearContainerFactor::LinearContainerFactor(
const HessianFactorOrdered& factor, const OrderingOrdered& ordering, const HessianFactor& factor, const Values& linearizationPoint)
const Values& linearizationPoint)
: factor_(factor.clone()) { : factor_(factor.clone()) {
rekeyFactor(ordering);
initializeLinearizationPoint(linearizationPoint); initializeLinearizationPoint(linearizationPoint);
} }
/* ************************************************************************* */ /* ************************************************************************* */
LinearContainerFactor::LinearContainerFactor( LinearContainerFactor::LinearContainerFactor(
const GaussianFactorOrdered::shared_ptr& factor, const OrderingOrdered& ordering, const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint)
const Values& linearizationPoint)
: factor_(factor->clone()) { : factor_(factor->clone()) {
rekeyFactor(ordering);
initializeLinearizationPoint(linearizationPoint); initializeLinearizationPoint(linearizationPoint);
} }
@ -112,19 +99,11 @@ double LinearContainerFactor::error(const Values& c) const {
csub.insert(key, c.at(key)); csub.insert(key, c.at(key));
// create dummy ordering for evaluation // create dummy ordering for evaluation
OrderingOrdered ordering = *csub.orderingArbitrary(); VectorValues delta = linearizationPoint_->localCoordinates(csub);
VectorValuesOrdered delta = linearizationPoint_->localCoordinates(csub, ordering);
// Change keys on stored factor
BOOST_FOREACH(gtsam::Index& index, factor_->keys())
index = ordering[index];
// compute error // compute error
double error = factor_->error(delta); double error = factor_->error(delta);
// change keys back
factor_->keys() = keys();
return error; return error;
} }
@ -137,103 +116,79 @@ size_t LinearContainerFactor::dim() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorOrdered::shared_ptr LinearContainerFactor::order(const OrderingOrdered& ordering) const { GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) const {
// clone factor
boost::shared_ptr<GaussianFactorOrdered> 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 {
if (!hasLinearizationPoint()) if (!hasLinearizationPoint())
return order(ordering); return factor_;
// Extract subset of values // Extract subset of values
Values subsetC; Values subsetC;
BOOST_FOREACH(const gtsam::Key& key, this->keys()) BOOST_FOREACH(const gtsam::Key& key, this->keys())
subsetC.insert(key, c.at(key)); 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 // 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 // clone and reorder linear factor to final ordering
GaussianFactorOrdered::shared_ptr linFactor = order(localOrdering); GaussianFactor::shared_ptr linFactor = factor_->clone();
if (isJacobian()) { if (isJacobian()) {
JacobianFactorOrdered::shared_ptr jacFactor = boost::dynamic_pointer_cast<JacobianFactorOrdered>(linFactor); JacobianFactor::shared_ptr jacFactor = boost::dynamic_pointer_cast<JacobianFactor>(linFactor);
jacFactor->getb() = -jacFactor->unweighted_error(delta); jacFactor->getb() = -jacFactor->unweighted_error(delta);
} else { } else {
HessianFactorOrdered::shared_ptr hesFactor = boost::dynamic_pointer_cast<HessianFactorOrdered>(linFactor); HessianFactor::shared_ptr hesFactor = boost::dynamic_pointer_cast<HessianFactor>(linFactor);
size_t dim = hesFactor->linearTerm().size(); size_t dim = hesFactor->linearTerm().size();
Eigen::Block<HessianFactorOrdered::Block> Gview = hesFactor->info().block(0, 0, dim, dim); Eigen::Block<HessianFactor::Block> Gview = hesFactor->info().block(0, 0, dim, dim);
Vector deltaVector = delta.asVector(); Vector deltaVector = delta.asVector();
Vector G_delta = Gview.selfadjointView<Eigen::Upper>() * deltaVector; Vector G_delta = Gview.selfadjointView<Eigen::Upper>() * deltaVector;
hesFactor->constantTerm() += deltaVector.dot(G_delta) - 2.0 * deltaVector.dot(hesFactor->linearTerm()); hesFactor->constantTerm() += deltaVector.dot(G_delta) - 2.0 * deltaVector.dot(hesFactor->linearTerm());
hesFactor->linearTerm() -= G_delta; hesFactor->linearTerm() -= G_delta;
} }
// reset ordering
BOOST_FOREACH(Index& idx, linFactor->keys())
idx = ordering[localOrdering.key(idx)];
return linFactor; return linFactor;
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool LinearContainerFactor::isJacobian() const { bool LinearContainerFactor::isJacobian() const {
return boost::dynamic_pointer_cast<JacobianFactorOrdered>(factor_); return boost::dynamic_pointer_cast<JacobianFactor>(factor_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool LinearContainerFactor::isHessian() const { bool LinearContainerFactor::isHessian() const {
return boost::dynamic_pointer_cast<HessianFactorOrdered>(factor_); return boost::dynamic_pointer_cast<HessianFactor>(factor_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactorOrdered::shared_ptr LinearContainerFactor::toJacobian() const { JacobianFactor::shared_ptr LinearContainerFactor::toJacobian() const {
return boost::dynamic_pointer_cast<JacobianFactorOrdered>(factor_); return boost::dynamic_pointer_cast<JacobianFactor>(factor_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactorOrdered::shared_ptr LinearContainerFactor::toHessian() const { HessianFactor::shared_ptr LinearContainerFactor::toHessian() const {
return boost::dynamic_pointer_cast<HessianFactorOrdered>(factor_); return boost::dynamic_pointer_cast<HessianFactor>(factor_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorOrdered::shared_ptr LinearContainerFactor::negate(const OrderingOrdered& ordering) const { GaussianFactor::shared_ptr LinearContainerFactor::negateToGaussian() const {
GaussianFactorOrdered::shared_ptr result = factor_->negate(); GaussianFactor::shared_ptr result = factor_->negate();
BOOST_FOREACH(Key& key, result->keys())
key = ordering[key];
return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactor::shared_ptr LinearContainerFactor::negate() const { NonlinearFactor::shared_ptr LinearContainerFactor::negateToNonlinear() const {
GaussianFactorOrdered::shared_ptr antifactor = factor_->negate(); // already has keys in place GaussianFactor::shared_ptr antifactor = factor_->negate(); // already has keys in place
return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor,linearizationPoint_)); return boost::make_shared<LinearContainerFactor>(antifactor, linearizationPoint_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( NonlinearFactorGraph LinearContainerFactor::convertLinearGraph(
const GaussianFactorGraphOrdered& linear_graph, const OrderingOrdered& ordering, const GaussianFactorGraph& linear_graph, const Values& linearizationPoint)
const Values& linearizationPoint) { {
NonlinearFactorGraph result; NonlinearFactorGraph result;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& f, linear_graph) BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph)
if (f) if (f)
result.push_back(NonlinearFactorGraph::sharedFactor( result.push_back(NonlinearFactorGraph::sharedFactor(
new LinearContainerFactor(f, ordering, linearizationPoint))); new LinearContainerFactor(f, linearizationPoint)));
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
} // \namespace gtsam } // \namespace gtsam

41
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -9,11 +9,13 @@
#pragma once #pragma once
#include <gtsam/linear/HessianFactorOrdered.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
namespace gtsam { namespace gtsam {
// Forward declarations
class HessianFactor;
/** /**
* Dummy version of a generic linear factor to be injected into a nonlinear factor graph * 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 { class GTSAM_EXPORT LinearContainerFactor : public NonlinearFactor {
protected: protected:
GaussianFactorOrdered::shared_ptr factor_; GaussianFactor::shared_ptr factor_;
boost::optional<Values> linearizationPoint_; boost::optional<Values> linearizationPoint_;
/** Default constructor - necessary for serialization */ /** Default constructor - necessary for serialization */
LinearContainerFactor() {} LinearContainerFactor() {}
/** direct copy constructor */ /** direct copy constructor */
LinearContainerFactor(const GaussianFactorOrdered::shared_ptr& factor, LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional<Values>& linearizationPoint);
const boost::optional<Values>& linearizationPoint);
public: public:
/** Primary constructor: store a linear factor and decode the ordering */ /** Primary constructor: store a linear factor and decode the ordering */
LinearContainerFactor(const JacobianFactorOrdered& factor, const OrderingOrdered& ordering, LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values());
const Values& linearizationPoint = Values());
/** Primary constructor: store a linear factor and decode the ordering */ /** Primary constructor: store a linear factor and decode the ordering */
LinearContainerFactor(const HessianFactorOrdered& factor, const OrderingOrdered& ordering, LinearContainerFactor(const HessianFactor& factor, const Values& linearizationPoint = Values());
const Values& linearizationPoint = Values());
/** Constructor from shared_ptr */ /** Constructor from shared_ptr */
LinearContainerFactor(const GaussianFactorOrdered::shared_ptr& factor, const OrderingOrdered& ordering, LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values());
const Values& linearizationPoint = Values());
/** Constructor from re-keyed factor: all indices assumed replaced with Key */ /** Constructor from re-keyed factor: all indices assumed replaced with Key */
LinearContainerFactor(const GaussianFactorOrdered::shared_ptr& factor, LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values());
const Values& linearizationPoint = Values());
// Access // Access
const GaussianFactorOrdered::shared_ptr& factor() const { return factor_; } const GaussianFactor::shared_ptr& factor() const { return factor_; }
// Testable // Testable
@ -81,9 +78,6 @@ public:
/** Extract the linearization point used in recalculating error */ /** Extract the linearization point used in recalculating error */
const boost::optional<Values>& linearizationPoint() const { return linearizationPoint_; } const boost::optional<Values>& 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 * Linearize to a GaussianFactor, with method depending on the presence of a linearizationPoint
* - With no linearization point, returns a reordered, but numerically identical, * - With no linearization point, returns a reordered, but numerically identical,
@ -101,18 +95,18 @@ public:
* TODO: better approximation of relinearization * TODO: better approximation of relinearization
* TODO: switchable modes for approximation technique * 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 * 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, * Creates the equivalent anti-factor as another LinearContainerFactor,
* so it remains independent of ordering. * 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 * Creates a shared_ptr clone of the factor - needs to be specialized to allow
@ -135,20 +129,19 @@ public:
bool isHessian() const; bool isHessian() const;
/** Casts to JacobianFactor */ /** Casts to JacobianFactor */
JacobianFactorOrdered::shared_ptr toJacobian() const; JacobianFactor::shared_ptr toJacobian() const;
/** Casts to HessianFactor */ /** Casts to HessianFactor */
HessianFactorOrdered::shared_ptr toHessian() const; HessianFactor::shared_ptr toHessian() const;
/** /**
* Utility function for converting linear graphs to nonlinear graphs * Utility function for converting linear graphs to nonlinear graphs
* consisting of LinearContainerFactors. * consisting of LinearContainerFactors.
*/ */
static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraphOrdered& linear_graph, static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph,
const OrderingOrdered& ordering, const Values& linearizationPoint = Values()); const Values& linearizationPoint = Values());
protected: protected:
void rekeyFactor(const OrderingOrdered& ordering);
void initializeLinearizationPoint(const Values& linearizationPoint); void initializeLinearizationPoint(const Values& linearizationPoint);
private: private:

110
gtsam/nonlinear/NonlinearFactor.h
View File

@ -20,19 +20,14 @@
#pragma once #pragma once
#include <list>
#include <limits>
#include <boost/serialization/base_object.hpp> #include <boost/serialization/base_object.hpp>
#include <boost/function.hpp> #include <boost/assign/list_of.hpp>
#include <gtsam/inference/FactorOrdered-inl.h>
#include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/JacobianFactorOrdered.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/OrderingOrdered.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/inference/Factor.h>
/** /**
* Macro to add a standard clone function to a derived factor * 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). * which are objects in non-linear manifolds (Lie groups).
* \nosubgrouping * \nosubgrouping
*/ */
class NonlinearFactor: public FactorOrdered<Key> { class NonlinearFactor: public Factor {
protected: protected:
// Some handy typedefs // Some handy typedefs
typedef FactorOrdered<Key> Base; typedef Factor Base;
typedef NonlinearFactor This; typedef NonlinearFactor This;
public: public:
typedef boost::shared_ptr<NonlinearFactor> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
/** Default constructor for I/O only */ /** 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<size_t>& keys) : template<typename CONTAINER>
NonlinearFactor(const CONTAINER& keys) :
Base(keys) {} Base(keys) {}
/**
* Constructor from iterators over the keys involved in this factor
*/
template<class ITERATOR>
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 /// @name Testable
/// @{ /// @{
/** print */ /** 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 = { "; std::cout << s << " keys = { ";
BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; } BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; }
std::cout << "}" << std::endl; std::cout << "}" << std::endl;
@ -140,19 +123,19 @@ public:
virtual bool active(const Values& c) const { return true; } virtual bool active(const Values& c) const { return true; }
/** linearize to a GaussianFactor */ /** linearize to a GaussianFactor */
virtual boost::shared_ptr<GaussianFactorOrdered> virtual boost::shared_ptr<GaussianFactor>
linearize(const Values& c, const OrderingOrdered& ordering) const = 0; linearize(const Values& c) const = 0;
/** /**
* Create a symbolic factor using the given ordering to determine the * Create a symbolic factor using the given ordering to determine the
* variable indices. * variable indices.
*/ */
virtual IndexFactorOrdered::shared_ptr symbolic(const OrderingOrdered& ordering) const { //virtual IndexFactorOrdered::shared_ptr symbolic(const OrderingOrdered& ordering) const {
std::vector<Index> indices(this->size()); // std::vector<Index> indices(this->size());
for(size_t j=0; j<this->size(); ++j) // for(size_t j=0; j<this->size(); ++j)
indices[j] = ordering[this->keys()[j]]; // indices[j] = ordering[this->keys()[j]];
return IndexFactorOrdered::shared_ptr(new IndexFactorOrdered(indices)); // return IndexFactorOrdered::shared_ptr(new IndexFactorOrdered(indices));
} //}
/** /**
* Creates a shared_ptr clone of the factor - needs to be specialized to allow * Creates a shared_ptr clone of the factor - needs to be specialized to allow
@ -218,11 +201,10 @@ protected:
public: public:
typedef boost::shared_ptr<NoiseModelFactor > shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
/** Default constructor for I/O only */ /** Default constructor for I/O only */
NoiseModelFactor() { NoiseModelFactor() {}
}
/** Destructor */ /** Destructor */
virtual ~NoiseModelFactor() {} virtual ~NoiseModelFactor() {}
@ -230,17 +212,9 @@ public:
/** /**
* Constructor * Constructor
*/ */
template<class ITERATOR> template<typename CONTAINER>
NoiseModelFactor(const SharedNoiseModel& noiseModel, ITERATOR beginKeys, ITERATOR endKeys) NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys) :
: Base(beginKeys, endKeys), noiseModel_(noiseModel) { Base(keys), 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
protected: protected:
@ -252,7 +226,9 @@ protected:
public: public:
/** Print */ /** 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); Base::print(s, keyFormatter);
this->noiseModel_->print(" noise model: "); 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$ * \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$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/ */
boost::shared_ptr<GaussianFactorOrdered> linearize(const Values& x, const OrderingOrdered& ordering) const { boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
// Only linearize if the factor is active // Only linearize if the factor is active
if (!this->active(x)) if (!this->active(x))
return boost::shared_ptr<JacobianFactorOrdered>(); return boost::shared_ptr<JacobianFactor>();
// Create the set of terms - Jacobians for each index // Create the set of terms - Jacobians for each index
Vector b; Vector b;
@ -332,7 +308,7 @@ public:
std::vector<std::pair<Index, Matrix> > terms(this->size()); std::vector<std::pair<Index, Matrix> > terms(this->size());
// Fill in terms // Fill in terms
for(size_t j=0; j<this->size(); ++j) { for(size_t j=0; j<this->size(); ++j) {
terms[j].first = ordering[this->keys()[j]]; terms[j].first = this->keys()[j];
terms[j].second.swap(A[j]); terms[j].second.swap(A[j]);
} }
@ -340,11 +316,9 @@ public:
noiseModel::Constrained::shared_ptr constrained = noiseModel::Constrained::shared_ptr constrained =
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_); boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if(constrained) if(constrained)
return GaussianFactorOrdered::shared_ptr( return GaussianFactor::shared_ptr(new JacobianFactor(terms, b, constrained->unit()));
new JacobianFactorOrdered(terms, b, constrained->unit()));
else else
return GaussianFactorOrdered::shared_ptr( return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
new JacobianFactorOrdered(terms, b, noiseModel::Unit::Create(b.size())));
} }
private: private:
@ -392,7 +366,7 @@ public:
* @param key1 by which to look up X value in Values * @param key1 by which to look up X value in Values
*/ */
NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) : 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 /** Calls the 1-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. * so must be implemented in the derived class.
@ -461,7 +435,7 @@ public:
* @param j2 key of the second variable * @param j2 key of the second variable
*/ */
NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) : NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) :
Base(noiseModel, j1, j2) {} Base(noiseModel, boost::assign::cref_list_of<2>(j1)(j2)) {}
virtual ~NoiseModelFactor2() {} virtual ~NoiseModelFactor2() {}
@ -538,7 +512,7 @@ public:
* @param j3 key of the third variable * @param j3 key of the third variable
*/ */
NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) : 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() {} virtual ~NoiseModelFactor3() {}
@ -617,7 +591,7 @@ public:
* @param j4 key of the fourth variable * @param j4 key of the fourth variable
*/ */
NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) : 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() {} virtual ~NoiseModelFactor4() {}
@ -700,7 +674,7 @@ public:
* @param j5 key of the fifth variable * @param j5 key of the fifth variable
*/ */
NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) : 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() {} virtual ~NoiseModelFactor5() {}
@ -787,7 +761,7 @@ public:
* @param j6 key of the fifth variable * @param j6 key of the fifth variable
*/ */
NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) : 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() {} virtual ~NoiseModelFactor6() {}

118
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -20,10 +20,12 @@
#include <cmath> #include <cmath>
#include <limits> #include <limits>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <gtsam/inference/FactorGraphOrdered.h> #include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/inference/inferenceOrdered.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
using namespace std; using namespace std;
@ -207,107 +209,61 @@ FastSet<Key> NonlinearFactorGraph::keys() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
OrderingOrdered::shared_ptr NonlinearFactorGraph::orderingCOLAMD( Ordering NonlinearFactorGraph::orderingCOLAMD() const
const Values& config) const
{ {
gttic(NonlinearFactorGraph_orderingCOLAMD); return Ordering::COLAMD(*this);
// 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;
} }
/* ************************************************************************* */ /* ************************************************************************* */
OrderingOrdered::shared_ptr NonlinearFactorGraph::orderingCOLAMDConstrained(const Values& config, Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const
const std::map<Key, int>& constraints) const
{ {
gttic(NonlinearFactorGraph_orderingCOLAMDConstrained); return Ordering::COLAMDConstrained(*this, constraints);
// 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<Key, int>::value_type constraint_type;
std::map<Index, int> 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;
} }
/* ************************************************************************* */ /* ************************************************************************* */
SymbolicFactorGraphOrdered::shared_ptr NonlinearFactorGraph::symbolic(const OrderingOrdered& ordering) const { //SymbolicFactorGraphOrdered::shared_ptr NonlinearFactorGraph::symbolic(const OrderingOrdered& ordering) const {
gttic(NonlinearFactorGraph_symbolic_from_Ordering); // gttic(NonlinearFactorGraph_symbolic_from_Ordering);
//
// Generate the symbolic factor graph // // Generate the symbolic factor graph
SymbolicFactorGraphOrdered::shared_ptr symbolicfg(new SymbolicFactorGraphOrdered); // SymbolicFactorGraphOrdered::shared_ptr symbolicfg(new SymbolicFactorGraphOrdered);
symbolicfg->reserve(this->size()); // symbolicfg->reserve(this->size());
//
BOOST_FOREACH(const sharedFactor& factor, this->factors_) { // BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor) // if(factor)
symbolicfg->push_back(factor->symbolic(ordering)); // symbolicfg->push_back(factor->symbolic(ordering));
else // else
symbolicfg->push_back(SymbolicFactorGraphOrdered::sharedFactor()); // symbolicfg->push_back(SymbolicFactorGraphOrdered::sharedFactor());
} // }
//
return symbolicfg; // return symbolicfg;
} //}
/* ************************************************************************* */ /* ************************************************************************* */
pair<SymbolicFactorGraphOrdered::shared_ptr, OrderingOrdered::shared_ptr> NonlinearFactorGraph::symbolic( //pair<SymbolicFactorGraphOrdered::shared_ptr, OrderingOrdered::shared_ptr> NonlinearFactorGraph::symbolic(
const Values& config) const // const Values& config) const
{ //{
gttic(NonlinearFactorGraph_symbolic_from_Values); // gttic(NonlinearFactorGraph_symbolic_from_Values);
//
// Generate an initial key ordering in iterator order // // Generate an initial key ordering in iterator order
OrderingOrdered::shared_ptr ordering(config.orderingArbitrary()); // OrderingOrdered::shared_ptr ordering(config.orderingArbitrary());
return make_pair(symbolic(*ordering), ordering); // return make_pair(symbolic(*ordering), ordering);
} //}
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraphOrdered::shared_ptr NonlinearFactorGraph::linearize( GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& linearizationPoint) const
const Values& config, const OrderingOrdered& ordering) const
{ {
gttic(NonlinearFactorGraph_linearize); gttic(NonlinearFactorGraph_linearize);
// create an empty linear FG // create an empty linear FG
GaussianFactorGraphOrdered::shared_ptr linearFG(new GaussianFactorGraphOrdered); GaussianFactorGraph::shared_ptr linearFG = boost::make_shared<GaussianFactorGraph>();
linearFG->reserve(this->size()); linearFG->reserve(this->size());
// linearize all factors // linearize all factors
BOOST_FOREACH(const sharedFactor& factor, this->factors_) { BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor) { if(factor) {
GaussianFactorOrdered::shared_ptr lf = factor->linearize(config, ordering); (*linearFG) += factor->linearize(linearizationPoint);
if (lf) linearFG->push_back(lf);
} else } else
linearFG->push_back(GaussianFactorOrdered::shared_ptr()); (*linearFG) += GaussianFactor::shared_ptr();
} }
return linearFG; return linearFG;

55
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -22,12 +22,16 @@
#pragma once #pragma once
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/inference/FactorGraph.h>
namespace gtsam { namespace gtsam {
// Forward declarations
class Values;
class Ordering;
class GaussianFactorGraph;
/** /**
* Formatting options when saving in GraphViz format using * Formatting options when saving in GraphViz format using
* NonlinearFactorGraph::saveGraph. * NonlinearFactorGraph::saveGraph.
@ -58,13 +62,28 @@ namespace gtsam {
* tangent vector space at the linearization point. Because the tangent space is a true * 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. * vector space, the config type will be an VectorValues in that linearized factor graph.
*/ */
class NonlinearFactorGraph: public FactorGraphOrdered<NonlinearFactor> { class NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
public: public:
typedef FactorGraphOrdered<NonlinearFactor> Base; typedef FactorGraph<NonlinearFactor> Base;
typedef boost::shared_ptr<NonlinearFactorGraph> shared_ptr; typedef NonlinearFactorGraph This;
typedef boost::shared_ptr<NonlinearFactor> sharedFactor; typedef boost::shared_ptr<This> shared_ptr;
/** Default constructor */
NonlinearFactorGraph() {}
/** Construct from iterator over factors */
template<typename ITERATOR>
NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
/** Construct from container of factors (shared_ptr or plain objects) */
template<class CONTAINER>
explicit NonlinearFactorGraph(const CONTAINER& factors) : Base(factors) {}
/** Implicit copy/downcast constructor to override explicit template container constructor */
template<class DERIVEDFACTOR>
NonlinearFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
/** print just calls base class */ /** print just calls base class */
GTSAM_EXPORT void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; GTSAM_EXPORT void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
@ -83,20 +102,10 @@ namespace gtsam {
/** Unnormalized probability. O(n) */ /** Unnormalized probability. O(n) */
GTSAM_EXPORT double probPrime(const Values& c) const; 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 * 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 * 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 * The graph and ordering should be permuted after such a fill-reducing
* ordering is found. * ordering is found.
*/ */
GTSAM_EXPORT std::pair<SymbolicFactorGraphOrdered::shared_ptr, OrderingOrdered::shared_ptr> //GTSAM_EXPORT std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr>
symbolic(const Values& config) const; // symbolic(const Values& config) const;
/** /**
* Compute a fill-reducing ordering using COLAMD. * 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 * 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 * indices need to appear in the constraints, unconstrained is assumed for all
* other variables * other variables
*/ */
GTSAM_EXPORT OrderingOrdered::shared_ptr orderingCOLAMDConstrained(const Values& config, GTSAM_EXPORT Ordering orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const;
const std::map<Key, int>& constraints) const;
/** /**
* linearize a nonlinear factor graph * linearize a nonlinear factor graph
*/ */
GTSAM_EXPORT boost::shared_ptr<GaussianFactorGraphOrdered > GTSAM_EXPORT boost::shared_ptr<GaussianFactorGraph> linearize(const Values& linearizationPoint) const;
linearize(const Values& config, const OrderingOrdered& ordering) const;
/** /**
* Clone() performs a deep-copy of the graph, including all of the factors * Clone() performs a deep-copy of the graph, including all of the factors

17
gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp
View File

@ -6,10 +6,10 @@
*/ */
#include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h> #include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h>
#include <gtsam/inference/EliminationTreeOrdered.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianJunctionTreeOrdered.h> #include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/linear/SubgraphSolver.h> #include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/VectorValuesOrdered.h> #include <gtsam/linear/VectorValues.h>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <stdexcept> #include <stdexcept>
@ -53,15 +53,14 @@ void SuccessiveLinearizationParams::print(const std::string& str) const {
std::cout.flush(); std::cout.flush();
} }
VectorValuesOrdered solveGaussianFactorGraph(const GaussianFactorGraphOrdered &gfg, const SuccessiveLinearizationParams &params) { VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const SuccessiveLinearizationParams &params)
{
gttic(solveGaussianFactorGraph); gttic(solveGaussianFactorGraph);
VectorValuesOrdered delta; VectorValues delta;
if (params.isMultifrontal()) { if (params.isMultifrontal()) {
delta = GaussianJunctionTreeOrdered(gfg).optimize(params.getEliminationFunction()); delta = gfg.optimize(*params.ordering, params.getEliminationFunction());
} else if(params.isSequential()) { } else if(params.isSequential()) {
const boost::shared_ptr<GaussianBayesNetOrdered> gbn = delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize();
EliminationTreeOrdered<GaussianFactorOrdered>::Create(gfg)->eliminate(params.getEliminationFunction());
delta = gtsam::optimize(*gbn);
} }
else if ( params.isCG() ) { else if ( params.isCG() ) {
if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ..."); if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ...");

7
gtsam/nonlinear/SuccessiveLinearizationOptimizer.h
View File

@ -20,6 +20,7 @@
#include <gtsam/nonlinear/NonlinearOptimizer.h> #include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/linear/SubgraphSolver.h> #include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/inference/Ordering.h>
namespace gtsam { namespace gtsam {
@ -36,7 +37,7 @@ public:
}; };
LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer
boost::optional<OrderingOrdered> ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) boost::optional<Ordering> 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. IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers.
SuccessiveLinearizationParams() : linearSolverType(MULTIFRONTAL_CHOLESKY) {} SuccessiveLinearizationParams() : linearSolverType(MULTIFRONTAL_CHOLESKY) {}
@ -75,7 +76,7 @@ public:
void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); } void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); }
void setIterativeParams(const SubgraphSolverParameters &params); void setIterativeParams(const SubgraphSolverParameters &params);
void setOrdering(const OrderingOrdered& ordering) { this->ordering = ordering; } void setOrdering(const Ordering& ordering) { this->ordering = ordering; }
private: private:
std::string linearSolverTranslator(LinearSolverType linearSolverType) const { std::string linearSolverTranslator(LinearSolverType linearSolverType) const {
@ -101,6 +102,6 @@ private:
}; };
/* a wrapper for solving a GaussianFactorGraph according to the parameters */ /* a wrapper for solving a GaussianFactorGraph according to the parameters */
GTSAM_EXPORT VectorValuesOrdered solveGaussianFactorGraph(const GaussianFactorGraphOrdered &gfg, const SuccessiveLinearizationParams &params) ; GTSAM_EXPORT VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const SuccessiveLinearizationParams &params);
} /* namespace gtsam */ } /* namespace gtsam */

46
gtsam/nonlinear/Values.cpp
View File

@ -23,6 +23,7 @@
*/ */
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/VectorValues.h>
#include <list> #include <list>
@ -76,12 +77,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValuesOrdered Values::zeroVectors(const OrderingOrdered& ordering) const { Values Values::retract(const VectorValues& delta) const
return VectorValuesOrdered::Zero(this->dims(ordering)); {
}
/* ************************************************************************* */
Values Values::retract(const VectorValuesOrdered& delta, const OrderingOrdered& ordering) const {
Values result; Values result;
for(const_iterator key_value = begin(); key_value != end(); ++key_value) { 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 { VectorValues Values::localCoordinates(const Values& cp) const {
VectorValuesOrdered result(this->dims(ordering));
if(this->size() != cp.size()) if(this->size() != cp.size())
throw DynamicValuesMismatched(); throw DynamicValuesMismatched();
VectorValues result;
for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) { for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) {
if(it1->key != it2->key) if(it1->key != it2->key)
throw DynamicValuesMismatched(); // If keys do not match throw DynamicValuesMismatched(); // If keys do not match
// Will throw a dynamic_cast exception if types 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 // 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; 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 { const Value& Values::at(Key j) const {
// Find the item // Find the item
@ -192,16 +177,6 @@ namespace gtsam {
return *this; return *this;
} }
/* ************************************************************************* */
vector<size_t> Values::dims(const OrderingOrdered& ordering) const {
assert(ordering.size() == this->size()); // reads off of end of array if difference in size
vector<size_t> 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 Values::dim() const {
size_t result = 0; size_t result = 0;
@ -211,15 +186,6 @@ namespace gtsam {
return result; 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() { const char* ValuesKeyAlreadyExists::what() const throw() {
if(message_.empty()) if(message_.empty())

24
gtsam/nonlinear/Values.h
View File

@ -45,13 +45,12 @@
#include <gtsam/base/Value.h> #include <gtsam/base/Value.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
#include <gtsam/linear/VectorValuesOrdered.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/OrderingOrdered.h>
namespace gtsam { namespace gtsam {
// Forward declarations / utilities // Forward declarations / utilities
class VectorValues;
class ValueCloneAllocator; class ValueCloneAllocator;
class ValueAutomaticCasting; class ValueAutomaticCasting;
template<typename T> static bool _truePredicate(const T&) { return true; } template<typename T> static bool _truePredicate(const T&) { return true; }
@ -206,9 +205,6 @@ namespace gtsam {
/** whether the config is empty */ /** whether the config is empty */
bool empty() const { return values_.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 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); } 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); } 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 */ /** 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) */ /** Get a delta config about a linearization point c0 (*this) */
VectorValuesOrdered localCoordinates(const Values& cp, const OrderingOrdered& ordering) const; VectorValues localCoordinates(const Values& cp) 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;
///@} ///@}
@ -262,20 +255,9 @@ namespace gtsam {
/** Remove all variables from the config */ /** Remove all variables from the config */
void clear() { values_.clear(); } void clear() { values_.clear(); }
/** Create an array of variable dimensions using the given ordering (\f$ O(n) \f$) */
std::vector<size_t> dims(const OrderingOrdered& ordering) const;
/** Compute the total dimensionality of all values (\f$ O(n) \f$) */ /** Compute the total dimensionality of all values (\f$ O(n) \f$) */
size_t dim() const; 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. * Return a filtered view of this Values class, without copying any data.
* When iterating over the filtered view, only the key-value pairs * When iterating over the filtered view, only the key-value pairs