109 lines
2.8 KiB
C++
109 lines
2.8 KiB
C++
/**
|
|
* @file simulated2D.h
|
|
* @brief measurement functions and derivatives for simulated 2D robot
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
// \callgraph
|
|
|
|
#pragma once
|
|
|
|
#include "VectorConfig.h"
|
|
#include "NonlinearFactor.h"
|
|
#include "Key.h"
|
|
|
|
// \namespace
|
|
|
|
namespace gtsam {
|
|
|
|
namespace simulated2D {
|
|
|
|
typedef gtsam::VectorConfig VectorConfig;
|
|
typedef gtsam::Symbol PoseKey;
|
|
typedef gtsam::Symbol PointKey;
|
|
|
|
/**
|
|
* Prior on a single pose, and optional derivative version
|
|
*/
|
|
inline Vector prior(const Vector& x) {return x;}
|
|
Vector prior(const Vector& x, boost::optional<Matrix&> H = boost::none);
|
|
|
|
/**
|
|
* odometry between two poses, and optional derivative version
|
|
*/
|
|
inline Vector odo(const Vector& x1, const Vector& x2) {return x2-x1;}
|
|
Vector odo(const Vector& x1, const Vector& x2, boost::optional<Matrix&> H1 =
|
|
boost::none, boost::optional<Matrix&> H2 = boost::none);
|
|
|
|
/**
|
|
* measurement between landmark and pose, and optional derivative version
|
|
*/
|
|
inline Vector mea(const Vector& x, const Vector& l) {return l-x;}
|
|
Vector mea(const Vector& x, const Vector& l, boost::optional<Matrix&> H1 =
|
|
boost::none, boost::optional<Matrix&> H2 = boost::none);
|
|
|
|
/**
|
|
* Unary factor encoding a soft prior on a vector
|
|
*/
|
|
struct Prior: public NonlinearFactor1<VectorConfig, PoseKey,
|
|
Vector> {
|
|
|
|
Vector z_;
|
|
|
|
Prior(const Vector& z, const sharedGaussian& model,
|
|
const PoseKey& key) :
|
|
NonlinearFactor1<VectorConfig, PoseKey, Vector>(model, key),
|
|
z_(z) {
|
|
}
|
|
|
|
Vector evaluateError(const Vector& x, boost::optional<Matrix&> H =
|
|
boost::none) const {
|
|
return prior(x, H) - z_;
|
|
}
|
|
|
|
};
|
|
|
|
/**
|
|
* Binary factor simulating "odometry" between two Vectors
|
|
*/
|
|
struct Odometry: public NonlinearFactor2<VectorConfig, PoseKey,
|
|
Vector, PointKey, Vector> {
|
|
Vector z_;
|
|
|
|
Odometry(const Vector& z, const sharedGaussian& model,
|
|
const PoseKey& j1, const PoseKey& j2) :
|
|
z_(z), NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
|
|
Vector>(model, j1, j2) {
|
|
}
|
|
|
|
Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
|
|
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
|
|
return odo(x1, x2, H1, H2) - z_;
|
|
}
|
|
|
|
};
|
|
|
|
/**
|
|
* Binary factor simulating "measurement" between two Vectors
|
|
*/
|
|
struct Measurement: public NonlinearFactor2<VectorConfig, PoseKey,
|
|
Vector, PointKey, Vector> {
|
|
|
|
Vector z_;
|
|
|
|
Measurement(const Vector& z, const sharedGaussian& model,
|
|
const PoseKey& j1, const PointKey& j2) :
|
|
z_(z), NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
|
|
Vector>(model, j1, j2) {
|
|
}
|
|
|
|
Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
|
|
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
|
|
return mea(x1, x2, H1, H2) - z_;
|
|
}
|
|
|
|
};
|
|
|
|
} // namespace simulated2D
|
|
} // namespace gtsam
|