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

352
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
@ -68,87 +70,87 @@ 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;

3
gtsam/slam/planarSLAM.cpp
View File

@ -21,8 +21,6 @@
#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) : Graph::Graph(const NonlinearFactorGraph<Values>& graph) :
@ -65,4 +63,3 @@ namespace gtsam {
} // planarSLAM } // planarSLAM
} // gtsam

6
gtsam/slam/planarSLAM.h
View File

@ -29,12 +29,11 @@
#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
namespace gtsam {
// Use planarSLAM namespace for specific SLAM instance // Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM { namespace planarSLAM {
using namespace gtsam;
/// 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;
@ -144,5 +143,4 @@ namespace gtsam {
} // planarSLAM } // planarSLAM
} // namespace gtsam

5
gtsam/slam/pose2SLAM.cpp
View File

@ -20,9 +20,8 @@
#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 {
@ -56,5 +55,3 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
} // pose2SLAM } // pose2SLAM
} // gtsam

6
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;
@ -113,5 +113,5 @@ namespace pose2SLAM {
} // pose2SLAM } // pose2SLAM
} // namespace gtsam

4
gtsam/slam/simulated2D.cpp
View File

@ -17,8 +17,6 @@
#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);
@ -48,4 +46,4 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace simulated2D } // namespace simulated2D
} // namespace gtsam

5
gtsam/slam/simulated2D.h
View File

@ -25,10 +25,10 @@
// \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;
@ -241,4 +241,3 @@ namespace gtsam {
}; };
} // namespace simulated2D } // namespace simulated2D
} // namespace gtsam

4
gtsam/slam/simulated2DConstraints.h
View File

@ -28,8 +28,6 @@
// \namespace // \namespace
namespace gtsam {
namespace simulated2D { namespace simulated2D {
namespace equality_constraints { namespace equality_constraints {
@ -201,4 +199,4 @@ namespace gtsam {
} // \namespace inequality_constraints } // \namespace inequality_constraints
} // \namespace simulated2D } // \namespace simulated2D
} // \namespace gtsam

4
gtsam/slam/simulated2DOriented.cpp
View File

@ -17,8 +17,6 @@
#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);
@ -38,4 +36,4 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace simulated2DOriented } // namespace simulated2DOriented
} // namespace gtsam

6
gtsam/slam/simulated2DOriented.h
View File

@ -24,11 +24,10 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
// \namespace // \namespace
namespace gtsam {
namespace simulated2DOriented { namespace simulated2DOriented {
using namespace gtsam;
// The types that take an oriented pose2 rather than point2 // The types that take an oriented pose2 rather than point2
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
@ -129,4 +128,3 @@ namespace gtsam {
}; };
} // namespace simulated2DOriented } // 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>
} }