From a32892c043d54432f30918794f97a01ff2db2aca Mon Sep 17 00:00:00 2001 From: Kai Ni Date: Wed, 7 Apr 2010 19:09:14 +0000 Subject: [PATCH] added oriented version of simulate2d --- cpp/Makefile.am | 7 ++-- cpp/Simulated2DOrientedConfig.h | 49 ++++++++++++++++++++++++++ cpp/Simulated2DOrientedPosePrior.h | 19 ++++++++++ cpp/gtsam.h | 18 ++++++++++ cpp/simulated2DOriented.cpp | 31 +++++++++++++++++ cpp/simulated2DOriented.h | 56 ++++++++++++++++++++++++++++++ 6 files changed, 177 insertions(+), 3 deletions(-) create mode 100644 cpp/Simulated2DOrientedConfig.h create mode 100644 cpp/Simulated2DOrientedPosePrior.h create mode 100644 cpp/simulated2DOriented.cpp create mode 100644 cpp/simulated2DOriented.h diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 552ee0589..f175b6baf 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -180,10 +180,11 @@ testLieConfig_LDADD = libgtsam.la testTupleConfig_LDADD = libgtsam.la # simulated2D example -headers += Simulated2DConfig.h -headers += Simulated2DPosePrior.h Simulated2DPointPrior.h +headers += simulated2D.h simulated2DOriented.h +headers += Simulated2DConfig.h Simulated2DOrientedConfig.h +headers += Simulated2DPosePrior.h Simulated2DPointPrior.h Simulated2DOrientedPosePrior.h headers += Simulated2DOdometry.h Simulated2DMeasurement.h -sources += simulated2D.cpp +sources += simulated2D.cpp simulated2DOriented.cpp testSimulated2D_SOURCES = testSimulated2D.cpp testSimulated2D_LDADD = libgtsam.la check_PROGRAMS += testSimulated2D diff --git a/cpp/Simulated2DOrientedConfig.h b/cpp/Simulated2DOrientedConfig.h new file mode 100644 index 000000000..41a3606e5 --- /dev/null +++ b/cpp/Simulated2DOrientedConfig.h @@ -0,0 +1,49 @@ +/* + * Simulated2DConfig.h + * + * Re-created on Feb 22, 2010 for compatibility with MATLAB + * Author: Frank Dellaert + */ + +#pragma once + +#include "simulated2DOriented.h" + +namespace gtsam { + + class Simulated2DOrientedConfig: public simulated2DOriented::Config { + public: + typedef boost::shared_ptr sharedPoint; + typedef boost::shared_ptr sharedPose; + + Simulated2DOrientedConfig() { + } + + void insertPose(const simulated2DOriented::PoseKey& i, const Pose2& p) { + insert(i, p); + } + + void insertPoint(const simulated2DOriented::PointKey& j, const Point2& p) { + insert(j, p); + } + + int nrPoses() const { + return this->first_.size(); + } + + int nrPoints() const { + return this->second_.size(); + } + + sharedPose pose(const simulated2DOriented::PoseKey& i) { + return sharedPose(new Pose2((*this)[i])); + } + + sharedPoint point(const simulated2DOriented::PointKey& j) { + return sharedPoint(new Point2((*this)[j])); + } + + }; + +} // namespace gtsam + diff --git a/cpp/Simulated2DOrientedPosePrior.h b/cpp/Simulated2DOrientedPosePrior.h new file mode 100644 index 000000000..c9a4a8681 --- /dev/null +++ b/cpp/Simulated2DOrientedPosePrior.h @@ -0,0 +1,19 @@ +/* + * Simulated2DPosePrior.h + * + * Re-created on Feb 22, 2010 for compatibility with MATLAB + * Author: Frank Dellaert + */ + +#pragma once + +#include "simulated2DOriented.h" +#include "Simulated2DOrientedConfig.h" + +namespace gtsam { + + /** Create a prior on a pose Point2 with key 'x1' etc... */ + typedef simulated2DOriented::GenericPosePrior Simulated2DOrientedPosePrior; + +} + diff --git a/cpp/gtsam.h b/cpp/gtsam.h index c79d17f51..026592995 100644 --- a/cpp/gtsam.h +++ b/cpp/gtsam.h @@ -170,6 +170,17 @@ class Simulated2DConfig { Point2* point(int j); }; +class Simulated2DOrientedConfig { + Simulated2DOrientedConfig(); + void print(string s) const; + void insertPose(int i, const Pose2& p); + void insertPoint(int j, const Point2& p); + int nrPoses() const; + int nrPoints() const; + Pose2* pose(int i); + Point2* point(int j); +}; + class Simulated2DPosePrior { Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i); void print(string s) const; @@ -177,6 +188,13 @@ class Simulated2DPosePrior { double error(const Simulated2DConfig& c) const; }; +class Simulated2DOrientedPosePrior { + Simulated2DOrientedPosePrior(Pose2& mu, const SharedDiagonal& model, int i); + void print(string s) const; + GaussianFactor* linearize(const Simulated2DOrientedConfig& config) const; + double error(const Simulated2DOrientedConfig& c) const; +}; + class Simulated2DPointPrior { Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i); void print(string s) const; diff --git a/cpp/simulated2DOriented.cpp b/cpp/simulated2DOriented.cpp new file mode 100644 index 000000000..744b1f7e9 --- /dev/null +++ b/cpp/simulated2DOriented.cpp @@ -0,0 +1,31 @@ +/** + * @file simulated2DOriented + * @brief measurement functions and derivatives for simulated 2D robot + * @author Frank Dellaert + */ + +#include "simulated2DOriented.h" +#include "TupleConfig-inl.h" + +namespace gtsam { + + using namespace simulated2DOriented; +// INSTANTIATE_LIE_CONFIG(PointKey, Point2) +// INSTANTIATE_PAIR_CONFIG(PoseKey, Pose2, PointKey, Point2) +// INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) +// INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) + + namespace simulated2DOriented { + + static Matrix I = gtsam::eye(3); + + /* ************************************************************************* */ + Pose2 prior(const Pose2& x, boost::optional H) { + if (H) *H = I; + return x; + } + + /* ************************************************************************* */ + + } // namespace simulated2DOriented +} // namespace gtsam diff --git a/cpp/simulated2DOriented.h b/cpp/simulated2DOriented.h new file mode 100644 index 000000000..7706dbc80 --- /dev/null +++ b/cpp/simulated2DOriented.h @@ -0,0 +1,56 @@ +/** + * @file simulated2D.h + * @brief measurement functions and derivatives for simulated 2D robot + * @author Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include "Pose2.h" +#include "TupleConfig.h" +#include "NonlinearFactor.h" + +// \namespace + +namespace gtsam { + + namespace simulated2DOriented { + + // The types that take an oriented pose2 rather than point2 + typedef TypedSymbol PointKey; + typedef TypedSymbol PoseKey; + typedef PairConfig Config; + + //TODO:: point prior is not implemented right now + + /** + * Prior on a single pose, and optional derivative version + */ + inline Pose2 prior(const Pose2& x) { + return x; + } + Pose2 prior(const Pose2& x, boost::optional H = boost::none); + + /** + * Unary factor encoding a soft prior on a vector + */ + template + struct GenericPosePrior: public NonlinearFactor1 { + + Pose2 z_; + + GenericPosePrior(const Pose2& z, const SharedGaussian& model, const Key& key) : + NonlinearFactor1 (model, key), z_(z) { + } + + Vector evaluateError(const Pose2& x, boost::optional H = + boost::none) const { + return logmap(z_, prior(x, H)); + } + + }; + + } // namespace simulated2DOriented +} // namespace gtsam