diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h new file mode 100644 index 000000000..d4c5f8172 --- /dev/null +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -0,0 +1,167 @@ +/* ---------------------------------------------------------------------------- + + * 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 TSAMFactors.h + * @brief TSAM 1 Factors, simpler than the hierarchical TSAM 2 + * @author Frank Dellaert + * @date May 2014 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * DeltaFactor: relative 2D measurement between Pose2 and Point2 + */ +class DeltaFactor: public NoiseModelFactor2 { + +public: + typedef DeltaFactor This; + typedef NoiseModelFactor2 Base; + typedef boost::shared_ptr shared_ptr; + +private: + Point2 measured_; ///< the measurement + +public: + + /// Constructor + DeltaFactor(Key i, Key j, const Point2& measured, + const SharedNoiseModel& model) : + Base(model, i, j), measured_(measured) { + } + + /// Evaluate measurement error h(x)-z + Vector evaluateError(const Pose2& pose, const Point2& point, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + Point2 d = pose.transform_to(point, H1, H2); + Point2 e = measured_.between(d); + return e.vector(); + } +}; + +/** + * DeltaFactorBase: relative 2D measurement between Pose2 and Point2, with Basenodes + */ +class DeltaFactorBase: public NoiseModelFactor4 { + +public: + typedef DeltaFactorBase This; + typedef NoiseModelFactor4 Base; + typedef boost::shared_ptr shared_ptr; + +private: + Point2 measured_; ///< the measurement + +public: + + /// Constructor + DeltaFactorBase(Key b1, Key i, Key b2, Key j, const Point2& measured, + const SharedNoiseModel& model) : + Base(model, b1, i, b2, j), measured_(measured) { + } + + /// Evaluate measurement error h(x)-z + Vector evaluateError(const Pose2& base1, const Pose2& pose, + const Pose2& base2, const Point2& point, // + boost::optional H1 = boost::none, // + boost::optional H2 = boost::none, // + boost::optional H3 = boost::none, // + boost::optional H4 = boost::none) const { + if (H1 || H2 || H3 || H4) { + // TODO use fixed-size matrices + Matrix D_pose_g_base1, D_pose_g_pose; + Pose2 pose_g = base1.compose(pose, D_pose_g_base1, D_pose_g_pose); + Matrix D_point_g_base2, D_point_g_point; + Point2 point_g = base2.transform_from(point, D_point_g_base2, + D_point_g_point); + Matrix D_e_pose_g, D_e_point_g; + Point2 d = pose_g.transform_to(point_g, D_e_pose_g, D_e_point_g); + if (H1) + *H1 = D_e_pose_g * D_pose_g_base1; + if (H2) + *H2 = D_e_pose_g * D_pose_g_pose; + if (H3) + *H3 = D_e_point_g * D_point_g_base2; + if (H4) + *H4 = D_e_point_g * D_point_g_point; + return measured_.localCoordinates(d); + } else { + Pose2 pose_g = base1.compose(pose); + Point2 point_g = base2.transform_from(point); + Point2 d = pose_g.transform_to(point_g); + return measured_.localCoordinates(d); + } + } +}; + +/** + * OdometryFactorBase: Pose2 odometry, with Basenodes + */ +class OdometryFactorBase: public NoiseModelFactor4 { + +public: + typedef OdometryFactorBase This; + typedef NoiseModelFactor4 Base; + typedef boost::shared_ptr shared_ptr; + +private: + Pose2 measured_; ///< the measurement + +public: + + /// Constructor + OdometryFactorBase(Key b1, Key i, Key b2, Key j, const Pose2& measured, + const SharedNoiseModel& model) : + Base(model, b1, i, b2, j), measured_(measured) { + } + + /// Evaluate measurement error h(x)-z + Vector evaluateError(const Pose2& base1, const Pose2& pose1, + const Pose2& base2, const Pose2& pose2, // + boost::optional H1 = boost::none, // + boost::optional H2 = boost::none, // + boost::optional H3 = boost::none, // + boost::optional H4 = boost::none) const { + if (H1 || H2 || H3 || H4) { + // TODO use fixed-size matrices + Matrix D_pose1_g_base1, D_pose1_g_pose1; + Pose2 pose1_g = base1.compose(pose1, D_pose1_g_base1, D_pose1_g_pose1); + Matrix D_pose2_g_base2, D_pose2_g_pose2; + Pose2 pose2_g = base2.compose(pose2, D_pose2_g_base2, D_pose2_g_pose2); + Matrix D_e_pose1_g, D_e_pose2_g; + Pose2 d = pose1_g.between(pose2_g, D_e_pose1_g, D_e_pose2_g); + if (H1) + *H1 = D_e_pose1_g * D_pose1_g_base1; + if (H2) + *H2 = D_e_pose1_g * D_pose1_g_pose1; + if (H3) + *H3 = D_e_pose2_g * D_pose2_g_base2; + if (H4) + *H4 = D_e_pose2_g * D_pose2_g_pose2; + return measured_.localCoordinates(d); + } else { + Pose2 pose1_g = base1.compose(pose1); + Pose2 pose2_g = base2.compose(pose2); + Pose2 d = pose1_g.between(pose2_g); + return measured_.localCoordinates(d); + } + } +}; + +} + diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp new file mode 100644 index 000000000..44dca9b8e --- /dev/null +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -0,0 +1,149 @@ +/* ---------------------------------------------------------------------------- + + * 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 testTSAMFactors.cpp + * @brief Unit tests for TSAM 1 Factors + * @author Frank Dellaert + * @date May 2014 + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +Key i(1), j(2); // Key for pose and point + +//************************************************************************* +TEST( DeltaFactor, all ) { + // Create a factor + Point2 measurement(1, 1); + static SharedNoiseModel model(noiseModel::Unit::Create(2)); + DeltaFactor factor(i, j, measurement, model); + + // Set the linearization point + Pose2 pose(1, 2, 0); + Point2 point(4, 11); + Vector2 expected(4 - 1 - 1, 11 - 2 - 1); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual; + Vector actual = factor.evaluateError(pose, point, H1Actual, H2Actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected; + H1Expected = numericalDerivative11( + boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none, + boost::none), pose); + H2Expected = numericalDerivative11( + boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none, + boost::none), point); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); +} + +//************************************************************************* +TEST( DeltaFactorBase, all ) { + // Create a factor + Key b1(10), b2(20); + Point2 measurement(1, 1); + static SharedNoiseModel model(noiseModel::Unit::Create(2)); + DeltaFactorBase factor(b1, i, b2, j, measurement, model); + + // Set the linearization point + Pose2 base1, base2(1, 0, 0); + Pose2 pose(1, 2, 0); + Point2 point(4, 11); + Vector2 expected(4 + 1 - 1 - 1, 11 - 2 - 1); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual, H4Actual; + Vector actual = factor.evaluateError(base1, pose, base2, point, H1Actual, + H2Actual, H3Actual, H4Actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected, H3Expected, H4Expected; + H1Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2, + point, boost::none, boost::none, boost::none, boost::none), base1); + H2Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2, + point, boost::none, boost::none, boost::none, boost::none), pose); + H3Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1, + point, boost::none, boost::none, boost::none, boost::none), base2); + H4Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, + _1, boost::none, boost::none, boost::none, boost::none), point); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); + EXPECT(assert_equal(H3Expected, H3Actual, 1e-9)); + EXPECT(assert_equal(H4Expected, H4Actual, 1e-9)); +} + +//************************************************************************* +TEST( OdometryFactorBase, all ) { + // Create a factor + Key b1(10), b2(20); + Pose2 measurement(1, 1, 0); + static SharedNoiseModel model(noiseModel::Unit::Create(2)); + OdometryFactorBase factor(b1, i, b2, j, measurement, model); + + // Set the linearization pose2 + Pose2 base1, base2(1, 0, 0); + Pose2 pose1(1, 2, 0), pose2(4, 11, 0); + Vector3 expected(4 + 1 - 1 - 1, 11 - 2 - 1, 0); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual, H4Actual; + Vector actual = factor.evaluateError(base1, pose1, base2, pose2, H1Actual, + H2Actual, H3Actual, H4Actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected, H3Expected, H4Expected; + H1Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2, + pose2, boost::none, boost::none, boost::none, boost::none), base1); + H2Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2, + pose2, boost::none, boost::none, boost::none, boost::none), pose1); + H3Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1, + pose2, boost::none, boost::none, boost::none, boost::none), base2); + H4Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, + base2, _1, boost::none, boost::none, boost::none, boost::none), + pose2); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); + EXPECT(assert_equal(H3Expected, H3Actual, 1e-9)); + EXPECT(assert_equal(H4Expected, H4Actual, 1e-9)); +} + +//************************************************************************* +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//************************************************************************* +