Added versions of partial priors for parts of poses - useful for GPS or inertial priors. Added interval interface to Pose2 and Pose3.
Alex Cunningham
parent
13057e7548
commit
d36b8b63e7
|
|
@ -259,12 +259,26 @@ public:
|
|||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> 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<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
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
|
|
|
|||
|
|
@ -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<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:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
|
|
|
|||
|
|
@ -49,6 +49,14 @@ private:
|
|||
static Translation checkTranslationMemberAccess(const POSE& p) {
|
||||
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:
|
||||
|
||||
// shorthand for a smart pointer to a factor
|
||||
typedef typename boost::shared_ptr<PartialPriorFactor> shared_ptr;
|
||||
|
||||
virtual ~PartialPriorFactor() {}
|
||||
|
||||
/** Single Element Constructor: acts on a single parameter specified by idx */
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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