From 54808f6d44d9a68294bc67647661381c139d6273 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Wed, 8 May 2013 13:23:35 +0000 Subject: [PATCH] Added several experimental versions of an Inverse Depth Factor --- gtsam_unstable/slam/InvDepthFactorVariant1.h | 139 +++++++++ gtsam_unstable/slam/InvDepthFactorVariant2.h | 148 ++++++++++ gtsam_unstable/slam/InvDepthFactorVariant3.h | 265 ++++++++++++++++++ .../slam/tests/testInvDepthFactorVariant1.cpp | 126 +++++++++ .../slam/tests/testInvDepthFactorVariant2.cpp | 118 ++++++++ .../slam/tests/testInvDepthFactorVariant3.cpp | 85 ++++++ 6 files changed, 881 insertions(+) create mode 100644 gtsam_unstable/slam/InvDepthFactorVariant1.h create mode 100644 gtsam_unstable/slam/InvDepthFactorVariant2.h create mode 100644 gtsam_unstable/slam/InvDepthFactorVariant3.h create mode 100644 gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp create mode 100644 gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp create mode 100644 gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h new file mode 100644 index 000000000..5e67a39d3 --- /dev/null +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -0,0 +1,139 @@ + +/** + * @file InvDepthFactorVariant1.h + * @brief Inverse Depth Factor based on Civera09tro, Montiel06rss. + * Landmarks are parameterized as (x,y,z,theta,phi,rho). The factor involves + * a single pose and a landmark. The landmark parameterization contains the + * reference point internally (and will thus be updated as well during optimization). + * @author Chris Beall, Stephen Williams + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * Binary factor representing a visual measurement using an inverse-depth parameterization + */ +class InvDepthFactorVariant1: public NoiseModelFactor2 { +protected: + + // Keep a copy of measurement and calibration for I/O + Point2 measured_; ///< 2D measurement + Cal3_S2::shared_ptr K_; ///< shared pointer to calibration object + +public: + + /// shorthand for base class type + typedef NoiseModelFactor2 Base; + + /// shorthand for this class + typedef InvDepthFactorVariant1 This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor + InvDepthFactorVariant1() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + + /** + * Constructor + * @param poseKey is the index of the camera pose + * @param pointKey is the index of the landmark + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param K shared pointer to the constant calibration + * @param model is the standard deviation + */ + InvDepthFactorVariant1(const Key poseKey, const Key landmarkKey, + const Point2& measured, const Cal3_S2::shared_ptr& K, const SharedNoiseModel& model) : + Base(model, poseKey, landmarkKey), measured_(measured), K_(K) {} + + /** Virtual destructor */ + virtual ~InvDepthFactorVariant1() {} + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "InvDepthFactorVariant1", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); + } + + /// equals + virtual bool equals(const gtsam::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); + } + + Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const { + try { + // Calculate the 3D coordinates of the landmark in the world frame + double x = landmark(0), y = landmark(1), z = landmark(2); + double theta = landmark(3), phi = landmark(4), rho = landmark(5); + Point3 world_P_landmark = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); + // Project landmark into Pose2 + PinholeCamera camera(pose, *K_); + gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_); + return reprojectionError.vector(); + } catch( CheiralityException& e) { + std::cout << e.what() + << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]" + << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]" + << std::endl; + return gtsam::ones(2) * 2.0 * K_->fx(); + } + return gtsam::Vector_(1, 0.0); + } + + /// Evaluate error h(x)-z and optionally derivatives + Vector evaluateError(const Pose3& pose, const LieVector& landmark, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + + if(H1) { + (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1, landmark), pose); + } + if(H2) { + (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, _1), landmark); + } + + return inverseDepthError(pose, landmark); + } + + /** return the measurement */ + const gtsam::Point2& imagePoint() const { + return measured_; + } + + /** return the calibration object */ + const Cal3_S2::shared_ptr calibration() const { + return K_; + } + +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_); + } +}; + +} // \ namespace gtsam diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h new file mode 100644 index 000000000..2da440425 --- /dev/null +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -0,0 +1,148 @@ + +/** + * @file InvDepthFactorVariant2.h + * @brief Inverse Depth Factor based on Civera09tro, Montiel06rss. + * Landmarks are parameterized as (theta,phi,rho) with the reference point + * created at landmark construction and then never updated (i.e. the point + * [x,y,z] is treated as fixed and not part of the optimization). The factor + * involves a single pose and a landmark. + * @author Chris Beall, Stephen Williams + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * Binary factor representing a visual measurement using an inverse-depth parameterization + */ +class InvDepthFactorVariant2: public NoiseModelFactor2 { +protected: + + // Keep a copy of measurement and calibration for I/O + Point2 measured_; ///< 2D measurement + Cal3_S2::shared_ptr K_; ///< shared pointer to calibration object + Point3 referencePoint_; ///< the reference point/origin for this landmark + +public: + + /// shorthand for base class type + typedef NoiseModelFactor2 Base; + + /// shorthand for this class + typedef InvDepthFactorVariant2 This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor + InvDepthFactorVariant2() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + + /** + * Constructor + * @param poseKey is the index of the camera pose + * @param pointKey is the index of the landmark + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param K shared pointer to the constant calibration + * @param model is the standard deviation + */ + InvDepthFactorVariant2(const Key poseKey, const Key landmarkKey, + const Point2& measured, const Cal3_S2::shared_ptr& K, const Point3 referencePoint, + const SharedNoiseModel& model) : + Base(model, poseKey, landmarkKey), measured_(measured), K_(K), referencePoint_(referencePoint) {} + + /** Virtual destructor */ + virtual ~InvDepthFactorVariant2() {} + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "InvDepthFactorVariant2", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); + } + + /// equals + virtual bool equals(const gtsam::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) + && this->referencePoint_.equals(e->referencePoint_, tol); + } + + Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const { + try { + // Calculate the 3D coordinates of the landmark in the world frame + double theta = landmark(0), phi = landmark(1), rho = landmark(2); + Point3 world_P_landmark = referencePoint_ + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); + // Project landmark into Pose2 + PinholeCamera camera(pose, *K_); + gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_); + return reprojectionError.vector(); + } catch( CheiralityException& e) { + std::cout << e.what() + << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]" + << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]" + << std::endl; + return gtsam::ones(2) * 2.0 * K_->fx(); + } + return gtsam::Vector_(1, 0.0); + } + + /// Evaluate error h(x)-z and optionally derivatives + Vector evaluateError(const Pose3& pose, const LieVector& landmark, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + + if(H1) { + (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1, landmark), pose); + } + if(H2) { + (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, _1), landmark); + } + + return inverseDepthError(pose, landmark); + } + + /** return the measurement */ + const gtsam::Point2& imagePoint() const { + return measured_; + } + + /** return the calibration object */ + const Cal3_S2::shared_ptr calibration() const { + return K_; + } + + /** return the calibration object */ + const Point3& referencePoint() const { + return referencePoint_; + } + +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(referencePoint_); + } +}; + +} // \ namespace gtsam diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h new file mode 100644 index 000000000..0d75d5e6b --- /dev/null +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -0,0 +1,265 @@ + +/** + * @file InvDepthFactorVariant3.h + * @brief Inverse Depth Factor based on Civera09tro, Montiel06rss. + * Landmarks are parameterized as (theta,phi,rho). The factor involves + * two poses and a landmark. The first pose is the reference frame + * from which (theta, phi, rho) is measured. + * @author Chris Beall, Stephen Williams + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * Binary factor representing the first visual measurement using an inverse-depth parameterization + */ +class InvDepthFactorVariant3a: public NoiseModelFactor2 { +protected: + + // Keep a copy of measurement and calibration for I/O + Point2 measured_; ///< 2D measurement + Cal3_S2::shared_ptr K_; ///< shared pointer to calibration object + +public: + + /// shorthand for base class type + typedef NoiseModelFactor2 Base; + + /// shorthand for this class + typedef InvDepthFactorVariant3a This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor + InvDepthFactorVariant3a() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + + /** + * Constructor + * 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 pose + * @param pointKey is the index of the landmark + * @param invDepthKey is the index of inverse depth + * @param K shared pointer to the constant calibration + */ + InvDepthFactorVariant3a(const Key poseKey, const Key landmarkKey, + const Point2& measured, const Cal3_S2::shared_ptr& K, const SharedNoiseModel& model) : + Base(model, poseKey, landmarkKey), measured_(measured), K_(K) {} + + /** Virtual destructor */ + virtual ~InvDepthFactorVariant3a() {} + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "InvDepthFactorVariant3a", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); + } + + /// equals + virtual bool equals(const gtsam::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); + } + + Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const { + try { + // Calculate the 3D coordinates of the landmark in the Pose frame + double theta = landmark(0), phi = landmark(1), rho = landmark(2); + Point3 pose_P_landmark(cos(phi)*sin(theta)/rho, sin(phi)/rho, cos(phi)*cos(theta)/rho); + // Convert the landmark to world coordinates + Point3 world_P_landmark = pose.transform_from(pose_P_landmark); + // Project landmark into Pose2 + PinholeCamera camera(pose, *K_); + gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_); + return reprojectionError.vector(); + } catch( CheiralityException& e) { + std::cout << e.what() + << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]" + << " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]" + << std::endl; + return gtsam::ones(2) * 2.0 * K_->fx(); + } + return gtsam::Vector_(1, 0.0); + } + + /// Evaluate error h(x)-z and optionally derivatives + Vector evaluateError(const Pose3& pose, const LieVector& landmark, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + + if(H1) { + (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); + } + if(H2) { + (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); + } + + return inverseDepthError(pose, landmark); + } + + /** return the measurement */ + const gtsam::Point2& imagePoint() const { + return measured_; + } + + /** return the calibration object */ + const Cal3_S2::shared_ptr calibration() const { + return K_; + } + +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_); + } +}; + +/** + * Ternary factor representing a visual measurement using an inverse-depth parameterization + */ +class InvDepthFactorVariant3b: public NoiseModelFactor3 { +protected: + + // Keep a copy of measurement and calibration for I/O + Point2 measured_; ///< 2D measurement + Cal3_S2::shared_ptr K_; ///< shared pointer to calibration object + +public: + + /// shorthand for base class type + typedef NoiseModelFactor3 Base; + + /// shorthand for this class + typedef InvDepthFactorVariant3b This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor + InvDepthFactorVariant3b() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + + /** + * Constructor + * 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 pose + * @param pointKey is the index of the landmark + * @param invDepthKey is the index of inverse depth + * @param K shared pointer to the constant calibration + */ + InvDepthFactorVariant3b(const Key poseKey1, const Key poseKey2, const Key landmarkKey, + const Point2& measured, const Cal3_S2::shared_ptr& K, const SharedNoiseModel& model) : + Base(model, poseKey1, poseKey2, landmarkKey), measured_(measured), K_(K) {} + + /** Virtual destructor */ + virtual ~InvDepthFactorVariant3b() {} + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "InvDepthFactorVariant3", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); + } + + /// equals + virtual bool equals(const gtsam::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); + } + + Vector inverseDepthError(const Pose3& pose1, const Pose3& pose2, const LieVector& landmark) const { + try { + // Calculate the 3D coordinates of the landmark in the Pose1 frame + double theta = landmark(0), phi = landmark(1), rho = landmark(2); + Point3 pose1_P_landmark(cos(phi)*sin(theta)/rho, sin(phi)/rho, cos(phi)*cos(theta)/rho); + // Convert the landmark to world coordinates + Point3 world_P_landmark = pose1.transform_from(pose1_P_landmark); + // Project landmark into Pose2 + PinholeCamera camera(pose2, *K_); + gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_); + return reprojectionError.vector(); + } catch( CheiralityException& e) { + std::cout << e.what() + << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]" + << " moved behind camera " << DefaultKeyFormatter(this->key2()) + << std::endl; + return gtsam::ones(2) * 2.0 * K_->fx(); + } + return gtsam::Vector_(1, 0.0); + } + + /// Evaluate error h(x)-z and optionally derivatives + Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const LieVector& landmark, + boost::optional H1=boost::none, + boost::optional H2=boost::none, + boost::optional H3=boost::none) const { + + if(H1) { + (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); + } + if(H2) { + (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); + } + if(H3) { + (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); + } + + return inverseDepthError(pose1, pose2, landmark); + } + + /** return the measurement */ + const gtsam::Point2& imagePoint() const { + return measured_; + } + + /** return the calibration object */ + const Cal3_S2::shared_ptr calibration() const { + return K_; + } + +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_); + } +}; + +} // \ namespace gtsam diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp new file mode 100644 index 000000000..b8f24dda8 --- /dev/null +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -0,0 +1,126 @@ +/* + * testInvDepthFactorVariant1.cpp + * + * Created on: Apr 13, 2012 + * Author: cbeall3 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( InvDepthFactorVariant1, optimize) { + + // Create two poses looking in the x-direction + Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); + Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); + + // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1)) + Point3 landmark(5, 0, 1); + + // Create image observations + Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); + PinholeCamera camera1(pose1, *K); + PinholeCamera camera2(pose2, *K); + Point2 pixel1 = camera1.project(landmark); + Point2 pixel2 = camera2.project(landmark); + + // Create expected landmark + double x = pose1.translation().x(); + double y = pose1.translation().y(); + double z = pose1.translation().z(); + Point3 ray = landmark - pose1.translation(); + double theta = atan2(ray.y(), ray.x()); + double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); + double rho = 1./ray.norm(); + LieVector expected(6, x, y, z, theta, phi, rho); + + + + // Create a factor graph with two inverse depth factors and two pose priors + Key poseKey1(1); + Key poseKey2(2); + Key landmarkKey(100); + SharedNoiseModel sigma(noiseModel::Unit::Create(2)); + NonlinearFactor::shared_ptr factor1(new NonlinearEquality(poseKey1, pose1, 100000)); + NonlinearFactor::shared_ptr factor2(new NonlinearEquality(poseKey2, pose2, 100000)); + NonlinearFactor::shared_ptr factor3(new InvDepthFactorVariant1(poseKey1, landmarkKey, pixel1, K, sigma)); + NonlinearFactor::shared_ptr factor4(new InvDepthFactorVariant1(poseKey2, landmarkKey, pixel2, K, sigma)); + NonlinearFactorGraph graph; + graph.push_back(factor1); + graph.push_back(factor2); + graph.push_back(factor3); + graph.push_back(factor4); + + // Create a values with slightly incorrect initial conditions + Values values; + values.insert(poseKey1, pose1.retract(Vector_(6, +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); + values.insert(poseKey2, pose2.retract(Vector_(6, +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); + values.insert(landmarkKey, expected.retract(Vector_(6, -0.20, +0.20, -0.35, +0.02, -0.04, +0.05))); + + // Optimize the graph to recover the actual landmark position + LevenbergMarquardtParams params; + Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); + LieVector actual = result.at(landmarkKey); + + + values.at(poseKey1).print("Pose1 Before:\n"); + result.at(poseKey1).print("Pose1 After:\n"); + pose1.print("Pose1 Truth:\n"); + std::cout << std::endl << std::endl; + values.at(poseKey2).print("Pose2 Before:\n"); + result.at(poseKey2).print("Pose2 After:\n"); + pose2.print("Pose2 Truth:\n"); + std::cout << std::endl << std::endl; + values.at(landmarkKey).print("Landmark Before:\n"); + result.at(landmarkKey).print("Landmark After:\n"); + expected.print("Landmark Truth:\n"); + std::cout << std::endl << std::endl; + + // Calculate world coordinates of landmark versions + Point3 world_landmarkBefore; + { + LieVector landmarkBefore = values.at(landmarkKey); + double x = landmarkBefore(0), y = landmarkBefore(1), z = landmarkBefore(2); + double theta = landmarkBefore(3), phi = landmarkBefore(4), rho = landmarkBefore(5); + world_landmarkBefore = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); + } + Point3 world_landmarkAfter; + { + LieVector landmarkAfter = result.at(landmarkKey); + double x = landmarkAfter(0), y = landmarkAfter(1), z = landmarkAfter(2); + double theta = landmarkAfter(3), phi = landmarkAfter(4), rho = landmarkAfter(5); + world_landmarkAfter = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); + } + + world_landmarkBefore.print("World Landmark Before:\n"); + world_landmarkAfter.print("World Landmark After:\n"); + landmark.print("World Landmark Truth:\n"); + std::cout << std::endl << std::endl; + + + + // Test that the correct landmark parameters have been recovered + // However, since this is an over-parameterization, there can be + // many 6D landmark values that equate to the same 3D world position + // Instead, directly test the recovered 3D landmark position + //EXPECT(assert_equal(expected, actual, 1e-9)); + EXPECT(assert_equal(landmark, world_landmarkAfter, 1e-9)); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp new file mode 100644 index 000000000..4706a9338 --- /dev/null +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -0,0 +1,118 @@ +/* + * testInvDepthFactorVariant1.cpp + * + * Created on: Apr 13, 2012 + * Author: cbeall3 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( InvDepthFactorVariant2, optimize) { + + // Create two poses looking in the x-direction + Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); + Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); + + // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1)) + Point3 landmark(5, 0, 1); + + // Create image observations + Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); + PinholeCamera camera1(pose1, *K); + PinholeCamera camera2(pose2, *K); + Point2 pixel1 = camera1.project(landmark); + Point2 pixel2 = camera2.project(landmark); + + // Create expected landmark + Point3 referencePoint = pose1.translation(); + Point3 ray = landmark - referencePoint; + double theta = atan2(ray.y(), ray.x()); + double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); + double rho = 1./ray.norm(); + LieVector expected(3, theta, phi, rho); + + + + // Create a factor graph with two inverse depth factors and two pose priors + Key poseKey1(1); + Key poseKey2(2); + Key landmarkKey(100); + SharedNoiseModel sigma(noiseModel::Unit::Create(2)); + NonlinearFactor::shared_ptr factor1(new NonlinearEquality(poseKey1, pose1, 100000)); + NonlinearFactor::shared_ptr factor2(new NonlinearEquality(poseKey2, pose2, 100000)); + NonlinearFactor::shared_ptr factor3(new InvDepthFactorVariant2(poseKey1, landmarkKey, pixel1, K, referencePoint, sigma)); + NonlinearFactor::shared_ptr factor4(new InvDepthFactorVariant2(poseKey2, landmarkKey, pixel2, K, referencePoint, sigma)); + NonlinearFactorGraph graph; + graph.push_back(factor1); + graph.push_back(factor2); + graph.push_back(factor3); + graph.push_back(factor4); + + // Create a values with slightly incorrect initial conditions + Values values; + values.insert(poseKey1, pose1.retract(Vector_(6, +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); + values.insert(poseKey2, pose2.retract(Vector_(6, +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); + values.insert(landmarkKey, expected.retract(Vector_(3, +0.02, -0.04, +0.05))); + + // Optimize the graph to recover the actual landmark position + LevenbergMarquardtParams params; + Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); + LieVector actual = result.at(landmarkKey); + + + values.at(poseKey1).print("Pose1 Before:\n"); + result.at(poseKey1).print("Pose1 After:\n"); + pose1.print("Pose1 Truth:\n"); + std::cout << std::endl << std::endl; + values.at(poseKey2).print("Pose2 Before:\n"); + result.at(poseKey2).print("Pose2 After:\n"); + pose2.print("Pose2 Truth:\n"); + std::cout << std::endl << std::endl; + values.at(landmarkKey).print("Landmark Before:\n"); + result.at(landmarkKey).print("Landmark After:\n"); + expected.print("Landmark Truth:\n"); + std::cout << std::endl << std::endl; + + // Calculate world coordinates of landmark versions + Point3 world_landmarkBefore; + { + LieVector landmarkBefore = values.at(landmarkKey); + double theta = landmarkBefore(0), phi = landmarkBefore(1), rho = landmarkBefore(2); + world_landmarkBefore = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); + } + Point3 world_landmarkAfter; + { + LieVector landmarkAfter = result.at(landmarkKey); + double theta = landmarkAfter(0), phi = landmarkAfter(1), rho = landmarkAfter(2); + world_landmarkAfter = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); + } + + world_landmarkBefore.print("World Landmark Before:\n"); + world_landmarkAfter.print("World Landmark After:\n"); + landmark.print("World Landmark Truth:\n"); + std::cout << std::endl << std::endl; + + + + // Test that the correct landmark parameters have been recovered + EXPECT(assert_equal(expected, actual, 1e-9)); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp new file mode 100644 index 000000000..c15998507 --- /dev/null +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -0,0 +1,85 @@ +/* + * testInvDepthFactorVariant3.cpp + * + * Created on: Apr 13, 2012 + * Author: cbeall3 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( InvDepthFactorVariant3, optimize) { + + // Create two poses looking in the x-direction + Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); + Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); + + // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1)) + Point3 landmark(5, 0, 1); + + // Create image observations + Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); + PinholeCamera camera1(pose1, *K); + PinholeCamera camera2(pose2, *K); + Point2 pixel1 = camera1.project(landmark); + Point2 pixel2 = camera2.project(landmark); + + // Create expected landmark + Point3 landmark_p1 = pose1.transform_to(landmark); + landmark_p1.print("Landmark in Pose1 Frame:\n"); + double theta = atan2(landmark_p1.x(), landmark_p1.z()); + double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z())); + double rho = 1./landmark_p1.norm(); + LieVector expected(3, theta, phi, rho); + + + + // Create a factor graph with two inverse depth factors and two pose priors + Key poseKey1(1); + Key poseKey2(2); + Key landmarkKey(100); + SharedNoiseModel sigma(noiseModel::Unit::Create(2)); + NonlinearFactor::shared_ptr factor1(new NonlinearEquality(poseKey1, pose1, 100000)); + NonlinearFactor::shared_ptr factor2(new NonlinearEquality(poseKey2, pose2, 100000)); + NonlinearFactor::shared_ptr factor3(new InvDepthFactorVariant3a(poseKey1, landmarkKey, pixel1, K, sigma)); + NonlinearFactor::shared_ptr factor4(new InvDepthFactorVariant3b(poseKey1, poseKey2, landmarkKey, pixel2, K, sigma)); + NonlinearFactorGraph graph; + graph.push_back(factor1); + graph.push_back(factor2); + graph.push_back(factor3); + graph.push_back(factor4); + + // Create a values with slightly incorrect initial conditions + Values values; + values.insert(poseKey1, pose1.retract(Vector_(6, +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); + values.insert(poseKey2, pose2.retract(Vector_(6, +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); + values.insert(landmarkKey, expected.retract(Vector_(3, +0.02, -0.04, +0.05))); + + // Optimize the graph to recover the actual landmark position + LevenbergMarquardtParams params; + Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); + LieVector actual = result.at(landmarkKey); + + + + // Test that the correct landmark parameters have been recovered + EXPECT(assert_equal(expected, actual, 1e-9)); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */