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

Debug printout and variable names

release/4.3a0
Frank Dellaert 2009-11-13 06:13:58 +00:00
parent 976b56fd44
commit 2178589263
1 changed files with 12 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
cpp/GaussianFactor.cpp
View File

@ -313,8 +313,9 @@ GaussianFactor::eliminate(const string& key) const
size_t n = A.size2(); size_t n = A.size2();
// Do in-place QR to get R, d of the augmented system // Do in-place QR to get R, d of the augmented system
if (verbose) ::print(A,"A before"); if (verbose) ::print(A,"A");
if (verbose) ::print(b,"b before"); if (verbose) ::print(b,"b = ");
if (verbose) ::print(sigmas_,"sigmas = ");
std::list<boost::tuple<Vector, double, double> > solution = std::list<boost::tuple<Vector, double, double> > solution =
weighted_eliminate(A, b, sigmas_); weighted_eliminate(A, b, sigmas_);
@ -339,30 +340,31 @@ GaussianFactor::eliminate(const string& key) const
} }
// create base conditional Gaussian // create base conditional Gaussian
GaussianConditional::shared_ptr cg(new GaussianConditional(key, GaussianConditional::shared_ptr conditional(new GaussianConditional(key,
sub(d, 0, n1), // form d vector sub(d, 0, n1), // form d vector
sub(R, 0, n1, 0, n1), // form R matrix sub(R, 0, n1, 0, n1), // form R matrix
sub(newSigmas, 0, n1))); // get standard deviations sub(newSigmas, 0, n1))); // get standard deviations
// extract the block matrices for parents in both CG and LF // extract the block matrices for parents in both CG and LF
GaussianFactor::shared_ptr lf(new GaussianFactor); GaussianFactor::shared_ptr factor(new GaussianFactor);
size_t j = n1; size_t j = n1;
BOOST_FOREACH(string cur_key, ordering) BOOST_FOREACH(string cur_key, ordering)
if (cur_key!=key) { if (cur_key!=key) {
size_t dim = getDim(cur_key); size_t dim = getDim(cur_key);
cg->add(cur_key, sub(R, 0, n1, j, j+dim)); conditional->add(cur_key, sub(R, 0, n1, j, j+dim));
lf->insert(cur_key, sub(R, n1, maxRank, j, j+dim)); factor->insert(cur_key, sub(R, n1, maxRank, j, j+dim));
j+=dim; j+=dim;
} }
// Set sigmas // Set sigmas
lf->sigmas_ = sub(newSigmas,n1,maxRank); factor->sigmas_ = sub(newSigmas,n1,maxRank);
// extract ds vector for the new b // extract ds vector for the new b
lf->set_b(sub(d, n1, maxRank)); factor->set_b(sub(d, n1, maxRank));
if (verbose) lf->print("lf"); if (verbose) conditional->print("Conditional");
if (verbose) factor->print("Factor");
return make_pair(cg, lf); return make_pair(conditional, factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */