From 7bd40e836dffa243d39f815efd59268b0bd76910 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 9 Nov 2009 04:46:34 +0000 Subject: [PATCH] Cleaned up and added test for VSLAMFactor --- cpp/VSLAMFactor.cpp | 59 +++++++++++++++++++---------------------- cpp/VSLAMFactor.h | 34 +++++++++++++----------- cpp/testVSLAMFactor.cpp | 48 ++++++++++++++++++++++++++++----- 3 files changed, 88 insertions(+), 53 deletions(-) diff --git a/cpp/VSLAMFactor.cpp b/cpp/VSLAMFactor.cpp index 60cd86491..ddfa9290b 100644 --- a/cpp/VSLAMFactor.cpp +++ b/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 -VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K) - : NonlinearFactor(z, sigma) +VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K) + : NonlinearFactor(z, sigma) { cameraFrameNumber_ = cn; landmarkNumber_ = ln; @@ -28,27 +29,36 @@ VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, } /* ************************************************************************* */ -template -void VSLAMFactor::print(const std::string& s) const { +void VSLAMFactor::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 -Vector VSLAMFactor::error_vector(const Config& c) const { +bool VSLAMFactor::equals(const NonlinearFactor& f, double tol) const { + const VSLAMFactor* p = dynamic_cast(&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_]; - - // Right-hand-side b = (z - h(x))/sigma - Vector h = project(SimpleCamera(K_,pose), landmark).vector(); - - return (ConvenientFactor::z_ - h); + return project(SimpleCamera(K_,pose), landmark).vector(); } /* ************************************************************************* */ -template -LinearFactor::shared_ptr VSLAMFactor::linearize(const Config& c) const +Vector VSLAMFactor::error_vector(const VectorConfig& 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::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 -bool VSLAMFactor::equals(const NonlinearFactor& f, double tol) const { - const VSLAMFactor* p = dynamic_cast(&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; -} - -/* ************************************************************************* */ diff --git a/cpp/VSLAMFactor.h b/cpp/VSLAMFactor.h index e32277bb0..6c22a8549 100644 --- a/cpp/VSLAMFactor.h +++ b/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 VSLAMFactor : public NonlinearFactor +class VSLAMFactor : public NonlinearFactor { - 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 ConvenientFactor; + typedef gtsam::NonlinearFactor ConvenientFactor; + public: typedef boost::shared_ptr shared_ptr; // shorthand for a smart pointer to a factor @@ -49,20 +48,25 @@ class VSLAMFactor : public NonlinearFactor */ void print(const std::string& s="VSLAMFactor") const; + /** + * equals + */ + bool equals(const NonlinearFactor&, 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 + * linerarization */ - LinearFactor::shared_ptr linearize(const Config&) const; - - /** - * equals - */ - bool equals(const NonlinearFactor&, double tol=1e-9) const; + LinearFactor::shared_ptr linearize(const VectorConfig&) const; int getCameraFrameNumber() const { return cameraFrameNumber_; } int getLandmarkNumber() const { return landmarkNumber_; } diff --git a/cpp/testVSLAMFactor.cpp b/cpp/testVSLAMFactor.cpp index 15f4b6d68..019067938 100644 --- a/cpp/testVSLAMFactor.cpp +++ b/cpp/testVSLAMFactor.cpp @@ -1,17 +1,53 @@ /********************************************************** -Written by Alireza Fathi, 2nd of April 2009 -**********************************************************/ + Written by Frank Dellaert, Nov 2009 + **********************************************************/ -#include #include -#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() {