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

CppUnitLite/Test.h

@@ -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))); }
 

gtsam/base/Matrix.h

@@ -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>

gtsam/base/timing.cpp

@@ -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);
+}

gtsam/base/timing.h

@@ -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) {
+  void add(size_t usecs);
-    t_ += usecs;
-    tIt_ += usecs;
-    ++ n_;
-  }
 
 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 {
+  TimingOutline(const std::string& label);
-    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_;
-  }
 
-  inline void print(const std::string& outline = "") const {
+  size_t time() 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);
-      }
-    }
-  }
 
-  inline const boost::shared_ptr<TimingOutline>& child(size_t child, const std::string& label, const boost::weak_ptr<TimingOutline>& thisPtr) {
+  void print(const std::string& outline = "") const;
-    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];
-  }
 
-  inline void tic() {
+  const boost::shared_ptr<TimingOutline>& child(size_t child, const std::string& label, const boost::weak_ptr<TimingOutline>& thisPtr);
-    assert(!timerActive_);
-    timerActive_ = true;
-    gettimeofday(&t0_, NULL);
-  }
 
-  inline void toc() {
+  void tic();
-    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;
-  }
 
-  inline void finishedIteration() {
+  void toc();
-    if(tIt_ > tMax_)
+
-      tMax_ = tIt_;
+  void finishedIteration();
-    if(tMin_ == 0 || tIt_ < tMin_)
-      tMin_ = tIt_;
-    tIt_ = 0;
-    for(size_t i=0; i<children_.size(); ++i)
-      if(children_[i])
-        children_[i]->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) {
+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();
-}
 
-inline void toc_(size_t id) {
+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_;
-}
 
-inline void toc_(size_t id, const std::string& label) {
+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);
-}
 
 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() {
+  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);
-    }
-  }
   double time(const std::string& id) {
     Stats& s = stats[id];
     return s.t;
   }
 };
 
-inline double _tic_() {
+double _tic_();
-  struct timeval t;
-  gettimeofday(&t, NULL);
-  return ((double)t.tv_sec + ((double)t.tv_usec)/1000000.);
-}
 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) {
+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);
-}
 inline void tictoc_print_() {
   timing.print();
   timingRoot->print();

gtsam/geometry/StereoPoint2.cpp

@@ -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;
+}
+/* ************************************************************************* */

gtsam/geometry/StereoPoint2.h

@@ -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 {
+		void print(const std::string& s="") const;
-			std::cout << s << "(" << uL_ << ", " << uR_ << ", " << v_ << ")"
-					<< std::endl;
-		}
 
 		/** equals */
 		bool equals(const StereoPoint2& q, double tol=1e-9) const {

gtsam/inference/ClusterTree-inl.h

@@ -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;

gtsam/inference/ClusterTree.h

@@ -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 {
+		void print(const std::string& str="") const;
-			std::cout << str << std::endl;
-			if (root_) root_->printTree("");
-		}
 
 		/** check equality */
 		bool equals(const ClusterTree<FG>& other, double tol = 1e-9) const;

gtsam/inference/tests/testClusterTree.cpp

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

gtsam/inference/tests/testEliminationTree.cpp

@@ -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   

gtsam/inference/tests/testFactorGraph.cpp

@@ -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() {
+int main() {	TestResult tr; return TestRegistry::runAllTests(tr); }
-	TestResult tr;
-	return TestRegistry::runAllTests(tr);
-}
 /* ************************************************************************* */

gtsam/inference/tests/testInference.cpp

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

gtsam/linear/GaussianConditional.cpp

@@ -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
+	// solve system - backsubstitution
-	Vector soln = permutation_.transpose()*backSubstituteUpper(get_R(), rhs, false);
+	Vector soln = permutation_.transpose() * get_R().triangularView<Eigen::Upper>().solve(rhs);
 
 	// apply solution: inlined manually due to permutation
   size_t solnStart = 0;

gtsam/linear/GaussianFactorGraph.cpp

@@ -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(
+	GaussianFactorGraph::EliminationResult EliminatePreferLDL(
-	      const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
+			const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
 
-	    typedef JacobianFactor J;
+		typedef JacobianFactor J;
-	    typedef HessianFactor H;
+		typedef HessianFactor H;
 
-	    // If any JacobianFactors have constrained noise models, we have to convert
+		// If any JacobianFactors have constrained noise models, we have to convert
-	    // all factors to JacobianFactors.  Otherwise, we can convert all factors
+		// all factors to JacobianFactors.  Otherwise, we can convert all factors
-	    // to HessianFactors.  This is because QR can handle constrained noise
+		// to HessianFactors.  This is because QR can handle constrained noise
-	    // models but LDL cannot.
+		// models but LDL cannot.
 
-	    // Decide whether to use QR or LDL
+		// Decide whether to use QR or LDL
-	    // Check if any JacobianFactors have constrained noise models.
+		// Check if any JacobianFactors have constrained noise models.
-	    bool useQR = false;
+		bool useQR = false;
-	    BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
+		BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
-	      J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
+			J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
-	      if (jacobian && jacobian->get_model()->isConstrained()) {
+			if (jacobian && jacobian->get_model()->isConstrained()) {
-	        useQR = true;
+				useQR = true;
-	        break;
+				break;
-	      }
+			}
-	    }
+		}
 
-	    // Convert all factors to the appropriate type
+		// Convert all factors to the appropriate type
-	    // and call the type-specific EliminateGaussian.
+		// and call the type-specific EliminateGaussian.
-	    if (useQR) return EliminateQR(factors, nrFrontals);
+		if (useQR) return EliminateQR(factors, nrFrontals);
 
-	    GaussianFactorGraph::EliminationResult ret;
+		GaussianFactorGraph::EliminationResult ret;
-	    try {
+		try {
-	      tic(2, "EliminateLDL");
+			tic(2, "EliminateLDL");
-	      ret = EliminateLDL(factors, nrFrontals);
+			ret = EliminateLDL(factors, nrFrontals);
-	      toc(2, "EliminateLDL");
+			toc(2, "EliminateLDL");
-	    } catch (const exception& e) {
+		} catch (const exception& e) {
-	      cout << "Exception in EliminateLDL: " << e.what() << endl;
+			cout << "Exception in EliminateLDL: " << e.what() << endl;
-	      SETDEBUG("EliminateLDL", true);
+			SETDEBUG("EliminateLDL", true);
-	      SETDEBUG("updateATA", true);
+			SETDEBUG("updateATA", true);
-	      SETDEBUG("JacobianFactor::eliminate", true);
+			SETDEBUG("JacobianFactor::eliminate", true);
-	      SETDEBUG("JacobianFactor::Combine", true);
+			SETDEBUG("JacobianFactor::Combine", true);
-	      SETDEBUG("ldlPartial", true);
+			SETDEBUG("ldlPartial", true);
-	      SETDEBUG("findScatterAndDims", true);
+			SETDEBUG("findScatterAndDims", true);
-	      factors.print("Combining factors: ");
+			factors.print("Combining factors: ");
-	      EliminateLDL(factors, nrFrontals);
+			EliminateLDL(factors, nrFrontals);
-	      throw;
+			throw;
-	    }
+		}
 
-	    const bool checkLDL = ISDEBUG("EliminateGaussian Check LDL");
+		const bool checkLDL = ISDEBUG("EliminateGaussian Check LDL");
-	    if (checkLDL) {
+		if (checkLDL) {
-	      GaussianFactorGraph::EliminationResult expected;
+			GaussianFactorGraph::EliminationResult expected;
-	      FactorGraph<J> jacobians = convertToJacobians(factors);
+			FactorGraph<J> jacobians = convertToJacobians(factors);
-	      try {
+			try {
-	        // Compare with QR
+				// Compare with QR
-	        expected = EliminateJacobians(jacobians, nrFrontals);
+				expected = EliminateJacobians(jacobians, nrFrontals);
-	      } catch (...) {
+			} catch (...) {
-	        cout << "Exception in QR" << endl;
+				cout << "Exception in QR" << endl;
-	        throw;
+				throw;
-	      }
+			}
 
-	      H actual_factor(*ret.second);
+			H actual_factor(*ret.second);
-	      H expected_factor(*expected.second);
+			H expected_factor(*expected.second);
-	      if (!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal(
+			if (!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal(
-	          expected_factor, actual_factor, 1.0)) {
+					expected_factor, actual_factor, 1.0)) {
-	        cout << "LDL and QR do not agree" << endl;
+				cout << "LDL and QR do not agree" << endl;
 
-	        SETDEBUG("EliminateLDL", true);
+				SETDEBUG("EliminateLDL", true);
-	        SETDEBUG("updateATA", true);
+				SETDEBUG("updateATA", true);
-	        SETDEBUG("JacobianFactor::eliminate", true);
+				SETDEBUG("JacobianFactor::eliminate", true);
-	        SETDEBUG("JacobianFactor::Combine", true);
+				SETDEBUG("JacobianFactor::Combine", true);
-	        jacobians.print("Jacobian Factors: ");
+				jacobians.print("Jacobian Factors: ");
-	        EliminateJacobians(jacobians, nrFrontals);
+				EliminateJacobians(jacobians, nrFrontals);
-	        EliminateLDL(factors, nrFrontals);
+				EliminateLDL(factors, nrFrontals);
-	        factors.print("Combining factors: ");
+				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

gtsam/linear/GaussianFactorGraph.h

@@ -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);
 

gtsam/linear/NoiseModel.h

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

gtsam/linear/tests/testNoiseModel.cpp

@@ -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));
 }
 

gtsam/nonlinear/NonlinearFactor.h

@@ -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

gtsam/slam/GeneralSFMFactor.h

@@ -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
  *

gtsam/slam/PartialPriorFactor.h

@@ -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

gtsam/slam/ProjectionFactor.h

@@ -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
  */
 

tests/testGaussianFactorGraph.cpp

@@ -306,8 +306,8 @@ TEST( GaussianFactorGraph, eliminateAll )
 	GaussianBayesNet actual = *GaussianSequentialSolver(fg1).eliminate();
 	EXPECT(assert_equal(expected,actual,tol));
 
-  GaussianBayesNet actualET = *GaussianSequentialSolver(fg1).eliminate();
+  GaussianBayesNet actualQR = *GaussianSequentialSolver(fg1, true).eliminate();
-  EXPECT(assert_equal(expected,actualET,tol));
+  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);

tests/testInference.cpp

@@ -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(
+  		*GaussianSequentialSolver(GaussianFactorGraph(cbn)).jointFactorGraph(xvar)).eliminate();
-  				GaussianFactorGraph(cbn), true).jointFactorGraph(xvar), true).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);
-  GaussianMultifrontalSolver solver(*gfg, true);
   solver.marginalFactor(ordering[PointKey(0)]);
 }