diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index f47edf5ea..551fdec0c 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -116,6 +116,9 @@ namespace gtsam { /// @name Advanced Interface /// @{ + /** Mutable version of nrFrontals */ + size_t& nrFrontals() { return nrFrontals_; } + /** Mutable iterator pointing to first frontal key. */ typename FACTOR::iterator beginFrontals() { return asFactor().begin(); } diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index a6cb2d82e..cc1f641e0 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -104,12 +104,12 @@ namespace gtsam { myData.myJTNode->keys.insert(myData.myJTNode->keys.begin(), childToMerge.keys.begin(), childToMerge.keys.end()); myData.myJTNode->factors.insert(myData.myJTNode->factors.end(), childToMerge.factors.begin(), childToMerge.factors.end()); myData.myJTNode->children.insert(myData.myJTNode->children.end(), childToMerge.children.begin(), childToMerge.children.end()); + // Increment problem size + combinedProblemSize = std::max(combinedProblemSize, childToMerge.problemSize_); // Remove child from list. myData.myJTNode->children.erase(myData.myJTNode->children.begin() + (child - nrMergedChildren)); // Increment number of merged children ++ nrMergedChildren; - // Increment problem size - combinedProblemSize = std::max(combinedProblemSize, childToMerge.problemSize_); } } myData.myJTNode->problemSize_ = combinedProblemSize; diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h index 1ce108321..a82eb7c0b 100644 --- a/gtsam/inference/VariableIndex-inl.h +++ b/gtsam/inference/VariableIndex-inl.h @@ -75,8 +75,10 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& template void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) { for(ITERATOR key = firstKey; key != lastKey; ++key) { - assert(internalAt(*key).empty()); - index_.erase(*key); + KeyMap::iterator entry = index_.find(*key); + if(!entry->second.empty()) + throw std::invalid_argument("Asking to remove variables from the variable index that are not unused"); + index_.erase(entry); } } diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 8ca12e1f2..50d03a7ad 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -134,6 +134,8 @@ namespace gtsam { model_ = noiseModel::Unit::Create(maxrank); } +#undef GTSAM_EXTRA_CONSISTENCY_CHECKS + /* ************************************************************************* */ // Helper functions for combine constructor namespace { @@ -144,43 +146,62 @@ namespace gtsam { #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS FastVector varDims(variableSlots.size(), numeric_limits::max()); #else - FastVector varDims(variableSlots.size()); + FastVector varDims(variableSlots.size(), numeric_limits::max()); #endif DenseIndex m = 0; DenseIndex n = 0; + for(size_t jointVarpos = 0; jointVarpos < variableSlots.size(); ++jointVarpos) { - size_t jointVarpos = 0; - BOOST_FOREACH(VariableSlots::const_iterator slots, variableSlots) - { - assert(slots->second.size() == factors.size()); + const VariableSlots::const_iterator& slots = variableSlots[jointVarpos]; + + assert(slots->second.size() == factors.size()); + + bool foundVariable = false; + for(size_t sourceFactorI = 0; sourceFactorI < slots->second.size(); ++sourceFactorI) + { + const size_t sourceVarpos = slots->second[sourceFactorI]; + if(sourceVarpos < numeric_limits::max()) { + const JacobianFactor& sourceFactor = *factors[sourceFactorI]; + if(sourceFactor.cols() > 0) { + foundVariable = true; + DenseIndex vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos); - size_t sourceFactorI = 0; - BOOST_FOREACH(const size_t sourceVarpos, slots->second) { - if(sourceVarpos < numeric_limits::max()) { - const JacobianFactor& sourceFactor = *factors[sourceFactorI]; - if(sourceFactor.rows() > 0) { - DenseIndex vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos); #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - if(varDims[jointVarpos] == numeric_limits::max()) { - varDims[jointVarpos] = vardim; - n += vardim; - } else - assert(varDims[jointVarpos] == vardim); -#else + if(varDims[jointVarpos] == numeric_limits::max()) { varDims[jointVarpos] = vardim; n += vardim; - break; -#endif + } else { + if(!(varDims[jointVarpos] == vardim)) { + cout << "Factor " << sourceFactorI << " variable " << DefaultKeyFormatter(sourceFactor.keys()[sourceVarpos]) << + " has different dimensionality of " << vardim << " instead of " << varDims[jointVarpos] << endl; + exit(1); + } } +#else + + varDims[jointVarpos] = vardim; + n += vardim; + break; +#endif } - ++ sourceFactorI; } - ++ jointVarpos; - } - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) { - m += factor->rows(); } + + if(!foundVariable) + GaussianFactorGraph(factors).print("factors: "); + assert(foundVariable); } + + BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) { + m += factor->rows(); + } + +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS + BOOST_FOREACH(DenseIndex d, varDims) { + assert(d != numeric_limits::max()); + } +#endif + return boost::make_tuple(varDims, m, n); } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 640b3db6f..79449204e 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -204,6 +204,9 @@ namespace gtsam { /** Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ const VerticalBlockMatrix& matrixObject() const { return Ab_; } + /** Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ + VerticalBlockMatrix& matrixObject() { return Ab_; } + /** * Construct the corresponding anti-factor to negate information * stored stored in this factor. diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 0895963f1..7526b4772 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -765,10 +765,28 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // multimap marginalFactors; map > marginalFactors; + // Keep track of factors that get summarized by removing cliques + FastSet factorIndicesToRemove; + + // Keep track of variables removed in subtrees + FastSet leafKeysRemoved; + // Remove each variable and its subtrees - BOOST_REVERSE_FOREACH(Key j, leafKeys) { - if(nodes_.count(j)) { // If the index was not already removed by removing another subtree + BOOST_FOREACH(Key j, leafKeys) { + if(!leafKeysRemoved.exists(j)) { // If the index was not already removed by removing another subtree + + // Traverse up the tree to find the root of the marginalized subtree sharedClique clique = nodes_[j]; + while(!clique->parent_._empty()) + { + // Check if parent contains a marginalized leaf variable. Only need to check the first + // variable because it is the closest to the leaves. + sharedClique parent = clique->parent(); + if(leafKeys.exists(parent->conditional()->front())) + clique = parent; + else + break; + } // See if we should remove the whole clique bool marginalizeEntireClique = true; @@ -791,9 +809,17 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques BOOST_FOREACH(const sharedClique& removedClique, removedCliques) { marginalFactors.erase(removedClique->conditional()->front()); - BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals()) { + leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); + BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals()) + { + // Add to factors to remove + const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; + factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); + + // Check for non-leaf keys if(!leafKeys.exists(frontal)) - throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); } + throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); + } } } else { @@ -823,68 +849,74 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), removedCliques.end()); BOOST_FOREACH(const sharedClique& removedClique, removedCliques) { marginalFactors.erase(removedClique->conditional()->front()); - BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals()) { + leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); + BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals()) + { + // Add to factors to remove + const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; + factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); + + // Check for non-leaf keys if(!leafKeys.exists(frontal)) - throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); } + throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); + } } } - // Gather remaining children after we removed marginalized subtrees - FastVector orphans(clique->children.begin(), clique->children.end()); - // Add the factors that are pulled into the current clique by the marginalized variables. // These are the factors that involve *marginalized* frontal variables in this clique // but do not involve frontal variables of any of its children. - FastSet factorsFromMarginalizedInClique; + // TODO: reuse cached linear factors + FastSet factorsFromMarginalizedInClique_step1; BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { if(leafKeys.exists(frontal)) - factorsFromMarginalizedInClique.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); } + factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); } + // Remove any factors in subtrees that we're removing at this step BOOST_FOREACH(const sharedClique& removedChild, childrenRemoved) { BOOST_FOREACH(Index indexInClique, removedChild->conditional()->frontals()) { BOOST_FOREACH(size_t factorInvolving, variableIndex_[indexInClique]) { - factorsFromMarginalizedInClique.erase(factorInvolving); } } } - BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique) { + factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } } + // Create factor graph from factor indices + BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique_step1) { graph.push_back(nonlinearFactors_[i]->linearize(theta_)); } - // Remove the current clique - sharedClique parent = clique->parent(); - this->removeClique(clique); - // Reeliminate the linear graph to get the marginal and discard the conditional const FastSet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); - FastSet cliqueFrontalsToEliminate; + FastVector cliqueFrontalsToEliminate; std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(), - std::inserter(cliqueFrontalsToEliminate, cliqueFrontalsToEliminate.end())); - FastVector cliqueFrontalsToEliminateV(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end()); + std::back_inserter(cliqueFrontalsToEliminate)); pair eliminationResult1 = - params_.getEliminationFunction()(graph, Ordering(cliqueFrontalsToEliminateV)); + params_.getEliminationFunction()(graph, Ordering(cliqueFrontalsToEliminate)); // Add the resulting marginal if(eliminationResult1.second) marginalFactors[clique->conditional()->front()].push_back(eliminationResult1.second); - // Recover the conditional on the remaining subset of frontal variables - // of this clique being partially marginalized. - GaussianConditional::iterator jPosition = std::find( - clique->conditional()->beginFrontals(), clique->conditional()->endFrontals(), j); - GaussianFactorGraph graph2; - graph2.push_back(clique->conditional()); - GaussianFactorGraph::EliminationResult eliminationResult2 = - params_.getEliminationFunction()(graph2, Ordering( - clique->conditional()->beginFrontals(), jPosition + 1)); - GaussianFactorGraph graph3; - graph3.push_back(eliminationResult2.second); - GaussianFactorGraph::EliminationResult eliminationResult3 = - params_.getEliminationFunction()(graph3, Ordering(jPosition + 1, clique->conditional()->endFrontals())); - sharedClique newClique = boost::make_shared(); - newClique->setEliminationResult(make_pair(eliminationResult3.first, clique->cachedFactor())); + // Split the current clique + // Find the position of the last leaf key in this clique + DenseIndex nToRemove = 0; + while(leafKeys.exists(clique->conditional()->keys()[nToRemove])) + ++ nToRemove; - // Add the marginalized clique to the BayesTree - this->addClique(newClique, parent); + // Make the clique's matrix appear as a subset + const DenseIndex dimToRemove = clique->conditional()->matrixObject().offset(nToRemove); + clique->conditional()->matrixObject().firstBlock() = nToRemove; + clique->conditional()->matrixObject().rowStart() = dimToRemove; - // Add the orphans - BOOST_FOREACH(const sharedClique& orphan, orphans) { - this->addClique(orphan, newClique); } + // Change the keys in the clique + FastVector originalKeys; originalKeys.swap(clique->conditional()->keys()); + clique->conditional()->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end()); + clique->conditional()->nrFrontals() -= nToRemove; + + // Add to factors to remove factors involved in frontals of current clique + BOOST_FOREACH(Key frontal, cliqueFrontalsToEliminate) + { + const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; + factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); + } + + // Add removed keys + leafKeysRemoved.insert(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end()); } } } @@ -912,9 +944,6 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, variableIndex_.augment(factorsToAdd); // Augment the variable index // Remove the factors to remove that have been summarized in the newly-added marginal factors - FastSet factorIndicesToRemove; - BOOST_FOREACH(Key j, leafKeys) { - factorIndicesToRemove.insert(variableIndex_[j].begin(), variableIndex_[j].end()); } NonlinearFactorGraph removedFactors; BOOST_FOREACH(size_t i, factorIndicesToRemove) { removedFactors.push_back(nonlinearFactors_[i]);