From d33f435eabd7fff9990a326a8d7d6f7568cec5c9 Mon Sep 17 00:00:00 2001 From: Vadim Indelman Date: Wed, 7 Aug 2013 15:25:00 +0000 Subject: [PATCH] Added BetweenFactorEM with 2 indicator variables. --- gtsam_unstable/gtsam_unstable.h | 10 + gtsam_unstable/slam/BetweenFactorEM.h | 276 ++++++++++ .../slam/tests/testBetweenFactorEM.cpp | 477 ++++++++++++++++++ 3 files changed, 763 insertions(+) create mode 100644 gtsam_unstable/slam/BetweenFactorEM.h create mode 100644 gtsam_unstable/slam/tests/testBetweenFactorEM.cpp diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 061dd6ec3..b48a47c54 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -13,6 +13,7 @@ virtual class gtsam::Point3; virtual class gtsam::Rot3; virtual class gtsam::Pose3; virtual class gtsam::noiseModel::Base; +virtual class gtsam::noiseModel::Gaussian; virtual class gtsam::imuBias::ConstantBias; virtual class gtsam::NonlinearFactor; virtual class gtsam::GaussianFactor; @@ -308,6 +309,15 @@ virtual class BetweenFactor : gtsam::NonlinearFactor { void serializable() const; // enabling serialization functionality }; +//#include +//template +//virtual class BetweenFactorEM : gtsam::NonlinearFactor { +// BetweenFactorEM(size_t key1, size_t key2, const T& relativePose, +// const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, +// double prior_inlier, double prior_outlier); +// +// void serializable() const; // enabling serialization functionality +//}; #include template diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h new file mode 100644 index 000000000..d856a6772 --- /dev/null +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -0,0 +1,276 @@ +/* ---------------------------------------------------------------------------- + + * 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 BetweenFactorEM.h + * @author Vadim Indelman + **/ +#pragma once + +#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 BetweenFactorEM: public NonlinearFactor { + + public: + + typedef VALUE T; + + private: + + typedef BetweenFactorEM This; + typedef gtsam::NonlinearFactor Base; + + gtsam::Key key1_; + gtsam::Key key2_; + + VALUE measured_; /** The measurement */ + + SharedGaussian model_inlier_; + SharedGaussian model_outlier_; + + double prior_inlier_; + double prior_outlier_; + + /** 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 */ + BetweenFactorEM() {} + + /** Constructor */ + BetweenFactorEM(Key key1, Key key2, const VALUE& measured, + const SharedGaussian& model_inlier, const SharedGaussian& model_outlier, + const double prior_inlier, const double prior_outlier) : + key1_(key1), key2_(key2), measured_(measured), + model_inlier_(model_inlier), model_outlier_(model_outlier), + prior_inlier_(prior_inlier), prior_outlier_(prior_outlier){ + } + + virtual ~BetweenFactorEM() {} + + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + 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 key1_ == t->key1_ && key2_ == t->key2_ && + // model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here + // model_outlier_->equals(t->model_outlier_ ) && + prior_outlier_ == t->prior_outlier_ && prior_inlier_ == t->prior_inlier_ && measured_.equals(t->measured_); + else + return false; + } + + /** implement functions needed to derive from Factor */ + + /* ************************************************************************* */ + 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 gtsam::Ordering& ordering) 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]; + A2 = A[1]; + + return gtsam::GaussianFactor::shared_ptr( + new gtsam::JacobianFactor(ordering[key1_], A1, ordering[key2_], A2, b, gtsam::noiseModel::Unit::Create(b.size()))); + } + + + /* ************************************************************************* */ + gtsam::Vector whitenedError(const gtsam::Values& x, + boost::optional&> H = boost::none) const { + + bool debug = true; + + const T& p1 = x.at(key1_); + const T& p2 = x.at(key2_); + + Matrix H1, H2; + + T hx = p1.between(p2, H1, H2); // h(x) + // manifold equivalent of h(x)-z -> log(z,h(x)) + + Vector err = measured_.localCoordinates(hx); + + // Calculate indicator probabilities (inlier and outlier) + Vector err_wh_inlier = model_inlier_->whiten(err); + Vector err_wh_outlier = model_outlier_->whiten(err); + + Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R(); + Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R(); + + double p_inlier = prior_inlier_ * invCov_inlier.norm() * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) ); + double p_outlier = prior_outlier_ * invCov_outlier.norm() * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) ); + + double sumP = p_inlier + p_outlier; + p_inlier /= sumP; + p_outlier /= sumP; + // TODO: possibly need to bump up near-zero probabilities (as in linerFlow.h) + + Vector err_wh_eq; + err_wh_eq.resize(err_wh_inlier.rows()*2); + err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array() , sqrt(p_outlier) * err_wh_outlier.array(); + + if (H){ + + // stack Jacobians for the two indicators for each of the key + + Matrix H1_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H1); + Matrix H1_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H1); + Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier); + + Matrix H2_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H2); + Matrix H2_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H2); + Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier); + + (*H)[0].resize(H1_aug.rows(),H1_aug.cols()); + (*H)[1].resize(H2_aug.rows(),H2_aug.cols()); + + (*H)[0] = H1_aug; + (*H)[1] = H2_aug; + } + + if (debug){ + std::cout<<"unwhitened error: "<(key1_); + const T& p2 = x.at(key2_); + + Matrix H1, H2; + + T hx = p1.between(p2, H1, H2); // h(x) + // manifold equivalent of h(x)-z -> log(z,h(x)) + + err = measured_.localCoordinates(hx); + + // Calculate indicator probabilities (inlier and outlier) + Vector err_wh_inlier = model_inlier_->whiten(err); + Vector err_wh_outlier = model_outlier_->whiten(err); + + Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R(); + Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R(); + + p_inlier = prior_inlier_ * invCov_inlier.norm() * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) ); + p_outlier = prior_outlier_ * invCov_outlier.norm() * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) ); + + double sumP = p_inlier + p_outlier; + p_inlier /= sumP; + p_outlier /= sumP; + // TODO: possibly need to bump up near-zero probabilities (as in linerFlow.h) + } + + /** return the measured */ + const VALUE& measured() const { + return measured_; + } + + /** number of variables attached to this factor */ + std::size_t size() const { + return 2; + } + + virtual size_t dim() const { + return model_inlier_->R().rows() + model_inlier_->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 BetweenFactorEM + +} /// namespace gtsam diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp new file mode 100644 index 000000000..08cbb856c --- /dev/null +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -0,0 +1,477 @@ +/** + * @file testBetweenFactorEM.cpp + * @brief Unit test for the BetweenFactorEM + * @author Vadim Indelman + */ + +#include + + +#include +#include +#include +#include +#include + +#include + +//#include +//#include +//#include + + +using namespace std; +using namespace gtsam; + + +/* ************************************************************************* */ +LieVector predictionError(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactorEM& factor){ + gtsam::Values values; + values.insert(key1, p1); + values.insert(key2, p2); + // 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& key1, const gtsam::Key& key2, const BetweenFactor& factor){ + gtsam::Values values; + values.insert(key1, p1); + values.insert(key2, p2); + // LieVector err = factor.whitenedError(values); + // return err; + return LieVector::Expmap(factor.whitenedError(values)); +} + +/* ************************************************************************* */ +TEST( BetweenFactorEM, ConstructorAndEquals) +{ + gtsam::Key key1(1); + gtsam::Key key2(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_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 5, 5, 1.0))); + + double prior_outlier = 0.5; + double prior_inlier = 0.5; + + // Constructor + BetweenFactorEM f(key1, key2, rel_pose_msr, model_inlier, model_outlier, + prior_inlier, prior_outlier); + BetweenFactorEM g(key1, key2, rel_pose_msr, model_inlier, model_outlier, + prior_inlier, prior_outlier); + + // Equals + CHECK(assert_equal(f, g, 1e-5)); +} + +/* ************************************************************************* */ +TEST( BetweenFactorEM, EvaluateError) +{ + gtsam::Key key1(1); + gtsam::Key key2(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::Vector_(3, 0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector_(3, 50.0, 50.0, 10.0))); + + gtsam::Values values; + values.insert(key1, p1); + values.insert(key2, p2); + + double prior_outlier = 0.5; + double prior_inlier = 0.5; + + BetweenFactorEM f(key1, key2, rel_pose_msr, model_inlier, model_outlier, + prior_inlier, prior_outlier); + + Vector actual_err_wh = f.whitenedError(values); + + Vector actual_err_wh_inlier = Vector_(3, actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + Vector actual_err_wh_outlier = Vector_(3, actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); + + // in case of inlier, inlier-mode whitented error should be dominant + assert(actual_err_wh_inlier.norm() > 1000.0*actual_err_wh_outlier.norm()); + + cout << "Inlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "< g(key1, key2, rel_pose_msr_test2, model_inlier, model_outlier, + prior_inlier, prior_outlier); + + actual_err_wh = g.whitenedError(values); + + actual_err_wh_inlier = Vector_(3, actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + actual_err_wh_outlier = Vector_(3, actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); + + // in case of outlier, outlier-mode whitented error should be dominant + assert(actual_err_wh_inlier.norm() < 1000.0*actual_err_wh_outlier.norm()); + + cout << "Outlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "< h_EM(key1, key2, rel_pose_msr, model_inlier, model_outlier, + prior_inlier, prior_outlier); + actual_err_wh = h_EM.whitenedError(values); + actual_err_wh_inlier = Vector_(3, actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + + BetweenFactor h(key1, key2, rel_pose_msr, model_inlier ); + Vector actual_err_wh_stnd = h.whitenedError(values); + + cout<<"actual_err_wh: "< f(key1, key2, 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(key1, key2, rel_pose_msr, model_inlier ); + Vector actual_err_wh_stnd = h.whitenedError(values); + Vector actual_err_wh_inlier = Vector_(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, key1, key2, f), p1, stepsize); + Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, key1, key2, 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, key1, key2, 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)); + +} + +/* ************************************************************************* */ +TEST( InertialNavFactor, Equals) +{ +// gtsam::Key Pose1(11); +// gtsam::Key Pose2(12); +// gtsam::Key Vel1(21); +// gtsam::Key Vel2(22); +// gtsam::Key Bias1(31); +// +// Vector measurement_acc(Vector_(3,0.1,0.2,0.4)); +// Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03)); +// +// double measurement_dt(0.1); +// Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); +// Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system +// gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); +// gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); +// +// SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); +// +// InertialNavFactor f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); +// InertialNavFactor g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); +// CHECK(assert_equal(f, g, 1e-5)); +} + +/* ************************************************************************* */ +TEST( InertialNavFactor, Predict) +{ +// gtsam::Key PoseKey1(11); +// gtsam::Key PoseKey2(12); +// gtsam::Key VelKey1(21); +// gtsam::Key VelKey2(22); +// gtsam::Key BiasKey1(31); +// +// double measurement_dt(0.1); +// Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); +// Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system +// gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); +// gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); +// +// SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); +// +// +// // First test: zero angular motion, some acceleration +// Vector measurement_acc(Vector_(3,0.1,0.2,0.3-9.81)); +// Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); +// +// InertialNavFactor f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); +// +// Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); +// LieVector Vel1(3, 0.50, -0.50, 0.40); +// imuBias::ConstantBias Bias1; +// Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); +// LieVector expectedVel2(3, 0.51, -0.48, 0.43); +// Pose3 actualPose2; +// LieVector actualVel2; +// f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); +// +// CHECK(assert_equal(expectedPose2, actualPose2, 1e-5)); +// CHECK(assert_equal(expectedVel2, actualVel2, 1e-5)); +} + +/* ************************************************************************* */ +TEST( InertialNavFactor, ErrorPosVel) +{ +// gtsam::Key PoseKey1(11); +// gtsam::Key PoseKey2(12); +// gtsam::Key VelKey1(21); +// gtsam::Key VelKey2(22); +// gtsam::Key BiasKey1(31); +// +// double measurement_dt(0.1); +// Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); +// Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system +// gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); +// gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); +// +// SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); +// +// +// // First test: zero angular motion, some acceleration +// Vector measurement_acc(Vector_(3,0.1,0.2,0.3-9.81)); +// Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); +// +// InertialNavFactor f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); +// +// Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); +// Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); +// LieVector Vel1(3, 0.50, -0.50, 0.40); +// LieVector Vel2(3, 0.51, -0.48, 0.43); +// imuBias::ConstantBias Bias1; +// +// Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); +// Vector ExpectedErr(zero(9)); +// +// CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); +} + +/* ************************************************************************* */ +TEST( InertialNavFactor, ErrorRot) +{ +// gtsam::Key PoseKey1(11); +// gtsam::Key PoseKey2(12); +// gtsam::Key VelKey1(21); +// gtsam::Key VelKey2(22); +// gtsam::Key BiasKey1(31); +// +// double measurement_dt(0.1); +// Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); +// Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system +// gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); +// gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); +// +// SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); +// +// // Second test: zero angular motion, some acceleration +// Vector measurement_acc(Vector_(3,0.0,0.0,0.0-9.81)); +// Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); +// +// InertialNavFactor f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); +// +// Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); +// Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0)); +// LieVector Vel1(3,0.0,0.0,0.0); +// LieVector Vel2(3,0.0,0.0,0.0); +// imuBias::ConstantBias Bias1; +// +// Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); +// Vector ExpectedErr(zero(9)); +// +// CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); +} + +/* ************************************************************************* */ +TEST( InertialNavFactor, ErrorRotPosVel) +{ +// gtsam::Key PoseKey1(11); +// gtsam::Key PoseKey2(12); +// gtsam::Key VelKey1(21); +// gtsam::Key VelKey2(22); +// gtsam::Key BiasKey1(31); +// +// double measurement_dt(0.1); +// Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); +// Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system +// gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); +// gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); +// +// SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); +// +// // Second test: zero angular motion, some acceleration - generated in matlab +// Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343)); +// Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); +// +// InertialNavFactor f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); +// +// Rot3 R1(0.487316618, 0.125253866, 0.86419557, +// 0.580273724, 0.693095498, -0.427669306, +// -0.652537293, 0.709880342, 0.265075427); +// Point3 t1(2.0,1.0,3.0); +// Pose3 Pose1(R1, t1); +// LieVector Vel1(3,0.5,-0.5,0.4); +// Rot3 R2(0.473618898, 0.119523052, 0.872582019, +// 0.609241153, 0.67099888, -0.422594037, +// -0.636011287, 0.731761397, 0.244979388); +// Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); +// Pose3 Pose2(R2, t2); +// LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000); +// imuBias::ConstantBias Bias1; +// +// Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); +// Vector ExpectedErr(zero(9)); +// +// CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); +} + + +/* ************************************************************************* */ +TEST (InertialNavFactor, Jacobian ) { + +// gtsam::Key PoseKey1(11); +// gtsam::Key PoseKey2(12); +// gtsam::Key VelKey1(21); +// gtsam::Key VelKey2(22); +// gtsam::Key BiasKey1(31); +// +// double measurement_dt(0.01); +// Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); +// Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system +// gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); +// gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); +// +// SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); +// +// Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343)); +// Vector measurement_gyro(Vector_(3, 3.14, 3.14/2, -3.14)); +// +// InertialNavFactor factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); +// +// Rot3 R1(0.487316618, 0.125253866, 0.86419557, +// 0.580273724, 0.693095498, -0.427669306, +// -0.652537293, 0.709880342, 0.265075427); +// Point3 t1(2.0,1.0,3.0); +// Pose3 Pose1(R1, t1); +// LieVector Vel1(3,0.5,-0.5,0.4); +// Rot3 R2(0.473618898, 0.119523052, 0.872582019, +// 0.609241153, 0.67099888, -0.422594037, +// -0.636011287, 0.731761397, 0.244979388); +// Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); +// Pose3 Pose2(R2, t2); +// LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000); +// imuBias::ConstantBias Bias1; +// +// Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; +// +// Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual)); +// +// // Checking for Pose part in the jacobians +// // ****** +// Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols())); +// Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols())); +// Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols())); +// Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols())); +// Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols())); +// +// // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function +// gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; +// H1_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); +// H2_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); +// H3_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); +// H4_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); +// H5_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); +// +// // Verify they are equal for this choice of state +// CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-6)); +// CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-6)); +// CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 1e-6)); +// CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-6)); +// CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-6)); +// +// // Checking for Vel part in the jacobians +// // ****** +// Matrix H1_actualVel(H1_actual.block(6,0,3,H1_actual.cols())); +// Matrix H2_actualVel(H2_actual.block(6,0,3,H2_actual.cols())); +// Matrix H3_actualVel(H3_actual.block(6,0,3,H3_actual.cols())); +// Matrix H4_actualVel(H4_actual.block(6,0,3,H4_actual.cols())); +// Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols())); +// +// // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function +// gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; +// H1_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); +// H2_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); +// H3_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); +// H4_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); +// H5_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); +// +// // Verify they are equal for this choice of state +// CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-6)); +// CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-6)); +// CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-6)); +// CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-6)); +// CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-6)); +} + + + +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */