12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Documentation

release/4.3a0
Frank Dellaert 2011-09-07 01:57:57 +00:00
parent d910b950c1
commit 92bdd2b43c
1 changed files with 20 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
gtsam/slam/simulated2DOriented.h
View File

@ -16,7 +16,6 @@
*/
// \callgraph
#pragma once
#include <gtsam/geometry/Pose2.h>
@ -31,42 +30,43 @@ namespace gtsam {
// The types that take an oriented pose2 rather than point2
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef LieValues<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues;
typedef TupleValues2<PoseValues, PointValues> Values;
//TODO:: point prior is not implemented right now
/**
* Prior on a single pose, and optional derivative version
*/
/// Prior on a single pose
inline Pose2 prior(const Pose2& x) {
return x;
}
/// Prior on a single pose, optional derivative version
Pose2 prior(const Pose2& x, boost::optional<Matrix&> H = boost::none);
/**
* odometry between two poses, and optional derivative version
*/
/// odometry between two poses
inline Pose2 odo(const Pose2& x1, const Pose2& x2) {
return x1.between(x2);
}
/// odometry between two poses, optional derivative version
Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none);
/**
* Unary factor encoding a soft prior on a vector
*/
/// Unary factor encoding a soft prior on a vector
template<class CFG = Values, class Key = PoseKey>
struct GenericPosePrior: public NonlinearFactor1<CFG, Key> {
Pose2 z_;
Pose2 z_; ///< measurement
GenericPosePrior(const Pose2& z, const SharedNoiseModel& model, const Key& key) :
NonlinearFactor1<CFG, Key> (model, key), z_(z) {
/// Create generic pose prior
GenericPosePrior(const Pose2& z, const SharedNoiseModel& model,
const Key& key) :
NonlinearFactor1<CFG, Key>(model, key), z_(z) {
}
/// Evaluate error and optionally derivative
Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
boost::none) const {
return z_.logmap(prior(x, H));
@ -81,13 +81,16 @@ namespace gtsam {
struct GenericOdometry: public NonlinearFactor2<CFG, KEY, KEY> {
Pose2 z_;
/// Create generic odometry factor
GenericOdometry(const Pose2& z, const SharedNoiseModel& model,
const KEY& i1, const KEY& i2) :
NonlinearFactor2<CFG, KEY, KEY> (model, i1, i2), z_(z) {
NonlinearFactor2<CFG, KEY, KEY>(model, i1, i2), z_(z) {
}
Vector evaluateError(const Pose2& x1, const Pose2& x2, boost::optional<
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
/// Evaluate error and optionally derivative
Vector evaluateError(const Pose2& x1, const Pose2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
return z_.logmap(odo(x1, x2, H1, H2));
}