error_vector and error unit-tested

cpp/UrbanFactor.cpp

@@ -11,7 +11,11 @@ namespace gtsam {
 
 	UrbanFactor::UrbanFactor() {
 		// TODO Auto-generated constructor stub
+	}
 
+	UrbanFactor::UrbanFactor(const Vector& z, const double sigma) :
+		NonlinearFactor<UrbanConfig> (z,sigma) {
+		// TODO
 	}
 
 	UrbanFactor::~UrbanFactor() {

cpp/UrbanFactor.h

@@ -13,20 +13,15 @@
 
 namespace gtsam {
 
+	/**
+	 * Base class for UrbanMeasurement and UrbanOdometry
+	 */
 	class UrbanFactor: public NonlinearFactor<UrbanConfig> {
 	public:
+
 		UrbanFactor();
+		UrbanFactor(const Vector& z, const double sigma);
 		virtual ~UrbanFactor();
-
-    /** Vector of errors */
-		Vector error_vector(const UrbanConfig& c) const { return zero(0); }
-
-    /** linearize to a GaussianFactor */
-    boost::shared_ptr<GaussianFactor> linearize(const UrbanConfig& c) const {
-    	boost::shared_ptr<GaussianFactor> factor(new GaussianFactor);
-    	return factor;
-    }
-
 	};
 
 }

cpp/UrbanGraph.cpp

@@ -33,9 +33,10 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	void UrbanGraph::addMeasurement(double x, double y, double sigma, int p1,
+	void UrbanGraph::addMeasurement(double x, double y, double sigma, int i, int j) {
-			int p2) {
+		Point2 z(x,y);
-		// TODO
+		sharedFactor factor(new UrbanMeasurement(z,sigma,i,j));
+		push_back(factor);
 	}
 	;
 

cpp/UrbanGraph.h

@@ -8,7 +8,7 @@
 #pragma once
 
 #include "NonlinearFactorGraph.h"
-#include "UrbanFactor.h"
+#include "UrbanMeasurement.h"
 
 namespace gtsam {
 
@@ -35,13 +35,12 @@ namespace gtsam {
 		/**
 		 *  Add a landmark constraint
 		 */
-		void addMeasurement(double x, double y, double sigma, int p1, int p2);
+		void addMeasurement(double x, double y, double sigma, int i, int j);
 
 		/**
 		 *  Add an odometry constraint
 		 */
-		void addOdometry(double dx, double yaw, double sigmadx, double sigmayaw,
+		void addOdometry(double dx, double yaw, double sigmadx, double sigmayaw, int i);
-				int p);
 
 		/**
 		 *  Add an initial constraint on the first pose

cpp/UrbanMeasurement.cpp

@@ -5,17 +5,34 @@
  * @author  Viorela Ila
  */
 
-#include "UrbanConfig.h"
 #include "UrbanMeasurement.h"
 #include "Pose3.h"
-#include "SimpleCamera.h"
 
 using namespace std;
+
 namespace gtsam {
-/** receives the 2D point in world coordinates and transforms it to Pose coordinates */
+
-Point3 transformPoint2_to(const Pose3& pose, const Point2& p);
+	/* ************************************************************************* */
-Matrix DtransformPoint2_to1(const Pose3& pose, const Point2& p);
+	Point2 transform_to(const Pose3& pose, const Point2& p) {
-Matrix DtransformPoint2_to2(const Pose3& pose); // does not depend on p !
+		// create a 3D point at our height (Z is assumed up)
+		Point3 global3D(p.x(),p.y(),pose.translation().z());
+		// transform into local 3D point
+		Point3 local3D = transform_to(pose,global3D);
+		// take x and y as the local measurement
+		Point2 local2D(local3D.x(),local3D.y());
+		return local2D;
+	}
+
+	/* ************************************************************************* */
+	Matrix Dtransform_to1(const Pose3& pose, const Point2& p) {
+		return zeros(2, 6);
+	}
+
+	/* ************************************************************************* */
+	Matrix Dtransform_to2(const Pose3& pose, const Point2& p) {
+		return zeros(2, 2);
+	}
+
 	/* ************************************************************************* */
 	UrbanMeasurement::UrbanMeasurement() {
 		/// Arbitrary values
@@ -26,12 +43,13 @@ UrbanMeasurement::UrbanMeasurement() {
 		keys_.push_back(robotPoseName_);
 		keys_.push_back(landmarkName_);
 	}
+
 	/* ************************************************************************* */
-UrbanMeasurement::UrbanMeasurement(const Point2& z, double sigma, int cn, int ln)
+	UrbanMeasurement::UrbanMeasurement(const Point2& z, double sigma, int i,
-: NonlinearFactor<UrbanConfig>(z.vector(), sigma)
+			int j) :
-  {
+		UrbanFactor(z.vector(), sigma) {
-	robotPoseNumber_ = cn;
+		robotPoseNumber_ = i;
-	landmarkNumber_    = ln;
+		landmarkNumber_ = j;
 		robotPoseName_ = symbol('x', robotPoseNumber_);
 		landmarkName_ = symbol('l', landmarkNumber_);
 		keys_.push_back(robotPoseName_);
@@ -40,35 +58,27 @@ UrbanMeasurement::UrbanMeasurement(const Point2& z, double sigma, int cn, int ln
 
 	/* ************************************************************************* */
 	void UrbanMeasurement::print(const std::string& s) const {
-  printf("%s %s %s\n", s.c_str(), robotPoseName_.c_str(), landmarkName_.c_str());
+		printf("%s %s %s\n", s.c_str(), robotPoseName_.c_str(),
+				landmarkName_.c_str());
 		gtsam::print(this->z_, s + ".z");
 	}
 
 	/* ************************************************************************* */
 	bool UrbanMeasurement::equals(const UrbanMeasurement& p, double tol) const {
 		if (&p == NULL) return false;
-  if (robotPoseNumber_ != p.robotPoseNumber_ || landmarkNumber_ != p.landmarkNumber_) return false;
+		if (robotPoseNumber_ != p.robotPoseNumber_ || landmarkNumber_
+				!= p.landmarkNumber_) return false;
 		if (!equal_with_abs_tol(this->z_, p.z_, tol)) return false;
 		return true;
 	}
 
-
-// the difference here that we have a 2d point in a 3D coordinate
-Vector UrbanMeasurement::predict(const UrbanConfig& c) const {
-  Pose3 pose = c.robotPose(robotPoseNumber_);
-  Point2 landmark = c.landmarkPoint(landmarkNumber_);
-  // TODO Implement predict function
-  Vector v;
-  return v;
-}
-
 	/* ************************************************************************* */
-Vector UrbanMeasurement::error_vector(const UrbanConfig& c) const {
+	Vector UrbanMeasurement::error_vector(const UrbanConfig& x) const {
+		Pose3 pose = x.robotPose(robotPoseNumber_);
+		Point2 landmark = x.landmarkPoint(landmarkNumber_);
 		// Right-hand-side b = (z - h(x))/sigma
-  Point2 h = predict(c);
+		Point2 hx = transform_to(pose,landmark);
-  // TODO Return z-h(x)
+		return (z_ - hx.vector());
-  Vector v;
-  return v;
 	}
 
 /* ************************************************************************* */

cpp/UrbanMeasurement.h

@@ -7,10 +7,7 @@
 
 #pragma once
 
-#include "NonlinearFactor.h"
+#include "UrbanFactor.h"
-#include "GaussianFactor.h"
-#include "Testable.h"
-#include "Point2.h"
 
 namespace gtsam {
 
@@ -19,8 +16,7 @@ class UrbanConfig;
 	/**
 	 * Non-linear factor for a constraint derived from a 2D measurement,
 	 */
-class UrbanMeasurement : public NonlinearFactor<UrbanConfig>, Testable<UrbanMeasurement>
+	class UrbanMeasurement: public UrbanFactor {
-{
 	private:
 
 		int robotPoseNumber_, landmarkNumber_;
@@ -47,8 +43,8 @@ public:
 		 *
 		 *
 		 */
-	UrbanMeasurement(const Point2& z, double sigma, int robotPoseNumber, int landmarkNumber);
+		UrbanMeasurement(const Point2& z, double sigma, int robotPoseNumber,
-
+				int landmarkNumber);
 
 		/**
 		 * print
@@ -61,11 +57,6 @@ public:
 		 */
 		bool equals(const UrbanMeasurement&, double tol = 1e-9) const;
 
-	/**
-	 * predict the measurement (is that update ??)
-	 */
-	Vector predict(const UrbanConfig&) const;
-
 		/**
 		 * calculate the error of the factor
 		 */
@@ -77,13 +68,14 @@ public:
 
 		GaussianFactor::shared_ptr linearize(const UrbanConfig&) const {
 			//TODO implement linearize
+		}
 
-	};
+		int getrobotPoseNumber() const {
-
+			return robotPoseNumber_;
-	int getrobotPoseNumber() const { return robotPoseNumber_; }
+		}
-	int getLandmarkNumber()    const { return landmarkNumber_;    }
+		int getLandmarkNumber() const {
-
+			return landmarkNumber_;
-
+		}
 
 	private:
 		/** Serialization function */
@@ -97,4 +89,11 @@ private:
 		}
 	};
 
-}
+	/**
+	 * Transform 2D landmark into 6D pose, and its derivatives
+	 */
+	Point2 transform_to(const Pose3& pose, const Point2& p);
+	Matrix Dtransform_to1(const Pose3& pose, const Point2& p);
+	Matrix Dtransform_to2(const Pose3& pose);
+
+} // namespace gtsam

cpp/testUrbanGraph.cpp

@@ -21,7 +21,7 @@ typedef NonlinearOptimizer<UrbanGraph,UrbanConfig> Optimizer;
 
 /* ************************************************************************* */
 
-Point2 landmark1(  2,  5);
+Point2 landmark(  2,  5);
 Point2 landmark2(  2, 10);
 Point2 landmark3( -2,  5);
 Point2 landmark4( -2,-10);
@@ -29,7 +29,7 @@ Point2 landmark4( -2,-10);
 /* Robot is at (0,0,0) looking in global "y" direction.
  * For the local frame we used Navlab convention,
  * x-looks forward, y-looks right, z- down*/
-Pose3 robotPose1(Matrix_(3,3,
+Pose3 robotPose(Matrix_(3,3,
 		       0., 1., 0.,
 		       1., 0., 0.,
 		       0., 0.,-1.
@@ -45,8 +45,28 @@ Pose3 robotPose2(Matrix_(3,3,
 	      Point3(0,1,0));
 
 /* ************************************************************************* */
-/* the measurements are relative to the robot pose so
+TEST( UrbanGraph, addMeasurement)
- * they are in robot coordinate frame different from the global (above)*/
+{
+	// Check adding a measurement
+  UrbanGraph g;
+  double sigma = 0.2;// 20 cm
+  g.addMeasurement(4, 2, sigma, 1, 1);
+  LONGS_EQUAL(1,g.size());
+
+	// Create a config
+	UrbanConfig config;
+	config.addRobotPose(1,robotPose);
+	config.addLandmark(1,landmark);
+
+  // Check error
+	double expected = 0.5/sigma/sigma;
+  DOUBLES_EQUAL(expected, g.error(config), 1e-9);
+}
+
+/* ************************************************************************* *
+ * the measurements are relative to the robot pose so
+ * they are in robot coordinate frame different from the global (above)
+ *
 UrbanGraph testGraph() {
 
   double sigma = 0.2;// 20 cm
@@ -68,7 +88,7 @@ UrbanGraph testGraph() {
   return g;
 }
 
-/* ************************************************************************* */
+/* ************************************************************************* *
 TEST( UrbanGraph, optimizeLM)
 {
   // build a graph
@@ -76,9 +96,9 @@ TEST( UrbanGraph, optimizeLM)
 
   // Create an initial configuration corresponding to the ground truth
   boost::shared_ptr<UrbanConfig> initialEstimate(new UrbanConfig);
-  initialEstimate->addRobotPose(1, robotPose1);
+  initialEstimate->addRobotPose(1, robotPose);
   initialEstimate->addRobotPose(2, robotPose2);
-  initialEstimate->addLandmark(1, landmark1);
+  initialEstimate->addLandmark(1, landmark);
   initialEstimate->addLandmark(2, landmark2);
   initialEstimate->addLandmark(3, landmark3);
   initialEstimate->addLandmark(4, landmark4);

cpp/testUrbanMeasurement.cpp

@@ -6,13 +6,53 @@
 
 #include "UrbanConfig.h"
 #include "UrbanMeasurement.h"
+#include "Vector.h"
 
 using namespace std;
 using namespace gtsam;
 
+Point2 landmark(2,5);
+
+/* Robot is at (0,0,0) looking in global "y" direction.
+ * For the local frame we used Navlab convention,
+ * x-looks forward, y-looks right, z- down*/
+Pose3 robotPose(Matrix_(3,3,
+		       0., 1., 0.,
+		       1., 0., 0.,
+		       0., 0.,-1.
+		       ),
+	      Point3(0,0,0));
+
 /* ************************************************************************* */
-TEST( UrbanMeasurement, linearize )
+TEST( UrbanMeasurement, transform_to )
 {
+	Point2 expected(5,2);
+	Point2 actual = transform_to(robotPose, landmark);
+	CHECK(assert_equal(expected,actual));
+}
+
+/* ************************************************************************* */
+TEST( UrbanMeasurement, error_vector )
+{
+	// Create a measurement, no-noise measurement would yield 5,2
+	Point2 z(4,2);
+	double sigma = 0.2;
+	UrbanMeasurement factor(z,sigma,44,66);
+
+	// Create a config
+	UrbanConfig config;
+	config.addRobotPose(44,robotPose);
+	config.addLandmark(66,landmark);
+
+	// test error_vector
+	Vector expected = Vector_(2, -1.0, 0.0);
+	Vector actual = factor.error_vector(config);
+	CHECK(assert_equal(expected,actual));
+
+	// test error
+	double expected2 = 12.5; // 0.5 * 5^2
+	double actual2 = factor.error(config);
+	DOUBLES_EQUAL(expected2,actual2,1e-9);
 }
 
 /* ************************************************************************* */