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

cleanup

release/4.3a0
Michael Kaess 2010-09-05 18:28:06 +00:00
parent dacd4b78a1
commit d850e2837c
2 changed files with 19 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
inference/ISAM2-inl.h
View File

@ -129,6 +129,7 @@ namespace gtsam {
return chordalBayesNet;
}
// special const version used in constructor below
GaussianBayesNet _eliminate_const(const FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) {
// make a copy that can be modified locally
FactorGraph<GaussianFactor> graph_ignored = graph;
@ -142,7 +143,8 @@ namespace gtsam {
/** Create a Bayes Tree from a nonlinear factor graph */
template<class Conditional, class Config>
ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& config)
: BayesTree<Conditional>(nlfg.linearize(config)->eliminate(ordering)), theta_(config), nonlinearFactors_(nlfg) {
: BayesTree<Conditional>(nlfg.linearize(config)->eliminate(ordering)),
theta_(config), delta_(VectorConfig()), nonlinearFactors_(nlfg) {
// todo: repeats calculation above, just to set "cached"
// De-referencing shared pointer can be quite expensive because creates temporary
_eliminate_const(*nlfg.linearize(config), cached_, ordering);
@ -213,7 +215,7 @@ namespace gtsam {
// Input: BayesTree(this), newFactors
//#define PRINT_STATS // todo: figures for paper, disable for timing
//#define PRINT_STATS // figures for paper, disable for timing
#ifdef PRINT_STATS
static int counter = 0;
int maxClique = 0;
@ -286,8 +288,8 @@ namespace gtsam {
// newKeys are passed in: those variables will be forced to the end in the ordering
set<Symbol> newKeysSet;
newKeysSet.insert(newKeys.begin(), newKeys.end());
Ordering ordering = factors.getConstrainedOrdering(newKeysSet);
// Ordering ordering = factors.getOrdering();
Ordering ordering = factors.getConstrainedOrdering(newKeysSet); // intelligent ordering
// Ordering ordering = factors.getOrdering(); // original ordering, yields in bad performance
// eliminate into a Bayes net
tic("linear_eliminate");

34
inference/ISAM2.h
View File

@ -35,19 +35,15 @@ namespace gtsam {
// current linearization point
Config theta_;
// the linear solution, an update to the estimate in theta
VectorConfig delta_;
// for keeping all original nonlinear factors
NonlinearFactorGraph<Config> nonlinearFactors_;
// cached intermediate results for restarting computation in the middle
CachedFactors cached_;
// the linear solution, an update to the estimate in theta
VectorConfig delta_;
VectorConfig deltaMarked_;
// variables that have been updated, requiring the corresponding factors to be relinearized
std::list<Symbol> marked_;
public:
/** Create an empty Bayes Tree */
@ -57,37 +53,29 @@ namespace gtsam {
ISAM2(const NonlinearFactorGraph<Config>& fg, const Ordering& ordering, const Config& config);
/** Destructor */
virtual ~ISAM2() {
}
virtual ~ISAM2() {}
typedef typename BayesTree<Conditional>::sharedClique sharedClique;
typedef typename BayesTree<Conditional>::Cliques Cliques;
/**
* ISAM2. (update_internal provides access to list of orphans for drawing purposes)
* ISAM2.
*/
void linear_update(const FactorGraph<GaussianFactor>& newFactors);
void find_all(sharedClique clique, std::list<Symbol>& keys, const std::list<Symbol>& marked); // helper function
void fluid_relinearization(double relinearize_threshold);
void update(const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
double wildfire_threshold = 0., double relinearize_threshold = 0., bool relinearize = true);
// needed to create initial estimates (note that this will be the linearization point in the next step!)
// needed to create initial estimates
const Config getLinearizationPoint() const {return theta_;}
// estimate based on incomplete delta (threshold!)
const Config calculateEstimate() const {return theta_.expmap(delta_);}
// estimate based on full delta (note that this is based on the actual current linearization point)
// estimate based on full delta (note that this is based on the current linearization point)
const Config calculateBestEstimate() const {return theta_.expmap(optimize2(*this, 0.));}
const std::list<Symbol>& getMarkedUnsafe() const { return marked_; }
const NonlinearFactorGraph<Config>& getFactorsUnsafe() const { return nonlinearFactors_; }
const Config& getThetaUnsafe() const { return theta_; }
const VectorConfig& getDeltaUnsafe() const { return delta_; }
size_t lastAffectedVariableCount;
size_t lastAffectedFactorCount;
size_t lastAffectedCliqueCount;
@ -98,6 +86,10 @@ namespace gtsam {
boost::shared_ptr<GaussianFactorGraph> relinearizeAffectedFactors(const std::set<Symbol>& affectedKeys) const;
FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans);
void linear_update(const FactorGraph<GaussianFactor>& newFactors);
void find_all(sharedClique clique, std::list<Symbol>& keys, const std::list<Symbol>& marked); // helper function
void fluid_relinearization(double relinearize_threshold);
}; // ISAM2
} /// namespace gtsam