From 381899640e6b4b7c0930b1f0877e3edfd834f4e5 Mon Sep 17 00:00:00 2001 From: vindelman3 Date: Tue, 4 Feb 2014 14:13:25 -0500 Subject: [PATCH] Moved TransformBtwRobotsUnaryFactor to gtsam_unstable from svn version. --- gtsam_unstable/gtsam_unstable.h | 14 + .../slam/TransformBtwRobotsUnaryFactor.h | 245 ++++++++++++++ .../testTransformBtwRobotsUnaryFactor.cpp | 317 ++++++++++++++++++ 3 files changed, 576 insertions(+) create mode 100644 gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h create mode 100644 gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index c4ad43a21..80ee41b22 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -352,6 +352,20 @@ virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor { void serializable() const; // enabling serialization functionality }; +#include +template +virtual class TransformBtwRobotsUnaryFactor : gtsam::NonlinearFactor { + TransformBtwRobotsUnaryFactor(size_t key, const T& relativePose, size_t keyA, size_t keyB, + const gtsam::Values& valA, const gtsam::Values& valB, + const gtsam::noiseModel::Gaussian* model); + + Vector whitenedError(const gtsam::Values& x); + Vector unwhitenedError(const gtsam::Values& x); + void setValAValB(const gtsam::Values valA, const gtsam::Values valB); + + void serializable() const; // enabling serialization functionality +}; + #include virtual class SmartRangeFactor : gtsam::NoiseModelFactor { SmartRangeFactor(double s); diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h new file mode 100644 index 000000000..c3564a748 --- /dev/null +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -0,0 +1,245 @@ +/* ---------------------------------------------------------------------------- + + * 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 TransformBtwRobotsUnaryFactor.h + * @brief Unary factor for determining transformation between given trajectories of two robots + * @author Vadim Indelman + **/ +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace gtsam { + + /** + * A class for a measurement predicted by "between(config[key1],config[key2])" + * @tparam VALUE the Value type + * @addtogroup SLAM + */ + template + class TransformBtwRobotsUnaryFactor: public NonlinearFactor { + + public: + + typedef VALUE T; + + private: + + typedef TransformBtwRobotsUnaryFactor This; + typedef gtsam::NonlinearFactor Base; + + gtsam::Key key_; + + VALUE measured_; /** The measurement */ + + gtsam::Values valA_; // given values for robot A map\trajectory + gtsam::Values valB_; // given values for robot B map\trajectory + gtsam::Key keyA_; // key of robot A to which the measurement refers + gtsam::Key keyB_; // key of robot B to which the measurement refers + + SharedGaussian model_; + + /** concept check by type */ + GTSAM_CONCEPT_LIE_TYPE(T) + GTSAM_CONCEPT_TESTABLE_TYPE(T) + + public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + TransformBtwRobotsUnaryFactor() {} + + /** Constructor */ + TransformBtwRobotsUnaryFactor(Key key, const VALUE& measured, Key keyA, Key keyB, + const gtsam::Values valA, const gtsam::Values valB, + const SharedGaussian& model) : + Base(cref_list_of<1>(key)), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB), + model_(model){ + + setValAValB(valA, valB); + + } + + virtual ~TransformBtwRobotsUnaryFactor() {} + + + /** Clone */ + virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::make_shared(*this); } + + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "TransformBtwRobotsUnaryFactor(" + << keyFormatter(key_) << ")\n"; + std::cout << "MR between factor keys: " + << keyFormatter(keyA_) << "," + << keyFormatter(keyB_) << "\n"; + measured_.print(" measured: "); + model_->print(" noise model: "); + // Base::print(s, keyFormatter); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& f, double tol=1e-9) const { + const This *t = dynamic_cast (&f); + + if(t && Base::equals(f)) + return key_ == t->key_ && measured_.equals(t->measured_); + else + return false; + } + + /** implement functions needed to derive from Factor */ + + /* ************************************************************************* */ + void setValAValB(const gtsam::Values valA, const gtsam::Values valB){ + if ( (!valA.exists(keyA_)) && (!valB.exists(keyA_)) && (!valA.exists(keyB_)) && (!valB.exists(keyB_)) ) + throw("something is wrong!"); + + // TODO: make sure the two keys belong to different robots + + if (valA.exists(keyA_)){ + valA_ = valA; + valB_ = valB; + } + else { + valA_ = valB; + valB_ = valA; + } + } + + /* ************************************************************************* */ + virtual double error(const gtsam::Values& x) const { + return whitenedError(x).squaredNorm(); + } + + /* ************************************************************************* */ + /** + * Linearize a non-linearFactorN to get a gtsam::GaussianFactor, + * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ + * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ + */ + /* This version of linearize recalculates the noise model each time */ + virtual boost::shared_ptr linearize(const gtsam::Values& x) const { + // Only linearize if the factor is active + if (!this->active(x)) + return boost::shared_ptr(); + + //std::cout<<"About to linearize"< A(this->size()); + gtsam::Vector b = -whitenedError(x, A); + A1 = A[0]; + + return gtsam::GaussianFactor::shared_ptr( + new gtsam::JacobianFactor(key_, A1, b, gtsam::noiseModel::Unit::Create(b.size()))); + } + + + /* ************************************************************************* */ + gtsam::Vector whitenedError(const gtsam::Values& x, + boost::optional&> H = boost::none) const { + + bool debug = true; + + Matrix H_compose, H_between1, H_dummy; + + T orgA_T_currA = valA_.at(keyA_); + T orgB_T_currB = valB_.at(keyB_); + + T orgA_T_orgB = x.at(key_); + + T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, H_dummy); + + T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, H_dummy, H_between1); + + T currA_T_currB_msr = measured_; + + Vector err_unw = currA_T_currB_msr.localCoordinates(currA_T_currB_pred); + + Vector err_wh = err_unw; + if (H) { + (*H)[0] = H_compose * H_between1; + model_->WhitenSystem(*H, err_wh); + } + else { + model_->whitenInPlace(err_wh); + } + + Vector err_wh2 = model_->whiten(err_wh); + + if (debug){ + // std::cout<<"err_wh: "<(keyA_); + T orgB_T_currB = valB_.at(keyB_); + + T orgA_T_orgB = x.at(key_); + + T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB); + + T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB); + + T currA_T_currB_msr = measured_; + + return currA_T_currB_msr.localCoordinates(currA_T_currB_pred); + } + + /* ************************************************************************* */ + + /** number of variables attached to this factor */ + std::size_t size() const { + return 1; + } + + virtual size_t dim() const { + return model_->R().rows() + model_->R().cols(); + } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor", + boost::serialization::base_object(*this)); + //ar & BOOST_SERIALIZATION_NVP(measured_); + } + }; // \class TransformBtwRobotsUnaryFactor + +} /// namespace gtsam diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp new file mode 100644 index 000000000..42eb943af --- /dev/null +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp @@ -0,0 +1,317 @@ +/** + * @file testBTransformBtwRobotsUnaryFactor.cpp + * @brief Unit test for the TransformBtwRobotsUnaryFactor + * @author Vadim Indelman + */ + +#include + + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +//#include + + +using namespace std; +using namespace gtsam; + +// Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below +// to reenable the test. +//#if 0 +/* ************************************************************************* */ +LieVector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactor& factor){ + gtsam::Values values; + values.insert(key, org1_T_org2); + // LieVector err = factor.whitenedError(values); + // return err; + return LieVector::Expmap(factor.whitenedError(values)); +} + +/* ************************************************************************* */ +//LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor& factor){ +// gtsam::Values values; +// values.insert(keyA, p1); +// values.insert(keyB, p2); +// // LieVector err = factor.whitenedError(values); +// // return err; +// return LieVector::Expmap(factor.whitenedError(values)); +//} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactor, ConstructorAndEquals) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 p1(10.0, 15.0, 0.1); + gtsam::Pose2 p2(15.0, 15.0, 0.3); + gtsam::Pose2 noise(0.5, 0.4, 0.01); + gtsam::Pose2 rel_pose_ideal = p1.between(p2); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); + + SharedGaussian model(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05))); + + gtsam::Values valA, valB; + valA.insert(keyA, p1); + valB.insert(keyB, p2); + + // Constructor + TransformBtwRobotsUnaryFactor g(key, rel_pose_msr, keyA, keyB, valA, valB, model); + TransformBtwRobotsUnaryFactor h(key, rel_pose_msr, keyA, keyB, valA, valB, model); + + // Equals + CHECK(assert_equal(g, h, 1e-5)); +} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactor, unwhitenedError) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1); + gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3); + + gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8); + + gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(orgB_T_2); + + gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal; + + SharedGaussian model(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05))); + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_1); + valB.insert(keyB, orgB_T_2); + + // Constructor + TransformBtwRobotsUnaryFactor g(key, rel_pose_msr, keyA, keyB, valA, valB, model); + + gtsam::Values values; + values.insert(key, orgA_T_orgB); + Vector err = g.unwhitenedError(values); + + // Equals + CHECK(assert_equal(err, zero(3), 1e-5)); +} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactor, unwhitenedError2) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0); + gtsam::Pose2 orgB_T_currB(-10.0, 15.0, 0.1); + + gtsam::Pose2 orgA_T_orgB(0.0, 0.0, 0.0); + + gtsam::Pose2 orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB); + + gtsam::Pose2 rel_pose_ideal = orgA_T_currA.between(orgA_T_currB); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal; + + SharedGaussian model(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05))); + + double prior_outlier = 0.01; + double prior_inlier = 0.99; + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_currA); + valB.insert(keyB, orgB_T_currB); + + // Constructor + TransformBtwRobotsUnaryFactor g(key, rel_pose_msr, keyA, keyB, valA, valB, model); + + gtsam::Values values; + values.insert(key, orgA_T_orgB); + Vector err = g.unwhitenedError(values); + + // Equals + CHECK(assert_equal(err, zero(3), 1e-5)); +} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactor, Optimize) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0); + gtsam::Pose2 orgB_T_currB(1.0, 2.0, 0.05); + + gtsam::Pose2 orgA_T_orgB_tr(10.0, -15.0, 0.0); + gtsam::Pose2 orgA_T_currB_tr = orgA_T_orgB_tr.compose(orgB_T_currB); + gtsam::Pose2 currA_T_currB_tr = orgA_T_currA.between(orgA_T_currB_tr); + + // some error in measurements + // gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, 0.01)); + // gtsam::Pose2 currA_Tmsr_currB2 = currA_T_currB_tr.compose(gtsam::Pose2(-0.1, 0.02, 0.01)); + // gtsam::Pose2 currA_Tmsr_currB3 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, -0.02, 0.01)); + // gtsam::Pose2 currA_Tmsr_currB4 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, -0.01)); + + // ideal measurements + gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.0, 0.0, 0.0)); + gtsam::Pose2 currA_Tmsr_currB2 = currA_Tmsr_currB1; + gtsam::Pose2 currA_Tmsr_currB3 = currA_Tmsr_currB1; + gtsam::Pose2 currA_Tmsr_currB4 = currA_Tmsr_currB1; + + SharedGaussian model(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05))); + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_currA); + valB.insert(keyB, orgB_T_currB); + + // Constructor + TransformBtwRobotsUnaryFactor g1(key, currA_Tmsr_currB1, keyA, keyB, valA, valB, model); + + TransformBtwRobotsUnaryFactor g2(key, currA_Tmsr_currB2, keyA, keyB, valA, valB, model); + + TransformBtwRobotsUnaryFactor g3(key, currA_Tmsr_currB3, keyA, keyB, valA, valB, model); + + TransformBtwRobotsUnaryFactor g4(key, currA_Tmsr_currB4, keyA, keyB, valA, valB, model); + + gtsam::Values values; + values.insert(key, gtsam::Pose2()); + + gtsam::NonlinearFactorGraph graph; + graph.push_back(g1); + graph.push_back(g2); + graph.push_back(g3); + graph.push_back(g4); + + gtsam::GaussNewtonParams params; + gtsam::GaussNewtonOptimizer optimizer(graph, values, params); + gtsam::Values result = optimizer.optimize(); + + gtsam::Pose2 orgA_T_orgB_opt = result.at(key); + + CHECK(assert_equal(orgA_T_orgB_opt, orgA_T_orgB_tr, 1e-5)); +} + + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactor, Jacobian) +{ + gtsam::Key key(0); + gtsam::Key keyA(1); + gtsam::Key keyB(2); + + gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1); + gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3); + + gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8); + + gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(orgB_T_2); + + gtsam::Pose2 noise(0.5, 0.4, 0.01); + + gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); + + SharedGaussian model(noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.05))); + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_1); + valB.insert(keyB, orgB_T_2); + + // Constructor + TransformBtwRobotsUnaryFactor g(key, rel_pose_msr, keyA, keyB, valA, valB, model); + + gtsam::Values values; + values.insert(key, orgA_T_orgB); + + std::vector H_actual(1); + Vector actual_err_wh = g.whitenedError(values, H_actual); + + Matrix H1_actual = H_actual[0]; + + double stepsize = 1.0e-9; + Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); +// CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); +} + + + + +/////* ************************************************************************** */ +//TEST (TransformBtwRobotsUnaryFactor, jacobian ) { +// +// gtsam::Key keyA(1); +// gtsam::Key keyB(2); +// +// // Inlier test +// gtsam::Pose2 p1(10.0, 15.0, 0.1); +// gtsam::Pose2 p2(15.0, 15.0, 0.3); +// gtsam::Pose2 noise(0.5, 0.4, 0.01); +// gtsam::Pose2 rel_pose_ideal = p1.between(p2); +// gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); +// +// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 0.5, 0.5, 0.05))); +// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vec(3) << 50.0, 50.0, 10.0))); +// +// gtsam::Values values; +// values.insert(keyA, p1); +// values.insert(keyB, p2); +// +// double prior_outlier = 0.0; +// double prior_inlier = 1.0; +// +// TransformBtwRobotsUnaryFactor f(keyA, keyB, rel_pose_msr, model_inlier, model_outlier, +// prior_inlier, prior_outlier); +// +// std::vector H_actual(2); +// Vector actual_err_wh = f.whitenedError(values, H_actual); +// +// Matrix H1_actual = H_actual[0]; +// Matrix H2_actual = H_actual[1]; +// +// // compare to standard between factor +// BetweenFactor h(keyA, keyB, rel_pose_msr, model_inlier ); +// Vector actual_err_wh_stnd = h.whitenedError(values); +// Vector actual_err_wh_inlier = (Vec(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); +// CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8)); +// std::vector H_actual_stnd_unwh(2); +// (void)h.unwhitenedError(values, H_actual_stnd_unwh); +// Matrix H1_actual_stnd_unwh = H_actual_stnd_unwh[0]; +// Matrix H2_actual_stnd_unwh = H_actual_stnd_unwh[1]; +// Matrix H1_actual_stnd = model_inlier->Whiten(H1_actual_stnd_unwh); +// Matrix H2_actual_stnd = model_inlier->Whiten(H2_actual_stnd_unwh); +//// CHECK( assert_equal(H1_actual_stnd, H1_actual, 1e-8)); +//// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); +// +// double stepsize = 1.0e-9; +// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); +// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); +// +// +// // try to check numerical derivatives of a standard between factor +// Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); +// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); +// +// +// CHECK( assert_equal(H1_expected, H1_actual, 1e-8)); +// CHECK( assert_equal(H2_expected, H2_actual, 1e-8)); +// +//} + +//#endif + +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */