From 651db9240b16848d3e90cc0206887801f8a42925 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Mon, 15 Nov 2010 16:04:30 +0000 Subject: [PATCH] moved StereoFactor from VO to gtsam --- gtsam/slam/Makefile.am | 4 + gtsam/slam/StereoFactor.h | 111 ++++++++++++++++++++++++++ gtsam/slam/tests/testStereoFactor.cpp | 94 ++++++++++++++++++++++ 3 files changed, 209 insertions(+) create mode 100644 gtsam/slam/StereoFactor.h create mode 100644 gtsam/slam/tests/testStereoFactor.cpp diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index 7bca039cd..25343a3b1 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -49,6 +49,10 @@ check_PROGRAMS += tests/testPose3Factor tests/testPose3Values tests/testPose3SLA sources += visualSLAM.cpp check_PROGRAMS += tests/testVSLAMFactor tests/testVSLAMGraph tests/testVSLAMValues +# StereoFactor +headers += StereoFactor.h +check_PROGRAMS += tests/testStereoFactor + #---------------------------------------------------------------------------------------------------- # Create a libtool library that is not installed # It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h new file mode 100644 index 000000000..819530f13 --- /dev/null +++ b/gtsam/slam/StereoFactor.h @@ -0,0 +1,111 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file StereoFactor.h + * @brief A non-linear factor for stereo measurements + * @author Alireza Fathi + * @author Chris Beall + */ + +#pragma once + +#include +#include + +namespace gtsam { + +using namespace gtsam; +using namespace gtsam::visualSLAM; + +class StereoFactor: public NonlinearFactor2, Testable { +private: + + // Keep a copy of measurement and calibration for I/O + StereoPoint2 z_; + boost::shared_ptr K_; + double baseline_; + +public: + + // shorthand for base class type + typedef NonlinearFactor2 Base; + + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** + * Default constructor + */ + StereoFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + + /** + * Constructor + * @param z is the 2 dimensional location of point in image (the measurement) + * @param sigma is the standard deviation + * @param cameraFrameNumber is basically the frame number + * @param landmarkNumber is the index of the landmark + * @param K the constant calibration + */ + StereoFactor(const StereoPoint2& z, const SharedGaussian& model, PoseKey j_pose, PointKey j_landmark, const shared_ptrK& K, double baseline) : + Base(model, j_pose, j_landmark), z_(z), K_(K), baseline_(baseline) {} + + /** + * print + * @param s optional string naming the factor + */ + void print(const std::string& s) const { + printf("%s %s %s\n", s.c_str(), ((string) key1_).c_str(), + ((string) key2_).c_str()); + z_.print(s+".z"); + } + + /** + * equals + */ + bool equals(const StereoFactor& f, double tol) const { + const StereoFactor* p = dynamic_cast(&f); + if (p == NULL) goto fail; + //if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail; + if (!z_.equals(p->z_,tol)) goto fail; + return true; + + fail: + print("actual"); + p->print("expected"); + return false; + } + + /** h(x)-z */ + Vector evaluateError(const Pose3& pose, const Point3& point, + boost::optional H1, boost::optional H2) const { + + const Cal3_S2& K = *K_; + StereoCamera stereoCam(pose, K, baseline_); + + return (stereoCam.project(point,H1,H2) - z_).vector(); + } + + StereoPoint2 z(){return z_;} + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + //ar & BOOST_SERIALIZATION_NVP(key1_); + //ar & BOOST_SERIALIZATION_NVP(key2_); + ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(K_); + } +}; + +} diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp new file mode 100644 index 000000000..e7024f1f6 --- /dev/null +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -0,0 +1,94 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testStereoFactor.cpp + * @brief Unit test for StereoFactor + * single camera + * @author Chris Beall + */ + +#include + +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::visualSLAM; +using namespace boost; +typedef NonlinearOptimizer Optimizer; + +Pose3 camera1(Matrix_(3,3, + 1., 0., 0., + 0.,-1., 0., + 0., 0.,-1. + ), + Point3(0,0,6.25)); + +Cal3_S2 K(1500, 1500, 0, 320, 240); +StereoCamera stereoCam(Pose3(), K, 0.5); + +// point X Y Z in meters +Point3 p(0, 0, 5); +static SharedGaussian sigma(noiseModel::Unit::Create(1)); + +/* ************************************************************************* */ +TEST( StereoFactor, singlePoint) +{ + //Cal3_S2 K(625, 625, 0, 320, 240); + boost::shared_ptr K(new Cal3_S2(625, 625, 0, 320, 240)); + boost::shared_ptr graph(new Graph()); + + graph->add(PoseConstraint(1,camera1)); + + StereoPoint2 z14(320,320.0-50, 240); + // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) + graph->add(StereoFactor(z14,sigma, 1, 1, K, 0.5)); + + // Create a configuration corresponding to the ground truth + boost::shared_ptr values(new Values()); + values->insert(1, camera1); // add camera at z=6.25m looking towards origin + + Point3 l1(0, 0, 0); + values->insert(1, l1); // add point at origin; + + Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); + + typedef gtsam::NonlinearOptimizer Optimizer; // optimization engine for this domain + + NonlinearOptimizationParameters parameters(1.0, 1e-3, 0, + 100, 1.0, 10, NonlinearOptimizationParameters::SILENT, NonlinearOptimizationParameters::BOUNDED); + + Optimizer optimizer(graph, values, ordering, make_shared(parameters)); + + // We expect the initial to be zero because config is the ground truth + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); + + // Iterate once, and the config should not have changed + Optimizer afterOneIteration = optimizer.iterate(); + DOUBLES_EQUAL(0.0, afterOneIteration.error(), 1e-9); + + // Complete solution + Optimizer final = optimizer.gaussNewton(1E-9, 1E-5); + + DOUBLES_EQUAL(0.0, final.error(), 1e-6); +} + +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */