diff --git a/inference/EliminationTree-inl.h b/inference/EliminationTree-inl.h index 7e88845d0..c1723cf6d 100644 --- a/inference/EliminationTree-inl.h +++ b/inference/EliminationTree-inl.h @@ -24,26 +24,28 @@ template typename EliminationTree::EliminationResult EliminationTree::eliminate_() const { - BayesNet bayesNet; + // Create the Bayes net, which will be returned to the parent. Initially empty... + BayesNet bayesNet; - set, boost::fast_pool_allocator > separator; - - // Create the list of factors to be eliminated + // Create the list of factors to be eliminated, initially empty, and reserve space FactorGraph factors; factors.reserve(this->factors_.size() + this->subTrees_.size()); - // add all factors associated with root + // Add all factors associated with the current node factors.push_back(this->factors_.begin(), this->factors_.end()); - // for all children, eliminate into Bayes net and add the eliminated factors + // for all subtrees, eliminate into Bayes net and a separator factor, added to [factors] BOOST_FOREACH(const shared_ptr& child, subTrees_) { EliminationResult result = child->eliminate_(); - bayesNet.push_back(result.first); - factors.push_back(result.second); + bayesNet.push_back(result.first); // Bayes net fragment added to Bayes net + factors.push_back(result.second); // Separator factor added to [factors] } - // eliminate the joint factor and add the conditional to the bayes net + // Combine all factors (from this node and from subtrees) into a joint factor sharedFactor jointFactor(FACTOR::Combine(factors, VariableSlots(factors))); + + // Eliminate the resulting joint factor and add the conditional to the bayes net + // What remains in the jointFactor will be passed to our parent node bayesNet.push_back(jointFactor->eliminateFirst()); return EliminationResult(bayesNet, jointFactor); diff --git a/linear/GaussianFactor.cpp b/linear/GaussianFactor.cpp index b9aada8f9..27567aba6 100644 --- a/linear/GaussianFactor.cpp +++ b/linear/GaussianFactor.cpp @@ -595,27 +595,26 @@ template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph& factorGraph, - const GaussianVariableIndex& variableIndex, const vector& factors, + const GaussianVariableIndex& variableIndex, const vector& factorIndices, const vector& variables, const std::vector >& variablePositions) { - shared_ptr ret(boost::make_shared()); + // Debugging flags static const bool verbose = false; static const bool debug = false; if (verbose) std::cout << "GaussianFactor::GaussianFactor (factors)" << std::endl; - - assert(factors.size() == variablePositions.size()); + assert(factorIndices.size() == variablePositions.size()); // Determine row and column counts and check if any noise models are constrained tic("Combine: count sizes"); size_t m = 0; bool constrained = false; - BOOST_FOREACH(const size_t factor, factors) { - assert(factorGraph[factor] != NULL); - assert(!factorGraph[factor]->keys_.empty()); - m += factorGraph[factor]->numberOfRows(); - if(debug) cout << "Combining factor " << factor << endl; - if(debug) factorGraph[factor]->print(" :"); - if (!constrained && factorGraph[factor]->model_->isConstrained()) { + BOOST_FOREACH(const size_t i, factorIndices) { + assert(factorGraph[i] != NULL); + assert(!factorGraph[i]->keys_.empty()); + m += factorGraph[i]->numberOfRows(); + if(debug) cout << "Combining factor " << i << endl; + if(debug) factorGraph[i]->print(" :"); + if (!constrained && factorGraph[i]->model_->isConstrained()) { constrained = true; if (debug) std::cout << "Found a constraint!" << std::endl; } @@ -634,50 +633,49 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraphAb_.copyStructureFrom(ab_type(ret->matrix_, dims, dims+variables.size()+1, m)); - ublas::noalias(ret->matrix_) = ublas::zero_matrix(ret->matrix_.size1(), ret->matrix_.size2()); - ret->firstNonzeroBlocks_.resize(m); + shared_ptr combinedFactor(boost::make_shared()); + combinedFactor->Ab_.copyStructureFrom(ab_type(combinedFactor->matrix_, dims, dims+variables.size()+1, m)); + ublas::noalias(combinedFactor->matrix_) = ublas::zero_matrix(combinedFactor->matrix_.size1(), combinedFactor->matrix_.size2()); + combinedFactor->firstNonzeroBlocks_.resize(m); Vector sigmas(m); // Copy keys - ret->keys_.reserve(variables.size()); - ret->keys_.insert(ret->keys_.end(), variables.begin(), variables.end()); + combinedFactor->keys_.reserve(variables.size()); + combinedFactor->keys_.insert(combinedFactor->keys_.end(), variables.begin(), variables.end()); toc("Combine: set up empty"); // Compute a row permutation that maintains a staircase pattern in the new // combined factor. To do this, we merge-sort the rows so that the column - // indices of the first structural non-zero in each row increases - // monotonically. + // indices of the first structural non-zero in each row increase monotonically. tic("Combine: sort rows"); vector<_RowSource> rowSources; rowSources.reserve(m); - size_t factorI = 0; - BOOST_FOREACH(const size_t factorII, factors) { + size_t i1 = 0; + BOOST_FOREACH(const size_t factorII, factorIndices) { const GaussianFactor& factor(*factorGraph[factorII]); size_t factorRowI = 0; assert(factor.firstNonzeroBlocks_.size() == factor.numberOfRows()); BOOST_FOREACH(const size_t factorFirstNonzeroVarpos, factor.firstNonzeroBlocks_) { Index firstNonzeroVar; firstNonzeroVar = factor.keys_[factorFirstNonzeroVarpos]; - rowSources.push_back(_RowSource(firstNonzeroVar, factorI, factorRowI)); + rowSources.push_back(_RowSource(firstNonzeroVar, i1, factorRowI)); ++ factorRowI; } assert(factorRowI == factor.numberOfRows()); - ++ factorI; + ++ i1; } assert(rowSources.size() == m); - assert(factorI == factors.size()); + assert(i1 == factorIndices.size()); sort(rowSources.begin(), rowSources.end()); toc("Combine: sort rows"); // Fill in the rows of the new factor in sorted order. Fill in the array of - // the left-most nonzero for each row and the first structural zero in each - // column. + // the left-most nonzero for each row and the first structural zero in each column. // todo SL: smarter ignoring of zero factor variables (store first possible like above) - if(debug) gtsam::print(ret->matrix_, "matrix_ before copying rows: "); + if(debug) gtsam::print(combinedFactor->matrix_, "matrix_ before copying rows: "); tic("Combine: copy rows"); #ifndef NDEBUG @@ -688,29 +686,28 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraphgetb()(row) = factor.getb()(factorRow); + combinedFactor->getb()(row) = factor.getb()(factorRow); sigmas(row) = factor.get_sigmas()(factorRow); // Copy the row of A variable by variable, starting at the first nonzero // variable. std::vector::const_iterator keyit = factor.keys_.begin() + factorFirstNonzeroVarpos; std::vector::const_iterator varposIt = variablePositions[factorI].begin() + factorFirstNonzeroVarpos; - ret->firstNonzeroBlocks_[row] = *varposIt; + combinedFactor->firstNonzeroBlocks_[row] = *varposIt; if(debug) cout << " copying starting at varpos " << *varposIt << " (variable " << variables[*varposIt] << ")" << endl; std::vector::const_iterator keyitend = factor.keys_.end(); while(keyit != keyitend) { const size_t varpos = *varposIt; assert(variables[varpos] == *keyit); - ab_type::block_type retBlock(ret->Ab_(varpos)); + ab_type::block_type retBlock(combinedFactor->Ab_(varpos)); const ab_type::const_block_type factorBlock(factor.getA(keyit)); ublas::noalias(ublas::row(retBlock, row)) = ublas::row(factorBlock, factorRow); ++ keyit; @@ -719,8 +716,8 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraphfirstNonzeroBlocks_[row] >= lastRowFirstVarpos); - lastRowFirstVarpos = ret->firstNonzeroBlocks_[row]; + assert(combinedFactor->firstNonzeroBlocks_[row] >= lastRowFirstVarpos); + lastRowFirstVarpos = combinedFactor->firstNonzeroBlocks_[row]; #endif } toc("Combine: copy rows"); @@ -728,18 +725,18 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraphmodel_ = noiseModel::Constrained::MixedSigmas(sigmas); - if (verbose) ret->model_->print("Just created Constraint ^"); + combinedFactor->model_ = noiseModel::Constrained::MixedSigmas(sigmas); + if (verbose) combinedFactor->model_->print("Just created Constraint ^"); } else { - ret->model_ = noiseModel::Diagonal::Sigmas(sigmas); - if (verbose) ret->model_->print("Just created Diagonal"); + combinedFactor->model_ = noiseModel::Diagonal::Sigmas(sigmas); + if (verbose) combinedFactor->model_->print("Just created Diagonal"); } - if(debug) ret->print("Combined factor: "); + if(debug) combinedFactor->print("Combined factor: "); - ret->assertInvariants(); + combinedFactor->assertInvariants(); - return ret; + return combinedFactor; } /* ************************************************************************* */ diff --git a/linear/GaussianFactor.h b/linear/GaussianFactor.h index c97819432..185cbe3fb 100644 --- a/linear/GaussianFactor.h +++ b/linear/GaussianFactor.h @@ -147,16 +147,16 @@ public: */ void permuteWithInverse(const Permutation& inversePermutation); - /** Named constructor for combining a set of factors with pre-computed set of - * variables. */ + /** + * Named constructor for combining a set of factors with pre-computed set of variables. + */ template static shared_ptr Combine(const FactorGraph& factorGraph, - const GaussianVariableIndex& variableIndex, const std::vector& factors, + const GaussianVariableIndex& variableIndex, const std::vector& factorIndices, const std::vector& variables, const std::vector >& variablePositions); /** - * Named constructor for combining a set of factors with pre-computed set of - * variables. + * Named constructor for combining a set of factors with pre-computed set of variables. */ static shared_ptr Combine(const FactorGraph& factors, const VariableSlots& variableSlots);