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

35
gtsam/slam/simulated2DOriented.h
View File

@ -16,7 +16,6 @@
*/ */
// \callgraph // \callgraph
#pragma once #pragma once
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
@ -38,35 +37,36 @@ namespace gtsam {
//TODO:: point prior is not implemented right now //TODO:: point prior is not implemented right now
/** /// Prior on a single pose
* Prior on a single pose, and optional derivative version
*/
inline Pose2 prior(const Pose2& x) { inline Pose2 prior(const Pose2& x) {
return x; return x;
} }
/// Prior on a single pose, optional derivative version
Pose2 prior(const Pose2& x, boost::optional<Matrix&> H = boost::none); Pose2 prior(const Pose2& x, boost::optional<Matrix&> H = boost::none);
/** /// odometry between two poses
* odometry between two poses, and optional derivative version
*/
inline Pose2 odo(const Pose2& x1, const Pose2& x2) { inline Pose2 odo(const Pose2& x1, const Pose2& x2) {
return x1.between(x2); return x1.between(x2);
} }
/// odometry between two poses, optional derivative version
Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1 = Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none); 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> template<class CFG = Values, class Key = PoseKey>
struct GenericPosePrior: public NonlinearFactor1<CFG, Key> { struct GenericPosePrior: public NonlinearFactor1<CFG, Key> {
Pose2 z_; Pose2 z_; ///< measurement
GenericPosePrior(const Pose2& z, const SharedNoiseModel& model, const Key& key) : /// Create generic pose prior
NonlinearFactor1<CFG, Key> (model, key), z_(z) { 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 = Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
boost::none) const { boost::none) const {
return z_.logmap(prior(x, H)); return z_.logmap(prior(x, H));
@ -81,13 +81,16 @@ namespace gtsam {
struct GenericOdometry: public NonlinearFactor2<CFG, KEY, KEY> { struct GenericOdometry: public NonlinearFactor2<CFG, KEY, KEY> {
Pose2 z_; Pose2 z_;
/// Create generic odometry factor
GenericOdometry(const Pose2& z, const SharedNoiseModel& model, GenericOdometry(const Pose2& z, const SharedNoiseModel& model,
const KEY& i1, const KEY& i2) : 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< /// Evaluate error and optionally derivative
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const { 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)); return z_.logmap(odo(x1, x2, H1, H2));
} }