diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp index d697ae261..d29b21fa9 100644 --- a/examples/PlanarSLAMExample_easy.cpp +++ b/examples/PlanarSLAMExample_easy.cpp @@ -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: diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index 998ed3af6..bb7f60516 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -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 diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp index afc7a217e..ab23436b0 100644 --- a/examples/Pose2SLAMExample_easy.cpp +++ b/examples/Pose2SLAMExample_easy.cpp @@ -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 diff --git a/examples/matlab/PlanarSLAMExample_easy.m b/examples/matlab/PlanarSLAMExample_easy.m index 8dc4ca5be..3e165e975 100644 --- a/examples/matlab/PlanarSLAMExample_easy.m +++ b/examples/matlab/PlanarSLAMExample_easy.m @@ -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); diff --git a/examples/matlab/Pose2SLAMExample_advanced.m b/examples/matlab/Pose2SLAMExample_advanced.m new file mode 100644 index 000000000..7a7e10802 --- /dev/null +++ b/examples/matlab/Pose2SLAMExample_advanced.m @@ -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); diff --git a/examples/matlab/Pose2SLAMExample_easy.m b/examples/matlab/Pose2SLAMExample_easy.m index 9acf35fb3..8df9c1c0e 100644 --- a/examples/matlab/Pose2SLAMExample_easy.m +++ b/examples/matlab/Pose2SLAMExample_easy.m @@ -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); diff --git a/gtsam.h b/gtsam.h index dc5433774..030a6ea01 100644 --- a/gtsam.h +++ b/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 @@ -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 @@ -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; diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp index c90e56be9..6942ab429 100644 --- a/gtsam/slam/planarSLAM.cpp +++ b/gtsam/slam/planarSLAM.cpp @@ -21,48 +21,45 @@ #include // Use planarSLAM namespace for specific SLAM instance -namespace gtsam { +namespace planarSLAM { - namespace planarSLAM { + Graph::Graph(const NonlinearFactorGraph& graph) : + NonlinearFactorGraph(graph) {} - Graph::Graph(const NonlinearFactorGraph& graph) : - NonlinearFactorGraph(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 diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 5776e989c..b8e887f8a 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -29,120 +29,118 @@ #include #include -// 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 PoseKey; + /// Typedef for a PoseKey with Pose2 data and 'x' symbol + typedef TypedSymbol PoseKey; - /// Typedef for a PointKey with Point2 data and 'l' symbol - typedef TypedSymbol PointKey; + /// Typedef for a PointKey with Point2 data and 'l' symbol + typedef TypedSymbol PointKey; - /// Typedef for Values structure with PoseKey type - typedef Values PoseValues; + /// Typedef for Values structure with PoseKey type + typedef Values PoseValues; - /// Typedef for Values structure with PointKey type - typedef Values PointValues; + /// Typedef for Values structure with PointKey type + typedef Values PointValues; - /// Values class, inherited from TupleValues2, using PoseKeys and PointKeys - struct Values: public TupleValues2 { + /// Values class, inherited from TupleValues2, using PoseKeys and PointKeys + struct Values: public TupleValues2 { - /// Default constructor - Values() {} + /// Default constructor + Values() {} - /// Copy constructor - Values(const TupleValues2& values) : - TupleValues2(values) { - } + /// Copy constructor + Values(const TupleValues2& values) : + TupleValues2(values) { + } - /// Copy constructor - Values(const TupleValues2::Base& values) : - TupleValues2(values) { - } + /// Copy constructor + Values(const TupleValues2::Base& values) : + TupleValues2(values) { + } - /// From sub-values - Values(const PoseValues& poses, const PointValues& points) : - TupleValues2(poses, points) { - } + /// From sub-values + Values(const PoseValues& poses, const PointValues& points) : + TupleValues2(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 Constraint; - /// A prior factor to bias the value of a PoseKey - typedef PriorFactor Prior; - /// A factor between two PoseKeys set with a Pose2 - typedef BetweenFactor Odometry; - /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2) - typedef BearingFactor Bearing; - /// A factor between a PoseKey and a PointKey to express distance between them (set with a double) - typedef RangeFactor Range; - /// A factor between a PoseKey and a PointKey to express difference in rotation and location - typedef BearingRangeFactor BearingRange; + /// A hard constraint for PoseKeys to enforce particular values + typedef NonlinearEquality Constraint; + /// A prior factor to bias the value of a PoseKey + typedef PriorFactor Prior; + /// A factor between two PoseKeys set with a Pose2 + typedef BetweenFactor Odometry; + /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2) + typedef BearingFactor Bearing; + /// A factor between a PoseKey and a PointKey to express distance between them (set with a double) + typedef RangeFactor Range; + /// A factor between a PoseKey and a PointKey to express difference in rotation and location + typedef BearingRangeFactor BearingRange; - /// Creates a NonlinearFactorGraph with the Values type - struct Graph: public NonlinearFactorGraph { + /// Creates a NonlinearFactorGraph with the Values type + struct Graph: public NonlinearFactorGraph { - /// Default constructor for a NonlinearFactorGraph - Graph(){} + /// Default constructor for a NonlinearFactorGraph + Graph(){} - /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph); + /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph + Graph(const NonlinearFactorGraph& 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 Optimizer; - return *Optimizer::optimizeLM(*this, initialEstimate, - NonlinearOptimizationParameters::LAMBDA); - } - }; + /// Optimize + Values optimize(const Values& initialEstimate) { + typedef NonlinearOptimizer Optimizer; + return *Optimizer::optimizeLM(*this, initialEstimate, + NonlinearOptimizationParameters::LAMBDA); + } + }; - /// Optimizer - typedef NonlinearOptimizer Optimizer; + /// Optimizer + typedef NonlinearOptimizer Optimizer; - } // planarSLAM +} // planarSLAM -} // namespace gtsam diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index 228ca3eed..e078505a6 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -20,41 +20,38 @@ #include // Use pose2SLAM namespace for specific SLAM instance -namespace gtsam { - template class NonlinearOptimizer; +template class gtsam::NonlinearOptimizer; - 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 diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index e283c7911..82a5a6efd 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -28,11 +28,11 @@ #include #include -namespace gtsam { - // Use pose2SLAM namespace for specific SLAM instance namespace pose2SLAM { + using namespace gtsam; + /// Keys with Pose2 and symbol 'x' typedef TypedSymbol PoseKey; @@ -111,7 +111,7 @@ namespace pose2SLAM { typedef NonlinearOptimizer Optimizer; - } // pose2SLAM +} // pose2SLAM + -} // namespace gtsam diff --git a/gtsam/slam/simulated2D.cpp b/gtsam/slam/simulated2D.cpp index ae30c6800..95dfef0e5 100644 --- a/gtsam/slam/simulated2D.cpp +++ b/gtsam/slam/simulated2D.cpp @@ -17,35 +17,33 @@ #include -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 H) { + if (H) *H = I; + return x; + } - /* ************************************************************************* */ - Point2 prior(const Point2& x, boost::optional H) { - if (H) *H = I; - return x; - } + /* ************************************************************************* */ + Point2 odo(const Point2& x1, const Point2& x2, boost::optional H1, + boost::optional H2) { + if (H1) *H1 = -I; + if (H2) *H2 = I; + return x2 - x1; + } - /* ************************************************************************* */ - Point2 odo(const Point2& x1, const Point2& x2, boost::optional H1, - boost::optional H2) { - if (H1) *H1 = -I; - if (H2) *H2 = I; - return x2 - x1; - } + /* ************************************************************************* */ + Point2 mea(const Point2& x, const Point2& l, boost::optional H1, + boost::optional H2) { + if (H1) *H1 = -I; + if (H2) *H2 = I; + return l - x; + } - /* ************************************************************************* */ - Point2 mea(const Point2& x, const Point2& l, boost::optional H1, - boost::optional H2) { - if (H1) *H1 = -I; - if (H2) *H2 = I; - return l - x; - } +/* ************************************************************************* */ - /* ************************************************************************* */ +} // namespace simulated2D - } // namespace simulated2D -} // namespace gtsam diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index cae60bb80..ffcfce04d 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -25,220 +25,219 @@ // \namespace -namespace gtsam { +namespace simulated2D { - namespace simulated2D { + using namespace gtsam; - // Simulated2D robots have no orientation, just a position - typedef TypedSymbol PoseKey; - typedef TypedSymbol PointKey; - typedef Values PoseValues; - typedef Values PointValues; + // Simulated2D robots have no orientation, just a position + typedef TypedSymbol PoseKey; + typedef TypedSymbol PointKey; + typedef Values PoseValues; + typedef Values PointValues; - /** - * Custom Values class that holds poses and points - */ - class Values: public TupleValues2 { - public: - typedef TupleValues2 Base; ///< base class - typedef boost::shared_ptr sharedPoint; ///< shortcut to shared Point type + /** + * Custom Values class that holds poses and points + */ + class Values: public TupleValues2 { + public: + typedef TupleValues2 Base; ///< base class + typedef boost::shared_ptr 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 H = boost::none); + /// Prior on a single pose, optionally returns derivative + Point2 prior(const Point2& x, boost::optional 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 H1 = - boost::none, boost::optional H2 = boost::none); + /// odometry between two poses, optionally returns derivative + Point2 odo(const Point2& x1, const Point2& x2, boost::optional H1 = + boost::none, boost::optional 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 H1 = - boost::none, boost::optional H2 = boost::none); + /// measurement between landmark and pose, optionally returns derivative + Point2 mea(const Point2& x, const Point2& l, boost::optional H1 = + boost::none, boost::optional H2 = boost::none); - /** - * Unary factor encoding a soft prior on a vector - */ - template - class GenericPrior: public NonlinearFactor1 { - public: - typedef NonlinearFactor1 Base; ///< base class - typedef boost::shared_ptr > shared_ptr; - typedef typename KEY::Value Pose; ///< shortcut to Pose type + /** + * Unary factor encoding a soft prior on a vector + */ + template + class GenericPrior: public NonlinearFactor1 { + public: + typedef NonlinearFactor1 Base; ///< base class + typedef boost::shared_ptr > 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(model, key), z_(z) { - } + /// Create generic prior + GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) : + NonlinearFactor1(model, key), z_(z) { + } - /// Return error and optional derivative - Vector evaluateError(const Pose& x, boost::optional H = - boost::none) const { - return (prior(x, H) - z_).vector(); - } + /// Return error and optional derivative + Vector evaluateError(const Pose& x, boost::optional 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 - 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 + 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 GenericOdometry: public NonlinearFactor2 { - public: - typedef NonlinearFactor2 Base; ///< base class - typedef boost::shared_ptr > shared_ptr; - typedef typename KEY::Value Pose; ///< shortcut to Pose type + /** + * Binary factor simulating "odometry" between two Vectors + */ + template + class GenericOdometry: public NonlinearFactor2 { + public: + typedef NonlinearFactor2 Base; ///< base class + typedef boost::shared_ptr > 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(model, i1, i2), z_(z) { - } + /// Create odometry + GenericOdometry(const Pose& z, const SharedNoiseModel& model, + const KEY& i1, const KEY& i2) : + NonlinearFactor2(model, i1, i2), z_(z) { + } - /// Evaluate error and optionally return derivatives - Vector evaluateError(const Pose& x1, const Pose& x2, - boost::optional H1 = boost::none, - boost::optional 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 H1 = boost::none, + boost::optional 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 - 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 + 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 GenericMeasurement: public NonlinearFactor2 { - public: - typedef NonlinearFactor2 Base; ///< base class - typedef boost::shared_ptr > 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 GenericMeasurement: public NonlinearFactor2 { + public: + typedef NonlinearFactor2 Base; ///< base class + typedef boost::shared_ptr > 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(model, i, j), z_(z) { - } + /// Create measurement factor + GenericMeasurement(const Point& z, const SharedNoiseModel& model, + const XKEY& i, const LKEY& j) : + NonlinearFactor2(model, i, j), z_(z) { + } - /// Evaluate error and optionally return derivatives - Vector evaluateError(const Pose& x1, const Point& x2, - boost::optional H1 = boost::none, - boost::optional 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 H1 = boost::none, + boost::optional 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 - 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 + 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 Prior; - typedef GenericOdometry Odometry; - typedef GenericMeasurement Measurement; + /** Typedefs for regular use */ + typedef GenericPrior Prior; + typedef GenericOdometry Odometry; + typedef GenericMeasurement Measurement; - // Specialization of a graph for this example domain - // TODO: add functions to add factor types - class Graph : public NonlinearFactorGraph { - public: - Graph() {} - }; + // Specialization of a graph for this example domain + // TODO: add functions to add factor types + class Graph : public NonlinearFactorGraph { + public: + Graph() {} + }; - } // namespace simulated2D -} // namespace gtsam +} // namespace simulated2D diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h index b09a5c638..2521e86e4 100644 --- a/gtsam/slam/simulated2DConstraints.h +++ b/gtsam/slam/simulated2DConstraints.h @@ -28,177 +28,175 @@ // \namespace -namespace gtsam { +namespace simulated2D { - namespace simulated2D { + namespace equality_constraints { - namespace equality_constraints { + /** Typedefs for regular use */ + typedef NonlinearEquality1 UnaryEqualityConstraint; + typedef NonlinearEquality1 UnaryEqualityPointConstraint; + typedef BetweenConstraint OdoEqualityConstraint; - /** Typedefs for regular use */ - typedef NonlinearEquality1 UnaryEqualityConstraint; - typedef NonlinearEquality1 UnaryEqualityPointConstraint; - typedef BetweenConstraint OdoEqualityConstraint; + /** Equality between variables */ + typedef NonlinearEquality2 PoseEqualityConstraint; + typedef NonlinearEquality2 PointEqualityConstraint; - /** Equality between variables */ - typedef NonlinearEquality2 PoseEqualityConstraint; - typedef NonlinearEquality2 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 + struct ScalarCoordConstraint1: public BoundingConstraint1 { + typedef BoundingConstraint1 Base; ///< Base class convenience typedef + typedef boost::shared_ptr > 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 - struct ScalarCoordConstraint1: public BoundingConstraint1 { - typedef BoundingConstraint1 Base; ///< Base class convenience typedef - typedef boost::shared_ptr > 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 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 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 PoseXInequality; ///< Simulated2D domain example factor constraining X + typedef ScalarCoordConstraint1 PoseYInequality; ///< Simulated2D domain example factor constraining Y - /** typedefs for use with simulated2D systems */ - typedef ScalarCoordConstraint1 PoseXInequality; ///< Simulated2D domain example factor constraining X - typedef ScalarCoordConstraint1 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 + 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 - 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 + struct MaxDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 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 - struct MaxDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 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 H1 = boost::none, + boost::optional H2 = boost::none) const { + if (H1) *H1 = numericalDerivative21(range_trait, x1, x2, 1e-5); + if (H1) *H2 = numericalDerivative22(range_trait, 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 H1 = boost::none, - boost::optional H2 = boost::none) const { - if (H1) *H1 = numericalDerivative21(range_trait, x1, x2, 1e-5); - if (H1) *H2 = numericalDerivative22(range_trait, x1, x2, 1e-5); - return range_trait(x1, x2); - } - }; + typedef MaxDistanceConstraint PoseMaxDistConstraint; ///< Simulated2D domain example factor - typedef MaxDistanceConstraint 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 + struct MinDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 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 - struct MinDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 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 H1 = boost::none, + boost::optional H2 = boost::none) const { + if (H1) *H1 = numericalDerivative21(range_trait, x1, x2, 1e-5); + if (H1) *H2 = numericalDerivative22(range_trait, 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 H1 = boost::none, - boost::optional H2 = boost::none) const { - if (H1) *H1 = numericalDerivative21(range_trait, x1, x2, 1e-5); - if (H1) *H2 = numericalDerivative22(range_trait, x1, x2, 1e-5); - return range_trait(x1, x2); - } - }; - - typedef MinDistanceConstraint LandmarkAvoid; ///< Simulated2D domain example factor + typedef MinDistanceConstraint LandmarkAvoid; ///< Simulated2D domain example factor - } // \namespace inequality_constraints + } // \namespace inequality_constraints + +} // \namespace simulated2D - } // \namespace simulated2D -} // \namespace gtsam diff --git a/gtsam/slam/simulated2DOriented.cpp b/gtsam/slam/simulated2DOriented.cpp index 0b68f0e46..947aaddc5 100644 --- a/gtsam/slam/simulated2DOriented.cpp +++ b/gtsam/slam/simulated2DOriented.cpp @@ -17,25 +17,23 @@ #include -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 H) { + if (H) *H = I; + return x; + } - /* ************************************************************************* */ - Pose2 prior(const Pose2& x, boost::optional H) { - if (H) *H = I; - return x; - } + /* ************************************************************************* */ + Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional H1, + boost::optional H2) { + return x1.between(x2, H1, H2); + } - /* ************************************************************************* */ - Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional H1, - boost::optional H2) { - return x1.between(x2, H1, H2); - } +/* ************************************************************************* */ - /* ************************************************************************* */ +} // namespace simulated2DOriented - } // namespace simulated2DOriented -} // namespace gtsam diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index b9cf150ef..18c0c96c2 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -24,109 +24,107 @@ #include // \namespace +namespace simulated2DOriented { -namespace gtsam { + using namespace gtsam; - namespace simulated2DOriented { + // The types that take an oriented pose2 rather than point2 + typedef TypedSymbol PointKey; + typedef TypedSymbol PoseKey; + typedef Values PoseValues; + typedef Values PointValues; - // The types that take an oriented pose2 rather than point2 - typedef TypedSymbol PointKey; - typedef TypedSymbol PoseKey; - typedef Values PoseValues; - typedef Values PointValues; + /// Specialized Values structure with syntactic sugar for + /// compatibility with matlab + class Values: public TupleValues2 { + public: + Values() {} - /// Specialized Values structure with syntactic sugar for - /// compatibility with matlab - class Values: public TupleValues2 { - 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 H = boost::none); - /// Prior on a single pose, optional derivative version - Pose2 prior(const Pose2& x, boost::optional 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 H1 = + boost::none, boost::optional H2 = boost::none); - /// odometry between two poses, optional derivative version - Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional H1 = - boost::none, boost::optional H2 = boost::none); + /// Unary factor encoding a soft prior on a vector + template + struct GenericPosePrior: public NonlinearFactor1 { - /// Unary factor encoding a soft prior on a vector - template - struct GenericPosePrior: public NonlinearFactor1 { + Pose2 z_; ///< measurement - Pose2 z_; ///< measurement + /// Create generic pose prior + GenericPosePrior(const Pose2& z, const SharedNoiseModel& model, + const Key& key) : + NonlinearFactor1(model, key), z_(z) { + } - /// Create generic pose prior - GenericPosePrior(const Pose2& z, const SharedNoiseModel& model, - const Key& key) : - NonlinearFactor1(model, key), z_(z) { - } + /// Evaluate error and optionally derivative + Vector evaluateError(const Pose2& x, boost::optional H = + boost::none) const { + return z_.localCoordinates(prior(x, H)); + } - /// Evaluate error and optionally derivative - Vector evaluateError(const Pose2& x, boost::optional H = - boost::none) const { - return z_.localCoordinates(prior(x, H)); - } + }; - }; + /** + * Binary factor simulating "odometry" between two Vectors + */ + template + struct GenericOdometry: public NonlinearFactor2 { + Pose2 z_; ///< Between measurement for odometry factor - /** - * Binary factor simulating "odometry" between two Vectors - */ - template - struct GenericOdometry: public NonlinearFactor2 { - 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(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(model, i1, i2), z_(z) { - } + /// Evaluate error and optionally derivative + Vector evaluateError(const Pose2& x1, const Pose2& x2, + boost::optional H1 = boost::none, + boost::optional 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 H1 = boost::none, - boost::optional H2 = boost::none) const { - return z_.localCoordinates(odo(x1, x2, H1, H2)); - } + }; - }; + typedef GenericOdometry Odometry; - typedef GenericOdometry Odometry; + /// Graph specialization for syntactic sugar use with matlab + class Graph : public NonlinearFactorGraph { + public: + Graph() {} + // TODO: add functions to add factors + }; - /// Graph specialization for syntactic sugar use with matlab - class Graph : public NonlinearFactorGraph { - public: - Graph() {} - // TODO: add functions to add factors - }; - - } // namespace simulated2DOriented -} // namespace gtsam +} // namespace simulated2DOriented diff --git a/gtsam/slam/tests/testSimulated2D.cpp b/gtsam/slam/tests/testSimulated2D.cpp index c9b2bfd61..2d28f11c3 100644 --- a/gtsam/slam/tests/testSimulated2D.cpp +++ b/gtsam/slam/tests/testSimulated2D.cpp @@ -25,6 +25,7 @@ using namespace std; using namespace gtsam; +using namespace simulated2D; /* ************************************************************************* */ TEST( simulated2D, Simulated2DValues ) diff --git a/gtsam/slam/tests/testSimulated2DOriented.cpp b/gtsam/slam/tests/testSimulated2DOriented.cpp index 57951f6e1..64844cb2a 100644 --- a/gtsam/slam/tests/testSimulated2DOriented.cpp +++ b/gtsam/slam/tests/testSimulated2DOriented.cpp @@ -26,6 +26,7 @@ using namespace std; using namespace gtsam; +using namespace simulated2DOriented; /* ************************************************************************* */ TEST( simulated2DOriented, Dprior ) diff --git a/gtsam/slam/tests/testSimulated3D.cpp b/gtsam/slam/tests/testSimulated3D.cpp index 30f41e2fd..b3086acf1 100644 --- a/gtsam/slam/tests/testSimulated3D.cpp +++ b/gtsam/slam/tests/testSimulated3D.cpp @@ -24,6 +24,7 @@ #include using namespace gtsam; +using namespace simulated3D; /* ************************************************************************* */ TEST( simulated3D, Values ) diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 062a04ed6..522d2d9a8 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -21,7 +21,7 @@ #include #include -namespace iq2D = gtsam::simulated2D::inequality_constraints; +namespace iq2D = simulated2D::inequality_constraints; using namespace std; using namespace gtsam; diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 2fb5ec315..fd1459a17 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -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; diff --git a/wrap/matlab.h b/wrap/matlab.h index 86dceb3d8..2c256a55d 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -22,6 +22,9 @@ #include #include +using gtsam::Vector; +using gtsam::Matrix; + extern "C" { #include }