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

linearize now always returns a shared pointer

release/4.3a0
Frank Dellaert 2010-02-21 21:17:47 +00:00
parent 517c82f62f
commit 694f6e4219
12 changed files with 46 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/GaussianFactorGraph.h
View File

@ -278,7 +278,7 @@ namespace gtsam {
* linearize the non-linear graph around the current config * linearize the non-linear graph around the current config
*/ */
boost::shared_ptr<GaussianFactorGraph> linearize(const NonlinearGraph& g, const Config& config) const { boost::shared_ptr<GaussianFactorGraph> linearize(const NonlinearGraph& g, const Config& config) const {
return g.linearize_(config); return g.linearize(config);
} }
}; };
} }

24
cpp/ISAM2-inl.h
View File

@ -67,9 +67,10 @@ namespace gtsam {
/** Create a Bayes Tree from a nonlinear factor graph */ /** Create a Bayes Tree from a nonlinear factor graph */
template<class Conditional, class Config> template<class Conditional, class Config>
ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& config) ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& config)
: BayesTree<Conditional>(nlfg.linearize(config).eliminate(ordering)), theta_(config), thetaFuture_(config), nonlinearFactors_(nlfg) { : BayesTree<Conditional>(nlfg.linearize(config)->eliminate(ordering)), theta_(config), thetaFuture_(config), nonlinearFactors_(nlfg) {
// todo: repeats calculation above, just to set "cached" // todo: repeats calculation above, just to set "cached"
_eliminate_const(nlfg.linearize(config), cached_, ordering); // De-referencing shared pointer can be quite expensive because creates temporary
_eliminate_const(*nlfg.linearize(config), cached_, ordering);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -90,7 +91,8 @@ namespace gtsam {
// retrieve all factors that ONLY contain the affected variables // retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors) // (note that the remaining stuff is summarized in the cached factors)
template<class Conditional, class Config> template<class Conditional, class Config>
FactorGraph<GaussianFactor> ISAM2<Conditional, Config>::relinearizeAffectedFactors(const set<Symbol>& affectedKeys) const { boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Config>::relinearizeAffectedFactors
(const set<Symbol>& affectedKeys) const {
list<Symbol> affectedKeysList; // todo: shouldn't have to convert back to list... list<Symbol> affectedKeysList; // todo: shouldn't have to convert back to list...
affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end()); affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end());
@ -110,6 +112,7 @@ namespace gtsam {
nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]); nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]);
} }
// TODO: temporary might be expensive, return shared pointer ?
return nonlinearAffectedFactors.linearize(theta_); return nonlinearAffectedFactors.linearize(theta_);
} }
@ -190,7 +193,7 @@ namespace gtsam {
//// 6 - find factors connected to affected variables //// 6 - find factors connected to affected variables
//// 7 - linearize //// 7 - linearize
FactorGraph<GaussianFactor> factors; boost::shared_ptr<GaussianFactorGraph> factors;
if (relinFromLast) { if (relinFromLast) {
// ordering provides all keys in conditionals, there cannot be others because path to root included // ordering provides all keys in conditionals, there cannot be others because path to root included
@ -207,25 +210,26 @@ namespace gtsam {
factors = relinearizeAffectedFactors(affectedKeys); factors = relinearizeAffectedFactors(affectedKeys);
// Save number of affected factors // Save number of affected factors
lastAffectedFactorCount = factors.size(); lastAffectedFactorCount = factors->size();
// add the cached intermediate results from the boundary of the orphans ... // add the cached intermediate results from the boundary of the orphans ...
FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans); FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
factors.push_back(cachedBoundary); factors->push_back(cachedBoundary);
} else { } else {
// reuse the old factors // reuse the old factors
FactorGraph<GaussianFactor> tmp(affectedBayesNet); FactorGraph<GaussianFactor> tmp(affectedBayesNet);
factors.push_back(tmp); factors.reset(new GaussianFactorGraph);
factors.push_back(newFactors.linearize(theta_)); factors->push_back(tmp);
factors->push_back(*newFactors.linearize(theta_)); // avoid temporary ?
} }
//// 8 - eliminate and add orphans back in //// 8 - eliminate and add orphans back in
// create an ordering for the new and contaminated factors // create an ordering for the new and contaminated factors
Ordering ordering = factors.getOrdering(); Ordering ordering = factors->getOrdering();
// eliminate into a Bayes net // eliminate into a Bayes net
BayesNet<Conditional> bayesNet = _eliminate(factors, cached_, ordering); BayesNet<Conditional> bayesNet = _eliminate(*factors, cached_, ordering);
// Create Index from ordering // Create Index from ordering
IndexTable<Symbol> index(ordering); IndexTable<Symbol> index(ordering);

2
cpp/ISAM2.h
View File

@ -96,7 +96,7 @@ namespace gtsam {
private: private:
std::list<int> getAffectedFactors(const std::list<Symbol>& keys) const; std::list<int> getAffectedFactors(const std::list<Symbol>& keys) const;
FactorGraph<GaussianFactor> relinearizeAffectedFactors(const std::set<Symbol>& affectedKeys) const; boost::shared_ptr<GaussianFactorGraph> relinearizeAffectedFactors(const std::set<Symbol>& affectedKeys) const;
FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans); FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans);
}; // ISAM2 }; // ISAM2

14
cpp/NonlinearFactorGraph-inl.h
View File

@ -42,24 +42,22 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class Config> template<class Config>
boost::shared_ptr<GaussianFactorGraph> NonlinearFactorGraph<Config>::linearize_( boost::shared_ptr<GaussianFactorGraph> NonlinearFactorGraph<Config>::linearize(
const Config& config) const{ const Config& config) const{
// create an empty linear FG // create an empty linear FG
boost::shared_ptr<GaussianFactorGraph> linearFG(new GaussianFactorGraph); boost::shared_ptr<GaussianFactorGraph> linearFG(new GaussianFactorGraph);
// linearize all factors // linearize all factors
BOOST_FOREACH(typename NonlinearFactorGraph<Config>::sharedFactor factor, this->factors_) { typedef typename NonlinearFactorGraph<Config>::sharedFactor Factor;
BOOST_FOREACH(const Factor& factor, this->factors_) {
boost::shared_ptr<GaussianFactor> lf = factor->linearize(config); boost::shared_ptr<GaussianFactor> lf = factor->linearize(config);
linearFG->push_back(lf); linearFG->push_back(lf);
} }
return linearFG; return linearFG;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Config>
GaussianFactorGraph NonlinearFactorGraph<Config>::linearize(
const Config& config) const {
return *linearize_(config);
}
} // namespace gtsam } // namespace gtsam

7
cpp/NonlinearFactorGraph.h
View File

@ -49,13 +49,8 @@ namespace gtsam {
/** /**
* linearize a nonlinear factor graph * linearize a nonlinear factor graph
*/ */
GaussianFactorGraph linearize(const Config& config) const;
/**
* shared pointer versions for MATLAB
*/
boost::shared_ptr<GaussianFactorGraph> boost::shared_ptr<GaussianFactorGraph>
linearize_(const Config& config) const; linearize(const Config& config) const;
}; };

8
cpp/SubgraphPreconditioner-inl.h
View File

@ -52,16 +52,18 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class T> template<class G, class T>
boost::shared_ptr<SubgraphPreconditioner> SubgraphPCG<G, T>::linearize(const G& g, const T& theta_bar) const { boost::shared_ptr<SubgraphPreconditioner> SubgraphPCG<G, T>::linearize(const G& g, const T& theta_bar) const {
SubgraphPreconditioner::sharedFG Ab1 = T_.linearize_(theta_bar); SubgraphPreconditioner::sharedFG Ab1 = T_.linearize(theta_bar);
SubgraphPreconditioner::sharedFG Ab2 = C_.linearize_(theta_bar); SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar);
#ifdef TIMING #ifdef TIMING
SubgraphPreconditioner::sharedBayesNet Rc1; SubgraphPreconditioner::sharedBayesNet Rc1;
SubgraphPreconditioner::sharedConfig xbar; SubgraphPreconditioner::sharedConfig xbar;
#else #else
GaussianFactorGraph sacrificialAb1 = T_.linearize(theta_bar); // duplicate !!!!! GaussianFactorGraph sacrificialAb1 = *Ab1; // duplicate !!!!!
SubgraphPreconditioner::sharedBayesNet Rc1 = sacrificialAb1.eliminate_(*ordering_); SubgraphPreconditioner::sharedBayesNet Rc1 = sacrificialAb1.eliminate_(*ordering_);
SubgraphPreconditioner::sharedConfig xbar = gtsam::optimize_(*Rc1); SubgraphPreconditioner::sharedConfig xbar = gtsam::optimize_(*Rc1);
#endif #endif
// TODO: there does not seem to be a good reason to have Ab1_
// It seems only be used to provide an ordering for creating sparse matrices
return boost::shared_ptr<SubgraphPreconditioner>(new SubgraphPreconditioner(Ab1, Ab2, Rc1, xbar)); return boost::shared_ptr<SubgraphPreconditioner>(new SubgraphPreconditioner(Ab1, Ab2, Rc1, xbar));
} }

7
cpp/smallExample.cpp
View File

@ -266,8 +266,7 @@ namespace example {
Config poses; Config poses;
boost::tie(nlfg, poses) = createNonlinearSmoother(T); boost::tie(nlfg, poses) = createNonlinearSmoother(T);
GaussianFactorGraph lfg = nlfg.linearize(poses); return *nlfg.linearize(poses);
return lfg;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -556,9 +555,7 @@ namespace example {
} }
// linearize around zero // linearize around zero
GaussianFactorGraph A = nlfg.linearize(zeros); return make_pair(*nlfg.linearize(zeros), xtrue);
return make_pair(A, xtrue);
} }
/* ************************************************************************* */ /* ************************************************************************* */

12
cpp/testIterative.cpp
View File

@ -122,8 +122,8 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint )
zeros.insert("x1",zero(3)); zeros.insert("x1",zero(3));
zeros.insert("x2",zero(3)); zeros.insert("x2",zero(3));
GaussianFactorGraph fg = graph.linearize(config); boost::shared_ptr<GaussianFactorGraph> fg = graph.linearize(config);
VectorConfig actual = conjugateGradientDescent(fg, zeros, verbose, 1e-3, 1e-5, 100); VectorConfig actual = conjugateGradientDescent(*fg, zeros, verbose, 1e-3, 1e-5, 100);
VectorConfig expected; VectorConfig expected;
expected.insert("x1", zero(3)); expected.insert("x1", zero(3));
@ -157,10 +157,10 @@ TEST( Iterative, subgraphPCG )
graph.split<Key, Pose2Factor>(tree, T, C); graph.split<Key, Pose2Factor>(tree, T, C);
// build the subgraph PCG system // build the subgraph PCG system
GaussianFactorGraph Ab1_ = T.linearize(theta_bar); boost::shared_ptr<GaussianFactorGraph> Ab1_ = T.linearize(theta_bar);
SubgraphPreconditioner::sharedFG Ab1 = T.linearize_(theta_bar); SubgraphPreconditioner::sharedFG Ab1 = T.linearize(theta_bar);
SubgraphPreconditioner::sharedFG Ab2 = C.linearize_(theta_bar); SubgraphPreconditioner::sharedFG Ab2 = C.linearize(theta_bar);
SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1_.eliminate_(ordering); SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1_->eliminate_(ordering);
SubgraphPreconditioner::sharedConfig xbar = optimize_(*Rc1); SubgraphPreconditioner::sharedConfig xbar = optimize_(*Rc1);
SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar); SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar);

4
cpp/testNonlinearFactorGraph.cpp
View File

@ -73,9 +73,9 @@ TEST( Graph, linearize )
{ {
Graph fg = createNonlinearFactorGraph(); Graph fg = createNonlinearFactorGraph();
Config initial = createNoisyConfig(); Config initial = createNoisyConfig();
GaussianFactorGraph linearized = fg.linearize(initial); boost::shared_ptr<GaussianFactorGraph> linearized = fg.linearize(initial);
GaussianFactorGraph expected = createGaussianFactorGraph(); GaussianFactorGraph expected = createGaussianFactorGraph();
CHECK(assert_equal(expected,linearized)); // Needs correct linearizations CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
cpp/testPose2SLAM.cpp
View File

@ -63,8 +63,8 @@ TEST( Pose2Graph, linearization )
config.insert(1,p1); config.insert(1,p1);
config.insert(2,p2); config.insert(2,p2);
// Linearize // Linearize
GaussianFactorGraph lfg_linearized = graph.linearize(config); boost::shared_ptr<GaussianFactorGraph> lfg_linearized = graph.linearize(config);
//lfg_linearized.print("lfg_actual"); //lfg_linearized->print("lfg_actual");
// the expected linear factor // the expected linear factor
GaussianFactorGraph lfg_expected; GaussianFactorGraph lfg_expected;
@ -83,7 +83,7 @@ TEST( Pose2Graph, linearization )
SharedDiagonal probModel1 = noiseModel::Unit::Create(3); SharedDiagonal probModel1 = noiseModel::Unit::Create(3);
lfg_expected.add("x1", A1, "x2", A2, b, probModel1); lfg_expected.add("x1", A1, "x2", A2, b, probModel1);
CHECK(assert_equal(lfg_expected, lfg_linearized)); CHECK(assert_equal(lfg_expected, *lfg_linearized));
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
cpp/testSQP.cpp
View File

@ -430,16 +430,16 @@ TEST (SQP, two_pose ) {
Config2D state(*initialEstimate); Config2D state(*initialEstimate);
// linearize the graph // linearize the graph
GaussianFactorGraph fg = graph->linearize(state); boost::shared_ptr<GaussianFactorGraph> fg = graph->linearize(state);
if (verbose) fg.print("Linearized graph"); if (verbose) fg->print("Linearized graph");
// create an ordering // create an ordering
Ordering ordering; Ordering ordering;
ordering += "x1", "x2", "l1", "l2", "L1"; ordering += "x1", "x2", "l1", "l2", "L1";
// optimize linear graph to get full delta config // optimize linear graph to get full delta config
GaussianBayesNet cbn = fg.eliminate(ordering); GaussianBayesNet cbn = fg->eliminate(ordering);
if (verbose) cbn.print("ChordalBayesNet"); if (verbose) cbn.print("ChordalBayesNet");
VectorConfig delta = optimize(cbn); //fg.optimize(ordering); VectorConfig delta = optimize(cbn); //fg.optimize(ordering);

4
cpp/testVSLAMFactor.cpp
View File

@ -62,8 +62,8 @@ TEST( ProjectionFactor, error )
graph.push_back(factor); graph.push_back(factor);
GaussianFactorGraph expected_lfg; GaussianFactorGraph expected_lfg;
expected_lfg.push_back(actual); expected_lfg.push_back(actual);
GaussianFactorGraph actual_lfg = graph.linearize(config); boost::shared_ptr<GaussianFactorGraph> actual_lfg = graph.linearize(config);
CHECK(assert_equal(expected_lfg,actual_lfg)); CHECK(assert_equal(expected_lfg,*actual_lfg));
// expmap on a config // expmap on a config
VectorConfig delta; VectorConfig delta;