246 lines
6.7 KiB
C++
246 lines
6.7 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file simulated2D.h
|
|
* @brief measurement functions and derivatives for simulated 2D robot
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
// \callgraph
|
|
#pragma once
|
|
|
|
#include <gtsam/geometry/Point2.h>
|
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
|
|
// \namespace
|
|
|
|
namespace gtsam {
|
|
|
|
namespace simulated2D {
|
|
|
|
// Simulated2D robots have no orientation, just a position
|
|
typedef TypedSymbol<Point2, 'x'> PoseKey;
|
|
typedef TypedSymbol<Point2, 'l'> PointKey;
|
|
|
|
/**
|
|
* Custom Values class that holds poses and points
|
|
*/
|
|
class Values: public DynamicValues {
|
|
size_t nrPoses_;
|
|
size_t nrPoints_;
|
|
public:
|
|
typedef DynamicValues Base; ///< base class
|
|
typedef boost::shared_ptr<Point2> sharedPoint; ///< shortcut to shared Point type
|
|
|
|
/// Constructor
|
|
Values() : nrPoses_(0), nrPoints_(0) {
|
|
}
|
|
|
|
/// Copy constructor
|
|
Values(const Base& base) :
|
|
Base(base), nrPoses_(0), nrPoints_(0) {
|
|
}
|
|
|
|
/// Insert a pose
|
|
void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
|
|
insert(i, p);
|
|
nrPoses_++;
|
|
}
|
|
|
|
/// Insert a point
|
|
void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
|
|
insert(j, p);
|
|
nrPoints_++;
|
|
}
|
|
|
|
/// Number of poses
|
|
int nrPoses() const {
|
|
return nrPoses_;
|
|
}
|
|
|
|
/// Number of points
|
|
int nrPoints() const {
|
|
return nrPoints_;
|
|
}
|
|
|
|
/// Return pose i
|
|
Point2 pose(const simulated2D::PoseKey& i) const {
|
|
return (*this)[i];
|
|
}
|
|
|
|
/// Return point j
|
|
Point2 point(const simulated2D::PointKey& j) const {
|
|
return (*this)[j];
|
|
}
|
|
};
|
|
|
|
/// Prior on a single pose
|
|
inline Point2 prior(const Point2& x) {
|
|
return x;
|
|
}
|
|
|
|
/// Prior on a single pose, optionally returns derivative
|
|
Point2 prior(const Point2& x, boost::optional<Matrix&> H = boost::none);
|
|
|
|
/// odometry between two poses
|
|
inline Point2 odo(const Point2& x1, const Point2& x2) {
|
|
return x2 - x1;
|
|
}
|
|
|
|
/// odometry between two poses, optionally returns derivative
|
|
Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1 =
|
|
boost::none, boost::optional<Matrix&> H2 = boost::none);
|
|
|
|
/// measurement between landmark and pose
|
|
inline Point2 mea(const Point2& x, const Point2& l) {
|
|
return l - x;
|
|
}
|
|
|
|
/// measurement between landmark and pose, optionally returns derivative
|
|
Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1 =
|
|
boost::none, boost::optional<Matrix&> H2 = boost::none);
|
|
|
|
/**
|
|
* Unary factor encoding a soft prior on a vector
|
|
*/
|
|
template<class KEY = PoseKey>
|
|
class GenericPrior: public NonlinearFactor1<KEY> {
|
|
public:
|
|
typedef NonlinearFactor1<KEY> Base; ///< base class
|
|
typedef boost::shared_ptr<GenericPrior<KEY> > shared_ptr;
|
|
typedef typename KEY::Value Pose; ///< shortcut to Pose type
|
|
|
|
Pose z_; ///< prior mean
|
|
|
|
/// Create generic prior
|
|
GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) :
|
|
NonlinearFactor1<KEY>(model, key), z_(z) {
|
|
}
|
|
|
|
/// Return error and optional derivative
|
|
Vector evaluateError(const Pose& x, boost::optional<Matrix&> H =
|
|
boost::none) const {
|
|
return (prior(x, H) - z_).vector();
|
|
}
|
|
|
|
private:
|
|
|
|
/// Default constructor
|
|
GenericPrior() {
|
|
}
|
|
|
|
/// Serialization function
|
|
friend class boost::serialization::access;
|
|
template<class ARCHIVE>
|
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
|
ar & BOOST_SERIALIZATION_NVP(z_);
|
|
}
|
|
};
|
|
|
|
/**
|
|
* Binary factor simulating "odometry" between two Vectors
|
|
*/
|
|
template<class KEY = PoseKey>
|
|
class GenericOdometry: public NonlinearFactor2<KEY, KEY> {
|
|
public:
|
|
typedef NonlinearFactor2<KEY, KEY> Base; ///< base class
|
|
typedef boost::shared_ptr<GenericOdometry<KEY> > shared_ptr;
|
|
typedef typename KEY::Value Pose; ///< shortcut to Pose type
|
|
|
|
Pose z_; ///< odometry measurement
|
|
|
|
/// Create odometry
|
|
GenericOdometry(const Pose& z, const SharedNoiseModel& model,
|
|
const KEY& i1, const KEY& i2) :
|
|
NonlinearFactor2<KEY, KEY>(model, i1, i2), z_(z) {
|
|
}
|
|
|
|
/// Evaluate error and optionally return derivatives
|
|
Vector evaluateError(const Pose& x1, const Pose& x2,
|
|
boost::optional<Matrix&> H1 = boost::none,
|
|
boost::optional<Matrix&> H2 = boost::none) const {
|
|
return (odo(x1, x2, H1, H2) - z_).vector();
|
|
}
|
|
|
|
private:
|
|
|
|
/// Default constructor
|
|
GenericOdometry() {
|
|
}
|
|
|
|
/// Serialization function
|
|
friend class boost::serialization::access;
|
|
template<class ARCHIVE>
|
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
|
ar & BOOST_SERIALIZATION_NVP(z_);
|
|
}
|
|
};
|
|
|
|
/**
|
|
* Binary factor simulating "measurement" between two Vectors
|
|
*/
|
|
template<class XKEY = PoseKey, class LKEY = PointKey>
|
|
class GenericMeasurement: public NonlinearFactor2<XKEY, LKEY> {
|
|
public:
|
|
typedef NonlinearFactor2<XKEY, LKEY> Base; ///< base class
|
|
typedef boost::shared_ptr<GenericMeasurement<XKEY, LKEY> > shared_ptr;
|
|
typedef typename XKEY::Value Pose; ///< shortcut to Pose type
|
|
typedef typename LKEY::Value Point; ///< shortcut to Point type
|
|
|
|
Point z_; ///< Measurement
|
|
|
|
/// Create measurement factor
|
|
GenericMeasurement(const Point& z, const SharedNoiseModel& model,
|
|
const XKEY& i, const LKEY& j) :
|
|
NonlinearFactor2<XKEY, LKEY>(model, i, j), z_(z) {
|
|
}
|
|
|
|
/// Evaluate error and optionally return derivatives
|
|
Vector evaluateError(const Pose& x1, const Point& x2,
|
|
boost::optional<Matrix&> H1 = boost::none,
|
|
boost::optional<Matrix&> H2 = boost::none) const {
|
|
return (mea(x1, x2, H1, H2) - z_).vector();
|
|
}
|
|
|
|
private:
|
|
|
|
/// Default constructor
|
|
GenericMeasurement() {
|
|
}
|
|
|
|
/// Serialization function
|
|
friend class boost::serialization::access;
|
|
template<class ARCHIVE>
|
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
|
ar & BOOST_SERIALIZATION_NVP(z_);
|
|
}
|
|
};
|
|
|
|
/** Typedefs for regular use */
|
|
typedef GenericPrior<PoseKey> Prior;
|
|
typedef GenericOdometry<PoseKey> Odometry;
|
|
typedef GenericMeasurement<PoseKey, PointKey> Measurement;
|
|
|
|
// Specialization of a graph for this example domain
|
|
// TODO: add functions to add factor types
|
|
class Graph : public NonlinearFactorGraph {
|
|
public:
|
|
Graph() {}
|
|
};
|
|
|
|
} // namespace simulated2D
|
|
} // namespace gtsam
|