Added versions of partial priors for parts of poses - useful for GPS or inertial priors. Added interval interface to Pose2 and Pose3.
parent
13057e7548
commit
d36b8b63e7
|
@ -259,12 +259,26 @@ public:
|
||||||
boost::optional<Matrix&> H1=boost::none,
|
boost::optional<Matrix&> H1=boost::none,
|
||||||
boost::optional<Matrix&> H2=boost::none) const;
|
boost::optional<Matrix&> H2=boost::none) const;
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
/// @name Advanced Interface
|
/// @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<size_t, size_t> 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<size_t, size_t> rotationInterval() { return std::make_pair(2, 2); }
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
// Serialization function
|
// Serialization function
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template<class Archive>
|
template<class Archive>
|
||||||
|
|
|
@ -228,6 +228,20 @@ namespace gtsam {
|
||||||
/// @name Advanced Interface
|
/// @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<size_t, size_t> 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<size_t, size_t> rotationInterval() { return std::make_pair(0, 2); }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
|
|
|
@ -49,6 +49,14 @@ private:
|
||||||
static Translation checkTranslationMemberAccess(const POSE& p) {
|
static Translation checkTranslationMemberAccess(const POSE& p) {
|
||||||
return p.translation();
|
return p.translation();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static std::pair<size_t, size_t> checkTranslationInterval() {
|
||||||
|
return POSE::translationInterval();
|
||||||
|
}
|
||||||
|
|
||||||
|
static std::pair<size_t, size_t> checkRotationInterval() {
|
||||||
|
return POSE::rotationInterval();
|
||||||
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
|
@ -66,9 +66,6 @@ namespace gtsam {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// shorthand for a smart pointer to a factor
|
|
||||||
typedef typename boost::shared_ptr<PartialPriorFactor> shared_ptr;
|
|
||||||
|
|
||||||
virtual ~PartialPriorFactor() {}
|
virtual ~PartialPriorFactor() {}
|
||||||
|
|
||||||
/** Single Element Constructor: acts on a single parameter specified by idx */
|
/** Single Element Constructor: acts on a single parameter specified by idx */
|
||||||
|
|
|
@ -3,8 +3,10 @@
|
||||||
set (gtsam_unstable_subdirs
|
set (gtsam_unstable_subdirs
|
||||||
base
|
base
|
||||||
discrete
|
discrete
|
||||||
|
# linear
|
||||||
dynamics
|
dynamics
|
||||||
nonlinear
|
nonlinear
|
||||||
|
slam
|
||||||
)
|
)
|
||||||
|
|
||||||
add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)
|
add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)
|
||||||
|
@ -14,7 +16,7 @@ foreach(subdir ${gtsam_unstable_subdirs})
|
||||||
# Build convenience libraries
|
# Build convenience libraries
|
||||||
file(GLOB subdir_srcs "${subdir}/*.cpp")
|
file(GLOB subdir_srcs "${subdir}/*.cpp")
|
||||||
set(${subdir}_srcs ${subdir_srcs})
|
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")
|
message(STATUS "Building Convenience Library: ${subdir}_unstable")
|
||||||
add_library("${subdir}_unstable" STATIC ${subdir_srcs})
|
add_library("${subdir}_unstable" STATIC ${subdir_srcs})
|
||||||
endif()
|
endif()
|
||||||
|
@ -29,8 +31,9 @@ set(gtsam_unstable_srcs
|
||||||
${base_srcs}
|
${base_srcs}
|
||||||
${discrete_srcs}
|
${discrete_srcs}
|
||||||
${dynamics_srcs}
|
${dynamics_srcs}
|
||||||
${linear_srcs}
|
#${linear_srcs}
|
||||||
${nonlinear_srcs}
|
${nonlinear_srcs}
|
||||||
|
${slam_srcs}
|
||||||
)
|
)
|
||||||
|
|
||||||
option (GTSAM_UNSTABLE_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam_unstable" ON)
|
option (GTSAM_UNSTABLE_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam_unstable" ON)
|
||||||
|
|
|
@ -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)
|
|
@ -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 <gtsam/slam/PartialPriorFactor.h>
|
||||||
|
#include <gtsam/geometry/concepts.h>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
template<class POSE>
|
||||||
|
class PoseRotationPrior : public PartialPriorFactor<POSE> {
|
||||||
|
public:
|
||||||
|
|
||||||
|
typedef PoseRotationPrior<POSE> This;
|
||||||
|
typedef PartialPriorFactor<POSE> 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<size_t, size_t> interval = Pose::rotationInterval();
|
||||||
|
|
||||||
|
std::vector<size_t> 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
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -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 <gtsam/slam/PartialPriorFactor.h>
|
||||||
|
#include <gtsam/geometry/concepts.h>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
template<class POSE>
|
||||||
|
class PoseTranslationPrior : public PartialPriorFactor<POSE> {
|
||||||
|
public:
|
||||||
|
|
||||||
|
typedef PoseTranslationPrior<POSE> This;
|
||||||
|
typedef PartialPriorFactor<POSE> 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<size_t, size_t> interval = Pose::translationInterval();
|
||||||
|
|
||||||
|
std::vector<size_t> 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
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -0,0 +1,76 @@
|
||||||
|
/**
|
||||||
|
* @file testPosePriors.cpp
|
||||||
|
*
|
||||||
|
* @brief Tests partial priors on poses
|
||||||
|
*
|
||||||
|
* @date Jun 14, 2012
|
||||||
|
* @author Alex Cunningham
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
|
#include <gtsam_unstable/slam/PoseRotationPrior.h>
|
||||||
|
#include <gtsam_unstable/slam/PoseTranslationPrior.h>
|
||||||
|
|
||||||
|
#include <gtsam/slam/planarSLAM.h>
|
||||||
|
#include <gtsam/slam/visualSLAM.h>
|
||||||
|
|
||||||
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
|
|
||||||
|
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<Pose2> 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<Pose3> 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<SimpleCamera> CamTPrior;
|
||||||
|
// CamTPrior actCamTprior(x1, Point3(2.0, 3.0, 4.0), model3);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( testPoseRotationPrior, planar) {
|
||||||
|
|
||||||
|
typedef PoseRotationPrior<Pose2> 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<Pose3> 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<SimpleCamera> CamRPrior;
|
||||||
|
// CamRPrior actCamRprior(x1, Rot3::RzRyRx(0.1, 0.2, 0.3), model3);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||||
|
/* ************************************************************************* */
|
Loading…
Reference in New Issue