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

assorted cleanup - mostly comments and adding implementations to cpp files, rather than in header files

release/4.3a0
Alex Cunningham 2011-06-13 20:01:58 +00:00
parent 322f61c537
commit 778001f63e
23 changed files with 395 additions and 345 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CppUnitLite/Test.h
View File

@ -113,6 +113,10 @@ protected:
result_.addFailure (Failure (name_, __FILE__,__LINE__, SimpleString("Wrong exception: ") + StringFrom(#condition) + StringFrom(", expected: ") + StringFrom(#exception_name))); \
return; } }
#define EQUALITY(expected,actual)\
{ if (!assert_equal(expected,actual)) \
result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); }
#define CHECK_EQUAL(expected,actual)\
{ if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, StringFrom(expected), StringFrom(actual))); }

11
gtsam/base/Matrix.h
View File

@ -331,9 +331,6 @@ void householder(Matrix& A, size_t k);
* @param unit, set true if unit triangular
* @return the solution x of U*x=b
*/
//FIXME: add back expression form
//template<class MATRIX, class VECTOR>
//Vector backSubstituteUpper(const MATRIX& U, const VECTOR& b, bool unit=false);
Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit=false);
/**
@ -343,10 +340,7 @@ Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit=false);
* @param unit, set true if unit triangular
* @return the solution x of x'*U=b'
*/
//FIXME: add back expression form
//TODO: is this function necessary? it isn't used
//template<class VECTOR, class MATRIX>
//Vector backSubstituteUpper(const VECTOR& b, const MATRIX& U, bool unit=false);
Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit=false);
/**
@ -453,11 +447,6 @@ DLT(const Matrix& A, double rank_tol = 1e-9);
*/
Matrix expm(const Matrix& A, size_t K=7);
// macro for unit tests
#define EQUALITY(expected,actual)\
{ if (!assert_equal(expected,actual)) \
result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); }
} // namespace gtsam
#include <boost/serialization/nvp.hpp>

192
gtsam/base/timing.cpp
View File

@ -16,6 +16,14 @@
* @created Oct 5, 2010
*/
#include <stdio.h>
#include <iostream>
#include <sys/time.h>
#include <stdlib.h>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
boost::shared_ptr<TimingOutline> timingRoot(new TimingOutline("Total"));
@ -24,3 +32,187 @@ boost::weak_ptr<TimingOutline> timingCurrent(timingRoot);
Timing timing;
std::string timingPrefix;
/* ************************************************************************* */
// Implementation of TimingOutline
/* ************************************************************************* */
/* ************************************************************************* */
void TimingOutline::add(size_t usecs) {
t_ += usecs;
tIt_ += usecs;
++ n_;
}
/* ************************************************************************* */
TimingOutline::TimingOutline(const std::string& label) :
t_(0), tIt_(0), tMax_(0), tMin_(0), n_(0), label_(label), timerActive_(false) {}
/* ************************************************************************* */
size_t TimingOutline::time() const {
size_t time = 0;
bool hasChildren = false;
BOOST_FOREACH(const boost::shared_ptr<TimingOutline>& child, children_) {
if(child) {
time += child->time();
hasChildren = true;
}
}
if(hasChildren)
return time;
else
return t_;
}
/* ************************************************************************* */
void TimingOutline::print(const std::string& outline) const {
std::cout << outline << " " << label_ << ": " << double(t_)/1000000.0 << " (" <<
n_ << " times, " << double(time())/1000000.0 << " children, min: " << double(tMin_)/1000000.0 <<
" max: " << double(tMax_)/1000000.0 << ")\n";
for(size_t i=0; i<children_.size(); ++i) {
if(children_[i]) {
std::string childOutline(outline);
#if 0
if(childOutline.size() > 0)
childOutline += ".";
childOutline += (boost::format("%d") % i).str();
#else
childOutline += " ";
#endif
children_[i]->print(childOutline);
}
}
}
/* ************************************************************************* */
const boost::shared_ptr<TimingOutline>& TimingOutline::child(size_t child, const std::string& label, const boost::weak_ptr<TimingOutline>& thisPtr) {
assert(thisPtr.lock().get() == this);
// Resize if necessary
if(child >= children_.size())
children_.resize(child + 1);
// Create child if necessary
if(children_[child]) {
#ifndef NDEBUG
if(children_[child]->label_ != label) {
timingRoot->print();
std::cerr << "gtsam timing: tic called with id=" << child << ", label=" << label << ", but this id already has the label " << children_[child]->label_ << std::endl;
exit(1);
}
#endif
} else {
children_[child].reset(new TimingOutline(label));
children_[child]->parent_ = thisPtr;
}
return children_[child];
}
/* ************************************************************************* */
void TimingOutline::tic() {
assert(!timerActive_);
timerActive_ = true;
gettimeofday(&t0_, NULL);
}
/* ************************************************************************* */
void TimingOutline::toc() {
struct timeval t;
gettimeofday(&t, NULL);
assert(timerActive_);
add(t.tv_sec*1000000 + t.tv_usec - (t0_.tv_sec*1000000 + t0_.tv_usec));
timerActive_ = false;
}
/* ************************************************************************* */
void TimingOutline::finishedIteration() {
if(tIt_ > tMax_)
tMax_ = tIt_;
if(tMin_ == 0 || tIt_ < tMin_)
tMin_ = tIt_;
tIt_ = 0;
for(size_t i=0; i<children_.size(); ++i)
if(children_[i])
children_[i]->finishedIteration();
}
/* ************************************************************************* */
void tic_(size_t id, const std::string& label) {
if(ISDEBUG("timing-verbose"))
std::cout << "tic(" << id << ", " << label << ")" << std::endl;
boost::shared_ptr<TimingOutline> node = timingCurrent.lock()->child(id, label, timingCurrent);
timingCurrent = node;
node->tic();
}
/* ************************************************************************* */
void toc_(size_t id) {
if(ISDEBUG("timing-verbose"))
std::cout << "toc(" << id << ")" << std::endl;
boost::shared_ptr<TimingOutline> current(timingCurrent.lock());
if(!(id < current->parent_.lock()->children_.size() && current->parent_.lock()->children_[id] == current)) {
if(std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current)
!= current->parent_.lock()->children_.end())
std::cout << "gtsam timing: Incorrect ID passed to toc, expected "
<< std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current) - current->parent_.lock()->children_.begin()
<< ", got " << id << std::endl;
else
std::cout << "gtsam timing: Incorrect ID passed to toc, id " << id << " does not exist" << std::endl;
timingRoot->print();
throw std::invalid_argument("gtsam timing: Incorrect ID passed to toc");
}
current->toc();
if(!current->parent_.lock()) {
std::cout << "gtsam timing: extra toc, already at the root" << std::endl;
timingRoot->print();
throw std::invalid_argument("gtsam timing: extra toc, already at the root");
}
timingCurrent = current->parent_;
}
/* ************************************************************************* */
void toc_(size_t id, const std::string& label) {
if(ISDEBUG("timing-verbose"))
std::cout << "toc(" << id << ", " << label << ")" << std::endl;
bool check = false;
#ifndef NDEBUG
// If NDEBUG is defined, still do this debug check if the granular debugging
// flag is enabled. If NDEBUG is not defined, always do this check.
check = true;
#endif
if(check || ISDEBUG("timing-debug")) {
if(label != timingCurrent.lock()->label_) {
std::cerr << "gtsam timing: toc called with id=" << id << ", label=\"" << label << "\", but expecting \"" << timingCurrent.lock()->label_ << "\"" << std::endl;
timingRoot->print();
exit(1);
}
}
toc_(id);
}
/* ************************************************************************* */
// Timing class implementation
void Timing::print() {
std::map<std::string, Timing::Stats>::iterator it;
for(it = this->stats.begin(); it!=stats.end(); it++) {
Stats& s = it->second;
printf("%s: %g (%i times, min: %g, max: %g)\n",
it->first.c_str(), s.t, s.n, s.t_min, s.t_max);
}
}
/* ************************************************************************* */
double _tic_() {
struct timeval t;
gettimeofday(&t, NULL);
return ((double)t.tv_sec + ((double)t.tv_usec)/1000000.);
}
/* ************************************************************************* */
void ticPop_(const std::string& prefix, const std::string& id) {
toc_(id);
if(timingPrefix.size() < prefix.size()) {
fprintf(stderr, "Seems to be a mismatched push/pop in timing, exiting\n");
exit(1);
} else if(timingPrefix.size() == prefix.size())
timingPrefix.resize(0);
else
timingPrefix.resize(timingPrefix.size() - prefix.size() - 1);
}

179
gtsam/base/timing.h
View File

@ -17,18 +17,11 @@
*/
#pragma once
#include <gtsam/base/debug.h>
#include <stdio.h>
#include <string>
#include <iostream>
#include <sys/time.h>
#include <map>
#include <stdlib.h>
#include <vector>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
class TimingOutline;
extern boost::shared_ptr<TimingOutline> timingRoot;
@ -47,151 +40,34 @@ protected:
struct timeval t0_;
bool timerActive_;
inline void add(size_t usecs) {
t_ += usecs;
tIt_ += usecs;
++ n_;
}
void add(size_t usecs);
public:
inline TimingOutline(const std::string& label) :
t_(0), tIt_(0), tMax_(0), tMin_(0), n_(0), label_(label), timerActive_(false) {}
inline size_t time() const {
size_t time = 0;
bool hasChildren = false;
BOOST_FOREACH(const boost::shared_ptr<TimingOutline>& child, children_) {
if(child) {
time += child->time();
hasChildren = true;
}
}
if(hasChildren)
return time;
else
return t_;
}
TimingOutline(const std::string& label);
inline void print(const std::string& outline = "") const {
std::cout << outline << " " << label_ << ": " << double(t_)/1000000.0 << " (" <<
n_ << " times, " << double(time())/1000000.0 << " children, min: " << double(tMin_)/1000000.0 <<
" max: " << double(tMax_)/1000000.0 << ")\n";
for(size_t i=0; i<children_.size(); ++i) {
if(children_[i]) {
std::string childOutline(outline);
#if 0
if(childOutline.size() > 0)
childOutline += ".";
childOutline += (boost::format("%d") % i).str();
#else
childOutline += " ";
#endif
children_[i]->print(childOutline);
}
}
}
size_t time() const;
inline const boost::shared_ptr<TimingOutline>& child(size_t child, const std::string& label, const boost::weak_ptr<TimingOutline>& thisPtr) {
assert(thisPtr.lock().get() == this);
// Resize if necessary
if(child >= children_.size())
children_.resize(child + 1);
// Create child if necessary
if(children_[child]) {
#ifndef NDEBUG
if(children_[child]->label_ != label) {
timingRoot->print();
std::cerr << "gtsam timing: tic called with id=" << child << ", label=" << label << ", but this id already has the label " << children_[child]->label_ << std::endl;
exit(1);
}
#endif
} else {
children_[child].reset(new TimingOutline(label));
children_[child]->parent_ = thisPtr;
}
return children_[child];
}
void print(const std::string& outline = "") const;
inline void tic() {
assert(!timerActive_);
timerActive_ = true;
gettimeofday(&t0_, NULL);
}
const boost::shared_ptr<TimingOutline>& child(size_t child, const std::string& label, const boost::weak_ptr<TimingOutline>& thisPtr);
inline void toc() {
struct timeval t;
gettimeofday(&t, NULL);
assert(timerActive_);
add(t.tv_sec*1000000 + t.tv_usec - (t0_.tv_sec*1000000 + t0_.tv_usec));
timerActive_ = false;
}
void tic();
inline void finishedIteration() {
if(tIt_ > tMax_)
tMax_ = tIt_;
if(tMin_ == 0 || tIt_ < tMin_)
tMin_ = tIt_;
tIt_ = 0;
for(size_t i=0; i<children_.size(); ++i)
if(children_[i])
children_[i]->finishedIteration();
}
void toc();
void finishedIteration();
friend class AutoTimer;
friend void toc_(size_t id);
friend void toc_(size_t id, const std::string& label);
};
}; // \TimingOutline
inline void tic_(size_t id, const std::string& label) {
if(ISDEBUG("timing-verbose"))
std::cout << "tic(" << id << ", " << label << ")" << std::endl;
boost::shared_ptr<TimingOutline> node = timingCurrent.lock()->child(id, label, timingCurrent);
timingCurrent = node;
node->tic();
}
void tic_(size_t id, const std::string& label);
inline void toc_(size_t id) {
if(ISDEBUG("timing-verbose"))
std::cout << "toc(" << id << ")" << std::endl;
boost::shared_ptr<TimingOutline> current(timingCurrent.lock());
if(!(id < current->parent_.lock()->children_.size() && current->parent_.lock()->children_[id] == current)) {
if(std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current)
!= current->parent_.lock()->children_.end())
std::cout << "gtsam timing: Incorrect ID passed to toc, expected "
<< std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current) - current->parent_.lock()->children_.begin()
<< ", got " << id << std::endl;
else
std::cout << "gtsam timing: Incorrect ID passed to toc, id " << id << " does not exist" << std::endl;
timingRoot->print();
throw std::invalid_argument("gtsam timing: Incorrect ID passed to toc");
}
current->toc();
if(!current->parent_.lock()) {
std::cout << "gtsam timing: extra toc, already at the root" << std::endl;
timingRoot->print();
throw std::invalid_argument("gtsam timing: extra toc, already at the root");
}
timingCurrent = current->parent_;
}
void toc_(size_t id);
inline void toc_(size_t id, const std::string& label) {
if(ISDEBUG("timing-verbose"))
std::cout << "toc(" << id << ", " << label << ")" << std::endl;
bool check = false;
#ifndef NDEBUG
// If NDEBUG is defined, still do this debug check if the granular debugging
// flag is enabled. If NDEBUG is not defined, always do this check.
check = true;
#endif
if(check || ISDEBUG("timing-debug")) {
if(label != timingCurrent.lock()->label_) {
std::cerr << "gtsam timing: toc called with id=" << id << ", label=\"" << label << "\", but expecting \"" << timingCurrent.lock()->label_ << "\"" << std::endl;
timingRoot->print();
exit(1);
}
}
toc_(id);
}
void toc_(size_t id, const std::string& label);
inline void tictoc_finishedIteration_() {
timingRoot->finishedIteration();
@ -261,25 +137,15 @@ public:
if (s.n==1 || s.t_max < dt) s.t_max = dt;
if (s.n==1 || s.t_min > dt) s.t_min = dt;
}
void print() {
std::map<std::string, Stats>::iterator it;
for(it = stats.begin(); it!=stats.end(); it++) {
Stats& s = it->second;
printf("%s: %g (%i times, min: %g, max: %g)\n",
it->first.c_str(), s.t, s.n, s.t_min, s.t_max);
}
}
void print();
double time(const std::string& id) {
Stats& s = stats[id];
return s.t;
}
};
inline double _tic_() {
struct timeval t;
gettimeofday(&t, NULL);
return ((double)t.tv_sec + ((double)t.tv_usec)/1000000.);
}
double _tic_();
inline double _toc_(double t) {
double s = _tic_();
return (std::max(0., s-t));
@ -301,16 +167,7 @@ inline void ticPush_(const std::string& prefix, const std::string& id) {
timingPrefix += prefix;
tic_(id);
}
inline void ticPop_(const std::string& prefix, const std::string& id) {
toc_(id);
if(timingPrefix.size() < prefix.size()) {
fprintf(stderr, "Seems to be a mismatched push/pop in timing, exiting\n");
exit(1);
} else if(timingPrefix.size() == prefix.size())
timingPrefix.resize(0);
else
timingPrefix.resize(timingPrefix.size() - prefix.size() - 1);
}
void ticPop_(const std::string& prefix, const std::string& id);
inline void tictoc_print_() {
timing.print();
timingRoot->print();

9
gtsam/geometry/StereoPoint2.cpp
View File

@ -16,5 +16,14 @@
* Author: dellaert
*/
#include <iostream>
#include <gtsam/geometry/StereoPoint2.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
void StereoPoint2::print(const string& s) const {
cout << s << "(" << uL_ << ", " << uR_ << ", " << v_ << ")" << endl;
}
/* ************************************************************************* */

6
gtsam/geometry/StereoPoint2.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <iostream>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -45,10 +44,7 @@ namespace gtsam {
}
/** print */
void print(const std::string& s) const {
std::cout << s << "(" << uL_ << ", " << uR_ << ", " << v_ << ")"
<< std::endl;
}
void print(const std::string& s="") const;
/** equals */
bool equals(const StereoPoint2& q, double tol=1e-9) const {

8
gtsam/inference/ClusterTree-inl.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <iostream>
#include <boost/foreach.hpp>
#include <gtsam/inference/ClusterTree.h>
@ -94,6 +95,13 @@ namespace gtsam {
* ClusterTree
* ************************************************************************* */
template<class FG>
void ClusterTree<FG>::print(const string& str) const {
cout << str << endl;
if (root_) root_->printTree("");
}
/* ************************************************************************* */
template<class FG>
bool ClusterTree<FG>::equals(const ClusterTree<FG>& other, double tol) const {
if (!root_ && !other.root_) return true;
if (!root_ || !other.root_) return false;

6
gtsam/inference/ClusterTree.h
View File

@ -21,7 +21,6 @@
#include <list>
#include <vector>
#include <iostream>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
@ -104,10 +103,7 @@ namespace gtsam {
sharedCluster root() const { return root_; }
// print the object
void print(const std::string& str) const {
std::cout << str << std::endl;
if (root_) root_->printTree("");
}
void print(const std::string& str="") const;
/** check equality */
bool equals(const ClusterTree<FG>& other, double tol = 1e-9) const;

5
gtsam/inference/tests/testClusterTree.cpp
View File

@ -30,6 +30,11 @@ using namespace gtsam;
template class ClusterTree<SymbolicFactorGraph>;
typedef ClusterTree<SymbolicFactorGraph> SymbolicClusterTree;
/* ************************************************************************* */
TEST(ClusterTree, constructor) {
SymbolicClusterTree tree;
}
/* ************************************************************************* */
int main() {
TestResult tr;

11
gtsam/inference/tests/testEliminationTree.cpp
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testEliminationTree.cpp
* @brief

7
gtsam/inference/tests/testFactorGraph.cpp
View File

@ -93,15 +93,12 @@ typedef boost::shared_ptr<SymbolicFactorGraph> shared;
// CHECK(singletonGraph_excepted.equals(singletonGraph));
//}
/* ************************************************************************* */
TEST(FactorGraph, dynamic_factor_cast) {
FactorGraph<IndexFactor> fg;
fg.dynamicCastFactors<FactorGraph<IndexFactor> >();
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

7
gtsam/inference/tests/testInference.cpp
View File

@ -36,12 +36,7 @@ TEST(Inference, UnobservedVariables) {
VariableIndex variableIndex(sfg);
Permutation::shared_ptr colamd(Inference::PermutationCOLAMD(variableIndex));
//colamd->print("colamd: ");
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

8
gtsam/linear/GaussianConditional.cpp
View File

@ -159,12 +159,11 @@ void GaussianConditional::rhs(VectorValues& x) const {
void GaussianConditional::solveInPlace(VectorValues& x) const {
static const bool debug = false;
if(debug) print("Solving conditional in place");
// Vector rhs(get_d());
Vector rhs = x.range(beginFrontals(), endFrontals());
for (const_iterator parent = beginParents(); parent != endParents(); ++parent) {
rhs += -get_S(parent) * x[*parent];
}
Vector soln = permutation_.transpose()*backSubstituteUpper(get_R(), rhs, false);
Vector soln = permutation_.transpose() * get_R().triangularView<Eigen::Upper>().solve(rhs);
if(debug) {
gtsam::print(Matrix(get_R()), "Calling backSubstituteUpper on ");
gtsam::print(rhs, "rhs: ");
@ -177,7 +176,6 @@ void GaussianConditional::solveInPlace(VectorValues& x) const {
void GaussianConditional::solveInPlace(Permuted<VectorValues>& x) const {
static const bool debug = false;
if(debug) print("Solving conditional in place (permuted)");
// Vector rhs(get_d());
// Extract RHS from values - inlined from VectorValues
size_t s = 0;
for (const_iterator it=beginFrontals(); it!=endFrontals(); ++it)
@ -195,8 +193,8 @@ void GaussianConditional::solveInPlace(Permuted<VectorValues>& x) const {
rhs += -get_S(parent) * x[*parent];
}
// solve system
Vector soln = permutation_.transpose()*backSubstituteUpper(get_R(), rhs, false);
// solve system - backsubstitution
Vector soln = permutation_.transpose() * get_R().triangularView<Eigen::Upper>().solve(rhs);
// apply solution: inlined manually due to permutation
size_t solnStart = 0;

170
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -30,8 +30,7 @@ using namespace gtsam;
namespace gtsam {
// Explicitly instantiate so we don't have to include everywhere
INSTANTIATE_FACTOR_GRAPH(GaussianFactor)
;
INSTANTIATE_FACTOR_GRAPH(GaussianFactor);
/* ************************************************************************* */
GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) :
@ -120,31 +119,6 @@ namespace gtsam {
return combined.matrix_augmented();
}
// VectorValues GaussianFactorGraph::allocateVectorValuesb() const {
// std::vector<size_t> dimensions(size()) ;
// Index i = 0 ;
// BOOST_FOREACH( const sharedFactor& factor, factors_) {
// dimensions[i] = factor->numberOfRows() ;
// i++;
// }
//
// return VectorValues(dimensions) ;
// }
//
// void GaussianFactorGraph::getb(VectorValues &b) const {
// Index i = 0 ;
// BOOST_FOREACH( const sharedFactor& factor, factors_) {
// b[i] = factor->getb();
// i++;
// }
// }
//
// VectorValues GaussianFactorGraph::getb() const {
// VectorValues b = allocateVectorValuesb() ;
// getb(b) ;
// return b ;
// }
/* ************************************************************************* */
// Helper functions for Combine
static boost::tuple<vector<size_t>, size_t, size_t> countDims(const std::vector<JacobianFactor::shared_ptr>& factors, const VariableSlots& variableSlots) {
@ -419,7 +393,7 @@ namespace gtsam {
toc(2, "Jacobian EliminateGaussian");
return make_pair(conditionals, factor);
} // EliminateQR
} // \EliminateQR
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminatePreferCholesky(
@ -499,8 +473,7 @@ namespace gtsam {
return ret;
} // EliminatePreferCholesky
} // \EliminatePreferCholesky
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminateLDL(
@ -537,7 +510,6 @@ namespace gtsam {
cout << " Index: " << p.first << ", " << p.second.toString() << endl;
}
HessianFactor::shared_ptr //
combinedFactor(new HessianFactor(factors, dimensions, scatter));
toc(3, "combine");
@ -561,86 +533,86 @@ namespace gtsam {
combinedFactor->assertInvariants();
return make_pair(conditionals, combinedFactor);
}
} // \EliminateLDL
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminatePreferLDL(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
GaussianFactorGraph::EliminationResult EliminatePreferLDL(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
typedef JacobianFactor J;
typedef HessianFactor H;
typedef JacobianFactor J;
typedef HessianFactor H;
// If any JacobianFactors have constrained noise models, we have to convert
// all factors to JacobianFactors. Otherwise, we can convert all factors
// to HessianFactors. This is because QR can handle constrained noise
// models but LDL cannot.
// If any JacobianFactors have constrained noise models, we have to convert
// all factors to JacobianFactors. Otherwise, we can convert all factors
// to HessianFactors. This is because QR can handle constrained noise
// models but LDL cannot.
// Decide whether to use QR or LDL
// Check if any JacobianFactors have constrained noise models.
bool useQR = false;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian && jacobian->get_model()->isConstrained()) {
useQR = true;
break;
}
}
// Decide whether to use QR or LDL
// Check if any JacobianFactors have constrained noise models.
bool useQR = false;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian && jacobian->get_model()->isConstrained()) {
useQR = true;
break;
}
}
// Convert all factors to the appropriate type
// and call the type-specific EliminateGaussian.
if (useQR) return EliminateQR(factors, nrFrontals);
// Convert all factors to the appropriate type
// and call the type-specific EliminateGaussian.
if (useQR) return EliminateQR(factors, nrFrontals);
GaussianFactorGraph::EliminationResult ret;
try {
tic(2, "EliminateLDL");
ret = EliminateLDL(factors, nrFrontals);
toc(2, "EliminateLDL");
} catch (const exception& e) {
cout << "Exception in EliminateLDL: " << e.what() << endl;
SETDEBUG("EliminateLDL", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
SETDEBUG("ldlPartial", true);
SETDEBUG("findScatterAndDims", true);
factors.print("Combining factors: ");
EliminateLDL(factors, nrFrontals);
throw;
}
GaussianFactorGraph::EliminationResult ret;
try {
tic(2, "EliminateLDL");
ret = EliminateLDL(factors, nrFrontals);
toc(2, "EliminateLDL");
} catch (const exception& e) {
cout << "Exception in EliminateLDL: " << e.what() << endl;
SETDEBUG("EliminateLDL", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
SETDEBUG("ldlPartial", true);
SETDEBUG("findScatterAndDims", true);
factors.print("Combining factors: ");
EliminateLDL(factors, nrFrontals);
throw;
}
const bool checkLDL = ISDEBUG("EliminateGaussian Check LDL");
if (checkLDL) {
GaussianFactorGraph::EliminationResult expected;
FactorGraph<J> jacobians = convertToJacobians(factors);
try {
// Compare with QR
expected = EliminateJacobians(jacobians, nrFrontals);
} catch (...) {
cout << "Exception in QR" << endl;
throw;
}
const bool checkLDL = ISDEBUG("EliminateGaussian Check LDL");
if (checkLDL) {
GaussianFactorGraph::EliminationResult expected;
FactorGraph<J> jacobians = convertToJacobians(factors);
try {
// Compare with QR
expected = EliminateJacobians(jacobians, nrFrontals);
} catch (...) {
cout << "Exception in QR" << endl;
throw;
}
H actual_factor(*ret.second);
H expected_factor(*expected.second);
if (!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal(
expected_factor, actual_factor, 1.0)) {
cout << "LDL and QR do not agree" << endl;
H actual_factor(*ret.second);
H expected_factor(*expected.second);
if (!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal(
expected_factor, actual_factor, 1.0)) {
cout << "LDL and QR do not agree" << endl;
SETDEBUG("EliminateLDL", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
jacobians.print("Jacobian Factors: ");
EliminateJacobians(jacobians, nrFrontals);
EliminateLDL(factors, nrFrontals);
factors.print("Combining factors: ");
SETDEBUG("EliminateLDL", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
jacobians.print("Jacobian Factors: ");
EliminateJacobians(jacobians, nrFrontals);
EliminateLDL(factors, nrFrontals);
factors.print("Combining factors: ");
throw runtime_error("LDL and QR do not agree");
}
}
throw runtime_error("LDL and QR do not agree");
}
}
return ret;
return ret;
} // EliminatePreferLDL
} // \EliminatePreferLDL
} // namespace gtsam

10
gtsam/linear/GaussianFactorGraph.h
View File

@ -163,13 +163,6 @@ namespace gtsam {
*/
Matrix denseJacobian() const;
// get b
// void getb(VectorValues &b) const ;
// VectorValues getb() const ;
//
// // allocate a vectorvalues of b's structure
// VectorValues allocateVectorValuesb() const ;
private:
/** Serialization function */
friend class boost::serialization::access;
@ -190,9 +183,6 @@ namespace gtsam {
GaussianFactorGraph::EliminationResult EliminateJacobians(const FactorGraph<
JacobianFactor>& factors, size_t nrFrontals = 1);
// GaussianFactorGraph::EliminationResult EliminateHessians(const FactorGraph<
// HessianFactor>& factors, size_t nrFrontals = 1);
GaussianFactorGraph::EliminationResult EliminateQR(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals = 1);

2
gtsam/linear/NoiseModel.h
View File

@ -25,7 +25,7 @@
namespace gtsam {
class SharedDiagonal; // forward declare, defined at end
class SharedDiagonal; // forward declare
namespace noiseModel {

19
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -208,23 +208,6 @@ TEST( NoiseModel, QR )
EXPECT(linear_dependent(expectedRd2,Ab2,1e-6)); // Ab was modified in place !!!
}
/* ************************************************************************* */
//TEST( NoiseModel, QRColumnWise )
//{
// // Call Gaussian version
// Matrix Ab = exampleQR::Ab; // otherwise overwritten !
// vector<int> firstZeroRows;
// firstZeroRows += 0,1,2,3,4,5; // FD: no idea as not documented :-(
// SharedDiagonal actual = exampleQR::diagonal->QRColumnWise(Ab,firstZeroRows);
// SharedDiagonal expected = noiseModel::Unit::Create(4);
// EXPECT(assert_equal(*expected,*actual));
// Matrix AbResized = ublas::triangular_adaptor<Matrix, ublas::upper>(Ab);
// print(exampleQR::Rd, "Rd: ");
// print(Ab, "Ab: ");
// print(AbResized, "AbResized: ");
// EXPECT(linear_dependent(exampleQR::Rd,AbResized,1e-4)); // Ab was modified in place !!!
//}
/* ************************************************************************* */
TEST(NoiseModel, Cholesky)
{
@ -234,8 +217,6 @@ TEST(NoiseModel, Cholesky)
EXPECT(assert_equal(*expected,*actual));
// Ab was modified in place !!!
Matrix actualRd = Ab.block(0, 0, actual->dim(), Ab.cols()).triangularView<Eigen::Upper>();
// ublas::project(ublas::triangular_adaptor<Matrix, ublas::upper>(Ab),
// ublas::range(0,actual->dim()), ublas::range(0, Ab.cols()));
EXPECT(linear_dependent(exampleQR::Rd,actualRd,1e-4));
}

12
gtsam/nonlinear/NonlinearFactor.h
View File

@ -153,7 +153,7 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(noiseModel_);
}
}; // NonlinearFactor
}; // \class NonlinearFactor
/**
@ -268,7 +268,7 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(key_);
}
};
};// \class NonlinearFactor1
/**
* A Gaussian nonlinear factor that takes 2 parameters
@ -394,7 +394,7 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(key2_);
}
};
}; // \class NonlinearFactor2
/* ************************************************************************* */
@ -501,7 +501,7 @@ namespace gtsam {
*/
virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_];
return IndexFactor::shared_ptr(new IndexFactor(ordering[key1_], ordering[key2_], ordering[key3_]));
return IndexFactor::shared_ptr(new IndexFactor(var1, var2, var3));
}
/** methods to retrieve keys */
@ -539,8 +539,8 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(key3_);
}
};
}; // \class NonlinearFactor3
/* ************************************************************************* */
}
} // \namespace gtsam

11
gtsam/slam/GeneralSFMFactor.h
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/*
* GeneralSFMFactor.h
*

11
gtsam/slam/PartialPriorFactor.h
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file PartialPriorFactor.h
* @brief A simple prior factor that allows for setting a prior only on a part of linear parameters

14
gtsam/slam/ProjectionFactor.h
View File

@ -1,6 +1,20 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ProjectionFactor.h
* @brief Basic bearing factor from 2D measurement
* @author Chris Beall
* @author Richard Roberts
* @author Frank Dellaert
* @author Alex Cunningham
*/

26
tests/testGaussianFactorGraph.cpp
View File

@ -306,8 +306,8 @@ TEST( GaussianFactorGraph, eliminateAll )
GaussianBayesNet actual = *GaussianSequentialSolver(fg1).eliminate();
EXPECT(assert_equal(expected,actual,tol));
GaussianBayesNet actualET = *GaussianSequentialSolver(fg1).eliminate();
EXPECT(assert_equal(expected,actualET,tol));
GaussianBayesNet actualQR = *GaussianSequentialSolver(fg1, true).eliminate();
EXPECT(assert_equal(expected,actualQR,tol));
}
///* ************************************************************************* */
@ -473,7 +473,7 @@ TEST( GaussianFactorGraph, getOrdering)
//}
/* ************************************************************************* */
TEST( GaussianFactorGraph, optimize )
TEST( GaussianFactorGraph, optimize_LDL )
{
// create an ordering
Ordering ord; ord += "x2","l1","x1";
@ -482,7 +482,25 @@ TEST( GaussianFactorGraph, optimize )
GaussianFactorGraph fg = createGaussianFactorGraph(ord);
// optimize the graph
VectorValues actual = *GaussianSequentialSolver(fg).optimize();
VectorValues actual = *GaussianSequentialSolver(fg, false).optimize();
// verify
VectorValues expected = createCorrectDelta(ord);
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, optimize_QR )
{
// create an ordering
Ordering ord; ord += "x2","l1","x1";
// create a graph
GaussianFactorGraph fg = createGaussianFactorGraph(ord);
// optimize the graph
VectorValues actual = *GaussianSequentialSolver(fg, true).optimize();
// verify
VectorValues expected = createCorrectDelta(ord);

12
tests/testInference.cpp
View File

@ -43,11 +43,10 @@ TEST(GaussianFactorGraph, createSmoother)
// eliminate
vector<Index> x3var; x3var.push_back(ordering["x3"]);
vector<Index> x1var; x1var.push_back(ordering["x1"]);
// NOTE: fails when using LDL, works with QR
GaussianBayesNet p_x3 = *GaussianSequentialSolver(
*GaussianSequentialSolver(fg2).jointFactorGraph(x3var), true).eliminate();
*GaussianSequentialSolver(fg2).jointFactorGraph(x3var)).eliminate();
GaussianBayesNet p_x1 = *GaussianSequentialSolver(
*GaussianSequentialSolver(fg2).jointFactorGraph(x1var), true).eliminate();
*GaussianSequentialSolver(fg2).jointFactorGraph(x1var)).eliminate();
CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry
}
@ -58,10 +57,8 @@ TEST( Inference, marginals )
// create and marginalize a small Bayes net on "x"
GaussianBayesNet cbn = createSmallGaussianBayesNet();
vector<Index> xvar; xvar.push_back(0);
// NOTE: fails when using LDL, works with QR
GaussianBayesNet actual = *GaussianSequentialSolver(
*GaussianSequentialSolver(
GaussianFactorGraph(cbn), true).jointFactorGraph(xvar), true).eliminate();
*GaussianSequentialSolver(GaussianFactorGraph(cbn)).jointFactorGraph(xvar)).eliminate();
// expected is just scalar Gaussian on x
GaussianBayesNet expected = scalarGaussian(0, 4, sqrt(2));
@ -92,8 +89,7 @@ TEST( Inference, marginals2)
Ordering ordering(*fg.orderingCOLAMD(init));
FactorGraph<GaussianFactor>::shared_ptr gfg(fg.linearize(init, ordering));
// NOTE: fails when using LDL, works with QR
GaussianMultifrontalSolver solver(*gfg, true);
GaussianMultifrontalSolver solver(*gfg);
solver.marginalFactor(ordering[PointKey(0)]);
}