diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h new file mode 100644 index 000000000..013d78c1a --- /dev/null +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2014, 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 EssentialMatrixConstraint.h + * @author Frank Dellaert + * @author Pablo Alcantarilla + * @date Jan 5, 2014 + **/ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace gtsam { + +/** + * Binary factor between two Pose3 variables induced by an EssentialMatrix measurement + * @addtogroup SLAM + */ +class EssentialMatrixConstraint: public NoiseModelFactor2 { + +private: + + typedef EssentialMatrixConstraint This; + typedef NoiseModelFactor2 Base; + + EssentialMatrix measuredE_; /** The measurement is an essential matrix */ + +public: + + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + EssentialMatrixConstraint() { + } + + /** + * Constructor + * @param key1 key for first pose + * @param key2 key for second pose + * @param measuredE measured EssentialMatrix + * @param model noise model, 5D ! + */ + EssentialMatrixConstraint(Key key1, Key key2, + const EssentialMatrix& measuredE, const SharedNoiseModel& model) : + Base(model, key1, key2), measuredE_(measuredE) { + } + + virtual ~EssentialMatrixConstraint() { + } + + /// @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))); + } + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key1()) + << "," << keyFormatter(this->key2()) << ")\n"; + measuredE_.print(" measured: "); + this->noiseModel_->print(" noise model: "); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) + && this->measuredE_.equals(e->measuredE_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const Pose3& p1, const Pose3& p2, + boost::optional Hp1 = boost::none, // + boost::optional Hp2 = boost::none) const { + // compute relative Pose3 between p1 and p2 + Pose3 _1P2_ = p1.between(p2, Hp1, Hp2); + // convert to EssentialMatrix + Matrix D_hx_1P2; + EssentialMatrix hx = EssentialMatrix::FromPose3(_1P2_, + (Hp1 || Hp2) ? boost::optional(D_hx_1P2) : boost::none); + // Calculate derivatives if needed + if (Hp1) { + // Hp1 will already contain the 6*6 derivative D_1P2_p1 + const Matrix& D_1P2_p1 = *Hp1; + // The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2: + *Hp1 = D_hx_1P2 * D_1P2_p1; + } + if (Hp2) { + // Hp2 will already contain the 6*6 derivative D_1P2_p1 + const Matrix& D_1P2_p2 = *Hp2; + // The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2: + *Hp2 = D_hx_1P2 * D_1P2_p2; + } + // manifold equivalent of h(x)-z -> log(z,h(x)) + return measuredE_.localCoordinates(hx); // 5D error + } + + /** return the measured */ + const EssentialMatrix& measured() const { + return measuredE_; + } + + /** number of variables attached to this factor */ + std::size_t size() const { + return 2; + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measuredE_); + } +}; +// \class EssentialMatrixConstraint + +}/// namespace gtsam diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp new file mode 100644 index 000000000..eb87100f6 --- /dev/null +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -0,0 +1,76 @@ +/* ---------------------------------------------------------------------------- + + * 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 testEssentialMatrixConstraint.cpp + * @brief Unit tests for EssentialMatrixConstraint Class + * @author Frank Dellaert + * @author Pablo Alcantarilla + * @date Jan 5, 2014 + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( EssentialMatrixConstraint, test ) { + // Create a factor + Key poseKey1(1); + Key poseKey2(2); + Rot3 trueRotation = Rot3::RzRyRx(0.15, 0.15, -0.20); + Point3 trueTranslation(+0.5, -1.0, +1.0); + Sphere2 trueDirection(trueTranslation); + EssentialMatrix measurement(trueRotation, trueDirection); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 0.25); + EssentialMatrixConstraint factor(poseKey1, poseKey2, measurement, model); + + // Create a linearization point at the zero-error point + Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0)); + Pose3 pose2( + Rot3::RzRyRx(0.179693265735950, 0.002945368776519, 0.102274823253840), + Point3(-3.37493895, 6.14660244, -8.93650986)); + + Vector expected = zero(5); + Vector actual = factor.evaluateError(pose1,pose2); + CHECK(assert_equal(expected, actual, 1e-8)); + + // Calculate numerical derivatives + Matrix expectedH1 = numericalDerivative11( + boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, + boost::none, boost::none), pose1); + Matrix expectedH2 = numericalDerivative11( + boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1, + boost::none, boost::none), pose2); + + // Use the factor to calculate the derivative + Matrix actualH1; + Matrix actualH2; + factor.evaluateError(pose1, pose2, actualH1, actualH2); + + // Verify we get the expected error + CHECK(assert_equal(expectedH1, actualH1, 1e-9)); + CHECK(assert_equal(expectedH2, actualH2, 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +