diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h new file mode 100644 index 000000000..621edf5de --- /dev/null +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -0,0 +1,311 @@ +/* ---------------------------------------------------------------------------- + + * 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 TransformBtwRobotsUnaryFactorEM.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 TransformBtwRobotsUnaryFactorEM: public NonlinearFactor { + + public: + + typedef VALUE T; + + private: + + typedef TransformBtwRobotsUnaryFactorEM 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 + + // TODO: create function to update valA_ and valB_ + + SharedGaussian model_inlier_; + SharedGaussian model_outlier_; + + double prior_inlier_; + double prior_outlier_; + + bool flag_bump_up_near_zero_probs_; + + /** 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 */ + TransformBtwRobotsUnaryFactorEM() {} + + /** Constructor */ + TransformBtwRobotsUnaryFactorEM(Key key, const VALUE& measured, Key keyA, Key keyB, + const gtsam::Values valA, const gtsam::Values valB, + const SharedGaussian& model_inlier, const SharedGaussian& model_outlier, + const double prior_inlier, const double prior_outlier) : + Base(key), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB), + model_inlier_(model_inlier), model_outlier_(model_outlier), + prior_inlier_(prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(false){ + + setValAValB(valA, valB); + + } + + virtual ~TransformBtwRobotsUnaryFactorEM() {} + + + /** 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 << "TransformBtwRobotsUnaryFactorEM(" + << keyFormatter(key_) << ")\n"; + std::cout << "MR between factor keys: " + << keyFormatter(keyA_) << "," + << keyFormatter(keyB_) << "\n"; + measured_.print(" measured: "); + model_inlier_->print(" noise model inlier: "); + model_outlier_->print(" noise model outlier: "); + std::cout << "(prior_inlier, prior_outlier_) = (" + << prior_inlier_ << "," + << prior_outlier_ << ")\n"; + // 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_) && + // 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_; + 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 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]; + + return gtsam::GaussianFactor::shared_ptr( + new gtsam::JacobianFactor(ordering[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 = currA_T_currB_msr.localCoordinates(currA_T_currB_pred); + + // Calculate indicator probabilities (inlier and outlier) + Vector p_inlier_outlier = calcIndicatorProb(x); + double p_inlier = p_inlier_outlier[0]; + double p_outlier = p_inlier_outlier[1]; + + 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(); + + 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(); + + Matrix H_unwh = H_compose * H_between1; + + if (H){ + + Matrix H_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H_unwh); + Matrix H_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H_unwh); + Matrix H_aug = gtsam::stack(2, &H_inlier, &H_outlier); + + (*H)[0].resize(H_aug.rows(),H_aug.cols()); + (*H)[0] = H_aug; + } + + if (debug){ + // std::cout<<"H_compose - rows, cols, : "<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_ * sqrt(invCov_inlier.norm()) * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) ); + double p_outlier = prior_outlier_ * sqrt(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; + + // if (flag_bump_up_near_zero_probs_){ + // Bump up near-zero probabilities (as in linerFlow.h) + double minP = 0.05; // == 0.1 / 2 indicator variables + if (p_inlier < minP || p_outlier < minP){ + if (p_inlier < minP) + p_inlier = minP; + if (p_outlier < minP) + p_outlier = minP; + sumP = p_inlier + p_outlier; + p_inlier /= sumP; + p_outlier /= sumP; + } + // } + + return Vector_(2, p_inlier, p_outlier); + } + + /* ************************************************************************* */ + gtsam::Vector unwhitenedError(const gtsam::Values& x) const { + + 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); + + 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_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 TransformBtwRobotsUnaryFactorEM + +} /// namespace gtsam diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp new file mode 100644 index 000000000..c7609b980 --- /dev/null +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp @@ -0,0 +1,335 @@ +/** + * @file testBTransformBtwRobotsUnaryFactorEM.cpp + * @brief Unit test for the TransformBtwRobotsUnaryFactorEM + * @author Vadim Indelman + */ + +#include + + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +//#include + + +using namespace std; +using namespace gtsam; + + +/* ************************************************************************* */ +LieVector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactorEM& 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( TransformBtwRobotsUnaryFactorEM, 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_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; + + gtsam::Values valA, valB; + valA.insert(keyA, p1); + valB.insert(keyB, p2); + + // Constructor + TransformBtwRobotsUnaryFactorEM g(key, rel_pose_msr, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + TransformBtwRobotsUnaryFactorEM h(key, rel_pose_msr, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + + // Equals + CHECK(assert_equal(g, h, 1e-5)); +} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactorEM, 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_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.01; + double prior_inlier = 0.99; + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_1); + valB.insert(keyB, orgB_T_2); + + // Constructor + TransformBtwRobotsUnaryFactorEM g(key, rel_pose_msr, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + + gtsam::Values values; + values.insert(key, orgA_T_orgB); + Vector err = g.unwhitenedError(values); + + // Equals + CHECK(assert_equal(err, zero(3), 1e-5)); +} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactorEM, 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_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.01; + double prior_inlier = 0.99; + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_currA); + valB.insert(keyB, orgB_T_currB); + + // Constructor + TransformBtwRobotsUnaryFactorEM g(key, rel_pose_msr, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + + gtsam::Values values; + values.insert(key, orgA_T_orgB); + Vector err = g.unwhitenedError(values); + + // Equals + CHECK(assert_equal(err, zero(3), 1e-5)); +} + +/* ************************************************************************* */ +TEST( TransformBtwRobotsUnaryFactorEM, 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_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.01; + double prior_inlier = 0.99; + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_currA); + valB.insert(keyB, orgB_T_currB); + + // Constructor + TransformBtwRobotsUnaryFactorEM g1(key, currA_Tmsr_currB1, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + + TransformBtwRobotsUnaryFactorEM g2(key, currA_Tmsr_currB2, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + + TransformBtwRobotsUnaryFactorEM g3(key, currA_Tmsr_currB3, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + + TransformBtwRobotsUnaryFactorEM g4(key, currA_Tmsr_currB4, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + + gtsam::Values values; + values.insert(key, gtsam::Pose2()); + + gtsam::NonlinearFactorGraph graph; + graph.add(g1); + graph.add(g2); + graph.add(g3); + graph.add(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( TransformBtwRobotsUnaryFactorEM, 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_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; + + gtsam::Values valA, valB; + valA.insert(keyA, orgA_T_1); + valB.insert(keyB, orgB_T_2); + + // Constructor + TransformBtwRobotsUnaryFactorEM g(key, rel_pose_msr, keyA, keyB, valA, valB, + model_inlier, model_outlier,prior_inlier, prior_outlier); + + 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 (TransformBtwRobotsUnaryFactorEM, 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::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(keyA, p1); +// values.insert(keyB, p2); +// +// double prior_outlier = 0.0; +// double prior_inlier = 1.0; +// +// TransformBtwRobotsUnaryFactorEM 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 = 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, 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)); +// +//} + +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */