diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h new file mode 100644 index 000000000..088bfd2ea --- /dev/null +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -0,0 +1,228 @@ +/* ---------------------------------------------------------------------------- + + * 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 ProjectionFactor.h + * @brief Basic bearing factor from 2D measurement + * @author Chris Beall + * @author Richard Roberts + * @author Frank Dellaert + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** + * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. + * i.e. the main building block for visual SLAM. + * @addtogroup SLAM + */ + template + class MultiProjectionFactor: public NoiseModelFactor { + protected: + + // Keep a copy of measurement and calibration for I/O + Vector measured_; ///< 2D measurement for each of the n views + boost::shared_ptr K_; ///< shared pointer to calibration object + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + + + // verbosity handling for Cheirality Exceptions + bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + + public: + + /// shorthand for base class type + typedef NoiseModelFactor Base; + + /// shorthand for this class + typedef MultiProjectionFactor This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor + MultiProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {} + + /** + * Constructor + * TODO: Mark argument order standard (keys, measurement, parameters) + * @param measured is the 2n dimensional location of the n points in the n views (the measurements) + * @param model is the standard deviation (current version assumes that the uncertainty is the same for all views) + * @param poseKeys is the set of indices corresponding to the cameras observing the same landmark + * @param pointKey is the index of the landmark + * @param K shared pointer to the constant calibration + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model, + FastSet poseKeys, Key pointKey, const boost::shared_ptr& K, + boost::optional body_P_sensor = boost::none) : + Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), + throwCheirality_(false), verboseCheirality_(false) { + keys_.assign(poseKeys.begin(), poseKeys.end()); + keys_.push_back(pointKey); + } + + /** + * Constructor with exception-handling flags + * TODO: Mark argument order standard (keys, measurement, parameters) + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation + * @param poseKey is the index of the camera + * @param pointKey is the index of the landmark + * @param K shared pointer to the constant calibration + * @param throwCheirality determines whether Cheirality exceptions are rethrown + * @param verboseCheirality determines whether exceptions are printed for Cheirality + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model, + FastSet poseKeys, Key pointKey, const boost::shared_ptr& K, + bool throwCheirality, bool verboseCheirality, + boost::optional body_P_sensor = boost::none) : + Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), + throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} + + /** Virtual destructor */ + virtual ~MultiProjectionFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "MultiProjectionFactor, z = "; + std::cout << measured_ << "measurements, z = "; + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + Base::print("", keyFormatter); + } + + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + return e + && Base::equals(p, tol) + //&& this->measured_.equals(e->measured_, tol) + && this->K_->equals(*e->K_, tol) + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } + + /// Evaluate error h(x)-z and optionally derivatives + Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const{ + + Vector a; + return a; + +// Point3 point = x.at(*keys_.end()); +// +// std::vector::iterator vit; +// for (vit = keys_.begin(); vit != keys_.end()-1; vit++) { +// Key key = (*vit); +// Pose3 pose = x.at(key); +// +// if(body_P_sensor_) { +// if(H1) { +// gtsam::Matrix H0; +// PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); +// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); +// *H1 = *H1 * H0; +// return reprojectionError.vector(); +// } else { +// PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); +// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); +// return reprojectionError.vector(); +// } +// } else { +// PinholeCamera camera(pose, *K_); +// Point2 reprojectionError(camera.project(point, H1, H2) - measured_); +// return reprojectionError.vector(); +// } +// } + + } + + + Vector evaluateError(const Pose3& pose, const Point3& point, + boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { + try { + if(body_P_sensor_) { + if(H1) { + gtsam::Matrix H0; + PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); + Point2 reprojectionError(camera.project(point, H1, H2) - measured_); + *H1 = *H1 * H0; + return reprojectionError.vector(); + } else { + PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); + Point2 reprojectionError(camera.project(point, H1, H2) - measured_); + return reprojectionError.vector(); + } + } else { + PinholeCamera camera(pose, *K_); + Point2 reprojectionError(camera.project(point, H1, H2) - measured_); + return reprojectionError.vector(); + } + } catch( CheiralityException& e) { + if (H1) *H1 = zeros(2,6); + if (H2) *H2 = zeros(2,3); + if (verboseCheirality_) + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << + " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + if (throwCheirality_) + throw e; + } + return ones(2) * 2.0 * K_->fx(); + } + + /** return the measurements */ + const Vector& measured() const { + return measured_; + } + + /** return the calibration object */ + inline const boost::shared_ptr calibration() const { + return K_; + } + + /** return verbosity */ + inline bool verboseCheirality() const { return verboseCheirality_; } + + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { return throwCheirality_; } + + private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(K_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + ar & BOOST_SERIALIZATION_NVP(throwCheirality_); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + } + }; +} // \ namespace gtsam diff --git a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp new file mode 100644 index 000000000..1cb338b4b --- /dev/null +++ b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp @@ -0,0 +1,235 @@ +/* ---------------------------------------------------------------------------- + + * 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 testProjectionFactor.cpp + * @brief Unit tests for ProjectionFactor Class + * @author Frank Dellaert + * @date Nov 2009 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +using namespace std; +using namespace gtsam; + +// make a realistic calibration matrix +static double fov = 60; // degrees +static size_t w=640,h=480; +static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); + +// Create a noise model for the pixel error +static SharedNoiseModel model(noiseModel::Unit::Create(2)); + +// Convenience for named keys +//using symbol_shorthand::X; +//using symbol_shorthand::L; + +//typedef GenericProjectionFactor TestProjectionFactor; + + +///* ************************************************************************* */ +TEST( MultiProjectionFactor, create ){ + Values theta; + NonlinearFactorGraph graph; + + Symbol x1('X', 1); + Symbol x2('X', 2); + Symbol x3('X', 3); + + Symbol l1('l', 1); + Vector n_measPixel(6); // Pixel measurements from 3 cameras observing landmark 1 + n_measPixel << 10, 10, 10, 10, 10, 10; + const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); + + FastSet views; + views.insert(x1); + views.insert(x2); + views.insert(x3); + + MultiProjectionFactor mpFactor(n_measPixel, noiseProjection, views, l1, K); + graph.add(mpFactor); + + +} + + + +///* ************************************************************************* */ +//TEST( ProjectionFactor, nonStandard ) { +// GenericProjectionFactor f; +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactor, Constructor) { +// Key poseKey(X(1)); +// Key pointKey(L(1)); +// +// Point2 measurement(323.0, 240.0); +// +// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactor, ConstructorWithTransform) { +// Key poseKey(X(1)); +// Key pointKey(L(1)); +// +// Point2 measurement(323.0, 240.0); +// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +// +// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactor, Equals ) { +// // Create two identical factors and make sure they're equal +// Point2 measurement(323.0, 240.0); +// +// TestProjectionFactor factor1(measurement, model, X(1), L(1), K); +// TestProjectionFactor factor2(measurement, model, X(1), L(1), K); +// +// CHECK(assert_equal(factor1, factor2)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactor, EqualsWithTransform ) { +// // Create two identical factors and make sure they're equal +// Point2 measurement(323.0, 240.0); +// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +// +// TestProjectionFactor factor1(measurement, model, X(1), L(1), K, body_P_sensor); +// TestProjectionFactor factor2(measurement, model, X(1), L(1), K, body_P_sensor); +// +// CHECK(assert_equal(factor1, factor2)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactor, Error ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K); +// +// // Set the linearization point +// Pose3 pose(Rot3(), Point3(0,0,-6)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the error +// Vector actualError(factor.evaluateError(pose, point)); +// +// // The expected error is (-3.0, 0.0) pixels / UnitCovariance +// Vector expectedError = Vector_(2, -3.0, 0.0); +// +// // Verify we get the expected error +// CHECK(assert_equal(expectedError, actualError, 1e-9)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactor, ErrorWithTransform ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor); +// +// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) +// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the error +// Vector actualError(factor.evaluateError(pose, point)); +// +// // The expected error is (-3.0, 0.0) pixels / UnitCovariance +// Vector expectedError = Vector_(2, -3.0, 0.0); +// +// // Verify we get the expected error +// CHECK(assert_equal(expectedError, actualError, 1e-9)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactor, Jacobian ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K); +// +// // Set the linearization point +// Pose3 pose(Rot3(), Point3(0,0,-6)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the Jacobians +// Matrix H1Actual, H2Actual; +// factor.evaluateError(pose, point, H1Actual, H2Actual); +// +// // The expected Jacobians +// Matrix H1Expected = Matrix_(2, 6, 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); +// Matrix H2Expected = Matrix_(2, 3, 92.376, 0., 0., 0., 92.376, 0.); +// +// // Verify the Jacobians are correct +// CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); +// CHECK(assert_equal(H2Expected, H2Actual, 1e-3)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactor, JacobianWithTransform ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor); +// +// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) +// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the Jacobians +// Matrix H1Actual, H2Actual; +// factor.evaluateError(pose, point, H1Actual, H2Actual); +// +// // The expected Jacobians +// Matrix H1Expected = Matrix_(2, 6, -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376); +// Matrix H2Expected = Matrix_(2, 3, 0., -92.376, 0., 0., 0., -92.376); +// +// // Verify the Jacobians are correct +// CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); +// CHECK(assert_equal(H2Expected, H2Actual, 1e-3)); +//} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ +