diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h new file mode 100644 index 000000000..87b9f4f5c --- /dev/null +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -0,0 +1,130 @@ +/* ---------------------------------------------------------------------------- + + * 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 BetweenFactor.h + * @author Frank Dellaert, Viorela Ila + **/ +#pragma once + +#include + +#include +#include +#include + +namespace gtsam { + + /** + * A class for a measurement predicted by "between(config[key1],config[key2])" + * @tparam POSE the Pose type + * @addtogroup SLAM + */ + template + class PoseBetweenFactor: public NoiseModelFactor2 { + + private: + + typedef PoseBetweenFactor This; + typedef NoiseModelFactor2 Base; + + POSE measured_; /** The measurement */ + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + + /** concept check by type */ + GTSAM_CONCEPT_TESTABLE_TYPE(POSE) + GTSAM_CONCEPT_POSE_TYPE(POSE) + public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + PoseBetweenFactor() {} + + /** Constructor */ + PoseBetweenFactor(Key key1, Key key2, const POSE& measured, + const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none) : + Base(model, key1, key2), measured_(measured), body_P_sensor_(body_P_sensor) { + } + + virtual ~PoseBetweenFactor() {} + + /// @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 << "BetweenFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n"; + measured_.print(" measured: "); + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + 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->measured_.equals(e->measured_, tol) + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const POSE& p1, const POSE& p2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + if(body_P_sensor_) { + POSE hx; + if(H1 || H2) { + Matrix H3; + hx = p1.compose(*body_P_sensor_,H3).between(p2.compose(*body_P_sensor_), H1, H2); // h(x) + if(H1) (*H1) *= H3; + if(H2) (*H2) *= H3; + } else { + hx = p1.compose(*body_P_sensor_).between(p2.compose(*body_P_sensor_)); // h(x) + } + // manifold equivalent of h(x)-z -> log(z,h(x)) + return measured_.localCoordinates(hx); + } else { + POSE hx = p1.between(p2, H1, H2); // h(x) + // manifold equivalent of h(x)-z -> log(z,h(x)) + return measured_.localCoordinates(hx); + } + } + + /** return the measured */ + const POSE& measured() const { + return measured_; + } + + 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_); + + } + }; // \class PoseBetweenFactor + +} /// namespace gtsam diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h new file mode 100644 index 000000000..24f083907 --- /dev/null +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -0,0 +1,111 @@ +/* ---------------------------------------------------------------------------- + + * 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 PosePriorFactor.h + * @author Frank Dellaert + **/ +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** + * A class for a soft prior on any Value type + * @addtogroup SLAM + */ + template + class PosePriorFactor: public NoiseModelFactor1 { + + private: + + typedef PosePriorFactor This; + typedef NoiseModelFactor1 Base; + + POSE prior_; /** The measurement */ + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + + /** concept check by type */ + GTSAM_CONCEPT_TESTABLE_TYPE(POSE) + GTSAM_CONCEPT_POSE_TYPE(POSE) + public: + + /// shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr > shared_ptr; + + /** default constructor - only use for serialization */ + PosePriorFactor() {} + + virtual ~PosePriorFactor() {} + + /** Constructor */ + PosePriorFactor(Key key, const POSE& prior, const SharedNoiseModel& model, + boost::optional body_P_sensor = boost::none) : + Base(model, key), prior_(prior), body_P_sensor_(body_P_sensor) { + } + + /// @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 << "PriorFactor on " << keyFormatter(this->key()) << "\n"; + prior_.print(" prior mean: "); + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + 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->prior_.equals(e->prior_, tol) + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const POSE& p, boost::optional H = boost::none) const { + if(body_P_sensor_) { + // manifold equivalent of h(x)-z -> log(z,h(x)) + return prior_.localCoordinates(p.compose(*body_P_sensor_, H)); + } else { + if(H) (*H) = eye(p.dim()); + // manifold equivalent of h(x)-z -> log(z,h(x)) + return prior_.localCoordinates(p); + } + } + + const POSE& prior() const { return prior_; } + + 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(prior_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } + }; + +} /// namespace gtsam diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp new file mode 100644 index 000000000..3063571cc --- /dev/null +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -0,0 +1,204 @@ +/* ---------------------------------------------------------------------------- + + * 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 testProjectionFactor.cpp + * @brief Unit tests for ProjectionFactor Class + * @author Frank Dellaert + * @date Nov 2009 + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +typedef PoseBetweenFactor TestPoseBetweenFactor; + +/* ************************************************************************* */ +TEST( PoseBetweenFactor, Constructor) { + Key poseKey1(1); + Key poseKey2(2); + Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25); + TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model); +} + +/* ************************************************************************* */ +TEST( PoseBetweenFactor, ConstructorWithTransform) { + Key poseKey1(1); + Key poseKey2(2); + Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model, body_P_sensor); +} + +/* ************************************************************************* */ +TEST( PoseBetweenFactor, Equals ) { + // Create two identical factors and make sure they're equal + Key poseKey1(1); + Key poseKey2(2); + Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25); + TestPoseBetweenFactor factor1(poseKey1, poseKey2, measurement, model); + TestPoseBetweenFactor factor2(poseKey1, poseKey2, measurement, model); + + CHECK(assert_equal(factor1, factor2)); + + // Create a third, different factor and check for inequality + Pose3 measurement2(Rot3::RzRyRx(0.20, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + TestPoseBetweenFactor factor3(poseKey1, poseKey2, measurement2, model); + + CHECK(assert_inequal(factor1, factor3)); +} + +/* ************************************************************************* */ +TEST( PoseBetweenFactor, EqualsWithTransform ) { + // Create two identical factors and make sure they're equal + Key poseKey1(1); + Key poseKey2(2); + Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + TestPoseBetweenFactor factor1(poseKey1, poseKey2, measurement, model, body_P_sensor); + TestPoseBetweenFactor factor2(poseKey1, poseKey2, measurement, model, body_P_sensor); + + CHECK(assert_equal(factor1, factor2)); + + // Create a third, different factor and check for inequality + Pose3 body_P_sensor2(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.30, -0.10, 1.0)); + TestPoseBetweenFactor factor3(poseKey1, poseKey2, measurement, model, body_P_sensor2); + + CHECK(assert_inequal(factor1, factor3)); +} + +/* ************************************************************************* */ +TEST( PoseBetweenFactor, Error ) { + // Create the measurement and linearization point + Pose3 measurement(Rot3::RzRyRx(0.15, 0.15, -0.20), Point3(+0.5, -1.0, +1.0)); + Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0)); + Pose3 pose2(Rot3::RzRyRx(0.15, 0.00, 0.20), Point3(-3.5, 6.0, -9.0)); + + // The expected error + Vector expectedError(6); + expectedError << -0.029839512616488, + 0.013145599455949, + 0.096971291682578, + -0.139187549519821, + -0.142346243063553, + -0.039088532100977; + + // Create a factor and calculate the error + Key poseKey1(1); + Key poseKey2(2); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25); + TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model); + Vector actualError(factor.evaluateError(pose1, pose2)); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); +} + +/* ************************************************************************* */ +TEST( PoseBetweenFactor, ErrorWithTransform ) { + // Create the measurement and linearization point + Pose3 measurement(Rot3::RzRyRx(-0.15, 0.10, 0.15), Point3(+1.25, -0.90, +.45)); + Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0)); + Pose3 pose2(Rot3::RzRyRx(0.15, 0.00, 0.20), Point3(-3.5, 6.0, -9.0)); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + + // The expected error + Vector expectedError(6); + expectedError << 0.017337193670445, + 0.022222830355243, + -0.012504190982804, + 0.026413288603739, + 0.005237695303536, + -0.000071612703633; + + // Create a factor and calculate the error + Key poseKey1(1); + Key poseKey2(2); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25); + TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model, body_P_sensor); + Vector actualError(factor.evaluateError(pose1, pose2)); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); +} + +/* ************************************************************************* */ +TEST( PoseBetweenFactor, Jacobian ) { + // Create a factor + Key poseKey1(1); + Key poseKey2(2); + Pose3 measurement(Rot3::RzRyRx(0.15, 0.15, -0.20), Point3(+0.5, -1.0, +1.0)); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25); + TestPoseBetweenFactor 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)); + + // Calculate numerical derivatives + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); + Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::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)); +} + +/* ************************************************************************* */ +TEST( PoseBetweenFactor, JacobianWithTransform ) { + // Create a factor + Key poseKey1(1); + Key poseKey2(2); + Pose3 measurement(Rot3::RzRyRx(-0.15, 0.10, 0.15), Point3(+1.25, -0.90, +.45)); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model, body_P_sensor); + + // 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.162672458989103, 0.013665177349534, 0.224649482928184), + Point3(-3.5257579, 6.02637531, -8.98382384)); + + // Calculate numerical derivatives + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); + Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); + + // Use the factor to calculate the derivative + Matrix actualH1; + Matrix actualH2; + Vector error = 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); } +/* ************************************************************************* */ + diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp new file mode 100644 index 000000000..c9825d582 --- /dev/null +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -0,0 +1,185 @@ +/* ---------------------------------------------------------------------------- + + * 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 testProjectionFactor.cpp + * @brief Unit tests for ProjectionFactor Class + * @author Frank Dellaert + * @date Nov 2009 + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +typedef PosePriorFactor TestPosePriorFactor; + +/* ************************************************************************* */ +TEST( PosePriorFactor, Constructor) { + Key poseKey(1); + Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25); + TestPosePriorFactor factor(poseKey, measurement, model); +} + +/* ************************************************************************* */ +TEST( PosePriorFactor, ConstructorWithTransform) { + Key poseKey(1); + Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + TestPosePriorFactor factor(poseKey, measurement, model, body_P_sensor); +} + +/* ************************************************************************* */ +TEST( PosePriorFactor, Equals ) { + // Create two identical factors and make sure they're equal + Key poseKey(1); + Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25); + TestPosePriorFactor factor1(poseKey, measurement, model); + TestPosePriorFactor factor2(poseKey, measurement, model); + + CHECK(assert_equal(factor1, factor2)); + + // Create a third, different factor and check for inequality + Pose3 measurement2(Rot3::RzRyRx(0.20, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + TestPosePriorFactor factor3(poseKey, measurement2, model); + + CHECK(assert_inequal(factor1, factor3)); +} + +/* ************************************************************************* */ +TEST( PosePriorFactor, EqualsWithTransform ) { + // Create two identical factors and make sure they're equal + Key poseKey(1); + Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + TestPosePriorFactor factor1(poseKey, measurement, model, body_P_sensor); + TestPosePriorFactor factor2(poseKey, measurement, model, body_P_sensor); + + CHECK(assert_equal(factor1, factor2)); + + // Create a third, different factor and check for inequality + Pose3 body_P_sensor2(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.30, -0.10, 1.0)); + TestPosePriorFactor factor3(poseKey, measurement, model, body_P_sensor2); + + CHECK(assert_inequal(factor1, factor3)); +} + +/* ************************************************************************* */ +TEST( PosePriorFactor, Error ) { + // Create the measurement and linearization point + Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + Pose3 pose(Rot3::RzRyRx(0.0, -0.15, 0.30), Point3(-4.0, 7.0, -10.0)); + + // The expected error + Vector expectedError(6); + expectedError << -0.184137861505414, + 0.139419283914526, + -0.158399296722242, + 0.740211733817804, + -1.198210282560946, + 1.008156093015192; + + // Create a factor and calculate the error + Key poseKey(1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25); + TestPosePriorFactor factor(poseKey, measurement, model); + Vector actualError(factor.evaluateError(pose)); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); +} + +/* ************************************************************************* */ +TEST( PosePriorFactor, ErrorWithTransform ) { + // Create the measurement and linearization point + Pose3 measurement(Rot3::RzRyRx(-M_PI_2+0.15, -0.30, -M_PI_2+0.45), Point3(-4.75, 7.90, -10.0)); + Pose3 pose(Rot3::RzRyRx(0.0, -0.15, 0.30), Point3(-4.0, 7.0, -10.0)); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + + // The expected error + Vector expectedError(6); + expectedError << -0.022712885347328, + 0.193765110165872, + 0.276418420819283, + 1.497519863757366, + -0.549375791422721, + 0.452761203187666; + + // Create a factor and calculate the error + Key poseKey(1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25); + TestPosePriorFactor factor(poseKey, measurement, model, body_P_sensor); + Vector actualError(factor.evaluateError(pose)); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); +} + +/* ************************************************************************* */ +TEST( PosePriorFactor, Jacobian ) { + // Create a factor + Key poseKey(1); + Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25); + TestPosePriorFactor factor(poseKey, measurement, model); + + // Create a linearization point at the zero-error point + Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + + // Calculate numerical derivatives + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); + + // Use the factor to calculate the derivative + Matrix actualH1; + factor.evaluateError(pose, actualH1); + + // Verify we get the expected error + CHECK(assert_equal(expectedH1, actualH1, 1e-9)); +} + +/* ************************************************************************* */ +TEST( PosePriorFactor, JacobianWithTransform ) { + // Create a factor + Key poseKey(1); + Pose3 measurement(Rot3::RzRyRx(-M_PI_2+0.15, -0.30, -M_PI_2+0.45), Point3(-4.75, 7.90, -10.0)); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + TestPosePriorFactor factor(poseKey, measurement, model, body_P_sensor); + + // Create a linearization point at the zero-error point + Pose3 pose(Rot3::RzRyRx(-0.303202977317447, -0.143253159173011, 0.494633847678171), + Point3(-4.74767676, 7.67044942, -11.00985)); + + // Calculate numerical derivatives + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); + + // Use the factor to calculate the derivative + Matrix actualH1; + factor.evaluateError(pose, actualH1); + + // Verify we get the expected error + CHECK(assert_equal(expectedH1, actualH1, 1e-9)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ +