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

Use VectorValues::vector rather than bespoke code for saving delta values.

release/4.3a0
Frank Dellaert 2018-09-30 10:30:27 -04:00
parent 34326c20ec
commit 5223713c18
2 changed files with 46 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
gtsam/nonlinear/ISAM2.cpp
View File

@ -85,10 +85,10 @@ class ISAM2JunctionTree
}; };
/* ************************************************************************* */ /* ************************************************************************* */
std::string ISAM2DoglegParams::adaptationModeTranslator( string ISAM2DoglegParams::adaptationModeTranslator(
const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode)
const { const {
std::string s; string s;
switch (adaptationMode) { switch (adaptationMode) {
case DoglegOptimizerImpl::SEARCH_EACH_ITERATION: case DoglegOptimizerImpl::SEARCH_EACH_ITERATION:
s = "SEARCH_EACH_ITERATION"; s = "SEARCH_EACH_ITERATION";
@ -106,8 +106,8 @@ std::string ISAM2DoglegParams::adaptationModeTranslator(
/* ************************************************************************* */ /* ************************************************************************* */
DoglegOptimizerImpl::TrustRegionAdaptationMode DoglegOptimizerImpl::TrustRegionAdaptationMode
ISAM2DoglegParams::adaptationModeTranslator( ISAM2DoglegParams::adaptationModeTranslator(
const std::string& adaptationMode) const { const string& adaptationMode) const {
std::string s = adaptationMode; string s = adaptationMode;
boost::algorithm::to_upper(s); boost::algorithm::to_upper(s);
if (s == "SEARCH_EACH_ITERATION") if (s == "SEARCH_EACH_ITERATION")
return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; return DoglegOptimizerImpl::SEARCH_EACH_ITERATION;
@ -120,8 +120,8 @@ ISAM2DoglegParams::adaptationModeTranslator(
/* ************************************************************************* */ /* ************************************************************************* */
ISAM2Params::Factorization ISAM2Params::factorizationTranslator( ISAM2Params::Factorization ISAM2Params::factorizationTranslator(
const std::string& str) { const string& str) {
std::string s = str; string s = str;
boost::algorithm::to_upper(s); boost::algorithm::to_upper(s);
if (s == "QR") return ISAM2Params::QR; if (s == "QR") return ISAM2Params::QR;
if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; if (s == "CHOLESKY") return ISAM2Params::CHOLESKY;
@ -131,9 +131,9 @@ ISAM2Params::Factorization ISAM2Params::factorizationTranslator(
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::string ISAM2Params::factorizationTranslator( string ISAM2Params::factorizationTranslator(
const ISAM2Params::Factorization& value) { const ISAM2Params::Factorization& value) {
std::string s; string s;
switch (value) { switch (value) {
case ISAM2Params::QR: case ISAM2Params::QR:
s = "QR"; s = "QR";
@ -171,13 +171,12 @@ bool ISAM2Clique::equals(const This& other, double tol) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ISAM2Clique::print(const std::string& s, void ISAM2Clique::print(const string& s, const KeyFormatter& formatter) const {
const KeyFormatter& formatter) const {
Base::print(s, formatter); Base::print(s, formatter);
if (cachedFactor_) if (cachedFactor_)
cachedFactor_->print(s + "Cached: ", formatter); cachedFactor_->print(s + "Cached: ", formatter);
else else
std::cout << s << "Cached empty" << std::endl; cout << s << "Cached empty" << endl;
if (gradientContribution_.rows() != 0) if (gradientContribution_.rows() != 0)
gtsam::print(gradientContribution_, "Gradient contribution: "); gtsam::print(gradientContribution_, "Gradient contribution: ");
} }
@ -207,31 +206,16 @@ bool ISAM2Clique::isDirty(const KeySet& replaced, const KeySet& changed) const {
} }
return dirty; return dirty;
} }
/* ************************************************************************* */
FastVector<Vector> ISAM2Clique::copyRelevantValues(
const VectorValues& delta) const {
FastVector<Vector> values;
values.reserve(conditional_->nrFrontals());
for (Key frontal : conditional_->frontals()) {
values.push_back(delta[frontal]);
}
return values;
}
/* ************************************************************************* */ /* ************************************************************************* */
bool ISAM2Clique::valuesChanged(const KeySet& replaced, bool ISAM2Clique::valuesChanged(const KeySet& replaced,
const FastVector<Vector>& originalValues, const Vector& originalValues,
const VectorValues& delta, const VectorValues& delta,
double threshold) const { double threshold) const {
if (replaced.exists(conditional_->frontals().front())) return true; auto frontals = conditional_->frontals();
size_t i = 0; if (replaced.exists(frontals.front())) return true;
for (Key frontal : conditional_->frontals()) { auto diff = originalValues - delta.vector(frontals);
const Vector diff = originalValues[i++] - delta[frontal]; return diff.lpNorm<Eigen::Infinity>() >= threshold;
if (diff.lpNorm<Eigen::Infinity>() >= threshold) {
return true;
}
}
return false;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -243,11 +227,13 @@ void ISAM2Clique::markFrontalsAsChanged(KeySet* changed) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ISAM2Clique::restoreFromOriginals(const FastVector<Vector>& originalValues, void ISAM2Clique::restoreFromOriginals(const Vector& originalValues,
VectorValues* delta) const { VectorValues* delta) const {
size_t i = 0; size_t pos = 0;
for (Key frontal : conditional_->frontals()) { for (Key frontal : conditional_->frontals()) {
delta->at(frontal) = originalValues[i++]; auto v = delta->at(frontal);
v = originalValues.segment(pos, v.size());
pos += v.size();
} }
} }
@ -257,7 +243,7 @@ void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold,
size_t* count) const { size_t* count) const {
if (isDirty(replaced, *changed)) { if (isDirty(replaced, *changed)) {
// Temporary copy of the original values, to check how much they change // Temporary copy of the original values, to check how much they change
auto originalValues = copyRelevantValues(*delta); auto originalValues = delta->vector(conditional_->frontals());
// Back-substitute // Back-substitute
delta->update(conditional_->solve(*delta)); delta->update(conditional_->solve(*delta));
@ -285,7 +271,7 @@ bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold,
bool dirty = isDirty(replaced, *changed); bool dirty = isDirty(replaced, *changed);
if (dirty) { if (dirty) {
// Temporary copy of the original values, to check how much they change // Temporary copy of the original values, to check how much they change
auto originalValues = copyRelevantValues(*delta); auto originalValues = delta->vector(conditional_->frontals());
// Back-substitute - special version stores solution pointers in cliques for // Back-substitute - special version stores solution pointers in cliques for
// fast access. // fast access.

52
gtsam/nonlinear/ISAM2.h
View File

@ -546,36 +546,12 @@ class GTSAM_EXPORT ISAM2Clique
void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const; const KeyFormatter& formatter = DefaultKeyFormatter) const;
/** void optimizeWildfire(const KeySet& replaced, double threshold,
* Check if clique was replaced, or if any parents were changed above the KeySet* changed, VectorValues* delta,
* threshold or themselves replaced.
*/
bool isDirty(const KeySet& replaced, const KeySet& changed) const;
/// Copy values in delta pertaining to this clique.
FastVector<Vector> copyRelevantValues(const VectorValues& delta) const;
/*
* Check whether the values changed above a threshold, or always true if the
* clique was replaced.
*/
bool valuesChanged(const KeySet& replaced,
const FastVector<Vector>& originalValues,
const VectorValues& delta, double threshold) const;
/// Set changed flag for each frontal variable
void markFrontalsAsChanged(KeySet* changed) const;
/// Restore delta to original values, guided by frontal keys.
void restoreFromOriginals(const FastVector<Vector>& originalValues,
VectorValues* delta) const;
void optimizeWildfire(const KeySet& replaced, double threshold, KeySet* changed,
VectorValues* delta,
size_t* count) const; size_t* count) const;
bool optimizeWildfireNode(const KeySet& replaced, double threshold, KeySet* changed, bool optimizeWildfireNode(const KeySet& replaced, double threshold,
VectorValues* delta, KeySet* changed, VectorValues* delta,
size_t* count) const; size_t* count) const;
/** /**
@ -586,6 +562,26 @@ class GTSAM_EXPORT ISAM2Clique
size_t calculate_nnz() const; size_t calculate_nnz() const;
private: private:
/**
* Check if clique was replaced, or if any parents were changed above the
* threshold or themselves replaced.
*/
bool isDirty(const KeySet& replaced, const KeySet& changed) const;
/*
* Check whether the values changed above a threshold, or always true if the
* clique was replaced.
*/
bool valuesChanged(const KeySet& replaced, const Vector& originalValues,
const VectorValues& delta, double threshold) const;
/// Set changed flag for each frontal variable
void markFrontalsAsChanged(KeySet* changed) const;
/// Restore delta to original values, guided by frontal keys.
void restoreFromOriginals(const Vector& originalValues,
VectorValues* delta) const;
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template <class ARCHIVE> template <class ARCHIVE>