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

(in branch) slight simplification of HessianFactor eliminate code - fill in keys when the combined factor is first constructed so it is a complete factor

release/4.3a0
Richard Roberts 2011-12-13 18:45:21 +00:00
parent ef2021f6da
commit e75e4321af
3 changed files with 12 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -367,10 +367,8 @@ namespace gtsam {
// Pull out keys and dimensions // Pull out keys and dimensions
tic(2, "keys"); tic(2, "keys");
vector<Index> keys(scatter.size());
vector<size_t> dimensions(scatter.size() + 1); vector<size_t> dimensions(scatter.size() + 1);
BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
keys[var_slot.second.slot] = var_slot.first;
dimensions[var_slot.second.slot] = var_slot.second.dimension; dimensions[var_slot.second.slot] = var_slot.second.dimension;
} }
// This is for the r.h.s. vector // This is for the r.h.s. vector
@ -401,7 +399,7 @@ namespace gtsam {
// Extract conditionals and fill in details of the remaining factor // Extract conditionals and fill in details of the remaining factor
tic(5, "split"); tic(5, "split");
GaussianConditional::shared_ptr conditionals = GaussianConditional::shared_ptr conditionals =
combinedFactor->splitEliminatedFactor(nrFrontals, keys); combinedFactor->splitEliminatedFactor(nrFrontals);
if (debug) { if (debug) {
conditionals->print("Extracted conditionals: "); conditionals->print("Extracted conditionals: ");
combinedFactor->print("Eliminated factor (L piece): "); combinedFactor->print("Eliminated factor (L piece): ");
@ -572,10 +570,8 @@ namespace gtsam {
// Pull out keys and dimensions // Pull out keys and dimensions
tic(2, "keys"); tic(2, "keys");
vector<Index> keys(scatter.size());
vector<size_t> dimensions(scatter.size() + 1); vector<size_t> dimensions(scatter.size() + 1);
BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
keys[var_slot.second.slot] = var_slot.first;
dimensions[var_slot.second.slot] = var_slot.second.dimension; dimensions[var_slot.second.slot] = var_slot.second.dimension;
} }
// This is for the r.h.s. vector // This is for the r.h.s. vector
@ -609,7 +605,7 @@ namespace gtsam {
// Extract conditionals and fill in details of the remaining factor // Extract conditionals and fill in details of the remaining factor
tic(5, "split"); tic(5, "split");
GaussianConditional::shared_ptr conditionals = GaussianConditional::shared_ptr conditionals =
combinedFactor->splitEliminatedFactor(nrFrontals, keys, permutation); combinedFactor->splitEliminatedFactor(nrFrontals, permutation);
if (debug) { if (debug) {
conditionals->print("Extracted conditionals: "); conditionals->print("Extracted conditionals: ");
combinedFactor->print("Eliminated factor (L piece): "); combinedFactor->print("Eliminated factor (L piece): ");

12
gtsam/linear/HessianFactor.cpp
View File

@ -23,6 +23,7 @@
#include <boost/lambda/bind.hpp> #include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp> #include <boost/lambda/lambda.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/bind.hpp>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
@ -193,6 +194,9 @@ HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors,
// Form Ab' * Ab // Form Ab' * Ab
tic(1, "allocate"); tic(1, "allocate");
info_.resize(dimensions.begin(), dimensions.end(), false); info_.resize(dimensions.begin(), dimensions.end(), false);
// Fill in keys
keys_.resize(scatter.size());
std::transform(scatter.begin(), scatter.end(), keys_.begin(), boost::bind(&Scatter::value_type::first, ::_1));
toc(1, "allocate"); toc(1, "allocate");
tic(2, "zero"); tic(2, "zero");
matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols()); matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols());
@ -406,7 +410,7 @@ Eigen::LDLT<Matrix>::TranspositionType HessianFactor::partialLDL(size_t nrFronta
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditional::shared_ptr GaussianConditional::shared_ptr
HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector<Index>& keys, const Eigen::LDLT<Matrix>::TranspositionType& permutation) { HessianFactor::splitEliminatedFactor(size_t nrFrontals, const Eigen::LDLT<Matrix>::TranspositionType& permutation) {
static const bool debug = false; static const bool debug = false;
@ -423,7 +427,7 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector<Index>& key
// Because of the pivoting permutation when using LDL, treating each variable separately doesn't make sense. // Because of the pivoting permutation when using LDL, treating each variable separately doesn't make sense.
tic(2, "construct cond"); tic(2, "construct cond");
Vector sigmas = Vector::Ones(varDim); Vector sigmas = Vector::Ones(varDim);
conditionals = boost::make_shared<ConditionalType>(keys.begin(), keys.end(), nrFrontals, Ab, sigmas, permutation); conditionals = boost::make_shared<ConditionalType>(keys_.begin(), keys_.end(), nrFrontals, Ab, sigmas, permutation);
toc(2, "construct cond"); toc(2, "construct cond");
if(debug) conditionals->print("Extracted conditional: "); if(debug) conditionals->print("Extracted conditional: ");
@ -433,7 +437,9 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector<Index>& key
tic(2, "remaining factor"); tic(2, "remaining factor");
info_.blockStart() = nrFrontals; info_.blockStart() = nrFrontals;
// Assign the keys // Assign the keys
keys_.assign(keys.begin() + nrFrontals, keys.end()); vector<Index> remainingKeys(keys_.size() - nrFrontals);
remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end());
keys_.swap(remainingKeys);
toc(2, "remaining factor"); toc(2, "remaining factor");
return conditionals; return conditionals;

2
gtsam/linear/HessianFactor.h
View File

@ -276,7 +276,7 @@ namespace gtsam {
/** split partially eliminated factor */ /** split partially eliminated factor */
boost::shared_ptr<GaussianConditional> splitEliminatedFactor( boost::shared_ptr<GaussianConditional> splitEliminatedFactor(
size_t nrFrontals, const std::vector<Index>& keys, const Eigen::LDLT<Matrix>::TranspositionType& permutation = Eigen::LDLT<Matrix>::TranspositionType()); size_t nrFrontals, const Eigen::LDLT<Matrix>::TranspositionType& permutation = Eigen::LDLT<Matrix>::TranspositionType());
/** assert invariants */ /** assert invariants */
void assertInvariants() const; void assertInvariants() const;