Moved DummyFactor to gtsam_unstable from MastSLAM - allows for adding sufficient connectivity for solvers to operate

gtsam_unstable/gtsam_unstable.h

@@ -199,4 +199,9 @@ virtual class RelativeElevationFactor: gtsam::NonlinearFactor {
 	void print(string s) const;
 };
 
+#include <gtsam_unstable/slam/DummyFactor.h>
+virtual class DummyFactor : gtsam::NonlinearFactor {
+	DummyFactor(size_t key1, size_t dim1, size_t key2, size_t dim2);
+};
+
 } //\namespace gtsam

gtsam_unstable/slam/DummyFactor.cpp

@@ -0,0 +1,64 @@
+/**
+ * @file DummyFactor.cpp
+ *
+ * @date Sep 10, 2012
+ * @author Alex Cunningham
+ */
+
+#include <gtsam_unstable/slam/DummyFactor.h>
+
+#include <boost/foreach.hpp>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+DummyFactor::DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2)
+: NonlinearFactor(key1, key2)
+{
+	dims_.push_back(dim1);
+	dims_.push_back(dim2);
+	if (dim1 > dim2)
+		rowDim_ = dim1;
+	else
+		rowDim_ = dim2;
+}
+
+/* ************************************************************************* */
+void DummyFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
+	std::cout << s << "  DummyFactor dim = " << rowDim_ << ", keys = { ";
+	BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; }
+	std::cout << "}" << std::endl;
+}
+
+/* ************************************************************************* */
+bool DummyFactor::equals(const NonlinearFactor& f, double tol) const {
+  const DummyFactor* e = dynamic_cast<const DummyFactor*>(&f);
+  return e && Base::equals(f, tol) && dims_ == e->dims_ && rowDim_ == e->rowDim_;
+}
+
+/* ************************************************************************* */
+boost::shared_ptr<GaussianFactor>
+DummyFactor::linearize(const Values& c, const Ordering& ordering) const {
+	// Only linearize if the factor is active
+	if (!this->active(c))
+		return boost::shared_ptr<JacobianFactor>();
+
+	 // Fill in terms with zero matrices
+  std::vector<std::pair<Index, Matrix> > terms(this->size());
+  for(size_t j=0; j<this->size(); ++j) {
+    terms[j].first = ordering[this->keys()[j]];
+    terms[j].second = zeros(rowDim_, dims_[j]);
+  }
+
+  noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(rowDim_);
+  return GaussianFactor::shared_ptr(
+  		new JacobianFactor(terms, zero(rowDim_), model));
+}
+
+/* ************************************************************************* */
+
+} // \namespace gtsam
+
+
+
+

gtsam_unstable/slam/DummyFactor.h

@@ -0,0 +1,76 @@
+/**
+ * @file DummyFactor.h
+ *
+ * @brief A simple factor that can be used to trick gtsam solvers into believing a graph is connected.
+ * 
+ * @date Sep 10, 2012
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+class DummyFactor : public NonlinearFactor {
+protected:
+
+	// Store the dimensions of the variables and the dimension of the full system
+	std::vector<size_t> dims_;
+	size_t rowDim_; ///< choose dimension for the rows
+
+public:
+
+	/** Default constructor: don't use directly */
+	DummyFactor() : rowDim_(1) { }
+
+	/** standard binary constructor */
+	DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2);
+
+	virtual ~DummyFactor() {}
+
+	// testable
+
+  /** print */
+  virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+
+  /** Check if two factors are equal */
+  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
+
+  // access
+
+  const std::vector<size_t>& dims() const { return dims_; }
+
+  // factor interface
+
+  /**
+   * Calculate the error of the factor - zero for dummy factors
+   */
+  virtual double error(const Values& c) const { return 0.0; }
+
+  /** get the dimension of the factor (number of rows on linearization) */
+  virtual size_t dim() const { return rowDim_; }
+
+  /** linearize to a GaussianFactor */
+  virtual boost::shared_ptr<GaussianFactor>
+  linearize(const Values& c, const Ordering& ordering) const;
+
+  /**
+   * Creates a shared_ptr clone of the factor - needs to be specialized to allow
+   * for subclasses
+   *
+   * By default, throws exception if subclass does not implement the function.
+   */
+  virtual NonlinearFactor::shared_ptr clone() const {
+	  return boost::static_pointer_cast<NonlinearFactor>(
+	      NonlinearFactor::shared_ptr(new DummyFactor(*this)));
+  }
+
+};
+
+} // \namespace gtsam
+
+
+
+

gtsam_unstable/slam/tests/testDummyFactor.cpp

@@ -0,0 +1,58 @@
+/**
+ * @file testDummyFactor.cpp
+ *
+ * @brief A simple dummy nonlinear factor that can be used to enforce connectivity
+ * without adding any real information
+ *
+ * @date Sep 10, 2012
+ * @author Alex Cunningham
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <gtsam/geometry/Point3.h>
+
+#include <gtsam_unstable/slam/DummyFactor.h>
+
+using namespace gtsam;
+
+const double tol = 1e-9;
+
+/* ************************************************************************* */
+TEST( testDummyFactor, basics ) {
+
+	gtsam::Key key1 = 7, key2 = 9;
+	size_t dim1 = 3, dim2 = 3;
+	DummyFactor dummyfactor(key1, dim1, key2, dim2);
+
+	// verify contents
+	LONGS_EQUAL(2, dummyfactor.size());
+	EXPECT_LONGS_EQUAL(key1, dummyfactor.keys()[0]);
+	EXPECT_LONGS_EQUAL(key2, dummyfactor.keys()[1]);
+
+	LONGS_EQUAL(2, dummyfactor.dims().size());
+	EXPECT_LONGS_EQUAL(dim1, dummyfactor.dims()[0]);
+	EXPECT_LONGS_EQUAL(dim2, dummyfactor.dims()[1]);
+
+	Values c;
+	c.insert(key1, Point3(1.0, 2.0, 3.0));
+	c.insert(key2, Point3(4.0, 5.0, 6.0));
+
+	// errors - all zeros
+	DOUBLES_EQUAL(0.0, dummyfactor.error(c), tol);
+
+	Ordering ordering;
+	ordering += key1, key2;
+
+	// linearization: all zeros
+	GaussianFactor::shared_ptr actLinearization = dummyfactor.linearize(c, ordering);
+	CHECK(actLinearization);
+	noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3);
+	GaussianFactor::shared_ptr expLinearization(new JacobianFactor(
+			ordering[key1], zeros(3,3), ordering[key2], zeros(3,3), zero(3), model3));
+	EXPECT(assert_equal(*expLinearization, *actLinearization, tol));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */