From d36b8b63e72fea808d1f19cd9cc195f1a52c4ec9 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 14 Jun 2012 20:00:51 +0000 Subject: [PATCH] Added versions of partial priors for parts of poses - useful for GPS or inertial priors. Added interval interface to Pose2 and Pose3. --- gtsam/geometry/Pose2.h | 18 ++++- gtsam/geometry/Pose3.h | 14 ++++ gtsam/geometry/concepts.h | 8 +++ gtsam/slam/PartialPriorFactor.h | 3 - gtsam_unstable/CMakeLists.txt | 9 ++- gtsam_unstable/slam/CMakeLists.txt | 26 +++++++ gtsam_unstable/slam/PoseRotationPrior.h | 64 +++++++++++++++++ gtsam_unstable/slam/PoseTranslationPrior.h | 64 +++++++++++++++++ gtsam_unstable/slam/tests/testPosePriors.cpp | 76 ++++++++++++++++++++ 9 files changed, 274 insertions(+), 8 deletions(-) create mode 100644 gtsam_unstable/slam/CMakeLists.txt create mode 100644 gtsam_unstable/slam/PoseRotationPrior.h create mode 100644 gtsam_unstable/slam/PoseTranslationPrior.h create mode 100644 gtsam_unstable/slam/tests/testPosePriors.cpp diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 60c74e7cb..d091ddac8 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -259,12 +259,26 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const; -private: - /// @} /// @name Advanced Interface /// @{ + /** + * Return the start and end indices (inclusive) of the translation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + inline static std::pair translationInterval() { return std::make_pair(0, 1); } + + /** + * Return the start and end indices (inclusive) of the rotation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + static std::pair rotationInterval() { return std::make_pair(2, 2); } + +private: + // Serialization function friend class boost::serialization::access; template diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 4d32a5608..dfae2d7a2 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -228,6 +228,20 @@ namespace gtsam { /// @name Advanced Interface /// @{ + /** + * Return the start and end indices (inclusive) of the translation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + inline static std::pair translationInterval() { return std::make_pair(3, 5); } + + /** + * Return the start and end indices (inclusive) of the rotation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + static std::pair rotationInterval() { return std::make_pair(0, 2); } + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/geometry/concepts.h b/gtsam/geometry/concepts.h index 3000530e5..bde16ee6e 100644 --- a/gtsam/geometry/concepts.h +++ b/gtsam/geometry/concepts.h @@ -49,6 +49,14 @@ private: static Translation checkTranslationMemberAccess(const POSE& p) { return p.translation(); } + + static std::pair checkTranslationInterval() { + return POSE::translationInterval(); + } + + static std::pair checkRotationInterval() { + return POSE::rotationInterval(); + } }; /** diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index bea3f3f46..f096ed9bc 100644 --- a/gtsam/slam/PartialPriorFactor.h +++ b/gtsam/slam/PartialPriorFactor.h @@ -66,9 +66,6 @@ namespace gtsam { public: - // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; - virtual ~PartialPriorFactor() {} /** Single Element Constructor: acts on a single parameter specified by idx */ diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index b03dc3d73..1c781453a 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -2,9 +2,11 @@ # and also build tests set (gtsam_unstable_subdirs base - discrete + discrete +# linear dynamics nonlinear + slam ) add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) @@ -14,7 +16,7 @@ foreach(subdir ${gtsam_unstable_subdirs}) # Build convenience libraries file(GLOB subdir_srcs "${subdir}/*.cpp") set(${subdir}_srcs ${subdir_srcs}) - if (GTSAM_BUILD_CONVENIENCE_LIBRARIES) + if (subdir_srcs AND GTSAM_BUILD_CONVENIENCE_LIBRARIES) message(STATUS "Building Convenience Library: ${subdir}_unstable") add_library("${subdir}_unstable" STATIC ${subdir_srcs}) endif() @@ -29,8 +31,9 @@ set(gtsam_unstable_srcs ${base_srcs} ${discrete_srcs} ${dynamics_srcs} - ${linear_srcs} + #${linear_srcs} ${nonlinear_srcs} + ${slam_srcs} ) option (GTSAM_UNSTABLE_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam_unstable" ON) diff --git a/gtsam_unstable/slam/CMakeLists.txt b/gtsam_unstable/slam/CMakeLists.txt new file mode 100644 index 000000000..88dd60edd --- /dev/null +++ b/gtsam_unstable/slam/CMakeLists.txt @@ -0,0 +1,26 @@ +# Install headers +file(GLOB slam_headers "*.h") +install(FILES ${slam_headers} DESTINATION include/gtsam_unstable/slam) + +# Components to link tests in this subfolder against +set(slam_local_libs + #slam_unstable # No sources currently, so no convenience lib + slam + nonlinear + linear + inference + geometry + base + ccolamd +) + +set (slam_full_libs + gtsam-static + gtsam_unstable-static) + +# Exclude tests that don't work +set (slam_excluded_tests "") + +# Add all tests +gtsam_add_subdir_tests(slam_unstable "${slam_local_libs}" "${slam_full_libs}" "${slam_excluded_tests}") +add_dependencies(check.unstable check.slam_unstable) diff --git a/gtsam_unstable/slam/PoseRotationPrior.h b/gtsam_unstable/slam/PoseRotationPrior.h new file mode 100644 index 000000000..c5f039f7f --- /dev/null +++ b/gtsam_unstable/slam/PoseRotationPrior.h @@ -0,0 +1,64 @@ +/** + * @file PoseRotationPrior.h + * + * @brief Implements a prior on the rotation component of a pose + * + * @date Jun 14, 2012 + * @author Alex Cunningham + */ + +#pragma once + +#include +#include + +namespace gtsam { + +template +class PoseRotationPrior : public PartialPriorFactor { +public: + + typedef PoseRotationPrior This; + typedef PartialPriorFactor Base; + typedef POSE Pose; + typedef typename POSE::Rotation Rotation; + + GTSAM_CONCEPT_POSE_TYPE(Pose) + GTSAM_CONCEPT_GROUP_TYPE(Pose) + GTSAM_CONCEPT_LIE_TYPE(Rotation) + + /** standard constructor */ + PoseRotationPrior(Key key, const Rotation& rot_z, const SharedNoiseModel& model) + : Base(key, model) + { + assert(rot_z.dim() == model->dim()); + + // Calculate the prior applied + this->prior_ = Rotation::Logmap(rot_z); + + // Create the mask for partial prior + size_t pose_dim = Pose::identity().dim(); + size_t rot_dim = rot_z.dim(); + + // get the interval of the lie coordinates corresponding to rotation + std::pair interval = Pose::rotationInterval(); + + std::vector mask; + for (size_t i=interval.first; i<=interval.second; ++i) + mask.push_back(i); + this->mask_ = mask; + + this->H_ = zeros(rot_dim, pose_dim); + this->fillH(); + } + + /** get the rotation used to create the measurement */ + Rotation priorRotation() const { return Rotation::Expmap(this->prior_); } + +}; + +} // \namespace gtsam + + + + diff --git a/gtsam_unstable/slam/PoseTranslationPrior.h b/gtsam_unstable/slam/PoseTranslationPrior.h new file mode 100644 index 000000000..0e4d34b8b --- /dev/null +++ b/gtsam_unstable/slam/PoseTranslationPrior.h @@ -0,0 +1,64 @@ +/** + * @file PoseTranslationPrior.h + * + * @brief Implements a prior on the translation component of a pose + * + * @date Jun 14, 2012 + * @author Alex Cunningham + */ + +#pragma once + +#include +#include + +namespace gtsam { + +template +class PoseTranslationPrior : public PartialPriorFactor { +public: + + typedef PoseTranslationPrior This; + typedef PartialPriorFactor Base; + typedef POSE Pose; + typedef typename POSE::Translation Translation; + + GTSAM_CONCEPT_POSE_TYPE(Pose) + GTSAM_CONCEPT_GROUP_TYPE(Pose) + GTSAM_CONCEPT_LIE_TYPE(Translation) + + /** standard constructor */ + PoseTranslationPrior(Key key, const Translation& rot_z, const SharedNoiseModel& model) + : Base(key, model) + { + assert(rot_z.dim() == model->dim()); + + // Calculate the prior applied + this->prior_ = Translation::Logmap(rot_z); + + // Create the mask for partial prior + size_t pose_dim = Pose::identity().dim(); + size_t rot_dim = rot_z.dim(); + + // get the interval of the lie coordinates corresponding to rotation + std::pair interval = Pose::translationInterval(); + + std::vector mask; + for (size_t i=interval.first; i<=interval.second; ++i) + mask.push_back(i); + this->mask_ = mask; + + this->H_ = zeros(rot_dim, pose_dim); + this->fillH(); + } + + /** get the rotation used to create the measurement */ + Translation priorTranslation() const { return Translation::Expmap(this->prior_); } + +}; + +} // \namespace gtsam + + + + diff --git a/gtsam_unstable/slam/tests/testPosePriors.cpp b/gtsam_unstable/slam/tests/testPosePriors.cpp new file mode 100644 index 000000000..4aa835266 --- /dev/null +++ b/gtsam_unstable/slam/tests/testPosePriors.cpp @@ -0,0 +1,76 @@ +/** + * @file testPosePriors.cpp + * + * @brief Tests partial priors on poses + * + * @date Jun 14, 2012 + * @author Alex Cunningham + */ + +#include + +#include +#include + +#include +#include + +#include + +using namespace gtsam; + +Key x1 = symbol_shorthand::X(1); + +const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas(Vector_(1, 0.1)); +const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); +const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); + +/* ************************************************************************* */ +TEST( testPoseTranslationPrior, planar) { + + typedef PoseTranslationPrior PlanarTPrior; + PlanarTPrior actTprior(x1, Point2(2.0, 3.0), model2); + EXPECT(assert_equal(Point2(2.0, 3.0), actTprior.priorTranslation())); + Matrix expH(2,3); expH << eye(2,2), zeros(2,1); + EXPECT(assert_equal(eye(2,3), actTprior.H())); +} + +/* ************************************************************************* */ +TEST( testPoseTranslationPrior, visual) { + + typedef PoseTranslationPrior VisualTPrior; + VisualTPrior actPose3Tprior(x1, Point3(2.0, 3.0, 4.0), model3); + EXPECT(assert_equal(Point3(2.0, 3.0, 4.0), actPose3Tprior.priorTranslation())); + Matrix expH(3,6); expH << zeros(3,3), eye(3,3); + EXPECT(assert_equal(expH, actPose3Tprior.H())); + +// typedef PoseTranslationPrior CamTPrior; +// CamTPrior actCamTprior(x1, Point3(2.0, 3.0, 4.0), model3); +} + +/* ************************************************************************* */ +TEST( testPoseRotationPrior, planar) { + + typedef PoseRotationPrior PlanarRPrior; + PlanarRPrior actRprior(x1, Rot2::fromDegrees(30.0), model1); + EXPECT(assert_equal(Rot2::fromDegrees(30.0), actRprior.priorRotation())); + Matrix expH(1,3); expH << 0.0, 0.0, 1.0; + EXPECT(assert_equal(expH, actRprior.H())); +} + +/* ************************************************************************* */ +TEST( testPoseRotationPrior, visual) { + + typedef PoseRotationPrior VisualRPrior; + VisualRPrior actPose3Rprior(x1, Rot3::RzRyRx(0.1, 0.2, 0.3), model3); + EXPECT(assert_equal(Rot3::RzRyRx(0.1, 0.2, 0.3), actPose3Rprior.priorRotation())); + Matrix expH(3,6); expH << eye(3,3), zeros(3,3); + EXPECT(assert_equal(expH, actPose3Rprior.H())); + +// typedef testPoseRotationPrior CamRPrior; +// CamRPrior actCamRprior(x1, Rot3::RzRyRx(0.1, 0.2, 0.3), model3); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */