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

Matlab wrapped classes now live within the gtsam namespace, meaning you have to use gtsamPose2 instead of just Pose2 in Matlab. This fixes the Point2 and Point3 issues we had with Matlab 2011b. 

promote pose2SLAM planarSLAM etc. namespaces to be next to gtsam instead of within
release/4.3a0
Chris Beall 2012-01-30 22:00:13 +00:00
parent 67aa0bf63d
commit 537a1a3fae
22 changed files with 882 additions and 829 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/PlanarSLAMExample_easy.cpp
View File

@ -24,7 +24,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::planarSLAM; using namespace planarSLAM;
/** /**
* In this version of the system we make the following assumptions: * In this version of the system we make the following assumptions:

2
examples/Pose2SLAMExample_advanced.cpp
View File

@ -30,7 +30,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace boost; using namespace boost;
using namespace gtsam::pose2SLAM; using namespace pose2SLAM;
int main(int argc, char** argv) { int main(int argc, char** argv) {
// create keys for robot positions // create keys for robot positions

2
examples/Pose2SLAMExample_easy.cpp
View File

@ -28,7 +28,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::pose2SLAM; using namespace pose2SLAM;
int main(int argc, char** argv) { int main(int argc, char** argv) {
// create keys for robot positions // create keys for robot positions

40
examples/matlab/PlanarSLAMExample_easy.m
View File

@ -28,31 +28,31 @@ graph = planarSLAMGraph;
%% Add prior %% Add prior
% gaussian for prior % gaussian for prior
prior_model = SharedNoiseModel_sharedSigmas([0.3; 0.3; 0.1]); prior_model = gtsamSharedNoiseModel_sharedSigmas([0.3; 0.3; 0.1]);
prior_measurement = Pose2(0.0, 0.0, 0.0); % prior at origin prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph
%% Add odometry %% Add odometry
% general noisemodel for odometry % general noisemodel for odometry
odom_model = SharedNoiseModel_sharedSigmas([0.2; 0.2; 0.1]); odom_model = gtsamSharedNoiseModel_sharedSigmas([0.2; 0.2; 0.1]);
odom_measurement = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
graph.addOdometry(x1, x2, odom_measurement, odom_model); graph.addOdometry(x1, x2, odom_measurement, odom_model);
graph.addOdometry(x2, x3, odom_measurement, odom_model); graph.addOdometry(x2, x3, odom_measurement, odom_model);
%% Add measurements %% Add measurements
% general noisemodel for measurements % general noisemodel for measurements
meas_model = SharedNoiseModel_sharedSigmas([0.1; 0.2]); meas_model = gtsamSharedNoiseModel_sharedSigmas([0.1; 0.2]);
% create the measurement values - indices are (pose id, landmark id) % create the measurement values - indices are (pose id, landmark id)
degrees = pi/180; degrees = pi/180;
bearing11 = Rot2(45*degrees); bearing11 = gtsamRot2(45*degrees);
bearing21 = Rot2(90*degrees); bearing21 = gtsamRot2(90*degrees);
bearing32 = Rot2(90*degrees); bearing32 = gtsamRot2(90*degrees);
range11 = sqrt(4+4); range11 = sqrt(4+4);
range21 = 2.0; range21 = 2.0;
range32 = 2.0; range32 = 2.0;
% create bearing/range factors and add them % % create bearing/range factors and add them
graph.addBearingRange(x1, l1, bearing11, range11, meas_model); graph.addBearingRange(x1, l1, bearing11, range11, meas_model);
graph.addBearingRange(x2, l1, bearing21, range21, meas_model); graph.addBearingRange(x2, l1, bearing21, range21, meas_model);
graph.addBearingRange(x3, l2, bearing32, range32, meas_model); graph.addBearingRange(x3, l2, bearing32, range32, meas_model);
@ -62,26 +62,14 @@ graph.print('full graph');
%% Initialize to noisy points %% Initialize to noisy points
initialEstimate = planarSLAMValues; initialEstimate = planarSLAMValues;
initialEstimate.insertPose(x1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insertPose(x1, gtsamPose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(x2, Pose2(2.3, 0.1,-0.2)); initialEstimate.insertPose(x2, gtsamPose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(x3, Pose2(4.1, 0.1, 0.1)); initialEstimate.insertPose(x3, gtsamPose2(4.1, 0.1, 0.1));
initialEstimate.insertPoint(l1, Point2(1.8, 2.1)); initialEstimate.insertPoint(l1, gtsamPoint2(1.8, 2.1));
initialEstimate.insertPoint(l2, Point2(4.1, 1.8)); initialEstimate.insertPoint(l2, gtsamPoint2(4.1, 1.8));
initialEstimate.print('initial estimate'); initialEstimate.print('initial estimate');
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate); result = graph.optimize(initialEstimate);
result.print('final result'); result.print('final result');
%% Get the corresponding dense matrix
ord = graph.orderingCOLAMD(result);
gfg = graph.linearize(result,ord);
denseAb = gfg.denseJacobian;
%% Get sparse matrix A and RHS b
IJS = gfg.sparseJacobian_();
Ab=sparse(IJS(1,:),IJS(2,:),IJS(3,:));
A = Ab(:,1:end-1);
b = full(Ab(:,end));
spy(A);

82
examples/matlab/Pose2SLAMExample_advanced.m Normal file
View File

@ -0,0 +1,82 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
%
% @brief Simple robotics example using the pre-built planar SLAM domain
% @author Alex Cunningham
% @author Frank Dellaert
% @author Chris Beall
% @author Vadim Indelman
% @author Can Erdogan
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Assumptions
% - All values are axis aligned
% - Robot poses are facing along the X axis (horizontal, to the right in images)
% - We have full odometry for measurements
% - The robot is on a grid, moving 2 meters each step
%% Create keys for variables
x1 = 1; x2 = 2; x3 = 3;
%% Create graph container and add factors to it
graph = pose2SLAMGraph;
%% Add prior
% gaussian for prior
prior_model = gtsamSharedNoiseModel_sharedSigmas([0.3; 0.3; 0.1]);
prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph
%% Add odometry
% general noisemodel for odometry
odom_model = gtsamSharedNoiseModel_sharedSigmas([0.2; 0.2; 0.1]);
odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
graph.addOdometry(x1, x2, odom_measurement, odom_model);
graph.addOdometry(x2, x3, odom_measurement, odom_model);
%% Add measurements
% general noisemodel for measurements
meas_model = gtsamSharedNoiseModel_sharedSigmas([0.1; 0.2]);
% print
graph.print('full graph');
%% Initialize to noisy points
initialEstimate = pose2SLAMValues;
initialEstimate.insertPose(x1, gtsamPose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(x2, gtsamPose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(x3, gtsamPose2(4.1, 0.1, 0.1));
initialEstimate.print('initial estimate');
%% set up solver, choose ordering and optimize
params = NonlinearOptimizationParameters_newDrecreaseThresholds(1e-15, 1e-15);
ord = graph.orderingCOLAMD(initialEstimate);
result = pose2SLAMOptimizer(graph,initialEstimate,ord,params);
result.print('final result');
% %
% % disp('\\\');
% %
% % %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
% % result = graph.optimize(initialEstimate);
% % result.print('final result');
%% Get the corresponding dense matrix
ord = graph.orderingCOLAMD(result);
gfg = graph.linearize(result,ord);
denseAb = gfg.denseJacobian;
%% Get sparse matrix A and RHS b
IJS = gfg.sparseJacobian_();
Ab=sparse(IJS(1,:),IJS(2,:),IJS(3,:));
A = Ab(:,1:end-1);
b = full(Ab(:,end));
spy(A);

26
examples/matlab/Pose2SLAMExample_easy.m
View File

@ -26,14 +26,14 @@ graph = pose2SLAMGraph;
%% Add prior %% Add prior
% gaussian for prior % gaussian for prior
prior_model = SharedNoiseModel_sharedSigmas([0.3; 0.3; 0.1]); prior_model = gtsamSharedNoiseModel_sharedSigmas([0.3; 0.3; 0.1]);
prior_measurement = Pose2(0.0, 0.0, 0.0); % prior at origin prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph
%% Add odometry %% Add odometry
% general noisemodel for odometry % general noisemodel for odometry
odom_model = SharedNoiseModel_sharedSigmas([0.2; 0.2; 0.1]); odom_model = gtsamSharedNoiseModel_sharedSigmas([0.2; 0.2; 0.1]);
odom_measurement = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
graph.addOdometry(x1, x2, odom_measurement, odom_model); graph.addOdometry(x1, x2, odom_measurement, odom_model);
graph.addOdometry(x2, x3, odom_measurement, odom_model); graph.addOdometry(x2, x3, odom_measurement, odom_model);
@ -46,24 +46,12 @@ graph.print('full graph');
%% Initialize to noisy points %% Initialize to noisy points
initialEstimate = pose2SLAMValues; initialEstimate = pose2SLAMValues;
initialEstimate.insertPose(x1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insertPose(x1, gtsamPose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(x2, Pose2(2.3, 0.1,-0.2)); initialEstimate.insertPose(x2, gtsamPose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(x3, Pose2(4.1, 0.1, 0.1)); initialEstimate.insertPose(x3, gtsamPose2(4.1, 0.1, 0.1));
initialEstimate.print('initial estimate'); initialEstimate.print('initial estimate');
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate); result = graph.optimize(initialEstimate);
result.print('final result'); result.print('final result');
%% Get the corresponding dense matrix
ord = graph.orderingCOLAMD(result);
gfg = graph.linearize(result,ord);
denseAb = gfg.denseJacobian;
%% Get sparse matrix A and RHS b
IJS = gfg.sparseJacobian_();
Ab=sparse(IJS(1,:),IJS(2,:),IJS(3,:));
A = Ab(:,1:end-1);
b = full(Ab(:,end));
spy(A);

356
gtsam.h
View File

@ -51,11 +51,13 @@
* - TODO: default values for arguments * - TODO: default values for arguments
* - TODO: overloaded functions * - TODO: overloaded functions
* - TODO: signatures for constructors can be ambiguous if two types have the same first letter * - TODO: signatures for constructors can be ambiguous if two types have the same first letter
* - TODO: Handle Rot3M conversions to quaternions * - TODO: Handle gtsam::Rot3M conversions to quaternions
*/ */
// Everything is in the gtsam namespace, so we avoid copying everything in // Everything is in the gtsam namespace, so we avoid copying everything in
using namespace gtsam; //using namespace gtsam;
namespace gtsam {
//************************************************************************* //*************************************************************************
// base // base
@ -66,89 +68,89 @@ using namespace gtsam;
//************************************************************************* //*************************************************************************
class Point2 { class Point2 {
Point2(); Point2();
Point2(double x, double y); Point2(double x, double y);
static Point2 Expmap(Vector v); static gtsam::Point2 Expmap(Vector v);
static Vector Logmap(const Point2& p); static Vector Logmap(const gtsam::Point2& p);
void print(string s) const; void print(string s) const;
double x(); double x();
double y(); double y();
Vector localCoordinates(const Point2& p); Vector localCoordinates(const gtsam::Point2& p);
Point2 compose(const Point2& p2); gtsam::Point2 compose(const gtsam::Point2& p2);
Point2 between(const Point2& p2); gtsam::Point2 between(const gtsam::Point2& p2);
Point2 retract(Vector v); gtsam::Point2 retract(Vector v);
}; };
class Point3 { class Point3 {
Point3(); Point3();
Point3(double x, double y, double z); Point3(double x, double y, double z);
Point3(Vector v); Point3(Vector v);
static Point3 Expmap(Vector v); static gtsam::Point3 Expmap(Vector v);
static Vector Logmap(const Point3& p); static Vector Logmap(const gtsam::Point3& p);
void print(string s) const; void print(string s) const;
bool equals(const Point3& p, double tol); bool equals(const gtsam::Point3& p, double tol);
Vector vector() const; Vector vector() const;
double x(); double x();
double y(); double y();
double z(); double z();
Vector localCoordinates(const Point3& p); Vector localCoordinates(const gtsam::Point3& p);
Point3 retract(Vector v); gtsam::Point3 retract(Vector v);
Point3 compose(const Point3& p2); gtsam::Point3 compose(const gtsam::Point3& p2);
Point3 between(const Point3& p2); gtsam::Point3 between(const gtsam::Point3& p2);
}; };
class Rot2 { class Rot2 {
Rot2(); Rot2();
Rot2(double theta); Rot2(double theta);
static Rot2 Expmap(Vector v); static gtsam::Rot2 Expmap(Vector v);
static Vector Logmap(const Rot2& p); static Vector Logmap(const gtsam::Rot2& p);
static Rot2 fromAngle(double theta); static gtsam::Rot2 fromAngle(double theta);
static Rot2 fromDegrees(double theta); static gtsam::Rot2 fromDegrees(double theta);
static Rot2 fromCosSin(double c, double s); static gtsam::Rot2 fromCosSin(double c, double s);
static Rot2 relativeBearing(const Point2& d); // Ignoring derivative static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative
static Rot2 atan2(double y, double x); static gtsam::Rot2 atan2(double y, double x);
void print(string s) const; void print(string s) const;
bool equals(const Rot2& rot, double tol) const; bool equals(const gtsam::Rot2& rot, double tol) const;
double theta() const; double theta() const;
double degrees() const; double degrees() const;
double c() const; double c() const;
double s() const; double s() const;
Vector localCoordinates(const Rot2& p); Vector localCoordinates(const gtsam::Rot2& p);
Rot2 retract(Vector v); gtsam::Rot2 retract(Vector v);
Rot2 compose(const Rot2& p2); gtsam::Rot2 compose(const gtsam::Rot2& p2);
Rot2 between(const Rot2& p2); gtsam::Rot2 between(const gtsam::Rot2& p2);
}; };
class Rot3 { class Rot3 {
Rot3(); Rot3();
Rot3(Matrix R); Rot3(Matrix R);
static Rot3 Rx(double t); static gtsam::Rot3 Rx(double t);
static Rot3 Ry(double t); static gtsam::Rot3 Ry(double t);
static Rot3 Rz(double t); static gtsam::Rot3 Rz(double t);
// static Rot3 RzRyRx(double x, double y, double z); // FIXME: overloaded functions don't work yet // static gtsam::Rot3 RzRyRx(double x, double y, double z); // FIXME: overloaded functions don't work yet
static Rot3 RzRyRx(Vector xyz); static gtsam::Rot3 RzRyRx(Vector xyz);
static Rot3 yaw (double t); // positive yaw is to right (as in aircraft heading) static gtsam::Rot3 yaw (double t); // positive yaw is to right (as in aircraft heading)
static Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude) static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude)
static Rot3 roll (double t); // positive roll is to right (increasing yaw in aircraft) static gtsam::Rot3 roll (double t); // positive roll is to right (increasing yaw in aircraft)
static Rot3 ypr(double y, double p, double r); static gtsam::Rot3 ypr(double y, double p, double r);
static Rot3 quaternion(double w, double x, double y, double z); static gtsam::Rot3 quaternion(double w, double x, double y, double z);
static Rot3 rodriguez(Vector v); static gtsam::Rot3 rodriguez(Vector v);
void print(string s) const; void print(string s) const;
bool equals(const Rot3& rot, double tol) const; bool equals(const gtsam::Rot3& rot, double tol) const;
static Rot3 identity(); static gtsam::Rot3 identity();
Rot3 compose(const Rot3& p2) const; gtsam::Rot3 compose(const gtsam::Rot3& p2) const;
Rot3 inverse() const; gtsam::Rot3 inverse() const;
Rot3 between(const Rot3& p2) const; gtsam::Rot3 between(const gtsam::Rot3& p2) const;
Point3 rotate(const Point3& p) const; gtsam::Point3 rotate(const gtsam::Point3& p) const;
Point3 unrotate(const Point3& p) const; gtsam::Point3 unrotate(const gtsam::Point3& p) const;
Rot3 retractCayley(Vector v) const; gtsam::Rot3 retractCayley(Vector v) const;
Rot3 retract(Vector v) const; gtsam::Rot3 retract(Vector v) const;
Vector localCoordinates(const Rot3& p) const; Vector localCoordinates(const gtsam::Rot3& p) const;
static Rot3 Expmap(Vector v); static gtsam::Rot3 Expmap(Vector v);
static Vector Logmap(const Rot3& p); static Vector Logmap(const gtsam::Rot3& p);
Matrix matrix() const; Matrix matrix() const;
Matrix transpose() const; Matrix transpose() const;
Point3 column(int index) const; gtsam::Point3 column(int index) const;
Vector xyz() const; Vector xyz() const;
Vector ypr() const; Vector ypr() const;
Vector rpy() const; Vector rpy() const;
@ -161,48 +163,48 @@ class Rot3 {
class Pose2 { class Pose2 {
Pose2(); Pose2();
Pose2(double x, double y, double theta); Pose2(double x, double y, double theta);
Pose2(double theta, const Point2& t); Pose2(double theta, const gtsam::Point2& t);
Pose2(const Rot2& r, const Point2& t); Pose2(const gtsam::Rot2& r, const gtsam::Point2& t);
Pose2(Vector v); Pose2(Vector v);
static Pose2 Expmap(Vector v); static gtsam::Pose2 Expmap(Vector v);
static Vector Logmap(const Pose2& p); static Vector Logmap(const gtsam::Pose2& p);
void print(string s) const; void print(string s) const;
bool equals(const Pose2& pose, double tol) const; bool equals(const gtsam::Pose2& pose, double tol) const;
double x() const; double x() const;
double y() const; double y() const;
double theta() const; double theta() const;
int dim() const; int dim() const;
Vector localCoordinates(const Pose2& p); Vector localCoordinates(const gtsam::Pose2& p);
Pose2 retract(Vector v); gtsam::Pose2 retract(Vector v);
Pose2 compose(const Pose2& p2); gtsam::Pose2 compose(const gtsam::Pose2& p2);
Pose2 between(const Pose2& p2); gtsam::Pose2 between(const gtsam::Pose2& p2);
Rot2 bearing(const Point2& point); gtsam::Rot2 bearing(const gtsam::Point2& point);
double range(const Point2& point); double range(const gtsam::Point2& point);
Point2 translation() const; gtsam::Point2 translation() const;
Rot2 rotation() const; gtsam::Rot2 rotation() const;
}; };
class Pose3 { class Pose3 {
Pose3(); Pose3();
Pose3(const Rot3& r, const Point3& t); Pose3(const gtsam::Rot3& r, const gtsam::Point3& t);
Pose3(Vector v); Pose3(Vector v);
Pose3(Matrix t); Pose3(Matrix t);
Pose3(const Pose2& pose2); Pose3(const gtsam::Pose2& pose2);
static Pose3 Expmap(Vector v); static gtsam::Pose3 Expmap(Vector v);
static Vector Logmap(const Pose3& p); static Vector Logmap(const gtsam::Pose3& p);
void print(string s) const; void print(string s) const;
bool equals(const Pose3& pose, double tol) const; bool equals(const gtsam::Pose3& pose, double tol) const;
double x() const; double x() const;
double y() const; double y() const;
double z() const; double z() const;
Matrix matrix() const; Matrix matrix() const;
Matrix adjointMap() const; Matrix adjointMap() const;
Pose3 compose(const Pose3& p2); gtsam::Pose3 compose(const gtsam::Pose3& p2);
Pose3 between(const Pose3& p2); gtsam::Pose3 between(const gtsam::Pose3& p2);
Pose3 retract(Vector v); gtsam::Pose3 retract(Vector v);
Pose3 retractFirstOrder(Vector v); gtsam::Pose3 retractFirstOrder(Vector v);
Point3 translation() const; gtsam::Point3 translation() const;
Rot3 rotation() const; gtsam::Rot3 rotation() const;
}; };
//************************************************************************* //*************************************************************************
@ -227,13 +229,13 @@ class SharedDiagonal {
}; };
class SharedNoiseModel { class SharedNoiseModel {
static SharedNoiseModel sharedSigmas(Vector sigmas); static gtsam::SharedNoiseModel sharedSigmas(Vector sigmas);
static SharedNoiseModel sharedSigma(size_t dim, double sigma); static gtsam::SharedNoiseModel sharedSigma(size_t dim, double sigma);
static SharedNoiseModel sharedPrecisions(Vector precisions); static gtsam::SharedNoiseModel sharedPrecisions(Vector precisions);
static SharedNoiseModel sharedPrecision(size_t dim, double precision); static gtsam::SharedNoiseModel sharedPrecision(size_t dim, double precision);
static SharedNoiseModel sharedUnit(size_t dim); static gtsam::SharedNoiseModel sharedUnit(size_t dim);
static SharedNoiseModel sharedSqrtInformation(Matrix R); static gtsam::SharedNoiseModel sharedSqrtInformation(Matrix R);
static SharedNoiseModel sharedCovariance(Matrix covariance); static gtsam::SharedNoiseModel sharedCovariance(Matrix covariance);
void print(string s) const; void print(string s) const;
}; };
@ -241,7 +243,7 @@ class VectorValues {
VectorValues(); VectorValues();
VectorValues(int nVars, int varDim); VectorValues(int nVars, int varDim);
void print(string s) const; void print(string s) const;
bool equals(const VectorValues& expected, double tol) const; bool equals(const gtsam::VectorValues& expected, double tol) const;
int size() const; int size() const;
void insert(int j, Vector value); void insert(int j, Vector value);
}; };
@ -253,7 +255,7 @@ class GaussianConditional {
GaussianConditional(int key, Vector d, Matrix R, int name1, Matrix S, GaussianConditional(int key, Vector d, Matrix R, int name1, Matrix S,
int name2, Matrix T, Vector sigmas); int name2, Matrix T, Vector sigmas);
void print(string s) const; void print(string s) const;
bool equals(const GaussianConditional &cg, double tol) const; bool equals(const gtsam::GaussianConditional &cg, double tol) const;
}; };
class GaussianDensity { class GaussianDensity {
@ -267,35 +269,35 @@ class GaussianDensity {
class GaussianBayesNet { class GaussianBayesNet {
GaussianBayesNet(); GaussianBayesNet();
void print(string s) const; void print(string s) const;
bool equals(const GaussianBayesNet& cbn, double tol) const; bool equals(const gtsam::GaussianBayesNet& cbn, double tol) const;
void push_back(GaussianConditional* conditional); void push_back(gtsam::GaussianConditional* conditional);
void push_front(GaussianConditional* conditional); void push_front(gtsam::GaussianConditional* conditional);
}; };
class GaussianFactor { class GaussianFactor {
void print(string s) const; void print(string s) const;
bool equals(const GaussianFactor& lf, double tol) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const VectorValues& c) const; double error(const gtsam::VectorValues& c) const;
}; };
class JacobianFactor { class JacobianFactor {
JacobianFactor(); JacobianFactor();
JacobianFactor(Vector b_in); JacobianFactor(Vector b_in);
JacobianFactor(int i1, Matrix A1, Vector b, const SharedDiagonal& model); JacobianFactor(int i1, Matrix A1, Vector b, const gtsam::SharedDiagonal& model);
JacobianFactor(int i1, Matrix A1, int i2, Matrix A2, Vector b, JacobianFactor(int i1, Matrix A1, int i2, Matrix A2, Vector b,
const SharedDiagonal& model); const gtsam::SharedDiagonal& model);
JacobianFactor(int i1, Matrix A1, int i2, Matrix A2, int i3, Matrix A3, JacobianFactor(int i1, Matrix A1, int i2, Matrix A2, int i3, Matrix A3,
Vector b, const SharedDiagonal& model); Vector b, const gtsam::SharedDiagonal& model);
void print(string s) const; void print(string s) const;
bool equals(const GaussianFactor& lf, double tol) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const;
bool empty() const; bool empty() const;
Vector getb() const; Vector getb() const;
double error(const VectorValues& c) const; double error(const gtsam::VectorValues& c) const;
GaussianConditional* eliminateFirst(); gtsam::GaussianConditional* eliminateFirst();
}; };
class HessianFactor { class HessianFactor {
HessianFactor(const HessianFactor& gf); HessianFactor(const gtsam::HessianFactor& gf);
HessianFactor(); HessianFactor();
HessianFactor(int j, Matrix G, Vector g, double f); HessianFactor(int j, Matrix G, Vector g, double f);
HessianFactor(int j, Vector mu, Matrix Sigma); HessianFactor(int j, Vector mu, Matrix Sigma);
@ -304,41 +306,41 @@ class HessianFactor {
HessianFactor(int j1, int j2, int j3, Matrix G11, Matrix G12, Matrix G13, HessianFactor(int j1, int j2, int j3, Matrix G11, Matrix G12, Matrix G13,
Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3,
double f); double f);
HessianFactor(const GaussianConditional& cg); HessianFactor(const gtsam::GaussianConditional& cg);
HessianFactor(const GaussianFactor& factor); HessianFactor(const gtsam::GaussianFactor& factor);
void print(string s) const; void print(string s) const;
bool equals(const GaussianFactor& lf, double tol) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const VectorValues& c) const; double error(const gtsam::VectorValues& c) const;
}; };
class GaussianFactorGraph { class GaussianFactorGraph {
GaussianFactorGraph(); GaussianFactorGraph();
GaussianFactorGraph(const GaussianBayesNet& CBN); GaussianFactorGraph(const gtsam::GaussianBayesNet& CBN);
// From FactorGraph // From FactorGraph
void push_back(GaussianFactor* factor); void push_back(gtsam::GaussianFactor* factor);
void print(string s) const; void print(string s) const;
bool equals(const GaussianFactorGraph& lfgraph, double tol) const; bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const;
int size() const; int size() const;
// Building the graph // Building the graph
void add(JacobianFactor* factor); void add(gtsam::JacobianFactor* factor);
void add(Vector b); void add(Vector b);
void add(int key1, Matrix A1, Vector b, const SharedDiagonal& model); void add(int key1, Matrix A1, Vector b, const gtsam::SharedDiagonal& model);
void add(int key1, Matrix A1, int key2, Matrix A2, Vector b, void add(int key1, Matrix A1, int key2, Matrix A2, Vector b,
const SharedDiagonal& model); const gtsam::SharedDiagonal& model);
void add(int key1, Matrix A1, int key2, Matrix A2, int key3, Matrix A3, void add(int key1, Matrix A1, int key2, Matrix A2, int key3, Matrix A3,
Vector b, const SharedDiagonal& model); Vector b, const gtsam::SharedDiagonal& model);
void add(HessianFactor* factor); void add(gtsam::HessianFactor* factor);
// error and probability // error and probability
double error(const VectorValues& c) const; double error(const gtsam::VectorValues& c) const;
double probPrime(const VectorValues& c) const; double probPrime(const gtsam::VectorValues& c) const;
// combining // combining
static GaussianFactorGraph combine2(const GaussianFactorGraph& lfg1, static gtsam::GaussianFactorGraph combine2(const gtsam::GaussianFactorGraph& lfg1,
const GaussianFactorGraph& lfg2); const gtsam::GaussianFactorGraph& lfg2);
void combine(const GaussianFactorGraph& lfg); void combine(const gtsam::GaussianFactorGraph& lfg);
// Conversion to matrices // Conversion to matrices
Matrix sparseJacobian_() const; Matrix sparseJacobian_() const;
@ -347,27 +349,27 @@ class GaussianFactorGraph {
}; };
class GaussianSequentialSolver { class GaussianSequentialSolver {
GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); GaussianSequentialSolver(const gtsam::GaussianFactorGraph& graph, bool useQR);
GaussianBayesNet* eliminate() const; gtsam::GaussianBayesNet* eliminate() const;
VectorValues* optimize() const; gtsam::VectorValues* optimize() const;
GaussianFactor* marginalFactor(int j) const; gtsam::GaussianFactor* marginalFactor(int j) const;
Matrix marginalCovariance(int j) const; Matrix marginalCovariance(int j) const;
}; };
class KalmanFilter { class KalmanFilter {
KalmanFilter(size_t n); KalmanFilter(size_t n);
GaussianDensity* init(Vector x0, const SharedDiagonal& P0); gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0);
GaussianDensity* init(Vector x0, Matrix P0); gtsam::GaussianDensity* init(Vector x0, Matrix P0);
void print(string s) const; void print(string s) const;
static int step(GaussianDensity* p); static int step(gtsam::GaussianDensity* p);
GaussianDensity* predict(GaussianDensity* p, Matrix F, Matrix B, Vector u, gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, Matrix B, Vector u,
const SharedDiagonal& modelQ); const gtsam::SharedDiagonal& modelQ);
GaussianDensity* predictQ(GaussianDensity* p, Matrix F, Matrix B, Vector u, gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, Matrix B, Vector u,
Matrix Q); Matrix Q);
GaussianDensity* predict2(GaussianDensity* p, Matrix A0, Matrix A1, Vector b, gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, Matrix A1, Vector b,
const SharedDiagonal& model); const gtsam::SharedDiagonal& model);
GaussianDensity* update(GaussianDensity* p, Matrix H, Vector z, gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, Vector z,
const SharedDiagonal& model); const gtsam::SharedDiagonal& model);
}; };
//************************************************************************* //*************************************************************************
@ -377,7 +379,7 @@ class KalmanFilter {
class Ordering { class Ordering {
Ordering(); Ordering();
void print(string s) const; void print(string s) const;
bool equals(const Ordering& ord, double tol) const; bool equals(const gtsam::Ordering& ord, double tol) const;
void push_back(string key); void push_back(string key);
}; };
@ -387,6 +389,9 @@ class NonlinearOptimizationParameters {
void print(string s) const; void print(string s) const;
}; };
}///\namespace gtsam
//************************************************************************* //*************************************************************************
// planarSLAM // planarSLAM
//************************************************************************* //*************************************************************************
@ -397,10 +402,10 @@ namespace planarSLAM {
class Values { class Values {
Values(); Values();
void print(string s) const; void print(string s) const;
Pose2 pose(int key) const; gtsam::Pose2 pose(int key) const;
Point2 point(int key) const; gtsam::Point2 point(int key) const;
void insertPose(int key, const Pose2& pose); void insertPose(int key, const gtsam::Pose2& pose);
void insertPoint(int key, const Point2& point); void insertPoint(int key, const gtsam::Point2& point);
}; };
class Graph { class Graph {
@ -409,37 +414,37 @@ class Graph {
void print(string s) const; void print(string s) const;
double error(const planarSLAM::Values& values) const; double error(const planarSLAM::Values& values) const;
Ordering* orderingCOLAMD(const planarSLAM::Values& values) const; gtsam::Ordering* orderingCOLAMD(const planarSLAM::Values& values) const;
GaussianFactorGraph* linearize(const planarSLAM::Values& values, gtsam::GaussianFactorGraph* linearize(const planarSLAM::Values& values,
const Ordering& ordering) const; const gtsam::Ordering& ordering) const;
void addPrior(int key, const Pose2& pose, const SharedNoiseModel& noiseModel); void addPrior(int key, const gtsam::Pose2& pose, const gtsam::SharedNoiseModel& noiseModel);
void addPoseConstraint(int key, const Pose2& pose); void addPoseConstraint(int key, const gtsam::Pose2& pose);
void addOdometry(int key1, int key2, const Pose2& odometry, const SharedNoiseModel& noiseModel); void addOdometry(int key1, int key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
void addBearing(int poseKey, int pointKey, const Rot2& bearing, const SharedNoiseModel& noiseModel); void addBearing(int poseKey, int pointKey, const gtsam::Rot2& bearing, const gtsam::SharedNoiseModel& noiseModel);
void addRange(int poseKey, int pointKey, double range, const SharedNoiseModel& noiseModel); void addRange(int poseKey, int pointKey, double range, const gtsam::SharedNoiseModel& noiseModel);
void addBearingRange(int poseKey, int pointKey, const Rot2& bearing, double range, void addBearingRange(int poseKey, int pointKey, const gtsam::Rot2& bearing, double range,
const SharedNoiseModel& noiseModel); const gtsam::SharedNoiseModel& noiseModel);
planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate); planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate);
}; };
class Odometry { class Odometry {
Odometry(int key1, int key2, const Pose2& measured, Odometry(int key1, int key2, const gtsam::Pose2& measured,
const SharedNoiseModel& model); const gtsam::SharedNoiseModel& model);
void print(string s) const; void print(string s) const;
GaussianFactor* linearize(const planarSLAM::Values& center, const Ordering& ordering) const; gtsam::GaussianFactor* linearize(const planarSLAM::Values& center, const gtsam::Ordering& ordering) const;
}; };
class Optimizer { class Optimizer {
Optimizer(planarSLAM::Graph* graph, planarSLAM::Values* values, Optimizer(planarSLAM::Graph* graph, planarSLAM::Values* values,
Ordering* ordering, NonlinearOptimizationParameters* parameters); gtsam::Ordering* ordering, gtsam::NonlinearOptimizationParameters* parameters);
void print(string s) const; void print(string s) const;
}; };
}///\namespace planarSLAM }///\namespace planarSLAM
//************************************************************************* //*************************************************************************
// pose2SLAM // gtsam::Pose2SLAM
//************************************************************************* //*************************************************************************
#include <gtsam/slam/pose2SLAM.h> #include <gtsam/slam/pose2SLAM.h>
@ -448,8 +453,8 @@ namespace pose2SLAM {
class Values { class Values {
Values(); Values();
void print(string s) const; void print(string s) const;
void insertPose(int key, const Pose2& pose); void insertPose(int key, const gtsam::Pose2& pose);
Pose2 pose(int i); gtsam::Pose2 pose(int i);
}; };
class Graph { class Graph {
@ -458,19 +463,19 @@ class Graph {
void print(string s) const; void print(string s) const;
double error(const pose2SLAM::Values& values) const; double error(const pose2SLAM::Values& values) const;
Ordering* orderingCOLAMD(const pose2SLAM::Values& values) const; gtsam::Ordering* orderingCOLAMD(const pose2SLAM::Values& values) const;
GaussianFactorGraph* linearize(const pose2SLAM::Values& values, gtsam::GaussianFactorGraph* linearize(const pose2SLAM::Values& values,
const Ordering& ordering) const; const gtsam::Ordering& ordering) const;
void addPrior(int key, const Pose2& pose, const SharedNoiseModel& noiseModel); void addPrior(int key, const gtsam::Pose2& pose, const gtsam::SharedNoiseModel& noiseModel);
void addPoseConstraint(int key, const Pose2& pose); void addPoseConstraint(int key, const gtsam::Pose2& pose);
void addOdometry(int key1, int key2, const Pose2& odometry, const SharedNoiseModel& noiseModel); void addOdometry(int key1, int key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate); pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate);
}; };
class Optimizer { class Optimizer {
Optimizer(pose2SLAM::Graph* graph, pose2SLAM::Values* values, Optimizer(pose2SLAM::Graph* graph, pose2SLAM::Values* values,
Ordering* ordering, NonlinearOptimizationParameters* parameters); gtsam::Ordering* ordering, gtsam::NonlinearOptimizationParameters* parameters);
void print(string s) const; void print(string s) const;
}; };
@ -485,12 +490,12 @@ namespace simulated2D {
class Values { class Values {
Values(); Values();
void insertPose(int i, const Point2& p); void insertPose(int i, const gtsam::Point2& p);
void insertPoint(int j, const Point2& p); void insertPoint(int j, const gtsam::Point2& p);
int nrPoses() const; int nrPoses() const;
int nrPoints() const; int nrPoints() const;
Point2 pose(int i); gtsam::Point2 pose(int i);
Point2 point(int j); gtsam::Point2 point(int j);
}; };
class Graph { class Graph {
@ -507,12 +512,12 @@ namespace simulated2DOriented {
class Values { class Values {
Values(); Values();
void insertPose(int i, const Pose2& p); void insertPose(int i, const gtsam::Pose2& p);
void insertPoint(int j, const Point2& p); void insertPoint(int j, const gtsam::Point2& p);
int nrPoses() const; int nrPoses() const;
int nrPoints() const; int nrPoints() const;
Pose2 pose(int i); gtsam::Pose2 pose(int i);
Point2 point(int j); gtsam::Point2 point(int j);
}; };
class Graph { class Graph {
@ -523,6 +528,7 @@ class Graph {
}///\namespace simulated2DOriented }///\namespace simulated2DOriented
//// These are considered to be broken and will be added back as they start working //// These are considered to be broken and will be added back as they start working
//// It's assumed that there have been interface changes that might break this //// It's assumed that there have been interface changes that might break this
// //
@ -611,28 +617,28 @@ class Graph {
// VectorValues* conjugateGradientDescent_(const VectorValues& x0) const; // VectorValues* conjugateGradientDescent_(const VectorValues& x0) const;
//}; //};
// //
//class Pose2Values{ //class gtsam::Pose2Values{
// Pose2Values(); // Pose2Values();
// Pose2 get(string key) const; // Pose2 get(string key) const;
// void insert(string name, const Pose2& val); // void insert(string name, const gtsam::Pose2& val);
// void print(string s) const; // void print(string s) const;
// void clear(); // void clear();
// int size(); // int size();
//}; //};
// //
//class Pose2Factor { //class gtsam::Pose2Factor {
// Pose2Factor(string key1, string key2, // Pose2Factor(string key1, string key2,
// const Pose2& measured, Matrix measurement_covariance); // const gtsam::Pose2& measured, Matrix measurement_covariance);
// void print(string name) const; // void print(string name) const;
// double error(const Pose2Values& c) const; // double error(const gtsam::Pose2Values& c) const;
// size_t size() const; // size_t size() const;
// GaussianFactor* linearize(const Pose2Values& config) const; // GaussianFactor* linearize(const gtsam::Pose2Values& config) const;
//}; //};
// //
//class Pose2Graph{ //class gtsam::Pose2Graph{
// Pose2Graph(); // Pose2Graph();
// void print(string s) const; // void print(string s) const;
// GaussianFactorGraph* linearize_(const Pose2Values& config) const; // GaussianFactorGraph* linearize_(const gtsam::Pose2Values& config) const;
// void push_back(Pose2Factor* factor); // void push_back(Pose2Factor* factor);
//}; //};
// //
@ -665,7 +671,7 @@ class Graph {
// GaussianFactor* linearize(const Simulated2DValues& config) const; // GaussianFactor* linearize(const Simulated2DValues& config) const;
//}; //};
// //
//class Pose2SLAMOptimizer { //class gtsam::Pose2SLAMOptimizer {
// Pose2SLAMOptimizer(string dataset_name); // Pose2SLAMOptimizer(string dataset_name);
// void print(string s) const; // void print(string s) const;
// void update(Vector x) const; // void update(Vector x) const;

69
gtsam/slam/planarSLAM.cpp
View File

@ -21,48 +21,45 @@
#include <gtsam/nonlinear/NonlinearOptimization.h> #include <gtsam/nonlinear/NonlinearOptimization.h>
// Use planarSLAM namespace for specific SLAM instance // Use planarSLAM namespace for specific SLAM instance
namespace gtsam { namespace planarSLAM {
namespace planarSLAM { Graph::Graph(const NonlinearFactorGraph<Values>& graph) :
NonlinearFactorGraph<Values>(graph) {}
Graph::Graph(const NonlinearFactorGraph<Values>& graph) : void Graph::addPrior(const PoseKey& i, const Pose2& p,
NonlinearFactorGraph<Values>(graph) {} const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model));
push_back(factor);
}
void Graph::addPrior(const PoseKey& i, const Pose2& p, void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) {
const SharedNoiseModel& model) { sharedFactor factor(new Constraint(i, p));
sharedFactor factor(new Prior(i, p, model)); push_back(factor);
push_back(factor); }
}
void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) { void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
sharedFactor factor(new Constraint(i, p)); const SharedNoiseModel& model) {
push_back(factor); sharedFactor factor(new Odometry(i, j, z, model));
} push_back(factor);
}
void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, void Graph::addBearing(const PoseKey& i, const PointKey& j, const Rot2& z,
const SharedNoiseModel& model) { const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(i, j, z, model)); sharedFactor factor(new Bearing(i, j, z, model));
push_back(factor); push_back(factor);
} }
void Graph::addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, void Graph::addRange(const PoseKey& i, const PointKey& j, double z,
const SharedNoiseModel& model) { const SharedNoiseModel& model) {
sharedFactor factor(new Bearing(i, j, z, model)); sharedFactor factor(new Range(i, j, z, model));
push_back(factor); push_back(factor);
} }
void Graph::addRange(const PoseKey& i, const PointKey& j, double z, void Graph::addBearingRange(const PoseKey& i, const PointKey& j, const Rot2& z1,
const SharedNoiseModel& model) { double z2, const SharedNoiseModel& model) {
sharedFactor factor(new Range(i, j, z, model)); sharedFactor factor(new BearingRange(i, j, z1, z2, model));
push_back(factor); push_back(factor);
} }
void Graph::addBearingRange(const PoseKey& i, const PointKey& j, const Rot2& z1, } // planarSLAM
double z2, const SharedNoiseModel& model) {
sharedFactor factor(new BearingRange(i, j, z1, z2, model));
push_back(factor);
}
} // planarSLAM
} // gtsam

170
gtsam/slam/planarSLAM.h
View File

@ -29,120 +29,118 @@
#include <gtsam/nonlinear/NonlinearOptimizer.h> #include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
// We use gtsam namespace for generally useful factors // Use planarSLAM namespace for specific SLAM instance
namespace gtsam { namespace planarSLAM {
// Use planarSLAM namespace for specific SLAM instance using namespace gtsam;
namespace planarSLAM {
/// Typedef for a PoseKey with Pose2 data and 'x' symbol /// Typedef for a PoseKey with Pose2 data and 'x' symbol
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
/// Typedef for a PointKey with Point2 data and 'l' symbol /// Typedef for a PointKey with Point2 data and 'l' symbol
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
/// Typedef for Values structure with PoseKey type /// Typedef for Values structure with PoseKey type
typedef Values<PoseKey> PoseValues; typedef Values<PoseKey> PoseValues;
/// Typedef for Values structure with PointKey type /// Typedef for Values structure with PointKey type
typedef Values<PointKey> PointValues; typedef Values<PointKey> PointValues;
/// Values class, inherited from TupleValues2, using PoseKeys and PointKeys /// Values class, inherited from TupleValues2, using PoseKeys and PointKeys
struct Values: public TupleValues2<PoseValues, PointValues> { struct Values: public TupleValues2<PoseValues, PointValues> {
/// Default constructor /// Default constructor
Values() {} Values() {}
/// Copy constructor /// Copy constructor
Values(const TupleValues2<PoseValues, PointValues>& values) : Values(const TupleValues2<PoseValues, PointValues>& values) :
TupleValues2<PoseValues, PointValues>(values) { TupleValues2<PoseValues, PointValues>(values) {
} }
/// Copy constructor /// Copy constructor
Values(const TupleValues2<PoseValues, PointValues>::Base& values) : Values(const TupleValues2<PoseValues, PointValues>::Base& values) :
TupleValues2<PoseValues, PointValues>(values) { TupleValues2<PoseValues, PointValues>(values) {
} }
/// From sub-values /// From sub-values
Values(const PoseValues& poses, const PointValues& points) : Values(const PoseValues& poses, const PointValues& points) :
TupleValues2<PoseValues, PointValues>(poses, points) { TupleValues2<PoseValues, PointValues>(poses, points) {
} }
// Convenience for MATLAB wrapper, which does not allow for identically named methods // Convenience for MATLAB wrapper, which does not allow for identically named methods
/// get a pose /// get a pose
Pose2 pose(int key) const { return (*this)[PoseKey(key)]; } Pose2 pose(int key) const { return (*this)[PoseKey(key)]; }
/// get a point /// get a point
Point2 point(int key) const { return (*this)[PointKey(key)]; } Point2 point(int key) const { return (*this)[PointKey(key)]; }
/// insert a pose /// insert a pose
void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); } void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); }
/// insert a point /// insert a point
void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); } void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); }
}; };
/** /**
* List of typedefs for factors * List of typedefs for factors
*/ */
/// A hard constraint for PoseKeys to enforce particular values /// A hard constraint for PoseKeys to enforce particular values
typedef NonlinearEquality<Values, PoseKey> Constraint; typedef NonlinearEquality<Values, PoseKey> Constraint;
/// A prior factor to bias the value of a PoseKey /// A prior factor to bias the value of a PoseKey
typedef PriorFactor<Values, PoseKey> Prior; typedef PriorFactor<Values, PoseKey> Prior;
/// A factor between two PoseKeys set with a Pose2 /// A factor between two PoseKeys set with a Pose2
typedef BetweenFactor<Values, PoseKey> Odometry; typedef BetweenFactor<Values, PoseKey> Odometry;
/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2) /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
typedef BearingFactor<Values, PoseKey, PointKey> Bearing; typedef BearingFactor<Values, PoseKey, PointKey> Bearing;
/// A factor between a PoseKey and a PointKey to express distance between them (set with a double) /// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
typedef RangeFactor<Values, PoseKey, PointKey> Range; typedef RangeFactor<Values, PoseKey, PointKey> Range;
/// A factor between a PoseKey and a PointKey to express difference in rotation and location /// A factor between a PoseKey and a PointKey to express difference in rotation and location
typedef BearingRangeFactor<Values, PoseKey, PointKey> BearingRange; typedef BearingRangeFactor<Values, PoseKey, PointKey> BearingRange;
/// Creates a NonlinearFactorGraph with the Values type /// Creates a NonlinearFactorGraph with the Values type
struct Graph: public NonlinearFactorGraph<Values> { struct Graph: public NonlinearFactorGraph<Values> {
/// Default constructor for a NonlinearFactorGraph /// Default constructor for a NonlinearFactorGraph
Graph(){} Graph(){}
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph<Values>& graph); Graph(const NonlinearFactorGraph<Values>& graph);
/// Biases the value of PoseKey key with Pose2 p given a noise model /// Biases the value of PoseKey key with Pose2 p given a noise model
void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel); void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel);
/// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value /// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose); void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose);
/// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement) /// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement)
void addOdometry(const PoseKey& poseKey, const PoseKey& pointKey, const Pose2& odometry, void addOdometry(const PoseKey& poseKey, const PoseKey& pointKey, const Pose2& odometry,
const SharedNoiseModel& model); const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation
void addBearing(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing, void addBearing(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing,
const SharedNoiseModel& model); const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location
void addRange(const PoseKey& poseKey, const PointKey& pointKey, double range, void addRange(const PoseKey& poseKey, const PointKey& pointKey, double range,
const SharedNoiseModel& model); const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location
void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey, void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey,
const Rot2& bearing, double range, const SharedNoiseModel& model); const Rot2& bearing, double range, const SharedNoiseModel& model);
/// Optimize /// Optimize
Values optimize(const Values& initialEstimate) { Values optimize(const Values& initialEstimate) {
typedef NonlinearOptimizer<Graph, Values> Optimizer; typedef NonlinearOptimizer<Graph, Values> Optimizer;
return *Optimizer::optimizeLM(*this, initialEstimate, return *Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA); NonlinearOptimizationParameters::LAMBDA);
} }
}; };
/// Optimizer /// Optimizer
typedef NonlinearOptimizer<Graph, Values> Optimizer; typedef NonlinearOptimizer<Graph, Values> Optimizer;
} // planarSLAM } // planarSLAM
} // namespace gtsam

57
gtsam/slam/pose2SLAM.cpp
View File

@ -20,41 +20,38 @@
#include <gtsam/nonlinear/NonlinearOptimizer.h> #include <gtsam/nonlinear/NonlinearOptimizer.h>
// Use pose2SLAM namespace for specific SLAM instance // Use pose2SLAM namespace for specific SLAM instance
namespace gtsam {
template class NonlinearOptimizer<pose2SLAM::Graph, pose2SLAM::Values, GaussianFactorGraph, GaussianSequentialSolver>; template class gtsam::NonlinearOptimizer<pose2SLAM::Graph, pose2SLAM::Values, gtsam::GaussianFactorGraph, gtsam::GaussianSequentialSolver>;
namespace pose2SLAM { namespace pose2SLAM {
/* ************************************************************************* */ /* ************************************************************************* */
Values circle(size_t n, double R) { Values circle(size_t n, double R) {
Values x; Values x;
double theta = 0, dtheta = 2 * M_PI / n; double theta = 0, dtheta = 2 * M_PI / n;
for (size_t i = 0; i < n; i++, theta += dtheta) for (size_t i = 0; i < n; i++, theta += dtheta)
x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta)); x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
return x; return x;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Graph::addPrior(const PoseKey& i, const Pose2& p, void Graph::addPrior(const PoseKey& i, const Pose2& p,
const SharedNoiseModel& model) { const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model)); sharedFactor factor(new Prior(i, p, model));
push_back(factor); push_back(factor);
} }
void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) { void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) {
sharedFactor factor(new HardConstraint(i, p)); sharedFactor factor(new HardConstraint(i, p));
push_back(factor); push_back(factor);
} }
void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
const SharedNoiseModel& model) { const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(i, j, z, model)); sharedFactor factor(new Odometry(i, j, z, model));
push_back(factor); push_back(factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
} // pose2SLAM } // pose2SLAM
} // gtsam

8
gtsam/slam/pose2SLAM.h
View File

@ -28,11 +28,11 @@
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h> #include <gtsam/linear/GaussianMultifrontalSolver.h>
namespace gtsam {
// Use pose2SLAM namespace for specific SLAM instance // Use pose2SLAM namespace for specific SLAM instance
namespace pose2SLAM { namespace pose2SLAM {
using namespace gtsam;
/// Keys with Pose2 and symbol 'x' /// Keys with Pose2 and symbol 'x'
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
@ -111,7 +111,7 @@ namespace pose2SLAM {
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph,
GaussianMultifrontalSolver> Optimizer; GaussianMultifrontalSolver> Optimizer;
} // pose2SLAM } // pose2SLAM
} // namespace gtsam

48
gtsam/slam/simulated2D.cpp
View File

@ -17,35 +17,33 @@
#include <gtsam/slam/simulated2D.h> #include <gtsam/slam/simulated2D.h>
namespace gtsam { namespace simulated2D {
namespace simulated2D { static Matrix I = gtsam::eye(2);
static Matrix I = gtsam::eye(2); /* ************************************************************************* */
Point2 prior(const Point2& x, boost::optional<Matrix&> H) {
if (H) *H = I;
return x;
}
/* ************************************************************************* */ /* ************************************************************************* */
Point2 prior(const Point2& x, boost::optional<Matrix&> H) { Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1,
if (H) *H = I; boost::optional<Matrix&> H2) {
return x; if (H1) *H1 = -I;
} if (H2) *H2 = I;
return x2 - x1;
}
/* ************************************************************************* */ /* ************************************************************************* */
Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1, Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) { boost::optional<Matrix&> H2) {
if (H1) *H1 = -I; if (H1) *H1 = -I;
if (H2) *H2 = I; if (H2) *H2 = I;
return x2 - x1; return l - x;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
if (H1) *H1 = -I;
if (H2) *H2 = I;
return l - x;
}
/* ************************************************************************* */ } // namespace simulated2D
} // namespace simulated2D
} // namespace gtsam

351
gtsam/slam/simulated2D.h
View File

@ -25,220 +25,219 @@
// \namespace // \namespace
namespace gtsam { namespace simulated2D {
namespace simulated2D { using namespace gtsam;
// Simulated2D robots have no orientation, just a position // Simulated2D robots have no orientation, just a position
typedef TypedSymbol<Point2, 'x'> PoseKey; typedef TypedSymbol<Point2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
typedef Values<PoseKey> PoseValues; typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues; typedef Values<PointKey> PointValues;
/** /**
* Custom Values class that holds poses and points * 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; ///< base class typedef TupleValues2<PoseValues, PointValues> Base; ///< base class
typedef boost::shared_ptr<Point2> sharedPoint; ///< shortcut to shared Point type typedef boost::shared_ptr<Point2> sharedPoint; ///< shortcut to shared Point type
/// Constructor /// Constructor
Values() { Values() {
} }
/// Copy constructor /// Copy constructor
Values(const Base& base) : Values(const Base& base) :
Base(base) { Base(base) {
} }
/// Insert a pose /// 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 /// 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 /// Number of poses
int nrPoses() const { int nrPoses() const {
return this->first_.size(); return this->first_.size();
} }
/// Number of points /// Number of points
int nrPoints() const { int nrPoints() const {
return this->second_.size(); return this->second_.size();
} }
/// Return pose i /// Return pose i
Point2 pose(const simulated2D::PoseKey& i) const { Point2 pose(const simulated2D::PoseKey& i) const {
return (*this)[i]; return (*this)[i];
} }
/// Return point j /// Return point j
Point2 point(const simulated2D::PointKey& j) const { Point2 point(const simulated2D::PointKey& j) const {
return (*this)[j]; return (*this)[j];
} }
}; };
/// Prior on a single pose /// Prior on a single pose
inline Point2 prior(const Point2& x) { inline Point2 prior(const Point2& x) {
return x; return x;
} }
/// Prior on a single pose, optionally returns derivative /// 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
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 /// 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
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 /// 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);
/** /**
* Unary factor encoding a soft prior on a vector * Unary factor encoding a soft prior on a vector
*/ */
template<class VALUES = Values, class KEY = PoseKey> template<class VALUES = Values, class KEY = PoseKey>
class GenericPrior: public NonlinearFactor1<VALUES, KEY> { class GenericPrior: public NonlinearFactor1<VALUES, KEY> {
public: public:
typedef NonlinearFactor1<VALUES, KEY> Base; ///< base class typedef NonlinearFactor1<VALUES, KEY> Base; ///< base class
typedef boost::shared_ptr<GenericPrior<VALUES, KEY> > shared_ptr; typedef boost::shared_ptr<GenericPrior<VALUES, KEY> > shared_ptr;
typedef typename KEY::Value Pose; ///< shortcut to Pose type typedef typename KEY::Value Pose; ///< shortcut to Pose type
Pose z_; ///< prior mean Pose z_; ///< prior mean
/// Create generic prior /// Create generic prior
GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) : GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) :
NonlinearFactor1<VALUES, KEY>(model, key), z_(z) { NonlinearFactor1<VALUES, KEY>(model, key), z_(z) {
} }
/// Return error and optional derivative /// 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:
/// Default constructor /// Default constructor
GenericPrior() { GenericPrior() {
} }
/// Serialization function /// 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) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(z_); ar & BOOST_SERIALIZATION_NVP(z_);
} }
}; };
/** /**
* Binary factor simulating "odometry" between two Vectors * Binary factor simulating "odometry" between two Vectors
*/ */
template<class VALUES = Values, class KEY = PoseKey> template<class VALUES = Values, class KEY = PoseKey>
class GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> { class GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> {
public: public:
typedef NonlinearFactor2<VALUES, KEY, KEY> Base; ///< base class typedef NonlinearFactor2<VALUES, KEY, KEY> Base; ///< base class
typedef boost::shared_ptr<GenericOdometry<VALUES, KEY> > shared_ptr; typedef boost::shared_ptr<GenericOdometry<VALUES, KEY> > shared_ptr;
typedef typename KEY::Value Pose; ///< shortcut to Pose type typedef typename KEY::Value Pose; ///< shortcut to Pose type
Pose z_; ///< odometry measurement Pose z_; ///< odometry measurement
/// Create odometry /// 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<VALUES, KEY, KEY>(model, i1, i2), z_(z) { NonlinearFactor2<VALUES, KEY, KEY>(model, i1, i2), z_(z) {
} }
/// Evaluate error and optionally return derivatives /// Evaluate error and optionally return derivatives
Vector evaluateError(const Pose& x1, const Pose& x2, Vector evaluateError(const Pose& x1, const Pose& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { 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:
/// Default constructor /// Default constructor
GenericOdometry() { GenericOdometry() {
} }
/// Serialization function /// 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) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(z_); ar & BOOST_SERIALIZATION_NVP(z_);
} }
}; };
/** /**
* Binary factor simulating "measurement" between two Vectors * Binary factor simulating "measurement" between two Vectors
*/ */
template<class VALUES = Values, class XKEY = PoseKey, class LKEY = PointKey> template<class VALUES = Values, class XKEY = PoseKey, class LKEY = PointKey>
class GenericMeasurement: public NonlinearFactor2<VALUES, XKEY, LKEY> { class GenericMeasurement: public NonlinearFactor2<VALUES, XKEY, LKEY> {
public: public:
typedef NonlinearFactor2<VALUES, XKEY, LKEY> Base; ///< base class typedef NonlinearFactor2<VALUES, XKEY, LKEY> Base; ///< base class
typedef boost::shared_ptr<GenericMeasurement<VALUES, XKEY, LKEY> > shared_ptr; typedef boost::shared_ptr<GenericMeasurement<VALUES, XKEY, LKEY> > shared_ptr;
typedef typename XKEY::Value Pose; ///< shortcut to Pose type typedef typename XKEY::Value Pose; ///< shortcut to Pose type
typedef typename LKEY::Value Point; ///< shortcut to Point type typedef typename LKEY::Value Point; ///< shortcut to Point type
Point z_; ///< Measurement Point z_; ///< Measurement
/// Create measurement factor /// 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<VALUES, XKEY, LKEY>(model, i, j), z_(z) { NonlinearFactor2<VALUES, XKEY, LKEY>(model, i, j), z_(z) {
} }
/// Evaluate error and optionally return derivatives /// Evaluate error and optionally return derivatives
Vector evaluateError(const Pose& x1, const Point& x2, Vector evaluateError(const Pose& x1, const Point& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { 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:
/// Default constructor /// Default constructor
GenericMeasurement() { GenericMeasurement() {
} }
/// Serialization function /// 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) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(z_); ar & BOOST_SERIALIZATION_NVP(z_);
} }
}; };
/** Typedefs for regular use */ /** Typedefs for regular use */
typedef GenericPrior<Values, PoseKey> Prior; typedef GenericPrior<Values, PoseKey> Prior;
typedef GenericOdometry<Values, PoseKey> Odometry; typedef GenericOdometry<Values, PoseKey> Odometry;
typedef GenericMeasurement<Values, PoseKey, PointKey> Measurement; typedef GenericMeasurement<Values, PoseKey, PointKey> Measurement;
// Specialization of a graph for this example domain // Specialization of a graph for this example domain
// TODO: add functions to add factor types // TODO: add functions to add factor types
class Graph : public NonlinearFactorGraph<Values> { class Graph : public NonlinearFactorGraph<Values> {
public: public:
Graph() {} Graph() {}
}; };
} // namespace simulated2D } // namespace simulated2D
} // namespace gtsam

296
gtsam/slam/simulated2DConstraints.h
View File

@ -28,177 +28,175 @@
// \namespace // \namespace
namespace gtsam { namespace simulated2D {
namespace simulated2D { namespace equality_constraints {
namespace equality_constraints { /** Typedefs for regular use */
typedef NonlinearEquality1<Values, PoseKey> UnaryEqualityConstraint;
typedef NonlinearEquality1<Values, PointKey> UnaryEqualityPointConstraint;
typedef BetweenConstraint<Values, PoseKey> OdoEqualityConstraint;
/** Typedefs for regular use */ /** Equality between variables */
typedef NonlinearEquality1<Values, PoseKey> UnaryEqualityConstraint; typedef NonlinearEquality2<Values, PoseKey> PoseEqualityConstraint;
typedef NonlinearEquality1<Values, PointKey> UnaryEqualityPointConstraint; typedef NonlinearEquality2<Values, PointKey> PointEqualityConstraint;
typedef BetweenConstraint<Values, PoseKey> OdoEqualityConstraint;
/** Equality between variables */ } // \namespace equality_constraints
typedef NonlinearEquality2<Values, PoseKey> PoseEqualityConstraint;
typedef NonlinearEquality2<Values, PointKey> PointEqualityConstraint;
} // \namespace equality_constraints namespace inequality_constraints {
namespace inequality_constraints { /**
* Unary inequality constraint forcing a coordinate to be greater/less than a fixed value (c)
* @tparam VALUES is the values structure for the graph
* @tparam KEY is the key type for the variable constrained
* @tparam IDX is an index in tangent space to constrain, must be less than KEY::VALUE::Dim()
*/
template<class VALUES, class KEY, unsigned int IDX>
struct ScalarCoordConstraint1: public BoundingConstraint1<VALUES, KEY> {
typedef BoundingConstraint1<VALUES, KEY> Base; ///< Base class convenience typedef
typedef boost::shared_ptr<ScalarCoordConstraint1<VALUES, KEY, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
typedef typename KEY::Value Point; ///< Constrained variable type
/** virtual ~ScalarCoordConstraint1() {}
* Unary inequality constraint forcing a coordinate to be greater/less than a fixed value (c)
* @tparam VALUES is the values structure for the graph
* @tparam KEY is the key type for the variable constrained
* @tparam IDX is an index in tangent space to constrain, must be less than KEY::VALUE::Dim()
*/
template<class VALUES, class KEY, unsigned int IDX>
struct ScalarCoordConstraint1: public BoundingConstraint1<VALUES, KEY> {
typedef BoundingConstraint1<VALUES, KEY> Base; ///< Base class convenience typedef
typedef boost::shared_ptr<ScalarCoordConstraint1<VALUES, KEY, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
typedef typename KEY::Value Point; ///< Constrained variable type
virtual ~ScalarCoordConstraint1() {} /**
* Constructor for constraint
* @param key is the label for the constrained variable
* @param c is the measured value for the fixed coordinate
* @param isGreaterThan is a flag to set inequality as greater than or less than
* @param mu is the penalty function gain
*/
ScalarCoordConstraint1(const KEY& key, double c,
bool isGreaterThan, double mu = 1000.0) :
Base(key, c, isGreaterThan, mu) {
}
/** /**
* Constructor for constraint * Access function for the constrained index
* @param key is the label for the constrained variable * @return the index for the constrained coordinate
* @param c is the measured value for the fixed coordinate */
* @param isGreaterThan is a flag to set inequality as greater than or less than inline unsigned int index() const { return IDX; }
* @param mu is the penalty function gain
*/
ScalarCoordConstraint1(const KEY& key, double c,
bool isGreaterThan, double mu = 1000.0) :
Base(key, c, isGreaterThan, mu) {
}
/** /**
* Access function for the constrained index * extracts a single value from the point to compute error
* @return the index for the constrained coordinate * @param x is the estimate of the constrained variable being evaluated
*/ * @param H is an optional Jacobian, linearized at x
inline unsigned int index() const { return IDX; } */
virtual double value(const Point& x, boost::optional<Matrix&> H =
boost::none) const {
if (H) {
Matrix D = zeros(1, x.dim());
D(0, IDX) = 1.0;
*H = D;
}
return Point::Logmap(x)(IDX);
}
};
/** /** typedefs for use with simulated2D systems */
* extracts a single value from the point to compute error typedef ScalarCoordConstraint1<Values, PoseKey, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
* @param x is the estimate of the constrained variable being evaluated typedef ScalarCoordConstraint1<Values, PoseKey, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
* @param H is an optional Jacobian, linearized at x
*/
virtual double value(const Point& x, boost::optional<Matrix&> H =
boost::none) const {
if (H) {
Matrix D = zeros(1, x.dim());
D(0, IDX) = 1.0;
*H = D;
}
return Point::Logmap(x)(IDX);
}
};
/** typedefs for use with simulated2D systems */ /**
typedef ScalarCoordConstraint1<Values, PoseKey, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X * Trait for distance constraints to provide distance
typedef ScalarCoordConstraint1<Values, PoseKey, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y * @tparam T1 is a Lie value for which distance functions exist
* @tparam T2 is a Lie value for which distance functions exist
* @param a is the first Lie element
* @param b is the second Lie element
* @return a scalar distance between values
*/
template<class T1, class T2>
double range_trait(const T1& a, const T2& b) { return a.dist(b); }
/** /**
* Trait for distance constraints to provide distance * Binary inequality constraint forcing the range between points
* @tparam T1 is a Lie value for which distance functions exist * to be less than or equal to a bound
* @tparam T2 is a Lie value for which distance functions exist * @tparam VALUES is the variable set for the graph
* @param a is the first Lie element * @tparam KEY is the type of the keys for the variables constrained
* @param b is the second Lie element */
* @return a scalar distance between values template<class VALUES, class KEY>
*/ struct MaxDistanceConstraint : public BoundingConstraint2<VALUES, KEY, KEY> {
template<class T1, class T2> typedef BoundingConstraint2<VALUES, KEY, KEY> Base; ///< Base class for factor
double range_trait(const T1& a, const T2& b) { return a.dist(b); } typedef typename KEY::Value Point; ///< Type of variable constrained
/** virtual ~MaxDistanceConstraint() {}
* Binary inequality constraint forcing the range between points
* to be less than or equal to a bound
* @tparam VALUES is the variable set for the graph
* @tparam KEY is the type of the keys for the variables constrained
*/
template<class VALUES, class KEY>
struct MaxDistanceConstraint : public BoundingConstraint2<VALUES, KEY, KEY> {
typedef BoundingConstraint2<VALUES, KEY, KEY> Base; ///< Base class for factor
typedef typename KEY::Value Point; ///< Type of variable constrained
virtual ~MaxDistanceConstraint() {} /**
* Primary constructor for factor
* @param key1 is the first variable key
* @param key2 is the second variable key
* @param range_bound is the maximum range allowed between the variables
* @param mu is the gain for the penalty function
*/
MaxDistanceConstraint(const KEY& key1, const KEY& key2, double range_bound, double mu = 1000.0)
: Base(key1, key2, range_bound, false, mu) {}
/** /**
* Primary constructor for factor * computes the range with derivatives
* @param key1 is the first variable key * @param x1 is the first variable value
* @param key2 is the second variable key * @param x2 is the second variable value
* @param range_bound is the maximum range allowed between the variables * @param H1 is an optional Jacobian in x1
* @param mu is the gain for the penalty function * @param H2 is an optional Jacobian in x2
*/ * @return the distance between the variables
MaxDistanceConstraint(const KEY& key1, const KEY& key2, double range_bound, double mu = 1000.0) */
: Base(key1, key2, range_bound, false, mu) {} virtual double value(const Point& x1, const Point& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = numericalDerivative21(range_trait<Point,Point>, x1, x2, 1e-5);
if (H1) *H2 = numericalDerivative22(range_trait<Point,Point>, x1, x2, 1e-5);
return range_trait(x1, x2);
}
};
/** typedef MaxDistanceConstraint<Values, PoseKey> PoseMaxDistConstraint; ///< Simulated2D domain example factor
* computes the range with derivatives
* @param x1 is the first variable value
* @param x2 is the second variable value
* @param H1 is an optional Jacobian in x1
* @param H2 is an optional Jacobian in x2
* @return the distance between the variables
*/
virtual double value(const Point& x1, const Point& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = numericalDerivative21(range_trait<Point,Point>, x1, x2, 1e-5);
if (H1) *H2 = numericalDerivative22(range_trait<Point,Point>, x1, x2, 1e-5);
return range_trait(x1, x2);
}
};
typedef MaxDistanceConstraint<Values, PoseKey> PoseMaxDistConstraint; ///< Simulated2D domain example factor /**
* Binary inequality constraint forcing a minimum range
* NOTE: this is not a convex function! Be careful with initialization.
* @tparam VALUES is the variable set for the graph
* @tparam XKEY is the type of the pose key constrained
* @tparam PKEY is the type of the point key constrained
*/
template<class VALUES, class XKEY, class PKEY>
struct MinDistanceConstraint : public BoundingConstraint2<VALUES, XKEY, PKEY> {
typedef BoundingConstraint2<VALUES, XKEY, PKEY> Base; ///< Base class for factor
typedef typename XKEY::Value Pose; ///< Type of pose variable constrained
typedef typename PKEY::Value Point; ///< Type of point variable constrained
/** virtual ~MinDistanceConstraint() {}
* Binary inequality constraint forcing a minimum range
* NOTE: this is not a convex function! Be careful with initialization.
* @tparam VALUES is the variable set for the graph
* @tparam XKEY is the type of the pose key constrained
* @tparam PKEY is the type of the point key constrained
*/
template<class VALUES, class XKEY, class PKEY>
struct MinDistanceConstraint : public BoundingConstraint2<VALUES, XKEY, PKEY> {
typedef BoundingConstraint2<VALUES, XKEY, PKEY> Base; ///< Base class for factor
typedef typename XKEY::Value Pose; ///< Type of pose variable constrained
typedef typename PKEY::Value Point; ///< Type of point variable constrained
virtual ~MinDistanceConstraint() {} /**
* Primary constructor for factor
* @param key1 is the first variable key
* @param key2 is the second variable key
* @param range_bound is the minimum range allowed between the variables
* @param mu is the gain for the penalty function
*/
MinDistanceConstraint(const XKEY& key1, const PKEY& key2,
double range_bound, double mu = 1000.0)
: Base(key1, key2, range_bound, true, mu) {}
/** /**
* Primary constructor for factor * computes the range with derivatives
* @param key1 is the first variable key * @param x1 is the first variable value
* @param key2 is the second variable key * @param x2 is the second variable value
* @param range_bound is the minimum range allowed between the variables * @param H1 is an optional Jacobian in x1
* @param mu is the gain for the penalty function * @param H2 is an optional Jacobian in x2
*/ * @return the distance between the variables
MinDistanceConstraint(const XKEY& key1, const PKEY& key2, */
double range_bound, double mu = 1000.0) virtual double value(const Pose& x1, const Point& x2,
: Base(key1, key2, range_bound, true, mu) {} boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = numericalDerivative21(range_trait<Pose,Point>, x1, x2, 1e-5);
if (H1) *H2 = numericalDerivative22(range_trait<Pose,Point>, x1, x2, 1e-5);
return range_trait(x1, x2);
}
};
/** typedef MinDistanceConstraint<Values, PoseKey, PointKey> LandmarkAvoid; ///< Simulated2D domain example factor
* computes the range with derivatives
* @param x1 is the first variable value
* @param x2 is the second variable value
* @param H1 is an optional Jacobian in x1
* @param H2 is an optional Jacobian in x2
* @return the distance between the variables
*/
virtual double value(const Pose& x1, const Point& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = numericalDerivative21(range_trait<Pose,Point>, x1, x2, 1e-5);
if (H1) *H2 = numericalDerivative22(range_trait<Pose,Point>, x1, x2, 1e-5);
return range_trait(x1, x2);
}
};
typedef MinDistanceConstraint<Values, PoseKey, PointKey> LandmarkAvoid; ///< Simulated2D domain example factor
} // \namespace inequality_constraints } // \namespace inequality_constraints
} // \namespace simulated2D
} // \namespace simulated2D
} // \namespace gtsam

30
gtsam/slam/simulated2DOriented.cpp
View File

@ -17,25 +17,23 @@
#include <gtsam/slam/simulated2DOriented.h> #include <gtsam/slam/simulated2DOriented.h>
namespace gtsam { namespace simulated2DOriented {
namespace simulated2DOriented { static Matrix I = gtsam::eye(3);
static Matrix I = gtsam::eye(3); /* ************************************************************************* */
Pose2 prior(const Pose2& x, boost::optional<Matrix&> H) {
if (H) *H = I;
return x;
}
/* ************************************************************************* */ /* ************************************************************************* */
Pose2 prior(const Pose2& x, boost::optional<Matrix&> H) { Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1,
if (H) *H = I; boost::optional<Matrix&> H2) {
return x; return x1.between(x2, H1, H2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
return x1.between(x2, H1, H2);
}
/* ************************************************************************* */ } // namespace simulated2DOriented
} // namespace simulated2DOriented
} // namespace gtsam

162
gtsam/slam/simulated2DOriented.h
View File

@ -24,109 +24,107 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
// \namespace // \namespace
namespace simulated2DOriented {
namespace gtsam { using namespace gtsam;
namespace simulated2DOriented { // The types that take an oriented pose2 rather than point2
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues;
// The types that take an oriented pose2 rather than point2 /// Specialized Values structure with syntactic sugar for
typedef TypedSymbol<Point2, 'l'> PointKey; /// compatibility with matlab
typedef TypedSymbol<Pose2, 'x'> PoseKey; class Values: public TupleValues2<PoseValues, PointValues> {
typedef Values<PoseKey> PoseValues; public:
typedef Values<PointKey> PointValues; Values() {}
/// Specialized Values structure with syntactic sugar for void insertPose(const PoseKey& i, const Pose2& p) {
/// compatibility with matlab insert(i, p);
class Values: public TupleValues2<PoseValues, PointValues> { }
public:
Values() {}
void insertPose(const PoseKey& i, const Pose2& p) { void insertPoint(const PointKey& j, const Point2& p) {
insert(i, p); insert(j, p);
} }
void insertPoint(const PointKey& j, const Point2& p) { int nrPoses() const { return this->first_.size(); }
insert(j, p); int nrPoints() const { return this->second_.size(); }
}
int nrPoses() const { return this->first_.size(); } Pose2 pose(const PoseKey& i) const { return (*this)[i]; }
int nrPoints() const { return this->second_.size(); } Point2 point(const PointKey& j) const { return (*this)[j]; }
};
Pose2 pose(const PoseKey& i) const { return (*this)[i]; } //TODO:: point prior is not implemented right now
Point2 point(const PointKey& j) const { return (*this)[j]; }
};
//TODO:: point prior is not implemented right now /// Prior on a single pose
inline Pose2 prior(const Pose2& x) {
return x;
}
/// Prior on a single pose /// Prior on a single pose, optional derivative version
inline Pose2 prior(const Pose2& x) { Pose2 prior(const Pose2& x, boost::optional<Matrix&> H = boost::none);
return x;
}
/// Prior on a single pose, optional derivative version /// odometry between two poses
Pose2 prior(const Pose2& x, boost::optional<Matrix&> H = boost::none); inline Pose2 odo(const Pose2& x1, const Pose2& x2) {
return x1.between(x2);
}
/// odometry between two poses /// odometry between two poses, optional derivative version
inline Pose2 odo(const Pose2& x1, const Pose2& x2) { Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1 =
return x1.between(x2); boost::none, boost::optional<Matrix&> H2 = boost::none);
}
/// odometry between two poses, optional derivative version /// Unary factor encoding a soft prior on a vector
Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1 = template<class VALUES = Values, class Key = PoseKey>
boost::none, boost::optional<Matrix&> H2 = boost::none); struct GenericPosePrior: public NonlinearFactor1<VALUES, Key> {
/// Unary factor encoding a soft prior on a vector Pose2 z_; ///< measurement
template<class VALUES = Values, class Key = PoseKey>
struct GenericPosePrior: public NonlinearFactor1<VALUES, Key> {
Pose2 z_; ///< measurement /// Create generic pose prior
GenericPosePrior(const Pose2& z, const SharedNoiseModel& model,
const Key& key) :
NonlinearFactor1<VALUES, Key>(model, key), z_(z) {
}
/// Create generic pose prior /// Evaluate error and optionally derivative
GenericPosePrior(const Pose2& z, const SharedNoiseModel& model, Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
const Key& key) : boost::none) const {
NonlinearFactor1<VALUES, Key>(model, key), z_(z) { return z_.localCoordinates(prior(x, H));
} }
/// Evaluate error and optionally derivative };
Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
boost::none) const {
return z_.localCoordinates(prior(x, H));
}
}; /**
* Binary factor simulating "odometry" between two Vectors
*/
template<class VALUES = Values, class KEY = PoseKey>
struct GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> {
Pose2 z_; ///< Between measurement for odometry factor
/** /**
* Binary factor simulating "odometry" between two Vectors * Creates an odometry factor between two poses
*/ */
template<class VALUES = Values, class KEY = PoseKey> GenericOdometry(const Pose2& z, const SharedNoiseModel& model,
struct GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> { const KEY& i1, const KEY& i2) :
Pose2 z_; ///< Between measurement for odometry factor NonlinearFactor2<VALUES, KEY, KEY>(model, i1, i2), z_(z) {
}
/** /// Evaluate error and optionally derivative
* Creates an odometry factor between two poses Vector evaluateError(const Pose2& x1, const Pose2& x2,
*/ boost::optional<Matrix&> H1 = boost::none,
GenericOdometry(const Pose2& z, const SharedNoiseModel& model, boost::optional<Matrix&> H2 = boost::none) const {
const KEY& i1, const KEY& i2) : return z_.localCoordinates(odo(x1, x2, H1, H2));
NonlinearFactor2<VALUES, KEY, KEY>(model, i1, i2), z_(z) { }
}
/// Evaluate error and optionally derivative };
Vector evaluateError(const Pose2& x1, const Pose2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
return z_.localCoordinates(odo(x1, x2, H1, H2));
}
}; typedef GenericOdometry<Values, PoseKey> Odometry;
typedef GenericOdometry<Values, PoseKey> Odometry; /// Graph specialization for syntactic sugar use with matlab
class Graph : public NonlinearFactorGraph<Values> {
public:
Graph() {}
// TODO: add functions to add factors
};
/// Graph specialization for syntactic sugar use with matlab } // namespace simulated2DOriented
class Graph : public NonlinearFactorGraph<Values> {
public:
Graph() {}
// TODO: add functions to add factors
};
} // namespace simulated2DOriented
} // namespace gtsam

1
gtsam/slam/tests/testSimulated2D.cpp
View File

@ -25,6 +25,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace simulated2D;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( simulated2D, Simulated2DValues ) TEST( simulated2D, Simulated2DValues )

1
gtsam/slam/tests/testSimulated2DOriented.cpp
View File

@ -26,6 +26,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace simulated2DOriented;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( simulated2DOriented, Dprior ) TEST( simulated2DOriented, Dprior )

1
gtsam/slam/tests/testSimulated3D.cpp
View File

@ -24,6 +24,7 @@
#include <gtsam/slam/simulated3D.h> #include <gtsam/slam/simulated3D.h>
using namespace gtsam; using namespace gtsam;
using namespace simulated3D;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( simulated3D, Values ) TEST( simulated3D, Values )

2
tests/testBoundingConstraint.cpp
View File

@ -21,7 +21,7 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h> #include <gtsam/nonlinear/NonlinearOptimizer.h>
namespace iq2D = gtsam::simulated2D::inequality_constraints; namespace iq2D = simulated2D::inequality_constraints;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;

2
tests/testNonlinearEquality.cpp
View File

@ -27,7 +27,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
namespace eq2D = gtsam::simulated2D::equality_constraints; namespace eq2D = simulated2D::equality_constraints;
static const double tol = 1e-5; static const double tol = 1e-5;

3
wrap/matlab.h
View File

@ -22,6 +22,9 @@
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
using gtsam::Vector;
using gtsam::Matrix;
extern "C" { extern "C" {
#include <mex.h> #include <mex.h>
} }