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

tested and documented ability to swap factors with ISAM2

release/4.3a0
Alex Cunningham 2012-05-01 18:54:44 +00:00
parent 99971df559
commit cb017fee80
3 changed files with 107 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/nonlinear/ISAM2.cpp
View File

@ -346,8 +346,11 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
lastAffectedFactorCount = linearFactors_.size();
// Reeliminated keys for detailed results
if(params_.enableDetailedResults)
BOOST_FOREACH(Key key, theta_.keys()) { result.detail->variableStatus[key].isReeliminated = true; }
if(params_.enableDetailedResults) {
BOOST_FOREACH(Key key, theta_.keys()) {
result.detail->variableStatus[key].isReeliminated = true;
}
}
toc(3,"batch");
@ -367,8 +370,11 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; }
// Reeliminated keys for detailed results
if(params_.enableDetailedResults)
BOOST_FOREACH(Index index, affectedAndNewKeys) { result.detail->variableStatus[inverseOrdering_->at(index)].isReeliminated = true; }
if(params_.enableDetailedResults) {
BOOST_FOREACH(Index index, affectedAndNewKeys) {
result.detail->variableStatus[inverseOrdering_->at(index)].isReeliminated = true;
}
}
result.variablesReeliminated = affectedAndNewKeys.size();
lastAffectedMarkedCount = markedKeys.size();
@ -477,8 +483,11 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
}
// Root clique variables for detailed results
if(params_.enableDetailedResults)
BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) { result.detail->variableStatus[inverseOrdering_->at(index)].inRootClique = true; }
if(params_.enableDetailedResults) {
BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) {
result.detail->variableStatus[inverseOrdering_->at(index)].inRootClique = true;
}
}
return affectedKeysSet;
}
@ -561,8 +570,11 @@ ISAM2Result ISAM2::update(
markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys
}
// Observed keys for detailed results
if(params_.enableDetailedResults)
BOOST_FOREACH(Index index, markedKeys) { result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true; }
if(params_.enableDetailedResults) {
BOOST_FOREACH(Index index, markedKeys) {
result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true;
}
}
// NOTE: we use assign instead of the iterator constructor here because this
// is a vector of size_t, so the constructor unintentionally resolves to
// vector(size_t count, Index value) instead of the iterator constructor.

4
gtsam/nonlinear/ISAM2.h
View File

@ -429,6 +429,9 @@ public:
/**
* Add new factors, updating the solution and relinearizing as needed.
*
* Optionally, this function remove existing factors from the system to enable
* behaviors such as swapping existing factors with new ones.
*
* Add new measurements, and optionally new variables, to the current system.
* This runs a full step of the ISAM2 algorithm, relinearizing and updating
* the solution as needed, according to the wildfire and relinearize
@ -439,6 +442,7 @@ public:
* You must include here all new variables occuring in newFactors (which were not already
* in the system). There must not be any variables here that do not occur in newFactors,
* and additionally, variables that were already in the system must not be included here.
* @param removeFactorIndices Indices of factors to remove from system
* @param force_relinearize Relinearize any variables whose delta magnitude is sufficiently
* large (Params::relinearizeThreshold), regardless of the relinearization interval
* (Params::relinearizeSkip).

85
tests/testGaussianISAM2.cpp
View File

@ -28,7 +28,10 @@ using boost::shared_ptr;
const double tol = 1e-4;
ISAM2 createSlamlikeISAM2() {
ISAM2 createSlamlikeISAM2(
boost::optional<Values&> init_values = boost::none,
boost::optional<planarSLAM::Graph&> full_graph = boost::none,
const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)) {
// Pose and landmark key types from planarSLAM
using planarSLAM::PoseKey;
@ -39,7 +42,8 @@ ISAM2 createSlamlikeISAM2() {
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
ISAM2 isam(params);
// ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
Values fullinit;
planarSLAM::Graph fullgraph;
@ -121,6 +125,12 @@ ISAM2 createSlamlikeISAM2() {
++ i;
}
if (full_graph)
*full_graph = fullgraph;
if (init_values)
*init_values = fullinit;
return isam;
}
@ -1071,6 +1081,7 @@ TEST(ISAM2, removeFactors)
// Remove the measurement on landmark 0
FastVector<size_t> toRemove;
EXPECT_LONGS_EQUAL(isam.getFactorsUnsafe().size()-2, result.newFactorsIndices[1]);
toRemove.push_back(result.newFactorsIndices[1]);
isam.update(planarSLAM::Graph(), Values(), toRemove);
}
@ -1107,6 +1118,76 @@ TEST(ISAM2, removeFactors)
EXPECT(assert_equal(expectedGradient, actualGradient));
}
/* ************************************************************************* */
TEST(ISAM2, swapFactors)
{
// SETDEBUG("ISAM2 update", true);
// SETDEBUG("ISAM2 update verbose", true);
// SETDEBUG("ISAM2 recalculate", true);
// This test builds a graph in the same way as the "slamlike" test above, but
// then swaps the 2nd-to-last landmark measurement with a different one
// Pose and landmark key types from planarSLAM
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
Values fullinit;
planarSLAM::Graph fullgraph;
ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph);
// Remove the measurement on landmark 0 and replace with a different one
{
size_t swap_idx = isam.getFactorsUnsafe().size()-2;
FastVector<size_t> toRemove;
toRemove.push_back(swap_idx);
fullgraph.remove(swap_idx);
planarSLAM::Graph swapfactors;
// swapfactors.addBearingRange(10, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); // original factor
swapfactors.addBearingRange(10, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise);
fullgraph.push_back(swapfactors);
isam.update(swapfactors, Values(), toRemove);
}
// Compare solutions
EXPECT(assert_equal(fullgraph, planarSLAM::Graph(isam.getFactorsUnsafe())));
EXPECT(isam_check(fullgraph, fullinit, isam));
// Check gradient at each node
typedef ISAM2::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraph<JacobianFactor> jfg;
jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(jfg, expectedGradient);
// Compare with actual gradients
int variablePosition = 0;
for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
const int dim = clique->conditional()->dim(jit);
Vector actual = clique->gradientContribution().segment(variablePosition, dim);
EXPECT(assert_equal(expectedGradient[*jit], actual));
variablePosition += dim;
}
EXPECT_LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
}
// Check gradient
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
VectorValues actualGradient(*allocateVectorValues(isam));
gradientAtZero(isam, actualGradient);
EXPECT(assert_equal(expectedGradient2, expectedGradient));
EXPECT(assert_equal(expectedGradient, actualGradient));
}
/* ************************************************************************* */
TEST(ISAM2, constrained_ordering)
{