diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h new file mode 100644 index 000000000..dc0fd0e64 --- /dev/null +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -0,0 +1,292 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#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 SmartProjectionFactor: public NonlinearFactor { + protected: + + // Keep a copy of measurement and calibration for I/O + std::vector measured_; ///< 2D measurement for each of the n views + ///< (important that the order is the same as the keys that we use to create the factor) + boost::shared_ptr K_; ///< shared pointer to calibration object + const SharedNoiseModel noise_; ///< noise model used + 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 NonlinearFactor Base; + + /// shorthand for this class + typedef SmartProjectionFactor This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor + SmartProjectionFactor() : 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 K shared pointer to the constant calibration + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + SmartProjectionFactor(const std::vector measured, const SharedNoiseModel& model, + std::vector poseKeys, const boost::shared_ptr& K, + boost::optional body_P_sensor = boost::none) : + measured_(measured), K_(K), noise_(model), body_P_sensor_(body_P_sensor), + throwCheirality_(false), verboseCheirality_(false) { + keys_.assign(poseKeys.begin(), poseKeys.end()); + } + + /** + * 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 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) + */ + SmartProjectionFactor(const std::vector measured, const SharedNoiseModel& model, + std::vector poseKeys, const boost::shared_ptr& K, + bool throwCheirality, bool verboseCheirality, + boost::optional body_P_sensor = boost::none) : + measured_(measured), K_(K), noise_(model), body_P_sensor_(body_P_sensor), + throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} + + /** Virtual destructor */ + virtual ~SmartProjectionFactor() {} + + /// @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 << "SmartProjectionFactor, z = "; + BOOST_FOREACH(const Point2& p, measured_) { + std::cout << "measurement, p = "<< p << std::endl; + } + 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); + + bool areMeasurementsEqual = true; + for(size_t i = 0; i < measured_.size(); i++) { + if(this->measured_.at(i).equals(e->measured_.at(i), tol) == false) + areMeasurementsEqual = false; + break; + } + + return e + && Base::equals(p, tol) + && areMeasurementsEqual + && 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(); +// } +// } + + } + + /// get the dimension of the factor (number of rows on linearization) + virtual size_t dim() const { + return 6*keys_.size(); + } + + /// linearize returns a Hessianfactor that is an approximation of error(p) + virtual boost::shared_ptr linearize(const Values& x, const Ordering& ordering) const { + + // fill in the keys + std::vector js; + BOOST_FOREACH(const Key& k, keys_) { + js += ordering[k]; + } + + + std::vector Gs; + + std::vector gs; + // Shur complement trick + +// double e = u + b - z , e2 = e * e; +// double c = 2 * logSqrt2PI - log(p) + e2 * p; +// Vector g1 = Vector_(1, -e * p); +// Vector g2 = Vector_(1, 0.5 / p - 0.5 * e2); +// Vector g3 = Vector_(1, -e * p); +// Matrix G11 = Matrix_(1, 1, p); +// Matrix G12 = Matrix_(1, 1, e); +// Matrix G13 = Matrix_(1, 1, p); +// Matrix G22 = Matrix_(1, 1, 0.5 / (p * p)); +// Matrix G23 = Matrix_(1, 1, e); +// Matrix G33 = Matrix_(1, 1, p); + + double f = 0; + + return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); + } + + /** + * Calculate the error of the factor. + * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. + * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model + * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. + */ + virtual double error(const Values& values) const { + if (this->active(values)) { + double overallError=0; + + // Collect all poses (Cameras) + std::vector cameraPoses; + + BOOST_FOREACH(const Key& k, keys_) { + if(body_P_sensor_) + cameraPoses.push_back(values.at(k).compose(*body_P_sensor_)); + else + cameraPoses.push_back(values.at(k)); + } + + // We triangulate the 3D position of the landmark + boost::optional point = triangulatePoint3(cameraPoses, measured_, *K_); + + if(point) + { // triangulation produced a good estimate of landmark position + + std::cout << "point " << *point << std::endl; + + for(size_t i = 0; i < measured_.size(); i++) { + Pose3 pose = cameraPoses.at(i); + PinholeCamera camera(pose, *K_); + std::cout << "pose.compose(*body_P_sensor_) " << pose << std::endl; + + Point2 reprojectionError(camera.project(*point) - measured_.at(i)); + std::cout << "reprojectionError " << reprojectionError << std::endl; + overallError += noise_->distance( reprojectionError.vector() ); + std::cout << "noise_->distance( reprojectionError.vector() ) " << noise_->distance( reprojectionError.vector() ) << std::endl; + } + return sqrt(overallError); + }else{ // triangulation failed: we deactivate the factor, then the error should not contribute to the overall error + return 0.0; + } + } else { + return 0.0; + } + } + + /** 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/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp new file mode 100644 index 000000000..c7bb9f483 --- /dev/null +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -0,0 +1,308 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#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, noiseless ){ + cout << " ************************ MultiProjectionFactor: noiseless ****************************" << endl; + Values theta; + NonlinearFactorGraph graph; + + Symbol x1('X', 1); + Symbol x2('X', 2); +// Symbol x3('X', 3); + + const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); + + std::vector views; + views += x1, x2; //, x3; + + Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera level_camera(level_pose, *K); + + // create second camera 1 meter to the right of first camera + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera level_camera_right(level_pose_right, *K); + + // landmark ~5 meters infront of camera + Point3 landmark(5, 0.5, 1.2); + + // 1. Project two landmarks into two cameras and triangulate + Point2 level_uv = level_camera.project(landmark); + Point2 level_uv_right = level_camera_right.project(landmark); + + Values value; + value.insert(x1, level_pose); + value.insert(x2, level_pose_right); + +// poses += level_pose, level_pose_right; + vector measurements; + measurements += level_uv, level_uv_right; + + SmartProjectionFactor smartFactor(measurements, noiseProjection, views, K); + + double actualError = smartFactor.error(value); + double expectedError = 0.0; + + DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +///* ************************************************************************* */ +TEST( MultiProjectionFactor, noisy ){ + cout << " ************************ MultiProjectionFactor: noisy ****************************" << endl; + + Values theta; + NonlinearFactorGraph graph; + + Symbol x1('X', 1); + Symbol x2('X', 2); +// Symbol x3('X', 3); + + const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); + + std::vector views; + views += x1, x2; //, x3; + + Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera level_camera(level_pose, *K); + + // create second camera 1 meter to the right of first camera + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera level_camera_right(level_pose_right, *K); + + // landmark ~5 meters infront of camera + Point3 landmark(5, 0.5, 1.2); + + // 1. Project two landmarks into two cameras and triangulate + Point2 pixelError(0.2,0.2); + Point2 level_uv = level_camera.project(landmark) + pixelError; + Point2 level_uv_right = level_camera_right.project(landmark); + + Values value; + value.insert(x1, level_pose); + value.insert(x2, level_pose_right); + +// poses += level_pose, level_pose_right; + vector measurements; + measurements += level_uv, level_uv_right; + + SmartProjectionFactor smartFactor(measurements, noiseProjection, views, K); + + double actualError = smartFactor.error(value); + double expectedError = sqrt(0.08); + + // we do not expect to be able to predict the error, since the error on the pixel will change + // the triangulation of the landmark which is internal to the factor. + // DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +///* ************************************************************************* */ +//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); } +/* ************************************************************************* */ +