Merge branch 'master' into wrap_mods_local

gtsam/slam/visualSLAM.cpp

@@ -31,7 +31,7 @@ namespace visualSLAM {
     if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K");
     if (Z.cols() != J.size()) throw std::invalid_argument(
           "insertBackProjections: J and Z must have same number of entries");
-    for(size_t k=0;k<Z.cols();k++) {
+    for(int k=0;k<Z.cols();k++) {
       Point2 p(Z(0,k),Z(1,k));
       Point3 P = camera.backproject(p, depth);
       insertPoint(J(k), P);
@@ -82,7 +82,7 @@ namespace visualSLAM {
     if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K");
     if (Z.cols() != J.size()) throw std::invalid_argument(
           "addMeasurements: J and Z must have same number of entries");
-    for (size_t k = 0; k < Z.cols(); k++)
+    for (int k = 0; k < Z.cols(); k++)
       addMeasurement(Point2(Z(0, k), Z(1, k)), model, i, J(k), K);
   }
 

gtsam_unstable/nonlinear/LinearContainerFactor.cpp

@@ -0,0 +1,148 @@
+/**
+ * @file LinearContainerFactor.cpp
+ *
+ * @date Jul 6, 2012
+ * @author Alex Cunningham
+ */
+
+#include <gtsam_unstable/nonlinear/LinearContainerFactor.h>
+
+#include <boost/foreach.hpp>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+void LinearContainerFactor::rekeyFactor(const Ordering& ordering) {
+	Ordering::InvertedMap invOrdering = ordering.invert(); // TODO: inefficient - make more selective ordering invert
+	rekeyFactor(invOrdering);
+}
+
+/* ************************************************************************* */
+void LinearContainerFactor::rekeyFactor(const Ordering::InvertedMap& invOrdering) {
+	BOOST_FOREACH(Index& idx, factor_->keys()) {
+		Key fullKey = invOrdering.find(idx)->second;
+		idx = fullKey;
+		keys_.push_back(fullKey);
+	}
+}
+
+/* ************************************************************************* */
+LinearContainerFactor::LinearContainerFactor(
+		const JacobianFactor& factor, const Ordering& ordering)
+: factor_(factor.clone()) {
+	rekeyFactor(ordering);
+}
+
+/* ************************************************************************* */
+LinearContainerFactor::LinearContainerFactor(
+		const HessianFactor& factor, const Ordering& ordering)
+: factor_(factor.clone()) {
+	rekeyFactor(ordering);
+}
+
+/* ************************************************************************* */
+LinearContainerFactor::LinearContainerFactor(
+		const GaussianFactor::shared_ptr& factor, const Ordering& ordering)
+: factor_(factor->clone()) {
+	rekeyFactor(ordering);
+}
+
+/* ************************************************************************* */
+LinearContainerFactor::LinearContainerFactor(
+		const GaussianFactor::shared_ptr& factor)
+: factor_(factor->clone())
+{
+}
+
+/* ************************************************************************* */
+LinearContainerFactor::LinearContainerFactor(const JacobianFactor& factor,
+		const Ordering::InvertedMap& inverted_ordering)
+: factor_(factor.clone()) {
+	rekeyFactor(inverted_ordering);
+}
+
+/* ************************************************************************* */
+LinearContainerFactor::LinearContainerFactor(const HessianFactor& factor,
+		const Ordering::InvertedMap& inverted_ordering)
+: factor_(factor.clone()) {
+	rekeyFactor(inverted_ordering);
+}
+
+/* ************************************************************************* */
+LinearContainerFactor::LinearContainerFactor(
+		const GaussianFactor::shared_ptr& factor,
+		const Ordering::InvertedMap& ordering)
+: factor_(factor->clone()) {
+	rekeyFactor(ordering);
+}
+
+/* ************************************************************************* */
+void LinearContainerFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
+	Base::print(s+"LinearContainerFactor", keyFormatter);
+	if (factor_)
+		factor_->print("   Stored Factor", keyFormatter);
+}
+
+/* ************************************************************************* */
+bool LinearContainerFactor::equals(const NonlinearFactor& f, double tol) const {
+	const LinearContainerFactor* jcf = dynamic_cast<const LinearContainerFactor*>(&f);
+	return jcf && factor_->equals(*jcf->factor_, tol) && NonlinearFactor::equals(f);
+}
+
+/* ************************************************************************* */
+double LinearContainerFactor::error(const Values& c) const {
+	//	VectorValues vecvalues;
+	//	// FIXME: add values correctly here
+	//	return factor_.error(vecvalues);
+		return 0; // FIXME: placeholder
+}
+
+/* ************************************************************************* */
+size_t LinearContainerFactor::dim() const {
+	if (isJacobian())
+		return toJacobian()->get_model()->dim();
+	else
+		return 1; // Hessians don't have true dimension
+}
+
+/* ************************************************************************* */
+boost::shared_ptr<GaussianFactor>
+LinearContainerFactor::linearize(const Values& c, const Ordering& ordering) const {
+	// clone factor
+	boost::shared_ptr<GaussianFactor> result = factor_->clone();
+
+	// rekey
+	BOOST_FOREACH(Index& key, result->keys())
+		key = ordering[key];
+
+	return result;
+}
+
+/* ************************************************************************* */
+bool LinearContainerFactor::isJacobian() const {
+	return boost::shared_dynamic_cast<JacobianFactor>(factor_);
+}
+
+/* ************************************************************************* */
+JacobianFactor::shared_ptr LinearContainerFactor::toJacobian() const {
+	return boost::shared_dynamic_cast<JacobianFactor>(factor_);
+}
+
+/* ************************************************************************* */
+HessianFactor::shared_ptr LinearContainerFactor::toHessian() const {
+	return boost::shared_dynamic_cast<HessianFactor>(factor_);
+}
+
+/* ************************************************************************* */
+GaussianFactor::shared_ptr LinearContainerFactor::negate(const Ordering& ordering) const {
+	GaussianFactor::shared_ptr result = factor_->negate();
+	BOOST_FOREACH(Key& key, result->keys())
+		key = ordering[key];
+	return result;
+}
+
+/* ************************************************************************* */
+} // \namespace gtsam
+
+
+

gtsam_unstable/nonlinear/LinearContainerFactor.h

@@ -0,0 +1,113 @@
+/**
+ * @file LinearContainerFactor.h
+ *
+ * @brief Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph
+ * 
+ * @date Jul 6, 2012
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <gtsam/linear/HessianFactor.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/**
+ * Dummy version of a generic linear factor to be injected into a nonlinear factor graph
+ */
+class LinearContainerFactor : public NonlinearFactor {
+protected:
+
+	GaussianFactor::shared_ptr factor_;
+
+public:
+
+	/** Primary constructor: store a linear factor and decode the ordering */
+	LinearContainerFactor(const JacobianFactor& factor, const Ordering& ordering);
+
+	/** Primary constructor: store a linear factor and decode the ordering */
+	LinearContainerFactor(const HessianFactor& factor, const Ordering& ordering);
+
+	/** Constructor from shared_ptr */
+	LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering);
+
+	/** Constructor from re-keyed factor: all indices assumed replaced with Key */
+	LinearContainerFactor(const GaussianFactor::shared_ptr& factor);
+
+	/** Alternate constructor: store a linear factor and decode keys with inverted ordering*/
+	LinearContainerFactor(const JacobianFactor& factor,
+			const Ordering::InvertedMap& inverted_ordering);
+
+	/** Alternate constructor: store a linear factor and decode keys with inverted ordering*/
+	LinearContainerFactor(const HessianFactor& factor,
+			const Ordering::InvertedMap& inverted_ordering);
+
+	/** Constructor from shared_ptr with inverted ordering*/
+	LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
+			const Ordering::InvertedMap& ordering);
+
+	// Access
+
+	const GaussianFactor::shared_ptr& factor() const { return factor_; }
+
+	// Testable
+
+  /** print */
+  void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
+
+  /** Check if two factors are equal */
+  bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
+
+	// NonlinearFactor
+
+  /**
+   * Calculate the error of the factor: uses the underlying linear factor to compute ordering
+   */
+  double error(const Values& c) const;
+
+  /** get the dimension of the factor: rows of linear factor */
+  size_t dim() const;
+
+  /** linearize to a GaussianFactor: values has no effect, just clones/rekeys underlying factor */
+  boost::shared_ptr<GaussianFactor>
+  linearize(const Values& c, const Ordering& ordering) const;
+
+  /**
+   * Creates an anti-factor directly and performs rekeying due to ordering
+   */
+  GaussianFactor::shared_ptr negate(const Ordering& ordering) const;
+
+  /**
+   * Creates a shared_ptr clone of the factor - needs to be specialized to allow
+   * for subclasses
+   *
+   * Clones the underlying linear factor
+   */
+  NonlinearFactor::shared_ptr clone() const {
+  	return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_));
+  }
+
+  // casting syntactic sugar
+
+  /**
+   * Simple check whether this is a Jacobian or Hessian factor
+   */
+  bool isJacobian() const;
+
+  /** Casts to JacobianFactor */
+  JacobianFactor::shared_ptr toJacobian() const;
+
+  /** Casts to HessianFactor */
+  HessianFactor::shared_ptr toHessian() const;
+
+protected:
+  void rekeyFactor(const Ordering& ordering);
+  void rekeyFactor(const Ordering::InvertedMap& invOrdering);
+
+}; // \class LinearContainerFactor
+
+} // \namespace gtsam
+
+

gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp

@@ -0,0 +1,113 @@
+/**
+ * @file testLinearContainerFactor.cpp
+ *
+ * @date Jul 6, 2012
+ * @author Alex Cunningham
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <gtsam_unstable/nonlinear/LinearContainerFactor.h>
+
+#include <gtsam/geometry/Pose2.h>
+
+using namespace gtsam;
+
+const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0));
+const double tol = 1e-5;
+
+gtsam::Key	l1 = 101, l2 = 102, x1 = 1, x2 = 2;
+
+Point2 landmark1(5.0, 1.5), landmark2(7.0, 1.5);
+Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0);
+
+/* ************************************************************************* */
+TEST( testLinearContainerFactor, generic_jacobian_factor ) {
+
+	Ordering initOrdering; initOrdering += x1, x2, l1, l2;
+
+	JacobianFactor expLinFactor1(
+			initOrdering[l1],
+			Matrix_(2,2,
+					 2.74222, -0.0067457,
+							 0.0,  2.63624),
+			initOrdering[l2],
+			Matrix_(2,2,
+					-0.0455167, -0.0443573,
+					-0.0222154, -0.102489),
+			Vector_(2, 0.0277052,
+								 -0.0533393),
+			diag_model2);
+
+	LinearContainerFactor actFactor1(expLinFactor1, initOrdering);
+	Values values;
+	values.insert(l1, landmark1);
+	values.insert(l2, landmark2);
+	values.insert(x1, poseA1);
+	values.insert(x2, poseA2);
+
+	// Check reconstruction from same ordering
+	GaussianFactor::shared_ptr actLinearizationA = actFactor1.linearize(values, initOrdering);
+	EXPECT(assert_equal(*expLinFactor1.clone(), *actLinearizationA, tol));
+
+	// Check reconstruction from new ordering
+	Ordering newOrdering; newOrdering += x1, l1, x2, l2;
+	GaussianFactor::shared_ptr actLinearizationB = actFactor1.linearize(values, newOrdering);
+
+	JacobianFactor expLinFactor2(
+			newOrdering[l1],
+			Matrix_(2,2,
+					 2.74222, -0.0067457,
+							 0.0,  2.63624),
+			newOrdering[l2],
+			Matrix_(2,2,
+					-0.0455167, -0.0443573,
+					-0.0222154, -0.102489),
+			Vector_(2, 0.0277052,
+								 -0.0533393),
+			diag_model2);
+
+	EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol));
+}
+
+/* ************************************************************************* */
+TEST( testLinearContainerFactor, generic_hessian_factor ) {
+  Matrix G11 = Matrix_(1,1, 1.0);
+  Matrix G12 = Matrix_(1,2, 2.0, 4.0);
+  Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0);
+
+  Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
+  Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, 1.0, 2.0, 4.0);
+
+  Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0);
+
+  Vector g1 = Vector_(1, -7.0);
+  Vector g2 = Vector_(2, -8.0, -9.0);
+  Vector g3 = Vector_(3,  1.0,  2.0,  3.0);
+
+  double f = 10.0;
+
+	Ordering initOrdering; initOrdering += x1, x2, l1, l2;
+  HessianFactor initFactor(initOrdering[x1], initOrdering[x2], initOrdering[l1],
+  		G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
+
+	Values values;
+	values.insert(l1, landmark1);
+	values.insert(l2, landmark2);
+	values.insert(x1, poseA1);
+	values.insert(x2, poseA2);
+
+  LinearContainerFactor actFactor(initFactor, initOrdering);
+	GaussianFactor::shared_ptr actLinearization1 = actFactor.linearize(values, initOrdering);
+	EXPECT(assert_equal(*initFactor.clone(), *actLinearization1, tol));
+
+	Ordering newOrdering; newOrdering += l1, x1, x2, l2;
+  HessianFactor expLinFactor(newOrdering[x1], newOrdering[x2], newOrdering[l1],
+  		G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
+  GaussianFactor::shared_ptr actLinearization2 = actFactor.linearize(values, newOrdering);
+ 	EXPECT(assert_equal(*expLinFactor.clone(), *actLinearization2, tol));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */

tests/testGaussianISAM2.cpp

@@ -431,7 +431,7 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, cons
 	bool totalGradOk = assert_equal(expectedGradient, actualGradient);
 	EXPECT(totalGradOk);
 
-	return nodeGradientsOk && expectedGradOk && totalGradOk;
+	return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual;
 }
 
 /* ************************************************************************* */