Fixed doxygen warnings
parent
cf72788bea
commit
d910b950c1
|
|
@ -16,7 +16,6 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// \callgraph
|
// \callgraph
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <gtsam/geometry/Point2.h>
|
#include <gtsam/geometry/Point2.h>
|
||||||
|
|
@ -35,62 +34,77 @@ namespace gtsam {
|
||||||
typedef LieValues<PoseKey> PoseValues;
|
typedef LieValues<PoseKey> PoseValues;
|
||||||
typedef LieValues<PointKey> PointValues;
|
typedef LieValues<PointKey> PointValues;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Custom Values class that holds poses and points
|
||||||
|
*/
|
||||||
class Values: public TupleValues2<PoseValues, PointValues> {
|
class Values: public TupleValues2<PoseValues, PointValues> {
|
||||||
public:
|
public:
|
||||||
typedef TupleValues2<PoseValues, PointValues> Base;
|
typedef TupleValues2<PoseValues, PointValues> Base; ///< base class
|
||||||
typedef boost::shared_ptr<Point2> sharedPoint;
|
typedef boost::shared_ptr<Point2> sharedPoint; ///< shortcut to shared Point type
|
||||||
|
|
||||||
Values() {}
|
/// Constructor
|
||||||
Values(const Base& base) : Base(base) {}
|
Values() {
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Copy constructor
|
||||||
|
Values(const Base& base) :
|
||||||
|
Base(base) {
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Insert a pose
|
||||||
void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
|
void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
|
||||||
insert(i, p);
|
insert(i, p);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Insert a point
|
||||||
void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
|
void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
|
||||||
insert(j, p);
|
insert(j, p);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Number of poses
|
||||||
int nrPoses() const {
|
int nrPoses() const {
|
||||||
return this->first_.size();
|
return this->first_.size();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Number of points
|
||||||
int nrPoints() const {
|
int nrPoints() const {
|
||||||
return this->second_.size();
|
return this->second_.size();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Return pose i
|
||||||
sharedPoint pose(const simulated2D::PoseKey& i) {
|
sharedPoint pose(const simulated2D::PoseKey& i) {
|
||||||
return sharedPoint(new Point2((*this)[i]));
|
return sharedPoint(new Point2((*this)[i]));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Return point j
|
||||||
sharedPoint point(const simulated2D::PointKey& j) {
|
sharedPoint point(const simulated2D::PointKey& j) {
|
||||||
return sharedPoint(new Point2((*this)[j]));
|
return sharedPoint(new Point2((*this)[j]));
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/// Prior on a single pose
|
||||||
* Prior on a single pose, and optional derivative version
|
|
||||||
*/
|
|
||||||
inline Point2 prior(const Point2& x) {
|
inline Point2 prior(const Point2& x) {
|
||||||
return x;
|
return x;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Prior on a single pose, optionally returns derivative
|
||||||
Point2 prior(const Point2& x, boost::optional<Matrix&> H = boost::none);
|
Point2 prior(const Point2& x, boost::optional<Matrix&> H = boost::none);
|
||||||
|
|
||||||
/**
|
/// odometry between two poses
|
||||||
* odometry between two poses, and optional derivative version
|
|
||||||
*/
|
|
||||||
inline Point2 odo(const Point2& x1, const Point2& x2) {
|
inline Point2 odo(const Point2& x1, const Point2& x2) {
|
||||||
return x2 - x1;
|
return x2 - x1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// odometry between two poses, optionally returns derivative
|
||||||
Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1 =
|
Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1 =
|
||||||
boost::none, boost::optional<Matrix&> H2 = boost::none);
|
boost::none, boost::optional<Matrix&> H2 = boost::none);
|
||||||
|
|
||||||
/**
|
/// measurement between landmark and pose
|
||||||
* measurement between landmark and pose, and optional derivative version
|
|
||||||
*/
|
|
||||||
inline Point2 mea(const Point2& x, const Point2& l) {
|
inline Point2 mea(const Point2& x, const Point2& l) {
|
||||||
return l - x;
|
return l - x;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// measurement between landmark and pose, optionally returns derivative
|
||||||
Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1 =
|
Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1 =
|
||||||
boost::none, boost::optional<Matrix&> H2 = boost::none);
|
boost::none, boost::optional<Matrix&> H2 = boost::none);
|
||||||
|
|
||||||
|
|
@ -100,23 +114,30 @@ namespace gtsam {
|
||||||
template<class CFG = Values, class KEY = PoseKey>
|
template<class CFG = Values, class KEY = PoseKey>
|
||||||
class GenericPrior: public NonlinearFactor1<CFG, KEY> {
|
class GenericPrior: public NonlinearFactor1<CFG, KEY> {
|
||||||
public:
|
public:
|
||||||
typedef NonlinearFactor1<CFG, KEY> Base;
|
typedef NonlinearFactor1<CFG, KEY> Base; ///< base class
|
||||||
typedef boost::shared_ptr<GenericPrior<CFG, KEY> > shared_ptr;
|
typedef boost::shared_ptr<GenericPrior<CFG, KEY> > shared_ptr;
|
||||||
typedef typename KEY::Value Pose;
|
typedef typename KEY::Value Pose; ///< shortcut to Pose type
|
||||||
Pose z_;
|
|
||||||
|
|
||||||
|
Pose z_; ///< prior mean
|
||||||
|
|
||||||
|
/// Create generic prior
|
||||||
GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) :
|
GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) :
|
||||||
NonlinearFactor1<CFG, KEY> (model, key), z_(z) {
|
NonlinearFactor1<CFG, KEY>(model, key), z_(z) {
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Return error and optional derivative
|
||||||
Vector evaluateError(const Pose& x, boost::optional<Matrix&> H =
|
Vector evaluateError(const Pose& x, boost::optional<Matrix&> H =
|
||||||
boost::none) const {
|
boost::none) const {
|
||||||
return (prior(x, H) - z_).vector();
|
return (prior(x, H) - z_).vector();
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
GenericPrior() {}
|
|
||||||
/** Serialization function */
|
/// Default constructor
|
||||||
|
GenericPrior() {
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Serialization function
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template<class ARCHIVE>
|
template<class ARCHIVE>
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||||
|
|
@ -131,24 +152,32 @@ namespace gtsam {
|
||||||
template<class CFG = Values, class KEY = PoseKey>
|
template<class CFG = Values, class KEY = PoseKey>
|
||||||
class GenericOdometry: public NonlinearFactor2<CFG, KEY, KEY> {
|
class GenericOdometry: public NonlinearFactor2<CFG, KEY, KEY> {
|
||||||
public:
|
public:
|
||||||
typedef NonlinearFactor2<CFG, KEY, KEY> Base;
|
typedef NonlinearFactor2<CFG, KEY, KEY> Base; ///< base class
|
||||||
typedef boost::shared_ptr<GenericOdometry<CFG, KEY> > shared_ptr;
|
typedef boost::shared_ptr<GenericOdometry<CFG, KEY> > shared_ptr;
|
||||||
typedef typename KEY::Value Pose;
|
typedef typename KEY::Value Pose; ///< shortcut to Pose type
|
||||||
Pose z_;
|
|
||||||
|
|
||||||
|
Pose z_; ///< odometry measurement
|
||||||
|
|
||||||
|
/// Create odometry
|
||||||
GenericOdometry(const Pose& z, const SharedNoiseModel& model,
|
GenericOdometry(const Pose& 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 Pose& x1, const Pose& x2, boost::optional<
|
/// Evaluate error and optionally return derivatives
|
||||||
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
|
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();
|
return (odo(x1, x2, H1, H2) - z_).vector();
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
GenericOdometry() {}
|
|
||||||
/** Serialization function */
|
/// Default constructor
|
||||||
|
GenericOdometry() {
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Serialization function
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template<class ARCHIVE>
|
template<class ARCHIVE>
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||||
|
|
@ -163,26 +192,33 @@ namespace gtsam {
|
||||||
template<class CFG = Values, class XKEY = PoseKey, class LKEY = PointKey>
|
template<class CFG = Values, class XKEY = PoseKey, class LKEY = PointKey>
|
||||||
class GenericMeasurement: public NonlinearFactor2<CFG, XKEY, LKEY> {
|
class GenericMeasurement: public NonlinearFactor2<CFG, XKEY, LKEY> {
|
||||||
public:
|
public:
|
||||||
typedef NonlinearFactor2<CFG, XKEY, LKEY> Base;
|
typedef NonlinearFactor2<CFG, XKEY, LKEY> Base; ///< base class
|
||||||
typedef boost::shared_ptr<GenericMeasurement<CFG, XKEY, LKEY> > shared_ptr;
|
typedef boost::shared_ptr<GenericMeasurement<CFG, XKEY, LKEY> > shared_ptr;
|
||||||
typedef typename XKEY::Value Pose;
|
typedef typename XKEY::Value Pose; ///< shortcut to Pose type
|
||||||
typedef typename LKEY::Value Point;
|
typedef typename LKEY::Value Point; ///< shortcut to Point type
|
||||||
|
|
||||||
Point z_;
|
Point z_; ///< Measurement
|
||||||
|
|
||||||
|
/// Create measurement factor
|
||||||
GenericMeasurement(const Point& z, const SharedNoiseModel& model,
|
GenericMeasurement(const Point& z, const SharedNoiseModel& model,
|
||||||
const XKEY& i, const LKEY& j) :
|
const XKEY& i, const LKEY& j) :
|
||||||
NonlinearFactor2<CFG, XKEY, LKEY> (model, i, j), z_(z) {
|
NonlinearFactor2<CFG, XKEY, LKEY>(model, i, j), z_(z) {
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector evaluateError(const Pose& x1, const Point& x2, boost::optional<
|
/// Evaluate error and optionally return derivatives
|
||||||
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
|
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();
|
return (mea(x1, x2, H1, H2) - z_).vector();
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
GenericMeasurement() {}
|
|
||||||
/** Serialization function */
|
/// Default constructor
|
||||||
|
GenericMeasurement() {
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Serialization function
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template<class ARCHIVE>
|
template<class ARCHIVE>
|
||||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue