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 withinrelease/4.3a0
Chris Beall
parent
67aa0bf63d
commit
537a1a3fae
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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,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);
|
||||
|
|
|
|||
352
gtsam.h
352
gtsam.h
|
|
@ -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
|
||||
|
|
@ -68,87 +70,87 @@ using namespace gtsam;
|
|||
class Point2 {
|
||||
Point2();
|
||||
Point2(double x, double y);
|
||||
static Point2 Expmap(Vector v);
|
||||
static Vector Logmap(const Point2& p);
|
||||
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;
|
||||
|
|
|
|||
|
|
@ -21,8 +21,6 @@
|
|||
#include <gtsam/nonlinear/NonlinearOptimization.h>
|
||||
|
||||
// Use planarSLAM namespace for specific SLAM instance
|
||||
namespace gtsam {
|
||||
|
||||
namespace planarSLAM {
|
||||
|
||||
Graph::Graph(const NonlinearFactorGraph<Values>& graph) :
|
||||
|
|
@ -65,4 +63,3 @@ namespace gtsam {
|
|||
|
||||
} // planarSLAM
|
||||
|
||||
} // gtsam
|
||||
|
|
|
|||
|
|
@ -29,12 +29,11 @@
|
|||
#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 {
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
/// Typedef for a PoseKey with Pose2 data and 'x' symbol
|
||||
typedef TypedSymbol<Pose2, 'x'> PoseKey;
|
||||
|
||||
|
|
@ -144,5 +143,4 @@ namespace gtsam {
|
|||
|
||||
} // planarSLAM
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -20,9 +20,8 @@
|
|||
#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 {
|
||||
|
||||
|
|
@ -56,5 +55,3 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
|
||||
} // pose2SLAM
|
||||
|
||||
} // gtsam
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
@ -113,5 +113,5 @@ namespace pose2SLAM {
|
|||
|
||||
} // pose2SLAM
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -17,8 +17,6 @@
|
|||
|
||||
#include <gtsam/slam/simulated2D.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
namespace simulated2D {
|
||||
|
||||
static Matrix I = gtsam::eye(2);
|
||||
|
|
@ -48,4 +46,4 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
|
||||
} // namespace simulated2D
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -25,10 +25,10 @@
|
|||
|
||||
// \namespace
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
namespace simulated2D {
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
// Simulated2D robots have no orientation, just a position
|
||||
typedef TypedSymbol<Point2, 'x'> PoseKey;
|
||||
typedef TypedSymbol<Point2, 'l'> PointKey;
|
||||
|
|
@ -241,4 +241,3 @@ namespace gtsam {
|
|||
};
|
||||
|
||||
} // namespace simulated2D
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -28,8 +28,6 @@
|
|||
|
||||
// \namespace
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
namespace simulated2D {
|
||||
|
||||
namespace equality_constraints {
|
||||
|
|
@ -201,4 +199,4 @@ namespace gtsam {
|
|||
} // \namespace inequality_constraints
|
||||
|
||||
} // \namespace simulated2D
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -17,8 +17,6 @@
|
|||
|
||||
#include <gtsam/slam/simulated2DOriented.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
namespace simulated2DOriented {
|
||||
|
||||
static Matrix I = gtsam::eye(3);
|
||||
|
|
@ -38,4 +36,4 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
|
||||
} // namespace simulated2DOriented
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -24,11 +24,10 @@
|
|||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
||||
// \namespace
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
namespace simulated2DOriented {
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
// The types that take an oriented pose2 rather than point2
|
||||
typedef TypedSymbol<Point2, 'l'> PointKey;
|
||||
typedef TypedSymbol<Pose2, 'x'> PoseKey;
|
||||
|
|
@ -129,4 +128,3 @@ namespace gtsam {
|
|||
};
|
||||
|
||||
} // namespace simulated2DOriented
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -25,6 +25,7 @@
|
|||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace simulated2D;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( simulated2D, Simulated2DValues )
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@
|
|||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace simulated2DOriented;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( simulated2DOriented, Dprior )
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
#include <gtsam/slam/simulated3D.h>
|
||||
|
||||
using namespace gtsam;
|
||||
using namespace simulated3D;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( simulated3D, Values )
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue