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

Further refactored pushNewFactors

release/4.3a0
Frank Dellaert 2019-06-01 13:45:07 -04:00
parent 5a819645a4
commit ecacda68c0
3 changed files with 45 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
gtsam/nonlinear/ISAM2-impl.h
View File

@ -117,17 +117,33 @@ struct GTSAM_EXPORT UpdateImpl {
UpdateImpl(const ISAM2Params& params, const ISAM2UpdateParams& updateParams) UpdateImpl(const ISAM2Params& params, const ISAM2UpdateParams& updateParams)
: params_(params), updateParams_(updateParams) {} : params_(params), updateParams_(updateParams) {}
// Provide some debugging information at the start of update
static void LogStartingUpdate(const NonlinearFactorGraph& newFactors,
const ISAM2& isam2) {
gttic(pushBackFactors);
const bool debug = ISDEBUG("ISAM2 update");
const bool verbose = ISDEBUG("ISAM2 update verbose");
if (verbose) {
std::cout << "ISAM2::update\n";
isam2.print("ISAM2: ");
}
// Add the new factor indices to the result struct
if (debug || verbose) {
newFactors.print("The new factors are: ");
}
}
/// Perform the first part of the bookkeeping updates for adding new factors. /// Perform the first part of the bookkeeping updates for adding new factors.
/// Adds them to the complete list of nonlinear factors, and populates the /// Adds them to the complete list of nonlinear factors, and populates the
/// list of new factor indices, both optionally finding and reusing empty /// list of new factor indices, both optionally finding and reusing empty
/// factor slots. /// factor slots.
static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, FactorIndices addFactorsStep1(const NonlinearFactorGraph& newFactors,
bool useUnusedSlots, NonlinearFactorGraph* nonlinearFactors) const {
NonlinearFactorGraph* nonlinearFactors, FactorIndices newFactorIndices(newFactors.size());
FactorIndices* newFactorIndices) {
newFactorIndices->resize(newFactors.size());
if (useUnusedSlots) { if (params_.findUnusedFactorSlots) {
size_t globalFactorIndex = 0; size_t globalFactorIndex = 0;
for (size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); for (size_t newFactorIndex = 0; newFactorIndex < newFactors.size();
++newFactorIndex) { ++newFactorIndex) {
@ -150,14 +166,15 @@ struct GTSAM_EXPORT UpdateImpl {
// Use the current slot, updating nonlinearFactors and newFactorSlots. // Use the current slot, updating nonlinearFactors and newFactorSlots.
(*nonlinearFactors)[globalFactorIndex] = newFactors[newFactorIndex]; (*nonlinearFactors)[globalFactorIndex] = newFactors[newFactorIndex];
(*newFactorIndices)[newFactorIndex] = globalFactorIndex; newFactorIndices[newFactorIndex] = globalFactorIndex;
} }
} else { } else {
// We're not looking for unused slots, so just add the factors at the end. // We're not looking for unused slots, so just add the factors at the end.
for (size_t i = 0; i < newFactors.size(); ++i) for (size_t i = 0; i < newFactors.size(); ++i)
(*newFactorIndices)[i] = i + nonlinearFactors->size(); newFactorIndices[i] = i + nonlinearFactors->size();
nonlinearFactors->push_back(newFactors); nonlinearFactors->push_back(newFactors);
} }
return newFactorIndices;
} }
// 1. Add any new factors \Factors:=\Factors\cup\Factors'. // 1. Add any new factors \Factors:=\Factors\cup\Factors'.
@ -167,13 +184,9 @@ struct GTSAM_EXPORT UpdateImpl {
VariableIndex* variableIndex, VariableIndex* variableIndex,
ISAM2Result* result) const { ISAM2Result* result) const {
gttic(pushBackFactors); gttic(pushBackFactors);
const bool debug = ISDEBUG("ISAM2 update");
const bool verbose = ISDEBUG("ISAM2 update verbose");
// Add the new factor indices to the result struct // Add the new factor indices to the result struct
if (debug || verbose) newFactors.print("The new factors are: "); result->newFactorsIndices = addFactorsStep1(newFactors, nonlinearFactors);
AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, nonlinearFactors,
&result->newFactorsIndices);
// Remove the removed factors // Remove the removed factors
NonlinearFactorGraph removedFactors; NonlinearFactorGraph removedFactors;
@ -378,8 +391,8 @@ struct GTSAM_EXPORT UpdateImpl {
* \c mask. Values are expmapped in-place. * \c mask. Values are expmapped in-place.
* \param mask Mask on linear indices, only \c true entries are expmapped * \param mask Mask on linear indices, only \c true entries are expmapped
*/ */
void expmapMasked(const VectorValues& delta, const KeySet& mask, static void ExpmapMasked(const VectorValues& delta, const KeySet& mask,
Values* theta) const { Values* theta) {
assert(theta->size() == delta.size()); assert(theta->size() == delta.size());
Values::iterator key_value; Values::iterator key_value;
VectorValues::const_iterator key_delta; VectorValues::const_iterator key_delta;
@ -468,7 +481,7 @@ struct GTSAM_EXPORT UpdateImpl {
gttic(expmap); gttic(expmap);
// 6. Update linearization point for marked variables: // 6. Update linearization point for marked variables:
// \Theta_{J}:=\Theta_{J}+\Delta_{J}. // \Theta_{J}:=\Theta_{J}+\Delta_{J}.
if (!relinKeys.empty()) expmapMasked(delta, markedRelinMask, theta); if (!relinKeys.empty()) ExpmapMasked(delta, markedRelinMask, theta);
gttoc(expmap); gttoc(expmap);
result->variablesRelinearized = result->markedKeys.size(); result->variablesRelinearized = result->markedKeys.size();
@ -511,7 +524,7 @@ struct GTSAM_EXPORT UpdateImpl {
} }
} }
void logRecalculateKeys(const ISAM2Result& result) const { static void LogRecalculateKeys(const ISAM2Result& result) {
const bool debug = ISDEBUG("ISAM2 recalculate"); const bool debug = ISDEBUG("ISAM2 recalculate");
if (debug) { if (debug) {
@ -528,8 +541,9 @@ struct GTSAM_EXPORT UpdateImpl {
} }
} }
FactorIndexSet getAffectedFactors(const KeyList& keys, static FactorIndexSet GetAffectedFactors(const KeyList& keys,
const VariableIndex& variableIndex) const { const VariableIndex& variableIndex) {
gttic(GetAffectedFactors);
FactorIndexSet indices; FactorIndexSet indices;
for (const Key key : keys) { for (const Key key : keys) {
const FactorIndices& factors(variableIndex[key]); const FactorIndices& factors(variableIndex[key]);
@ -540,8 +554,8 @@ struct GTSAM_EXPORT UpdateImpl {
// find intermediate (linearized) factors from cache that are passed into the // find intermediate (linearized) factors from cache that are passed into the
// affected area // affected area
GaussianFactorGraph getCachedBoundaryFactors( static GaussianFactorGraph GetCachedBoundaryFactors(
const ISAM2::Cliques& orphans) const { const ISAM2::Cliques& orphans) {
GaussianFactorGraph cachedBoundary; GaussianFactorGraph cachedBoundary;
for (const auto& orphan : orphans) { for (const auto& orphan : orphans) {
@ -559,9 +573,7 @@ struct GTSAM_EXPORT UpdateImpl {
const NonlinearFactorGraph& nonlinearFactors, const NonlinearFactorGraph& nonlinearFactors,
const VariableIndex& variableIndex, const Values& theta, const VariableIndex& variableIndex, const Values& theta,
GaussianFactorGraph* linearFactors) const { GaussianFactorGraph* linearFactors) const {
gttic(getAffectedFactors); FactorIndexSet candidates = GetAffectedFactors(affectedKeys, variableIndex);
FactorIndexSet candidates = getAffectedFactors(affectedKeys, variableIndex);
gttoc(getAffectedFactors);
gttic(affectedKeysSet); gttic(affectedKeysSet);
// for fast lookup below // for fast lookup below
@ -614,7 +626,7 @@ struct GTSAM_EXPORT UpdateImpl {
GaussianFactorGraph* linearFactors, ISAM2::Roots* roots, GaussianFactorGraph* linearFactors, ISAM2::Roots* roots,
ISAM2::Nodes* nodes, ISAM2Result* result) const { ISAM2::Nodes* nodes, ISAM2Result* result) const {
gttic(recalculate); gttic(recalculate);
logRecalculateKeys(*result); LogRecalculateKeys(*result);
// FactorGraph<GaussianFactor> factors(affectedBayesNet); // FactorGraph<GaussianFactor> factors(affectedBayesNet);
// bug was here: we cannot reuse the original factors, because then the // bug was here: we cannot reuse the original factors, because then the
@ -754,7 +766,7 @@ struct GTSAM_EXPORT UpdateImpl {
gttic(cached); gttic(cached);
// add the cached intermediate results from the boundary of the orphans // add the cached intermediate results from the boundary of the orphans
// ... // ...
GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); GaussianFactorGraph cachedBoundary = GetCachedBoundaryFactors(orphans);
if (debug) cachedBoundary.print("Boundary factors: "); if (debug) cachedBoundary.print("Boundary factors: ");
factors.push_back(cachedBoundary); factors.push_back(cachedBoundary);
gttoc(cached); gttoc(cached);
@ -846,6 +858,5 @@ struct GTSAM_EXPORT UpdateImpl {
return affectedKeysSet; return affectedKeysSet;
} }
}; };
/* ************************************************************************* */
} // namespace gtsam } // namespace gtsam

8
gtsam/nonlinear/ISAM2.cpp
View File

@ -116,8 +116,8 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
const Values& newTheta, const Values& newTheta,
const ISAM2UpdateParams& updateParams) { const ISAM2UpdateParams& updateParams) {
gttic(ISAM2_update); gttic(ISAM2_update);
this->update_count_++; this->update_count_++;
UpdateImpl::LogStartingUpdate(newFactors, *this);
ISAM2Result result; ISAM2Result result;
if (params_.enableDetailedResults) if (params_.enableDetailedResults)
@ -127,12 +127,6 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
(params_.enableRelinearization && (params_.enableRelinearization &&
update_count_ % params_.relinearizeSkip == 0); update_count_ % params_.relinearizeSkip == 0);
const bool verbose = ISDEBUG("ISAM2 update verbose");
if (verbose) {
cout << "ISAM2::update\n";
this->print("ISAM2: ");
}
// Update delta if we need it to check relinearization later // Update delta if we need it to check relinearization later
if (relinearizeThisStep) { if (relinearizeThisStep) {
updateDelta(updateParams.forceFullSolve); updateDelta(updateParams.forceFullSolve);

9
tests/testGaussianISAM2.cpp
View File

@ -305,9 +305,12 @@ TEST(ISAM2, AddFactorsStep1)
const FactorIndices expectedNewFactorIndices = list_of(1)(3); const FactorIndices expectedNewFactorIndices = list_of(1)(3);
FactorIndices actualNewFactorIndices; ISAM2Params params;
ISAM2UpdateParams updateParams;
UpdateImpl::AddFactorsStep1(newFactors, true, &nonlinearFactors, &actualNewFactorIndices); params.findUnusedFactorSlots = true;
UpdateImpl update(params, updateParams);
FactorIndices actualNewFactorIndices =
update.addFactorsStep1(newFactors, &nonlinearFactors);
EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors));
EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices)); EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices));