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

Bug fix and unit test HessianFactor::CombineAndEliminate when keys are not sorted

release/4.3a0
Richard Roberts 2011-01-26 22:53:14 +00:00
parent 38324ce080
commit d87ffe7ece
4 changed files with 133 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam/inference/BayesTree-inl.h
View File

@ -43,13 +43,29 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
BayesTree<CONDITIONAL>::Clique::Clique() {}
void BayesTree<CONDITIONAL>::Clique::assertInvariants() const {
#ifndef NDEBUG
// We rely on the keys being sorted
typename BayesNet<CONDITIONAL>::const_iterator cond = this->begin();
if(cond != this->end()) {
Index lastKey = (*cond)->key();
++cond;
for(; cond != this->end(); ++cond)
assert(lastKey < (*cond)->key());
}
#endif
}
/* ************************************************************************* */
template<class CONDITIONAL>
BayesTree<CONDITIONAL>::Clique::Clique() { assertInvariants(); }
/* ************************************************************************* */
template<class CONDITIONAL>
BayesTree<CONDITIONAL>::Clique::Clique(const sharedConditional& conditional) {
separator_.assign(conditional->parents().begin(), conditional->parents().end());
this->push_back(conditional);
assertInvariants();
}
/* ************************************************************************* */
@ -84,6 +100,7 @@ namespace gtsam {
// separator_.assign((*bayesNet.rbegin())->parents().begin(), (*bayesNet.rbegin())->parents().end());
separator_.assign((*bayesNet.rbegin())->beginParents(), (*bayesNet.rbegin())->endParents());
}
assertInvariants();
}
/* ************************************************************************* */
@ -137,6 +154,7 @@ namespace gtsam {
BOOST_FOREACH(const shared_ptr& child, children_) {
child->permuteWithInverse(inversePermutation);
}
assertInvariants();
}
/* ************************************************************************* */
@ -158,6 +176,7 @@ namespace gtsam {
(void)child->permuteSeparatorWithInverse(inversePermutation);
}
}
assertInvariants();
return changed;
}
@ -350,6 +369,7 @@ namespace gtsam {
p_S_R.permuteWithInverse(toBack);
// return the parent shortcut P(Sp|R)
assertInvariants();
return p_S_R;
}
@ -372,6 +392,7 @@ namespace gtsam {
// Find marginal on the keys we are interested in
FactorGraph<typename CONDITIONAL::Factor> p_FSRfg(p_FSR);
assertInvariants();
return *GenericSequentialSolver<typename CONDITIONAL::Factor>(p_FSR).jointFactorGraph(keys());
}
@ -400,6 +421,7 @@ namespace gtsam {
// Calculate the marginal
vector<Index> keys12vector; keys12vector.reserve(keys12.size());
keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end());
assertInvariants();
return *GenericSequentialSolver<typename CONDITIONAL::Factor>(joint).jointFactorGraph(keys12vector);
}
@ -429,6 +451,7 @@ namespace gtsam {
new_clique->parent_ = parent_clique;
parent_clique->children_.push_back(new_clique);
}
new_clique->assertInvariants();
return new_clique;
}
@ -443,6 +466,7 @@ namespace gtsam {
new_clique->children_ = child_cliques;
BOOST_FOREACH(sharedClique& child, child_cliques)
child->parent_ = new_clique;
new_clique->assertInvariants();
return new_clique;
}
@ -477,6 +501,7 @@ namespace gtsam {
nodes_[key] = clique;
clique->push_front(conditional);
if(debug) clique->print("Expanded clique is ");
clique->assertInvariants();
}
/* ************************************************************************* */

4
gtsam/inference/BayesTree.h
View File

@ -53,6 +53,10 @@ namespace gtsam {
*/
struct Clique: public BayesNet<CONDITIONAL> {
protected:
void assertInvariants() const;
public:
typedef typename boost::shared_ptr<Clique> shared_ptr;
typedef typename boost::weak_ptr<Clique> weak_ptr;
weak_ptr parent_;

29
gtsam/linear/HessianFactor.cpp
View File

@ -61,8 +61,8 @@ namespace gtsam {
/* ************************************************************************* */
HessianFactor::HessianFactor(const Vector& b_in) : info_(matrix_) {
JacobianFactor jf(b_in);
ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
info_.copyStructureFrom(jf.Ab_);
ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
}
/* ************************************************************************* */
@ -70,8 +70,8 @@ namespace gtsam {
const Vector& b, const SharedDiagonal& model) :
GaussianFactor(i1), info_(matrix_) {
JacobianFactor jf(i1, A1, b, model);
ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
info_.copyStructureFrom(jf.Ab_);
ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
}
/* ************************************************************************* */
@ -79,8 +79,8 @@ namespace gtsam {
const Vector& b, const SharedDiagonal& model) :
GaussianFactor(i1,i2), info_(matrix_) {
JacobianFactor jf(i1, A1, i2, A2, b, model);
ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
info_.copyStructureFrom(jf.Ab_);
ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
}
/* ************************************************************************* */
@ -88,8 +88,8 @@ namespace gtsam {
Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) :
GaussianFactor(i1,i2,i3), info_(matrix_) {
JacobianFactor jf(i1, A1, i2, A2, i3, A3, b, model);
ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
info_.copyStructureFrom(jf.Ab_);
ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
}
/* ************************************************************************* */
@ -97,8 +97,8 @@ namespace gtsam {
const Vector &b, const SharedDiagonal& model) : info_(matrix_) {
JacobianFactor jf(terms, b, model);
keys_ = jf.keys_;
ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
info_.copyStructureFrom(jf.Ab_);
ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
}
/* ************************************************************************* */
@ -106,16 +106,16 @@ namespace gtsam {
const Vector &b, const SharedDiagonal& model) : info_(matrix_) {
JacobianFactor jf(terms, b, model);
keys_ = jf.keys_;
ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
info_.copyStructureFrom(jf.Ab_);
ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
}
/* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianConditional& cg) : GaussianFactor(cg), info_(matrix_) {
JacobianFactor jf(cg);
info_.copyStructureFrom(jf.Ab_);
ublas::noalias(ublas::symmetric_adaptor<MatrixColMajor,ublas::upper>(matrix_)) =
ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
info_.copyStructureFrom(jf.Ab_);
}
/* ************************************************************************* */
@ -126,8 +126,8 @@ namespace gtsam {
if(dynamic_cast<const JacobianFactor*>(&gf)) {
const JacobianFactor& jf(static_cast<const JacobianFactor&>(gf));
JacobianFactor whitened(jf.whiten());
matrix_ = ublas::prod(ublas::trans(whitened.matrix_), whitened.matrix_);
info_.copyStructureFrom(whitened.Ab_);
matrix_ = ublas::prod(ublas::trans(whitened.matrix_), whitened.matrix_);
} else if(dynamic_cast<const HessianFactor*>(&gf)) {
const HessianFactor& hf(static_cast<const HessianFactor&>(gf));
info_.assignNoalias(hf.info_);
@ -289,9 +289,22 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
for(size_t j1=0; j1<=j2; ++j1) {
size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
if(slot2 > slot1) {
if(debug)
cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
ublas::noalias(this->info_(slot1, slot2)) += update.info_(j1,j2);
} else if(slot1 > slot2) {
if(debug)
cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl;
ublas::noalias(this->info_(slot2, slot1)) += ublas::trans(update.info_(j1,j2));
} else {
if(debug)
cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
Block thisBlock(this->info_(slot2, slot1));
constBlock updateBlock(update.info_(j1,j2));
ublas::noalias(ublas::symmetric_adaptor<Block,ublas::upper>(thisBlock)) +=
ublas::symmetric_adaptor<constBlock,ublas::upper>(updateBlock);
}
}
}
toc(2, "update");

79
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -205,6 +205,85 @@ TEST(GaussianFactor, eliminate2 )
EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3));
}
/* ************************************************************************* */
TEST_UNSAFE(GaussianFactor, eliminateUnsorted) {
JacobianFactor::shared_ptr factor1(
new JacobianFactor(0,
Matrix_(3,3,
44.7214, 0.0, 0.0,
0.0, 44.7214, 0.0,
0.0, 0.0, 44.7214),
1,
Matrix_(3,3,
-0.179168, -44.721, 0.717294,
44.721, -0.179168, -44.9138,
0.0, 0.0, -44.7214),
Vector_(3, 1.98916e-17, -4.96503e-15, -7.75792e-17),
noiseModel::Unit::Create(3)));
HessianFactor::shared_ptr unsorted_factor2(
new HessianFactor(0,
Matrix_(6,3,
25.8367, 0.1211, 0.0593,
0.0, 23.4099, 30.8733,
0.0, 0.0, 25.8729,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0),
1,
Matrix_(6,3,
25.7429, -1.6897, 0.4587,
1.6400, 23.3095, -8.4816,
0.0034, 0.0509, -25.7855,
0.9997, -0.0002, 0.0824,
0.0, 0.9973, 0.9517,
0.0, 0.0, 0.9973),
Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
noiseModel::Unit::Create(6)));
Permutation permutation(2);
permutation[0] = 1;
permutation[1] = 0;
unsorted_factor2->permuteWithInverse(permutation);
HessianFactor::shared_ptr sorted_factor2(
new HessianFactor(0,
Matrix_(6,3,
25.7429, -1.6897, 0.4587,
1.6400, 23.3095, -8.4816,
0.0034, 0.0509, -25.7855,
0.9997, -0.0002, 0.0824,
0.0, 0.9973, 0.9517,
0.0, 0.0, 0.9973),
1,
Matrix_(6,3,
25.8367, 0.1211, 0.0593,
0.0, 23.4099, 30.8733,
0.0, 0.0, 25.8729,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0),
Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
noiseModel::Unit::Create(6)));
GaussianFactorGraph sortedGraph;
// sortedGraph.push_back(factor1);
sortedGraph.push_back(sorted_factor2);
GaussianFactorGraph unsortedGraph;
// unsortedGraph.push_back(factor1);
unsortedGraph.push_back(unsorted_factor2);
GaussianBayesNet::shared_ptr expected_bn;
GaussianFactor::shared_ptr expected_factor;
boost::tie(expected_bn, expected_factor) = GaussianFactor::CombineAndEliminate(sortedGraph, 1, GaussianFactor::SOLVE_PREFER_CHOLESKY);
GaussianBayesNet::shared_ptr actual_bn;
GaussianFactor::shared_ptr actual_factor;
boost::tie(actual_bn, actual_factor) = GaussianFactor::CombineAndEliminate(unsortedGraph, 1, GaussianFactor::SOLVE_PREFER_CHOLESKY);
CHECK(assert_equal(*expected_bn, *actual_bn, 1e-10));
CHECK(assert_equal(*expected_factor, *actual_factor, 1e-10));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */