Merge branch 'master' into retraction_name

gtsam/inference/FactorGraph-inl.h

@@ -22,11 +22,9 @@
 
 #pragma once
 
-#include <stdio.h>
-#include <list>
-#include <sstream>
-#include <stdexcept>
-#include <functional>
+#include <gtsam/inference/FactorGraph.h>
+#include <gtsam/inference/graph-inl.h>
+#include <gtsam/base/DSF.h>
 
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
@@ -36,9 +34,11 @@
 #include <boost/graph/prim_minimum_spanning_tree.hpp>
 #include <boost/iterator/transform_iterator.hpp>
 
-#include <gtsam/inference/FactorGraph.h>
-#include <gtsam/inference/graph-inl.h>
-#include <gtsam/base/DSF.h>
+#include <stdio.h>
+#include <list>
+#include <sstream>
+#include <stdexcept>
+#include <functional>
 
 #define INSTANTIATE_FACTOR_GRAPH(F) \
   template class FactorGraph<F>; \
@@ -55,7 +55,7 @@ namespace gtsam {
 		cout << "size: " << size() << endl;
 		for (size_t i = 0; i < factors_.size(); i++) {
 			stringstream ss;
-			ss << "factor " << i;
+			ss << "factor " << i << ": ";
 			if (factors_[i] != NULL) factors_[i]->print(ss.str());
 		}
 	}

gtsam/nonlinear/NonlinearFactorGraph-inl.h

@@ -19,11 +19,11 @@
 
 #pragma once
 
-#include <cmath>
-#include <boost/foreach.hpp>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/inference/FactorGraph-inl.h>
 #include <gtsam/inference/inference.h>
+#include <boost/foreach.hpp>
+#include <cmath>
 
 #define INSTANTIATE_NONLINEAR_FACTOR_GRAPH(C) \
   INSTANTIATE_FACTOR_GRAPH(NonlinearFactor<C>); \
@@ -57,7 +57,6 @@ namespace gtsam {
 		return total_error;
 	}
 
-	/* ************************************************************************* */
 	/* ************************************************************************* */
 	template<class VALUES>
 	std::set<Symbol> NonlinearFactorGraph<VALUES>::keys() const {

gtsam/slam/PriorFactor.h

@@ -15,8 +15,8 @@
  **/
 #pragma once
 
-#include <gtsam/base/Testable.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/base/Testable.h>
 
 namespace gtsam {
 
@@ -63,7 +63,7 @@ namespace gtsam {
 
 		/** print */
 		virtual void print(const std::string& s) const {
-			std::cout << s << ": PriorFactor(" << (std::string) this->key_ << ")\n";
+			std::cout << s << "PriorFactor(" << (std::string) this->key_ << ")\n";
 			prior_.print("  prior");
 			this->noiseModel_->print("  noise model");
 		}

gtsam/slam/tests/testPose2SLAM.cpp

@@ -10,28 +10,28 @@
  * -------------------------------------------------------------------------- */
 
 /**
- *  @file  testPose2Graph.cpp
+ *  @file  testPose2SLAM.cpp
  *  @author Frank Dellaert, Viorela Ila
  **/
 
-#include <iostream>
+// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
+#define GTSAM_MAGIC_KEY
+
+#include <gtsam/slam/pose2SLAM.h>
+#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
+#include <gtsam/inference/FactorGraph-inl.h>
+#include <gtsam/base/numericalDerivative.h>
+using namespace gtsam;
+
+#include <CppUnitLite/TestHarness.h>
+
 #include <boost/shared_ptr.hpp>
 #include <boost/assign/std/list.hpp>
 using namespace boost;
 using namespace boost::assign;
 
-#include <CppUnitLite/TestHarness.h>
-
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
-#define GTSAM_MAGIC_KEY
-
-#include <gtsam/base/numericalDerivative.h>
-#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
-#include <gtsam/inference/FactorGraph-inl.h>
-#include <gtsam/slam/pose2SLAM.h>
-
+#include <iostream>
 using namespace std;
-using namespace gtsam;
 
 // common measurement covariance
 static double sx=0.5, sy=0.5,st=0.1;
@@ -44,11 +44,32 @@ static noiseModel::Gaussian::shared_ptr covariance(
 //static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3));
 
 /* ************************************************************************* */
-TEST( Pose2Graph, constructor )
+Vector f(const Pose2& pose1, const Pose2& pose2) {
+	Pose2 z(2,2,M_PI_2);
+	Pose2Factor constraint(1, 2, z, covariance);
+	return constraint.evaluateError(pose1, pose2);
+}
+
+TEST( Pose2SLAM, constraint )
+{
+	// create a factor between unknown poses p1 and p2
+	Pose2 pose1, pose2(2,2,M_PI_2);
+	Pose2Factor constraint(1, 2, pose2, covariance);
+	Matrix H1, H2;
+	Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
+
+	Matrix numericalH1 = numericalDerivative21(&f , pose1, pose2);
+	EXPECT(assert_equal(numericalH1,H1,5e-3));
+
+	Matrix numericalH2 = numericalDerivative22(&f , pose1, pose2);
+	EXPECT(assert_equal(numericalH2,H2));
+}
+
+/* ************************************************************************* */
+TEST( Pose2SLAM, constructor )
 {
 	// create a factor between unknown poses p1 and p2
 	Pose2 measured(2,2,M_PI_2);
-	Pose2Factor constraint(1,2,measured, covariance);
 	Pose2Graph graph;
 	graph.addConstraint(1,2,measured, covariance);
 	// get the size of the graph
@@ -56,11 +77,10 @@ TEST( Pose2Graph, constructor )
 	// verify
 	size_t expected = 1;
 	CHECK(actual == expected);
-
 }
 
 /* ************************************************************************* */
-TEST( Pose2Graph, linearization )
+TEST( Pose2SLAM, linearization )
 {
 	// create a factor between unknown poses p1 and p2
 	Pose2 measured(2,2,M_PI_2);