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 gtsam;
using namespace gtsam::planarSLAM;
using namespace planarSLAM;
/**
* 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 gtsam;
using namespace boost;
using namespace gtsam::pose2SLAM;
using namespace pose2SLAM;
int main(int argc, char** argv) {
// create keys for robot positions

2
examples/Pose2SLAMExample_easy.cpp
View File

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

40
examples/matlab/PlanarSLAMExample_easy.m
View File

@ -28,31 +28,31 @@ graph = planarSLAMGraph;
%% Add prior
% gaussian for prior
prior_model = SharedNoiseModel_sharedSigmas([0.3; 0.3; 0.1]);
prior_measurement = Pose2(0.0, 0.0, 0.0); % prior at origin
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 = SharedNoiseModel_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_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 = SharedNoiseModel_sharedSigmas([0.1; 0.2]);
meas_model = gtsamSharedNoiseModel_sharedSigmas([0.1; 0.2]);
% create the measurement values - indices are (pose id, landmark id)
degrees = pi/180;
bearing11 = Rot2(45*degrees);
bearing21 = Rot2(90*degrees);
bearing32 = Rot2(90*degrees);
bearing11 = gtsamRot2(45*degrees);
bearing21 = gtsamRot2(90*degrees);
bearing32 = gtsamRot2(90*degrees);
range11 = sqrt(4+4);
range21 = 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(x2, l1, bearing21, range21, meas_model);
graph.addBearingRange(x3, l2, bearing32, range32, meas_model);
@ -62,26 +62,14 @@ graph.print('full graph');
%% Initialize to noisy points
initialEstimate = planarSLAMValues;
initialEstimate.insertPose(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(x3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insertPoint(l1, Point2(1.8, 2.1));
initialEstimate.insertPoint(l2, Point2(4.1, 1.8));
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.insertPoint(l1, gtsamPoint2(1.8, 2.1));
initialEstimate.insertPoint(l2, gtsamPoint2(4.1, 1.8));
initialEstimate.print('initial estimate');
%% 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);

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
% gaussian for prior
prior_model = SharedNoiseModel_sharedSigmas([0.3; 0.3; 0.1]);
prior_measurement = Pose2(0.0, 0.0, 0.0); % prior at origin
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 = SharedNoiseModel_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_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);
@ -46,24 +46,12 @@ graph.print('full graph');
%% Initialize to noisy points
initialEstimate = pose2SLAMValues;
initialEstimate.insertPose(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(x3, Pose2(4.1, 0.1, 0.1));
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');
%% 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);

356
gtsam.h
View File

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

69
gtsam/slam/planarSLAM.cpp
View File

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

170
gtsam/slam/planarSLAM.h
View File

@ -29,120 +29,118 @@
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/geometry/Pose2.h>
// We use gtsam namespace for generally useful factors
namespace gtsam {
// Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM {
// Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM {
using namespace gtsam;
/// Typedef for a PoseKey with Pose2 data and 'x' symbol
typedef TypedSymbol<Pose2, 'x'> PoseKey;
/// Typedef for a PoseKey with Pose2 data and 'x' symbol
typedef TypedSymbol<Pose2, 'x'> PoseKey;
/// Typedef for a PointKey with Point2 data and 'l' symbol
typedef TypedSymbol<Point2, 'l'> PointKey;
/// Typedef for a PointKey with Point2 data and 'l' symbol
typedef TypedSymbol<Point2, 'l'> PointKey;
/// Typedef for Values structure with PoseKey type
typedef Values<PoseKey> PoseValues;
/// Typedef for Values structure with PoseKey type
typedef Values<PoseKey> PoseValues;
/// Typedef for Values structure with PointKey type
typedef Values<PointKey> PointValues;
/// Typedef for Values structure with PointKey type
typedef Values<PointKey> PointValues;
/// Values class, inherited from TupleValues2, using PoseKeys and PointKeys
struct Values: public TupleValues2<PoseValues, PointValues> {
/// Values class, inherited from TupleValues2, using PoseKeys and PointKeys
struct Values: public TupleValues2<PoseValues, PointValues> {
/// Default constructor
Values() {}
/// Default constructor
Values() {}
/// Copy constructor
Values(const TupleValues2<PoseValues, PointValues>& values) :
TupleValues2<PoseValues, PointValues>(values) {
}
/// Copy constructor
Values(const TupleValues2<PoseValues, PointValues>& values) :
TupleValues2<PoseValues, PointValues>(values) {
}
/// Copy constructor
Values(const TupleValues2<PoseValues, PointValues>::Base& values) :
TupleValues2<PoseValues, PointValues>(values) {
}
/// Copy constructor
Values(const TupleValues2<PoseValues, PointValues>::Base& values) :
TupleValues2<PoseValues, PointValues>(values) {
}
/// From sub-values
Values(const PoseValues& poses, const PointValues& points) :
TupleValues2<PoseValues, PointValues>(poses, points) {
}
/// From sub-values
Values(const PoseValues& poses, const PointValues& 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
Pose2 pose(int key) const { return (*this)[PoseKey(key)]; }
/// get a pose
Pose2 pose(int key) const { return (*this)[PoseKey(key)]; }
/// get a point
Point2 point(int key) const { return (*this)[PointKey(key)]; }
/// get a point
Point2 point(int key) const { return (*this)[PointKey(key)]; }
/// insert a pose
void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); }
/// insert a pose
void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); }
/// insert a point
void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); }
};
/// insert a 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
typedef NonlinearEquality<Values, PoseKey> Constraint;
/// A prior factor to bias the value of a PoseKey
typedef PriorFactor<Values, PoseKey> Prior;
/// A factor between two PoseKeys set with a Pose2
typedef BetweenFactor<Values, PoseKey> Odometry;
/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
typedef BearingFactor<Values, PoseKey, PointKey> Bearing;
/// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
typedef RangeFactor<Values, PoseKey, PointKey> Range;
/// A factor between a PoseKey and a PointKey to express difference in rotation and location
typedef BearingRangeFactor<Values, PoseKey, PointKey> BearingRange;
/// A hard constraint for PoseKeys to enforce particular values
typedef NonlinearEquality<Values, PoseKey> Constraint;
/// A prior factor to bias the value of a PoseKey
typedef PriorFactor<Values, PoseKey> Prior;
/// A factor between two PoseKeys set with a Pose2
typedef BetweenFactor<Values, PoseKey> Odometry;
/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
typedef BearingFactor<Values, PoseKey, PointKey> Bearing;
/// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
typedef RangeFactor<Values, PoseKey, PointKey> Range;
/// A factor between a PoseKey and a PointKey to express difference in rotation and location
typedef BearingRangeFactor<Values, PoseKey, PointKey> BearingRange;
/// Creates a NonlinearFactorGraph with the Values type
struct Graph: public NonlinearFactorGraph<Values> {
/// Creates a NonlinearFactorGraph with the Values type
struct Graph: public NonlinearFactorGraph<Values> {
/// Default constructor for a NonlinearFactorGraph
Graph(){}
/// Default constructor for a NonlinearFactorGraph
Graph(){}
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph<Values>& graph);
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph<Values>& graph);
/// Biases the value of PoseKey key with Pose2 p given a noise model
void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel);
/// Biases the value of PoseKey key with Pose2 p given a noise model
void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel);
/// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose);
/// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose);
/// 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,
const SharedNoiseModel& model);
/// 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,
const SharedNoiseModel& model);
/// 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,
const SharedNoiseModel& model);
/// 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,
const SharedNoiseModel& model);
/// 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,
const SharedNoiseModel& model);
/// 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,
const SharedNoiseModel& model);
/// 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,
const Rot2& bearing, double range, const SharedNoiseModel& model);
/// 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,
const Rot2& bearing, double range, const SharedNoiseModel& model);
/// Optimize
Values optimize(const Values& initialEstimate) {
typedef NonlinearOptimizer<Graph, Values> Optimizer;
return *Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA);
}
};
/// Optimize
Values optimize(const Values& initialEstimate) {
typedef NonlinearOptimizer<Graph, Values> Optimizer;
return *Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA);
}
};
/// Optimizer
typedef NonlinearOptimizer<Graph, Values> Optimizer;
/// 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>
// 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 x;
double theta = 0, dtheta = 2 * M_PI / n;
for (size_t i = 0; i < n; i++, theta += dtheta)
x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
return x;
}
/* ************************************************************************* */
Values circle(size_t n, double R) {
Values x;
double theta = 0, dtheta = 2 * M_PI / n;
for (size_t i = 0; i < n; i++, theta += dtheta)
x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
return x;
}
/* ************************************************************************* */
void Graph::addPrior(const PoseKey& i, const Pose2& p,
const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPrior(const PoseKey& i, const Pose2& p,
const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model));
push_back(factor);
}
void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) {
sharedFactor factor(new HardConstraint(i, p));
push_back(factor);
}
void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) {
sharedFactor factor(new HardConstraint(i, p));
push_back(factor);
}
void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(i, j, z, model));
push_back(factor);
}
void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(i, j, z, model));
push_back(factor);
}
/* ************************************************************************* */
/* ************************************************************************* */
} // pose2SLAM
} // gtsam
} // pose2SLAM

8
gtsam/slam/pose2SLAM.h
View File

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

48
gtsam/slam/simulated2D.cpp
View File

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

296
gtsam/slam/simulated2DConstraints.h
View File

@ -28,177 +28,175 @@
// \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 */
typedef NonlinearEquality1<Values, PoseKey> UnaryEqualityConstraint;
typedef NonlinearEquality1<Values, PointKey> UnaryEqualityPointConstraint;
typedef BetweenConstraint<Values, PoseKey> OdoEqualityConstraint;
/** Equality between variables */
typedef NonlinearEquality2<Values, PoseKey> PoseEqualityConstraint;
typedef NonlinearEquality2<Values, PointKey> PointEqualityConstraint;
/** Equality between variables */
typedef NonlinearEquality2<Values, PoseKey> PoseEqualityConstraint;
typedef NonlinearEquality2<Values, PointKey> PointEqualityConstraint;
} // \namespace equality_constraints
} // \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
/**
* 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() {}
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
* @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) {
}
/**
* Access function for the constrained index
* @return the index for the constrained coordinate
*/
inline unsigned int index() const { return IDX; }
/**
* Access function for the constrained index
* @return the index for the constrained coordinate
*/
inline unsigned int index() const { return IDX; }
/**
* extracts a single value from the point to compute error
* @param x is the estimate of the constrained variable being evaluated
* @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);
}
};
/**
* extracts a single value from the point to compute error
* @param x is the estimate of the constrained variable being evaluated
* @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
typedef ScalarCoordConstraint1<Values, PoseKey, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
/** typedefs for use with simulated2D systems */
typedef ScalarCoordConstraint1<Values, PoseKey, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
typedef ScalarCoordConstraint1<Values, PoseKey, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
/**
* Trait for distance constraints to provide distance
* @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
* @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); }
/**
* 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
/**
* 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() {}
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
* @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) {}
/**
* 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);
}
};
/**
* 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
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
/**
* 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() {}
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
* @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) {}
/**
* 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);
}
};
/**
* 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
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>
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) {
if (H) *H = I;
return x;
}
/* ************************************************************************* */
Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
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>
// \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
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues;
/// Specialized Values structure with syntactic sugar for
/// compatibility with matlab
class Values: public TupleValues2<PoseValues, PointValues> {
public:
Values() {}
/// Specialized Values structure with syntactic sugar for
/// compatibility with matlab
class Values: public TupleValues2<PoseValues, PointValues> {
public:
Values() {}
void insertPose(const PoseKey& i, const Pose2& p) {
insert(i, p);
}
void insertPose(const PoseKey& i, const Pose2& p) {
insert(i, p);
}
void insertPoint(const PointKey& j, const Point2& p) {
insert(j, p);
}
void insertPoint(const PointKey& j, const Point2& p) {
insert(j, p);
}
int nrPoses() const { return this->first_.size(); }
int nrPoints() const { return this->second_.size(); }
int nrPoses() const { return this->first_.size(); }
int nrPoints() const { return this->second_.size(); }
Pose2 pose(const PoseKey& i) const { return (*this)[i]; }
Point2 point(const PointKey& j) const { return (*this)[j]; }
};
Pose2 pose(const PoseKey& i) const { return (*this)[i]; }
Point2 point(const PointKey& j) const { return (*this)[j]; }
};
//TODO:: point prior is not implemented right now
//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
inline Pose2 prior(const Pose2& x) {
return x;
}
/// Prior on a single pose, optional derivative version
Pose2 prior(const Pose2& x, boost::optional<Matrix&> H = boost::none);
/// Prior on a single pose, optional derivative version
Pose2 prior(const Pose2& x, boost::optional<Matrix&> H = boost::none);
/// odometry between two poses
inline Pose2 odo(const Pose2& x1, const Pose2& x2) {
return x1.between(x2);
}
/// odometry between two poses
inline Pose2 odo(const Pose2& x1, const Pose2& x2) {
return x1.between(x2);
}
/// odometry between two poses, optional derivative version
Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none);
/// odometry between two poses, optional derivative version
Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none);
/// Unary factor encoding a soft prior on a vector
template<class VALUES = Values, class Key = PoseKey>
struct GenericPosePrior: public NonlinearFactor1<VALUES, Key> {
/// Unary factor encoding a soft prior on a vector
template<class VALUES = Values, class Key = PoseKey>
struct GenericPosePrior: public NonlinearFactor1<VALUES, Key> {
Pose2 z_; ///< measurement
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
GenericPosePrior(const Pose2& z, const SharedNoiseModel& model,
const Key& key) :
NonlinearFactor1<VALUES, Key>(model, key), z_(z) {
}
/// Evaluate error and optionally derivative
Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
boost::none) const {
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
*/
template<class VALUES = Values, class KEY = PoseKey>
struct GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> {
Pose2 z_; ///< Between measurement for odometry factor
/**
* Creates an odometry factor between two poses
*/
GenericOdometry(const Pose2& z, const SharedNoiseModel& model,
const KEY& i1, const KEY& i2) :
NonlinearFactor2<VALUES, KEY, KEY>(model, i1, i2), z_(z) {
}
/**
* Creates an odometry factor between two poses
*/
GenericOdometry(const Pose2& z, const SharedNoiseModel& model,
const KEY& i1, const KEY& i2) :
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));
}
/// 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
class Graph : public NonlinearFactorGraph<Values> {
public:
Graph() {}
// TODO: add functions to add factors
};
} // namespace simulated2DOriented
} // namespace gtsam
} // namespace simulated2DOriented

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

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

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

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

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

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

2
tests/testBoundingConstraint.cpp
View File

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

2
tests/testNonlinearEquality.cpp
View File

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

3
wrap/matlab.h
View File

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