12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed christian01 example, polluting gtsam with extra header files for MATLAB. Need to look for a better solution.

release/4.3a0
Frank Dellaert 2010-02-23 05:06:16 +00:00
parent ab218f11d2
commit a178023a27
15 changed files with 323 additions and 140 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cpp/Makefile.am
View File

@ -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

48
cpp/Simulated2DConfig.h Normal file
View File

@ -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<Point2> 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

18
cpp/Simulated2DMeasurement.h Normal file
View File

@ -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;
}

18
cpp/Simulated2DOdometry.h Normal file
View File

@ -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;
}

19
cpp/Simulated2DPointPrior.h Normal file
View File

@ -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<Simulated2DConfig, simulated2D::PointKey> Simulated2DPointPrior;
}

19
cpp/Simulated2DPosePrior.h Normal file
View File

@ -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<Simulated2DConfig, simulated2D::PoseKey> Simulated2DPosePrior;
}

30
cpp/gtsam-broken.h
View File

@ -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;

93
cpp/gtsam.h
View File

@ -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,Vector> matrix(const Ordering& ordering) const;
pair<GaussianConditional*,GaussianFactor*> 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;
};

37
cpp/simulated2D.h
View File

@ -52,7 +52,7 @@ namespace gtsam {
/**
* Unary factor encoding a soft prior on a vector
*/
template<class Cfg=Config, class Key=PoseKey>
template<class Cfg = Config, class Key = PoseKey>
struct GenericPrior: public NonlinearFactor1<Cfg, Key, Point2> {
Point2 z_;
@ -71,15 +71,14 @@ namespace gtsam {
/**
* Binary factor simulating "odometry" between two Vectors
*/
template<class Cfg=Config, class Key=PoseKey>
template<class Cfg = Config, class Key = PoseKey>
struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Point2, Key,
Point2> {
Point2 z_;
GenericOdometry(const Point2& z, const SharedGaussian& model, const Key& j1,
const Key& j2) :
z_(z), NonlinearFactor2<Cfg, Key, Point2, Key, Point2> (
model, j1, j2) {
GenericOdometry(const Point2& z, const SharedGaussian& model,
const Key& i1, const Key& i2) :
z_(z), NonlinearFactor2<Cfg, Key, Point2, Key, Point2> (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 Cfg=Config, class XKey=PoseKey, class LKey=PointKey>
class GenericMeasurement: public NonlinearFactor2<Cfg, XKey, Point2, LKey, Point2> {
public:
template<class Cfg = Config, class XKey = PoseKey, class LKey = PointKey>
class GenericMeasurement: public NonlinearFactor2<Cfg, XKey, Point2, LKey,
Point2> {
public:
Point2 z_;
Point2 z_;
GenericMeasurement(const Point2& z, const SharedGaussian& model,
const XKey& j1, const LKey& j2) :
z_(z), NonlinearFactor2<Cfg, XKey, Point2, LKey, Point2> (
model, j1, j2) {
}
GenericMeasurement(const Point2& z, const SharedGaussian& model,
const XKey& i, const LKey& j) :
z_(z), NonlinearFactor2<Cfg, XKey, Point2, LKey, Point2> (model, i, j) {
}
Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional<
Matrix&> H1 = boost::none, boost::optional<Matrix&> 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<Matrix&> H2 = boost::none) const {
return (mea(x1, x2, H1, H2) - z_).vector();
}
};

10
cpp/testSimulated2D.cpp
View File

@ -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 )
{

58
matlab/example/christian01.m Normal file
View File

@ -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;

13
matlab/example/create_config.m
View File

@ -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

36
matlab/example/create_gaussian_factor_graph.m
View File

@ -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
end

42
matlab/example/create_linear_factor_graph.m
View File

@ -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

19
matlab/example/plot_config.m Normal file
View File

@ -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