Cleaned up and added test for VSLAMFactor

cpp/VSLAMFactor.cpp

@@ -4,16 +4,17 @@
  * @author  Alireza Fathi
  */
 
+#include "VectorConfig.h"
 #include "VSLAMFactor.h"
+#include "Pose3.h"
 #include "SimpleCamera.h"
 
 using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
-template <class Config>
+VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K)
-VSLAMFactor<Config>::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K)
+  : NonlinearFactor<VectorConfig>(z, sigma)
-  : NonlinearFactor<Config>(z, sigma)
 {
   cameraFrameNumber_ = cn;
   landmarkNumber_ = ln;
@@ -28,27 +29,36 @@ VSLAMFactor<Config>::VSLAMFactor(const Vector& z, double sigma, int cn, int ln,
 }
 
 /* ************************************************************************* */
-template <class Config>
+void VSLAMFactor::print(const std::string& s) const {
-void VSLAMFactor<Config>::print(const std::string& s) const {
   printf("%s %s %s\n", s.c_str(), cameraFrameName_.c_str(), landmarkName_.c_str());
-  ::print(ConvenientFactor::z_, s+".z");
+  ::print(this->z_, s+".z");
 }
 
 /* ************************************************************************* */
-template <class Config>
+bool VSLAMFactor::equals(const NonlinearFactor<VectorConfig>& f, double tol) const {
-Vector VSLAMFactor<Config>::error_vector(const Config& c) const {
+  const VSLAMFactor* p = dynamic_cast<const VSLAMFactor*>(&f);
+  if (p == NULL) return false;
+  if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) return false;
+  if (!equal_with_abs_tol(this->z_,p->z_,tol)) return false;
+  return true;
+}
+
+/* ************************************************************************* */
+Vector VSLAMFactor::predict(const VectorConfig& c) const {
   Pose3  pose     = c[cameraFrameName_];
   Point3 landmark = c[landmarkName_];
-
+  return project(SimpleCamera(K_,pose), landmark).vector();
-  // Right-hand-side b = (z - h(x))/sigma
-  Vector h = project(SimpleCamera(K_,pose), landmark).vector();
-
-  return (ConvenientFactor::z_ - h);
 }
 
 /* ************************************************************************* */
-template <class Config>
+Vector VSLAMFactor::error_vector(const VectorConfig& c) const {
-LinearFactor::shared_ptr VSLAMFactor<Config>::linearize(const Config& c) const
+  // Right-hand-side b = (z - h(x))/sigma
+  Vector h = predict(c);
+  return (this->z_ - h);
+}
+
+/* ************************************************************************* */
+LinearFactor::shared_ptr VSLAMFactor::linearize(const VectorConfig& c) const
 {
   // get arguments from config
   Pose3  pose     = c[cameraFrameName_]; // cast from Vector to Pose3 !!!
@@ -62,27 +72,12 @@ LinearFactor::shared_ptr VSLAMFactor<Config>::linearize(const Config& c) const
 
   // Right-hand-side b = (z - h(x))
   Vector h = project(camera, landmark).vector();
-  Vector b = ConvenientFactor::z_ - h;
+  Vector b = this->z_ - h;
 
   // Make new linearfactor, divide by sigma
   LinearFactor::shared_ptr
-    p(new LinearFactor(cameraFrameName_, Dh1, landmarkName_, Dh2, b, ConvenientFactor::sigma_));
+    p(new LinearFactor(cameraFrameName_, Dh1, landmarkName_, Dh2, b, this->sigma_));
   return p;
 }
 
 /* ************************************************************************* */
-template <class Config>
-bool VSLAMFactor<Config>::equals(const NonlinearFactor<Config>& f, double tol) const {
-  const VSLAMFactor* p = dynamic_cast<const VSLAMFactor*>(&f);
-  if (p == NULL) goto fail;
-  if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail;
-  if (!equal_with_abs_tol(ConvenientFactor::z_,p->z_,tol)) goto fail;
-  return true;
-
- fail:
-  print("actual");
-  p->print("expected");
-  return false;
-}
-
-/* ************************************************************************* */

cpp/VSLAMFactor.h

@@ -8,26 +8,25 @@
 
 #include "NonlinearFactor.h"
 #include "LinearFactor.h"
-#include "VectorConfig.h"
 #include "Cal3_S2.h"
-#include "Pose3.h"
 
 namespace gtsam {
 
+class VectorConfig;
 
 /**
  * Non-linear factor for a constraint derived from a 2D measurement,
  * i.e. the main building block for visual SLAM.
  */
-template <class Config>
+class VSLAMFactor : public NonlinearFactor<VectorConfig>
-class VSLAMFactor : public NonlinearFactor<Config>
 {
-
  private:
-  int cameraFrameNumber_, landmarkNumber_;
+
+	int cameraFrameNumber_, landmarkNumber_;
   std::string cameraFrameName_, landmarkName_;
   Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this.
-  typedef gtsam::NonlinearFactor<Config> ConvenientFactor;
+  typedef gtsam::NonlinearFactor<VectorConfig> ConvenientFactor;
+
  public:
 
   typedef boost::shared_ptr<VSLAMFactor> shared_ptr; // shorthand for a smart pointer to a factor
@@ -49,20 +48,25 @@ class VSLAMFactor : public NonlinearFactor<Config>
    */
   void print(const std::string& s="VSLAMFactor") const;
 
+  /**
+   * equals
+   */
+  bool equals(const NonlinearFactor<VectorConfig>&, double tol=1e-9) const;
+
+  /**
+   * predict the measurement
+   */
+  Vector predict(const VectorConfig&) const;
+
   /**
    * calculate the error of the factor
    */
-  Vector error_vector(const Config&) const;
+  Vector error_vector(const VectorConfig&) const;
 
   /**
    * linerarization
    */
-  LinearFactor::shared_ptr linearize(const Config&) const;
+  LinearFactor::shared_ptr linearize(const VectorConfig&) const;
-
-  /**
-   * equals 
-   */
-  bool equals(const NonlinearFactor<Config>&, double tol=1e-9) const;
 
   int getCameraFrameNumber() const { return cameraFrameNumber_; }
   int getLandmarkNumber()    const { return landmarkNumber_;    }

cpp/testVSLAMFactor.cpp

@@ -1,17 +1,53 @@
 /**********************************************************
-Written by Alireza Fathi, 2nd of April 2009
+ Written by Frank Dellaert, Nov 2009
-**********************************************************/
+ **********************************************************/
 
-#include <iostream>
 #include <CppUnitLite/TestHarness.h>
-#include "numericalDerivative.h"
+
-#include "NonlinearFactorGraph.h"
 #include "VSLAMFactor.h"
+#include "Point3.h"
+#include "Pose3.h"
 
 using namespace std;
 using namespace gtsam;
 
-// TODO: test VSLAMFactor !!!
+// make cube
+Point3 x000(-1, -1, -1), x001(-1, -1, +1), x010(-1, +1, -1), x011(-1, +1, +1),
+		x100(-1, -1, -1), x101(-1, -1, +1), x110(-1, +1, -1), x111(-1, +1, +1);
+
+// make a realistic calibration matrix
+double fov = 60; // degrees
+size_t w=640,h=480;
+Cal3_S2 K(fov,w,h);
+
+// make cameras
+
+/* ************************************************************************* */
+TEST( VSLAMFactor, error )
+{
+	// Create the factor with a measurement that is 3 pixels off in x
+	Vector z = Vector_(2,323.,240.);
+	double sigma=1.0;
+	int cameraFrameNumber=1, landmarkNumber=1;
+  VSLAMFactor factor(z, sigma, cameraFrameNumber, landmarkNumber, K);
+
+  // For the following configuration, the factor predicts 320,240
+  VectorConfig config;
+  Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert("x1",x1.vector());
+  Point3 l1; config.insert("l1",l1.vector());
+  CHECK(assert_equal(Vector_(2,320.,240.),factor.predict(config)));
+
+  // Which yields an error of 3^2/2 = 4.5
+  DOUBLES_EQUAL(4.5,factor.error(config),1e-9);
+
+  // Check linearize
+  Matrix Al1 = Matrix_(2, 3, 61.584, 0., 0., 0., 61.584, 0.);
+  Matrix Ax1 = Matrix_(2, 6, 0., -369.504, 0., -61.584, 0., 0., 369.504, 0., 0., 0., -61.584, 0.);
+  Vector b = Vector_(2,3.,0.);
+  LinearFactor expected("l1", Al1, "x1", Ax1, b, 1);
+  LinearFactor::shared_ptr actual = factor.linearize(config);
+  CHECK(assert_equal(expected,*actual,1e-3));
+}
 
 /* ************************************************************************* */
 int main() {