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

Re-added ISAM2::marginalizeLeaves optional output arguments from Steves changes in r18185

release/4.3a0
Richard Roberts 2013-08-14 15:21:10 +00:00
parent 4dfa2506ab
commit 46beb6c6a4
2 changed files with 12 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/nonlinear/ISAM2.cpp
View File

@ -754,7 +754,9 @@ ISAM2Result ISAM2::update(
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList) void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
boost::optional<std::vector<size_t>&> marginalFactorsIndices,
boost::optional<std::vector<size_t>&> deletedFactorsIndices)
{ {
// Convert to ordered set // Convert to ordered set
FastSet<Key> leafKeys(leafKeysList.begin(), leafKeysList.end()); FastSet<Key> leafKeys(leafKeysList.begin(), leafKeysList.end());
@ -895,6 +897,8 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList)
BOOST_FOREACH(const Clique_Factor& clique_factor, marginalFactors) { BOOST_FOREACH(const Clique_Factor& clique_factor, marginalFactors) {
if(clique_factor.second) if(clique_factor.second)
factorsToAdd.push_back(clique_factor.second); factorsToAdd.push_back(clique_factor.second);
if(marginalFactorsIndices)
marginalFactorsIndices->push_back(nonlinearFactors_.size());
nonlinearFactors_.push_back(boost::make_shared<LinearContainerFactor>( nonlinearFactors_.push_back(boost::make_shared<LinearContainerFactor>(
clique_factor.second)); clique_factor.second));
if(params_.cacheLinearizedFactors) if(params_.cacheLinearizedFactors)
@ -908,16 +912,17 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList)
FastSet<size_t> factorIndicesToRemove; FastSet<size_t> factorIndicesToRemove;
BOOST_FOREACH(Key j, leafKeys) { BOOST_FOREACH(Key j, leafKeys) {
factorIndicesToRemove.insert(variableIndex_[j].begin(), variableIndex_[j].end()); } factorIndicesToRemove.insert(variableIndex_[j].begin(), variableIndex_[j].end()); }
vector<size_t> removedFactorIndices;
NonlinearFactorGraph removedFactors; NonlinearFactorGraph removedFactors;
BOOST_FOREACH(size_t i, factorIndicesToRemove) { BOOST_FOREACH(size_t i, factorIndicesToRemove) {
removedFactorIndices.push_back(i);
removedFactors.push_back(nonlinearFactors_[i]); removedFactors.push_back(nonlinearFactors_[i]);
nonlinearFactors_.remove(i); nonlinearFactors_.remove(i);
if(params_.cacheLinearizedFactors) if(params_.cacheLinearizedFactors)
linearFactors_.remove(i); linearFactors_.remove(i);
} }
variableIndex_.remove(removedFactorIndices.begin(), removedFactorIndices.end(), removedFactors); variableIndex_.remove(factorIndicesToRemove.begin(), factorIndicesToRemove.end(), removedFactors);
if(deletedFactorsIndices)
deletedFactorsIndices->assign(factorIndicesToRemove.begin(), factorIndicesToRemove.end());
// Remove the marginalized variables // Remove the marginalized variables
Impl::RemoveVariables(FastSet<Key>(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, Impl::RemoveVariables(FastSet<Key>(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,

4
gtsam/nonlinear/ISAM2.h
View File

@ -524,7 +524,9 @@ public:
* If provided, 'deletedFactorsIndices' will be augmented with the factor graph * If provided, 'deletedFactorsIndices' will be augmented with the factor graph
* indices of any factor that was removed during the 'marginalizeLeaves' call * indices of any factor that was removed during the 'marginalizeLeaves' call
*/ */
void marginalizeLeaves(const FastList<Key>& leafKeys); void marginalizeLeaves(const FastList<Key>& leafKeys,
boost::optional<std::vector<size_t>&> marginalFactorsIndices = boost::none,
boost::optional<std::vector<size_t>&> deletedFactorsIndices = boost::none);
/** Access the current linearization point */ /** Access the current linearization point */
const Values& getLinearizationPoint() const { return theta_; } const Values& getLinearizationPoint() const { return theta_; }