Skip to content

Commit

Permalink
Merge pull request #1599 from borglab/fix/iSAM2
Browse files Browse the repository at this point in the history 
Fix/iSAM2 #1101
  • Loading branch information
@dellaert
dellaert authored Aug 10, 2023
2 parents e36590a + 44d948e commit 8214bdc
Show file tree
Hide file tree
Showing 2 changed files with 223 additions and 25 deletions.
66 changes: 41 additions & 25 deletions gtsam/nonlinear/ISAM2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -556,9 +556,12 @@ void ISAM2::marginalizeLeaves(
// We do not need the marginal factors associated with this clique
// because their information is already incorporated in the new
// marginal factor. So, now associate this marginal factor with the
// parent of this clique.
marginalFactors[clique->parent()->conditional()->front()].push_back(
marginalFactor);
// parent of this clique. If the clique is a root and has no parent, then
// we can discard it without keeping track of the marginal factor.
if (clique->parent()) {
marginalFactors[clique->parent()->conditional()->front()].push_back(
marginalFactor);
}
// Now remove this clique and its subtree - all of its marginal
// information has been stored in marginalFactors.
trackingRemoveSubtree(clique);
Expand Down Expand Up @@ -636,7 +639,7 @@ void ISAM2::marginalizeLeaves(

// Make the clique's matrix appear as a subset
const DenseIndex dimToRemove = cg->matrixObject().offset(nToRemove);
cg->matrixObject().firstBlock() = nToRemove;
cg->matrixObject().firstBlock() += nToRemove;
cg->matrixObject().rowStart() = dimToRemove;

// Change the keys in the clique
Expand All @@ -662,42 +665,55 @@ void ISAM2::marginalizeLeaves(
// At this point we have updated the BayesTree, now update the remaining iSAM2
// data structures

// Remove the factors to remove that will be summarized in marginal factors
NonlinearFactorGraph removedFactors;
for (const auto index : factorIndicesToRemove) {
removedFactors.push_back(nonlinearFactors_[index]);
nonlinearFactors_.remove(index);
if (params_.cacheLinearizedFactors) {
linearFactors_.remove(index);
}
}
variableIndex_.remove(factorIndicesToRemove.begin(),
factorIndicesToRemove.end(), removedFactors);

// Gather factors to add - the new marginal factors
GaussianFactorGraph factorsToAdd;
GaussianFactorGraph factorsToAdd{};
NonlinearFactorGraph nonlinearFactorsToAdd{};
for (const auto& key_factors : marginalFactors) {
for (const auto& factor : key_factors.second) {
if (factor) {
factorsToAdd.push_back(factor);
if (marginalFactorsIndices)
marginalFactorsIndices->push_back(nonlinearFactors_.size());
nonlinearFactors_.push_back(
std::make_shared<LinearContainerFactor>(factor));
if (params_.cacheLinearizedFactors) linearFactors_.push_back(factor);
nonlinearFactorsToAdd.emplace_shared<LinearContainerFactor>(factor);
for (Key factorKey : *factor) {
fixedVariables_.insert(factorKey);
}
}
}
}
variableIndex_.augment(factorsToAdd); // Augment the variable index

// Remove the factors to remove that have been summarized in the newly-added
// marginal factors
NonlinearFactorGraph removedFactors;
for (const auto index : factorIndicesToRemove) {
removedFactors.push_back(nonlinearFactors_[index]);
nonlinearFactors_.remove(index);
if (params_.cacheLinearizedFactors) linearFactors_.remove(index);
// Add the nonlinear factors and keep track of the new factor indices
auto newFactorIndices = nonlinearFactors_.add_factors(nonlinearFactorsToAdd,
params_.findUnusedFactorSlots);
// Add cached linear factors.
if (params_.cacheLinearizedFactors){
linearFactors_.resize(nonlinearFactors_.size());
for (std::size_t i = 0; i < nonlinearFactorsToAdd.size(); ++i){
linearFactors_[newFactorIndices[i]] = factorsToAdd[i];
}
}
variableIndex_.remove(factorIndicesToRemove.begin(),
factorIndicesToRemove.end(), removedFactors);

if (deletedFactorsIndices)
deletedFactorsIndices->assign(factorIndicesToRemove.begin(),
factorIndicesToRemove.end());
// Augment the variable index
variableIndex_.augment(factorsToAdd, newFactorIndices);

// Remove the marginalized variables
removeVariables(KeySet(leafKeys.begin(), leafKeys.end()));

if (deletedFactorsIndices) {
deletedFactorsIndices->assign(factorIndicesToRemove.begin(),
factorIndicesToRemove.end());
}
if (marginalFactorsIndices){
*marginalFactorsIndices = std::move(newFactorIndices);
}
}

/* ************************************************************************* */
Expand Down
182 changes: 182 additions & 0 deletions tests/testGaussianISAM2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Marginals.h>
Expand Down Expand Up @@ -657,6 +658,77 @@ namespace {
bool ok = treeEqual && /*linEqual &&*/ nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect;
return ok;
}

std::optional<FastMap<Key, int>> createOrderingConstraints(const ISAM2& isam, const KeyVector& newKeys, const KeySet& marginalizableKeys)
{
if (marginalizableKeys.empty()) {
return {};
} else {
FastMap<Key, int> constrainedKeys = FastMap<Key, int>();
// Generate ordering constraints so that the marginalizable variables will be eliminated first
// Set all existing and new variables to Group1
for (const auto& key_val : isam.getDelta()) {
constrainedKeys.emplace(key_val.first, 1);
}
for (const auto& key : newKeys) {
constrainedKeys.emplace(key, 1);
}
// And then re-assign the marginalizable variables to Group0 so that they'll all be leaf nodes
for (const auto& key : marginalizableKeys) {
constrainedKeys.at(key) = 0;
}
return constrainedKeys;
}
}

void markAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& rootClique, KeyList& additionalKeys)
{
std::stack<ISAM2Clique::shared_ptr> frontier;
frontier.push(rootClique);
// Basic DFS to find additional keys
while (!frontier.empty()) {
// Get the top of the stack
const ISAM2Clique::shared_ptr clique = frontier.top();
frontier.pop();
// Check if we have more keys and children to add
if (std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) !=
clique->conditional()->endParents()) {
for (Key i : clique->conditional()->frontals()) {
additionalKeys.push_back(i);
}
for (const ISAM2Clique::shared_ptr& child : clique->children) {
frontier.push(child);
}
}
}
}

bool updateAndMarginalize(const NonlinearFactorGraph& newFactors, const Values& newValues, const KeySet& marginalizableKeys, ISAM2& isam)
{
// Force ISAM2 to put marginalizable variables at the beginning
auto orderingConstraints = createOrderingConstraints(isam, newValues.keys(), marginalizableKeys);

// Mark additional keys between the marginalized keys and the leaves
KeyList markedKeys;
for (Key key : marginalizableKeys) {
markedKeys.push_back(key);
ISAM2Clique::shared_ptr clique = isam[key];
for (const ISAM2Clique::shared_ptr& child : clique->children) {
markAffectedKeys(key, child, markedKeys);
}
}

// Update
isam.update(newFactors, newValues, FactorIndices{}, orderingConstraints, {}, markedKeys);

if (!marginalizableKeys.empty()) {
FastList<Key> leafKeys(marginalizableKeys.begin(), marginalizableKeys.end());
return checkMarginalizeLeaves(isam, leafKeys);
}
else {
return true;
}
}
}

/* ************************************************************************* */
Expand Down Expand Up @@ -790,6 +862,116 @@ TEST(ISAM2, marginalizeLeaves5)
EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys));
}

/* ************************************************************************* */
TEST(ISAM2, marginalizeLeaves6)
{
auto nm = noiseModel::Isotropic::Sigma(6, 1.0);

int gridDim = 10;

auto idxToKey = [gridDim](int i, int j){return i * gridDim + j;};

NonlinearFactorGraph factors;
Values values;
ISAM2 isam;

// Create a grid of pose variables
for (int i = 0; i < gridDim; ++i) {
for (int j = 0; j < gridDim; ++j) {
Pose3 pose = Pose3(Rot3::Identity(), Point3(i, j, 0));
Key key = idxToKey(i, j);
// Place a prior on the first pose
factors.addPrior(key, Pose3(Rot3::Identity(), Point3(i, j, 0)), nm);
values.insert(key, pose);
if (i > 0) {
factors.emplace_shared<BetweenFactor<Pose3>>(idxToKey(i - 1, j), key, Pose3(Rot3::Identity(), Point3(1, 0, 0)),nm);
}
if (j > 0) {
factors.emplace_shared<BetweenFactor<Pose3>>(idxToKey(i, j - 1), key, Pose3(Rot3::Identity(), Point3(0, 1, 0)),nm);
}
}
}

// Optimize the graph
EXPECT(updateAndMarginalize(factors, values, {}, isam));
auto estimate = isam.calculateBestEstimate();

// Get the list of keys
std::vector<Key> key_list(gridDim * gridDim);
std::iota(key_list.begin(), key_list.end(), 0);

// Shuffle the keys -> we will marginalize the keys one by one in this order
std::shuffle(key_list.begin(), key_list.end(), std::default_random_engine(1234));

for (const auto& key : key_list) {
KeySet marginalKeys;
marginalKeys.insert(key);
EXPECT(updateAndMarginalize({}, {}, marginalKeys, isam));
estimate = isam.calculateBestEstimate();
}
}

/* ************************************************************************* */
TEST(ISAM2, MarginalizeRoot)
{
auto nm = noiseModel::Isotropic::Sigma(6, 1.0);

NonlinearFactorGraph factors;
Values values;
ISAM2 isam;

// Create a factor graph with one variable and a prior
Pose3 root = Pose3::Identity();
Key rootKey(0);
values.insert(rootKey, root);
factors.addPrior(rootKey, Pose3::Identity(), nm);

// Optimize the graph
EXPECT(updateAndMarginalize(factors, values, {}, isam));
auto estimate = isam.calculateBestEstimate();
EXPECT(estimate.size() == 1);

// And remove the node from the graph
KeySet marginalizableKeys;
marginalizableKeys.insert(rootKey);

EXPECT(updateAndMarginalize({}, {}, marginalizableKeys, isam));

estimate = isam.calculateBestEstimate();
EXPECT(estimate.empty());
}

/* ************************************************************************* */
TEST(ISAM2, marginalizationSize)
{
auto nm = noiseModel::Isotropic::Sigma(6, 1.0);

NonlinearFactorGraph factors;
Values values;
ISAM2Params params;
params.findUnusedFactorSlots = true;
ISAM2 isam{params};

// Create a pose variable
Key aKey(0);
values.insert(aKey, Pose3::Identity());
factors.addPrior(aKey, Pose3::Identity(), nm);
// Create another pose variable linked to the first
Pose3 b = Pose3::Identity();
Key bKey(1);
values.insert(bKey, Pose3::Identity());
factors.emplace_shared<BetweenFactor<Pose3>>(aKey, bKey, Pose3::Identity(), nm);
// Optimize the graph
EXPECT(updateAndMarginalize(factors, values, {}, isam));

// Now remove a variable -> we should not see the number of factors increase
gtsam::KeySet to_remove;
to_remove.insert(aKey);
const auto numFactorsBefore = isam.getFactorsUnsafe().size();
updateAndMarginalize({}, {}, to_remove, isam);
EXPECT(numFactorsBefore == isam.getFactorsUnsafe().size());
}

/* ************************************************************************* */
TEST(ISAM2, marginalCovariance)
{
Expand Down

0 comments on commit 8214bdc

Please sign in to comment.