diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 671877c6f..fff8c76ca 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -178,6 +178,9 @@ testLieConfig_LDADD = libgtsam.la testTupleConfig_LDADD = libgtsam.la # simulated2D example +headers += Simulated2DConfig.h +headers += Simulated2DPosePrior.h Simulated2DPointPrior.h +headers += Simulated2DOdometry.h Simulated2DMeasurement.h sources += simulated2D.cpp testSimulated2D_SOURCES = testSimulated2D.cpp testSimulated2D_LDADD = libgtsam.la diff --git a/cpp/Simulated2DConfig.h b/cpp/Simulated2DConfig.h new file mode 100644 index 000000000..997577aed --- /dev/null +++ b/cpp/Simulated2DConfig.h @@ -0,0 +1,48 @@ +/* + * Simulated2DConfig.h + * + * Re-created on Feb 22, 2010 for compatibility with MATLAB + * Author: Frank Dellaert + */ + +#pragma once + +#include "simulated2D.h" + +namespace gtsam { + + class Simulated2DConfig: public simulated2D::Config { + public: + typedef boost::shared_ptr sharedPoint; + + Simulated2DConfig() { + } + + void insertPose(const simulated2D::PoseKey& i, const Point2& p) { + insert(i, p); + } + + void insertPoint(const simulated2D::PointKey& j, const Point2& p) { + insert(j, p); + } + + int nrPoses() const { + return this->first_.size(); + } + + int nrPoints() const { + return this->second_.size(); + } + + sharedPoint pose(const simulated2D::PoseKey& i) { + return sharedPoint(new Point2((*this)[i])); + } + + sharedPoint point(const simulated2D::PointKey& j) { + return sharedPoint(new Point2((*this)[j])); + } + + }; + +} // namespace gtsam + diff --git a/cpp/Simulated2DMeasurement.h b/cpp/Simulated2DMeasurement.h new file mode 100644 index 000000000..61840b15d --- /dev/null +++ b/cpp/Simulated2DMeasurement.h @@ -0,0 +1,18 @@ +/* + * Simulated2DMeasurement.h + * + * Re-created on Feb 22, 2010 for compatibility with MATLAB + * Author: Frank Dellaert + */ + +#pragma once + +#include "simulated2D.h" +#include "Simulated2DConfig.h" + +namespace gtsam { + + typedef simulated2D::Measurement Simulated2DMeasurement; + +} + diff --git a/cpp/Simulated2DOdometry.h b/cpp/Simulated2DOdometry.h new file mode 100644 index 000000000..93aca0ce6 --- /dev/null +++ b/cpp/Simulated2DOdometry.h @@ -0,0 +1,18 @@ +/* + * Simulated2DOdometry.h + * + * Re-created on Feb 22, 2010 for compatibility with MATLAB + * Author: Frank Dellaert + */ + +#pragma once + +#include "simulated2D.h" +#include "Simulated2DConfig.h" + +namespace gtsam { + + typedef simulated2D::Odometry Simulated2DOdometry; + +} + diff --git a/cpp/Simulated2DPointPrior.h b/cpp/Simulated2DPointPrior.h new file mode 100644 index 000000000..e563978db --- /dev/null +++ b/cpp/Simulated2DPointPrior.h @@ -0,0 +1,19 @@ +/* + * Simulated2DPointPrior.h + * + * Re-created on Feb 22, 2010 for compatibility with MATLAB + * Author: Frank Dellaert + */ + +#pragma once + +#include "simulated2D.h" +#include "Simulated2DConfig.h" + +namespace gtsam { + + /** Create a prior on a landmark Point2 with key 'l1' etc... */ + typedef simulated2D::GenericPrior Simulated2DPointPrior; + +} + diff --git a/cpp/Simulated2DPosePrior.h b/cpp/Simulated2DPosePrior.h new file mode 100644 index 000000000..47da3d500 --- /dev/null +++ b/cpp/Simulated2DPosePrior.h @@ -0,0 +1,19 @@ +/* + * Simulated2DPosePrior.h + * + * Re-created on Feb 22, 2010 for compatibility with MATLAB + * Author: Frank Dellaert + */ + +#pragma once + +#include "simulated2D.h" +#include "Simulated2DConfig.h" + +namespace gtsam { + + /** Create a prior on a pose Point2 with key 'x1' etc... */ + typedef simulated2D::GenericPrior Simulated2DPosePrior; + +} + diff --git a/cpp/gtsam-broken.h b/cpp/gtsam-broken.h index 0a8f78622..e83eaba79 100644 --- a/cpp/gtsam-broken.h +++ b/cpp/gtsam-broken.h @@ -2,36 +2,6 @@ // Solve by parsing a namespace pose2SLAM::Config and making a Pose2SLAMConfig class // We also have to solve the shared pointer mess to avoid duplicate methods -class Point2Prior { - Point2Prior(Vector mu, double sigma, string key); - Vector error_vector(const VectorConfig& c) const; - GaussianFactor* linearize(const VectorConfig& c) const; - double sigma(); - Vector measurement(); - double error(const VectorConfig& c) const; - void print(string s) const; -}; - -class Simulated2DOdometry { - Simulated2DOdometry(Vector odo, double sigma, string key, string key2); - Vector error_vector(const VectorConfig& c) const; - GaussianFactor* linearize(const VectorConfig& c) const; - double sigma(); - Vector measurement(); - double error(const VectorConfig& c) const; - void print(string s) const; -}; - -class Simulated2DMeasurement { - Simulated2DMeasurement(Vector odo, double sigma, string key, string key2); - Vector error_vector(const VectorConfig& c) const; - GaussianFactor* linearize(const VectorConfig& c) const; - double sigma(); - Vector measurement(); - double error(const VectorConfig& c) const; - void print(string s) const; -}; - class Pose2Config{ Pose2Config(); Pose2 get(string key) const; diff --git a/cpp/gtsam.h b/cpp/gtsam.h index 278bc4f3f..c79d17f51 100644 --- a/cpp/gtsam.h +++ b/cpp/gtsam.h @@ -1,18 +1,6 @@ -class Pose2SLAMOptimizer { - Pose2SLAMOptimizer(string dataset_name); - void print(string s) const; - void update(Vector x) const; - Vector optimize() const; - double error() const; - Matrix a1() const; - Matrix a2() const; - Vector b1() const; - Vector b2() const; -}; - class SharedGaussian { - SharedGaussian(Matrix covariance); - SharedGaussian(Vector sigmas); + SharedGaussian(Matrix covariance); + SharedGaussian(Vector sigmas); }; class SharedDiagonal { @@ -23,9 +11,9 @@ class Ordering { Ordering(); Ordering(string key); Ordering subtract(const Ordering& keys) const; - void push_back(string s); void print(string s) const; bool equals(const Ordering& ord, double tol) const; + void push_back(string s); void unique (); void reverse (); }; @@ -37,13 +25,12 @@ class SymbolicFactor{ class VectorConfig { VectorConfig(); + void print(string s) const; + bool equals(const VectorConfig& expected, double tol) const; + void insert(string name, Vector val); Vector get(string name) const; bool contains(string name) const; size_t size() const; - void insert(string name, Vector val); - void print(string s) const; - bool equals(const VectorConfig& expected, double tol) const; - void clear(); }; class GaussianFactor { @@ -65,13 +52,13 @@ class GaussianFactor { Matrix A3, Vector b_in, const SharedDiagonal& model); + void print(string s) const; + bool equals(const GaussianFactor& lf, double tol) const; bool empty() const; Vector get_b() const; Matrix get_A(string key) const; double error(const VectorConfig& c) const; bool involves(string key) const; - void print(string s) const; - bool equals(const GaussianFactor& lf, double tol) const; pair matrix(const Ordering& ordering) const; pair eliminate(string key) const; }; @@ -102,9 +89,9 @@ class GaussianConditional { Matrix T, Vector sigmas); void print(string s) const; - Vector solve(const VectorConfig& x); - void add(string key, Matrix S); bool equals(const GaussianConditional &cg, double tol) const; + void add(string key, Matrix S); + Vector solve(const VectorConfig& x); }; class GaussianBayesNet { @@ -117,13 +104,13 @@ class GaussianBayesNet { class GaussianFactorGraph { GaussianFactorGraph(); + void print(string s) const; + bool equals(const GaussianFactorGraph& lfgraph, double tol) const; size_t size() const; void push_back(GaussianFactor* ptr_f); double error(const VectorConfig& c) const; double probPrime(const VectorConfig& c) const; - void print(string s) const; - bool equals(const GaussianFactorGraph& lfgraph, double tol) const; void combine(const GaussianFactorGraph& lfg); GaussianConditional* eliminateOne(string key); @@ -138,20 +125,20 @@ class GaussianFactorGraph { class Point2 { Point2(); Point2(double x, double y); + void print(string s) const; double x(); double y(); - void print(string s) const; }; class Point3 { Point3(); Point3(double x, double y, double z); Point3(Vector v); + void print(string s) const; Vector vector() const; double x(); double y(); double z(); - void print(string s) const; }; class Pose2 { @@ -171,3 +158,55 @@ class Pose2 { Point2 t() const; Rot2 r() const; }; + +class Simulated2DConfig { + Simulated2DConfig(); + void print(string s) const; + void insertPose(int i, const Point2& p); + void insertPoint(int j, const Point2& p); + int nrPoses() const; + int nrPoints() const; + Point2* pose(int i); + Point2* point(int j); +}; + +class Simulated2DPosePrior { + Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i); + void print(string s) const; + GaussianFactor* linearize(const Simulated2DConfig& config) const; + double error(const Simulated2DConfig& c) const; +}; + +class Simulated2DPointPrior { + Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i); + void print(string s) const; + GaussianFactor* linearize(const Simulated2DConfig& config) const; + double error(const Simulated2DConfig& c) const; +}; + +class Simulated2DOdometry { + Simulated2DOdometry(Point2& mu, const SharedDiagonal& model, int i1, int i2); + void print(string s) const; + GaussianFactor* linearize(const Simulated2DConfig& config) const; + double error(const Simulated2DConfig& c) const; +}; + +class Simulated2DMeasurement { + Simulated2DMeasurement(Point2& mu, const SharedDiagonal& model, int i, int j); + void print(string s) const; + GaussianFactor* linearize(const Simulated2DConfig& config) const; + double error(const Simulated2DConfig& c) const; +}; + +class Pose2SLAMOptimizer { + Pose2SLAMOptimizer(string dataset_name); + void print(string s) const; + void update(Vector x) const; + Vector optimize() const; + double error() const; + Matrix a1() const; + Matrix a2() const; + Vector b1() const; + Vector b2() const; +}; + diff --git a/cpp/simulated2D.h b/cpp/simulated2D.h index 97191090a..974ba31b3 100644 --- a/cpp/simulated2D.h +++ b/cpp/simulated2D.h @@ -52,7 +52,7 @@ namespace gtsam { /** * Unary factor encoding a soft prior on a vector */ - template + template struct GenericPrior: public NonlinearFactor1 { Point2 z_; @@ -71,15 +71,14 @@ namespace gtsam { /** * Binary factor simulating "odometry" between two Vectors */ - template + template struct GenericOdometry: public NonlinearFactor2 { Point2 z_; - GenericOdometry(const Point2& z, const SharedGaussian& model, const Key& j1, - const Key& j2) : - z_(z), NonlinearFactor2 ( - model, j1, j2) { + GenericOdometry(const Point2& z, const SharedGaussian& model, + const Key& i1, const Key& i2) : + z_(z), NonlinearFactor2 (model, i1, i2) { } Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< @@ -92,22 +91,22 @@ namespace gtsam { /** * Binary factor simulating "measurement" between two Vectors */ - template - class GenericMeasurement: public NonlinearFactor2 { - public: + template + class GenericMeasurement: public NonlinearFactor2 { + public: - Point2 z_; + Point2 z_; - GenericMeasurement(const Point2& z, const SharedGaussian& model, - const XKey& j1, const LKey& j2) : - z_(z), NonlinearFactor2 ( - model, j1, j2) { - } + GenericMeasurement(const Point2& z, const SharedGaussian& model, + const XKey& i, const LKey& j) : + z_(z), NonlinearFactor2 (model, i, j) { + } - Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< - Matrix&> H1 = boost::none, boost::optional H2 = boost::none) const { - return (mea(x1, x2, H1, H2) - z_).vector(); - } + Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< + Matrix&> H1 = boost::none, boost::optional H2 = boost::none) const { + return (mea(x1, x2, H1, H2) - z_).vector(); + } }; diff --git a/cpp/testSimulated2D.cpp b/cpp/testSimulated2D.cpp index ec8fc136b..4f7b00e45 100644 --- a/cpp/testSimulated2D.cpp +++ b/cpp/testSimulated2D.cpp @@ -10,11 +10,21 @@ #include "numericalDerivative.h" #include "simulated2D.h" +#include "Simulated2DConfig.h" using namespace gtsam; using namespace std; using namespace simulated2D; +/* ************************************************************************* */ +TEST( simulated2D, Simulated2DConfig ) +{ + Simulated2DConfig actual; + actual.insertPose(1,Point2(1,1)); + actual.insertPoint(2,Point2(2,2)); + CHECK(assert_equal(actual,actual,1e-9)); +} + /* ************************************************************************* */ TEST( simulated2D, Dprior ) { diff --git a/matlab/example/christian01.m b/matlab/example/christian01.m new file mode 100644 index 000000000..65856310d --- /dev/null +++ b/matlab/example/christian01.m @@ -0,0 +1,58 @@ +% Set up a small SLAM example +% Christian Potthast, Frank Dellaert + +clear; +close all; + +n = 100; +m = 20; + +% have the robot move in this world +trajectory = random_walk([0.1,0.1],5,m); +plot(trajectory(1,:),trajectory(2,:),'b+'); +axis([0 100 0 100]);axis square; + +% Set up the map +mappingArea=max(trajectory,[],2); +map = create_random_landmarks(n, mappingArea); +figure(1);clf; +plot(map(1,:), map(2,:),'g.'); hold on; +axis([0 mappingArea(1) 0 mappingArea(2)]);axis square; + +% Check visibility and plot this on the problem figure +visibility = create_visibility(map, trajectory,10); +gplot(visibility,[map trajectory]'); + +% simulate the measurements +measurement_sigma = 1; +odo_sigma = 0.1; +[measurements, odometry] = simulate_measurements(map, trajectory, visibility, measurement_sigma, odo_sigma); + +% create a configuration of all zeroes +config = create_config(n,m); + +% create the factor graph +factorGraph = create_gaussian_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n); + +% create an ordering +ord = create_ordering(n,m); + +% show the matrix +figure(2); clf; +A = factorGraph.matrix(ord); +spy(A); + +% optimizing a BayesNet is not possible from MATLAB as +% GaussianBayesNet is a typedef not a real class :-( +% BayesNet = factorGraph.eliminate_(ord); +% optimal = BayesNet.optimize; + +% However, we can call the optimize_ method of a GaussianFactorGraph +optimal = factorGraph.optimize_(ord); + +% plot the solution +figure(3);clf; +plot_config(optimal,n,m);hold on +plot(trajectory(1,:),trajectory(2,:),'b+'); +plot(map(1,:), map(2,:),'g.'); +axis([0 mappingArea(1) 0 mappingArea(2)]);axis square; diff --git a/matlab/example/create_config.m b/matlab/example/create_config.m index 63ae0eb7c..9340a1c57 100644 --- a/matlab/example/create_config.m +++ b/matlab/example/create_config.m @@ -3,12 +3,13 @@ function config = create_config(n,m) -config = VectorConfig(); - -for j = 1:n - config.insert(sprintf('l%d',j), [0;0]); -end +config = Simulated2DConfig(); +origin = Point2; for i = 1:m - config.insert(sprintf('x%d',i), [0;0]); + config.insertPose(i, origin); +end + +for j = 1:n + config.insertPoint(j, origin); end \ No newline at end of file diff --git a/matlab/example/create_gaussian_factor_graph.m b/matlab/example/create_gaussian_factor_graph.m index 35dc824cb..d4bc146d2 100644 --- a/matlab/example/create_gaussian_factor_graph.m +++ b/matlab/example/create_gaussian_factor_graph.m @@ -8,35 +8,39 @@ m = size(measurements,2); % create linear factor graph lfg = GaussianFactorGraph(); +% Point2 at origin +origin = Point2; + % create prior for initial robot pose -prior = Point2Prior([0;0],0.2,'x1'); +model0_2 = SharedDiagonal([0.2;0.2]); +prior = Simulated2DPosePrior(origin,model0_2,1); lf = prior.linearize(config); lfg.push_back(lf); % add prior for landmarks +model1000 = SharedDiagonal([1000;1000]); for j = 1:n - key = sprintf('l%d',j); - prior = Point2Prior([0;0],1000,key); + prior = Simulated2DPointPrior(origin,model1000,j); lf = prior.linearize(config); lfg.push_back(lf); end -% add measurement factors -for k = 1 : size(measurements,2) - measurement = measurements{k}; - i = sprintf('x%d',measurement.i); - j = sprintf('l%d',measurement.j); - nlf = Simulated2DMeasurement(measurement.z, measurement_sigma, i, j); +% add odometry factors +odo_model = SharedDiagonal([odo_sigma;odo_sigma]); +for i = 2 : size(odometry,2) + odo = Point2(odometry{i}(1),odometry{i}(2)); + nlf = Simulated2DOdometry(odo, odo_model, i-1, i); lf = nlf.linearize(config); lfg.push_back(lf); end -% add odometry factors -for i = 2 : size(odometry,2) - odo = odometry{i}; - p = sprintf('x%d',i-1); - c = sprintf('x%d',i); - nlf = Simulated2DOdometry(odo, odo_sigma, p, c); +% add measurement factors +measurement_model = SharedDiagonal([measurement_sigma;measurement_sigma]); +for k = 1 : size(measurements,2) + measurement = measurements{k}; + point = Point2(measurement.z(1),measurement.z(2)); + nlf = Simulated2DMeasurement(point, measurement_model, measurement.i, measurement.j); lf = nlf.linearize(config); lfg.push_back(lf); -end \ No newline at end of file +end + diff --git a/matlab/example/create_linear_factor_graph.m b/matlab/example/create_linear_factor_graph.m deleted file mode 100644 index 0098e311e..000000000 --- a/matlab/example/create_linear_factor_graph.m +++ /dev/null @@ -1,42 +0,0 @@ -% Christan Potthast -% create linear factor graph - -function lfg = create_linear_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n) - -m = size(measurements,2); - -% create linear factor graph -lfg = GaussianFactorGraph(); - -% create prior for initial robot pose -prior = Point2Prior([0;0],0.2,'x1'); -lf = prior.linearize(config); -lfg.push_back(lf); - -% add prior for landmarks -for j = 1:n - key = sprintf('l%d',j); - prior = Point2Prior([0;0],1000,key); - lf = prior.linearize(config); - lfg.push_back(lf); -end - -% add measurement factors -for k = 1 : size(measurements,2) - measurement = measurements{k}; - i = sprintf('x%d',measurement.i); - j = sprintf('l%d',measurement.j); - nlf = Simulated2DMeasurement(measurement.z, measurement_sigma, i, j); - lf = nlf.linearize(config); - lfg.push_back(lf); -end - -% add odometry factors -for i = 2 : size(odometry,2) - odo = odometry{i}; - p = sprintf('x%d',i-1); - c = sprintf('x%d',i); - nlf = Simulated2DOdometry(odo, odo_sigma, p, c); - lf = nlf.linearize(config); - lfg.push_back(lf); -end \ No newline at end of file diff --git a/matlab/example/plot_config.m b/matlab/example/plot_config.m new file mode 100644 index 000000000..0554b9b06 --- /dev/null +++ b/matlab/example/plot_config.m @@ -0,0 +1,19 @@ +% Christian Potthast +% plot a configuration + +function plot_config(config,n,m) + +hold on; + +for j = 1:n + key = sprintf('l%d',j); + mj = config.get(key); + plot(mj(1), mj(2),'r*'); +end + +for i = 1:m + key = sprintf('x%d',i); + xi = config.get(key); + plot(xi(1), xi(2),'rx'); +end +