diff --git a/.cproject b/.cproject index f54873f0e..a64d177ca 100644 --- a/.cproject +++ b/.cproject @@ -1097,6 +1097,22 @@ true true + + make + + SLAMSelfContained.run + true + true + true + + + make + + PlanarSLAMExample.run + true + true + true + make diff --git a/examples/Makefile.am b/examples/Makefile.am index 7756fa171..32b2c65e5 100644 --- a/examples/Makefile.am +++ b/examples/Makefile.am @@ -11,7 +11,9 @@ sources = check_PROGRAMS = # Examples -noinst_PROGRAMS = SimpleRotation # Optimizes a single nonlinear rotation variable +noinst_PROGRAMS = SimpleRotation # Optimizes a single nonlinear rotation variable +noinst_PROGRAMS += SLAMSelfContained # Solves SLAM example from tutorial with all typedefs in the script +noinst_PROGRAMS += PlanarSLAMExample # Solves SLAM example from tutorial by using planarSLAM #---------------------------------------------------------------------------------------------------- # rules to build local programs diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp new file mode 100644 index 000000000..3529f2b19 --- /dev/null +++ b/examples/PlanarSLAMExample.cpp @@ -0,0 +1,87 @@ +/** + * @file PlanarSLAMExample.cpp + * @brief Simple robotics example from tutorial Figure 1.1 (left) by using the + * pre-built planar SLAM domain + * @author Alex Cunningham + */ + +#include +#include + +// This is should probably be pulled in elsewhere +#include + +// pull in the planar SLAM domain with all typedefs and helper functions defined +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::planarSLAM; + +/** + * In this version of the system we make the following assumptions: + * - All values are axis aligned + * - Robot poses are facing along the X axis (horizontal, to the right in images) + * - We have bearing and range information for measurements + * - We have full odometry for measurements + * - The robot and landmarks are on a grid, moving 2 meters each step + * - Landmarks are 2 meters away from the robot trajectory + */ +int main(int argc, char** argv) { + // create keys for variables + PoseKey x1(1), x2(2), x3(3); + PointKey l1(1), l2(2); + + // create graph container and add factors to it + Graph graph; + + /* add prior */ + // gaussian for prior + SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + Pose2 prior_measurement(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 + SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + Pose2 odom_measurement(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 + SharedDiagonal meas_model = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); + + // create the measurement values - indices are (pose id, landmark id) + Rot2 bearing11 = Rot2::fromDegrees(45), + bearing21 = Rot2::fromDegrees(90), + bearing32 = Rot2::fromDegrees(90); + double range11 = sqrt(4+4), + range21 = 2.0, + range32 = 2.0; + + // 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); + + graph.print("Full Graph"); + + // initialize to noisy points + Config initialEstimate; + initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); + initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); + initialEstimate.insert(l1, Point2(1.8, 2.1)); + initialEstimate.insert(l2, Point2(4.1, 1.8)); + + initialEstimate.print("Initial Estimate"); + + // optimize using Levenburg-Marquadt optimization with an ordering from colamd + Optimizer::shared_config result = Optimizer::optimizeLM(graph, initialEstimate); + + result->print("Final Result"); + + return 0; +} + diff --git a/examples/SLAMSelfContained.cpp b/examples/SLAMSelfContained.cpp new file mode 100644 index 000000000..757af1915 --- /dev/null +++ b/examples/SLAMSelfContained.cpp @@ -0,0 +1,118 @@ +/** + * @file SLAMSelfContained.cpp + * @brief Simple robotics example from tutorial Figure 1.1 (left), with all typedefs + * internal to this script. + * @author Alex Cunningham + */ + +#include +#include + +// for all nonlinear keys +#include + +// for points and poses +#include +#include + +// for modeling measurement uncertainty - all models included here +#include + +// add in headers for specific factors +#include +#include +#include + +// implementations for structures - needed if self-contained, and these should be included last +#include +#include +#include + +// Main typedefs +typedef gtsam::TypedSymbol PoseKey; // Key for poses, with type included +typedef gtsam::TypedSymbol PointKey; // Key for points, with type included +typedef gtsam::LieConfig PoseConfig; // config type for poses +typedef gtsam::LieConfig PointConfig; // config type for points +typedef gtsam::TupleConfig2 Config; // main config with two variable classes +typedef gtsam::NonlinearFactorGraph Graph; // graph structure +typedef gtsam::NonlinearOptimizer Optimizer; // optimization engine for this domain + +using namespace std; +using namespace gtsam; + +/** + * In this version of the system we make the following assumptions: + * - All values are axis aligned + * - Robot poses are facing along the X axis (horizontal, to the right in images) + * - We have bearing and range information for measurements + * - We have full odometry for measurements + * - The robot and landmarks are on a grid, moving 2 meters each step + * - Landmarks are 2 meters away from the robot trajectory + */ +int main(int argc, char** argv) { + // create keys for variables + PoseKey x1(1), x2(2), x3(3); + PointKey l1(1), l2(2); + + // create graph container and add factors to it + Graph graph; + + /* add prior */ + // gaussian for prior + SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin + PriorFactor posePrior(x1, prior_measurement, prior_model); // create the factor + graph.add(posePrior); // add the factor to the graph + + /* add odometry */ + // general noisemodel for odometry + SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) + // create between factors to represent odometry + BetweenFactor odom12(x1, x2, odom_measurement, odom_model); + BetweenFactor odom23(x2, x3, odom_measurement, odom_model); + graph.add(odom12); // add both to graph + graph.add(odom23); + + /* add measurements */ + // general noisemodel for measurements + SharedDiagonal meas_model = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); + + // create the measurement values - indices are (pose id, landmark id) + Rot2 bearing11 = Rot2::fromDegrees(45), + bearing21 = Rot2::fromDegrees(90), + bearing32 = Rot2::fromDegrees(90); + double range11 = sqrt(4+4), + range21 = 2.0, + range32 = 2.0; + + // create bearing/range factors + BearingRangeFactor meas11(x1, l1, bearing11, range11, meas_model); + BearingRangeFactor meas21(x2, l1, bearing21, range21, meas_model); + BearingRangeFactor meas32(x3, l2, bearing32, range32, meas_model); + + // add the factors + graph.add(meas11); + graph.add(meas21); + graph.add(meas32); + + graph.print("Full Graph"); + + // initialize to noisy points + Config initialEstimate; + initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); + initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); + initialEstimate.insert(l1, Point2(1.8, 2.1)); + initialEstimate.insert(l2, Point2(4.1, 1.8)); + + initialEstimate.print("Initial Estimate"); + + // optimize using Levenburg-Marquadt optimization with an ordering from colamd + Optimizer::shared_config result = Optimizer::optimizeLM(graph, initialEstimate); + + result->print("Final Result"); + + return 0; +} + diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index d41aa5ffc..b17022c6a 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -24,7 +24,7 @@ * TODO: make factors independent of Config * TODO: get rid of excessive shared pointer stuff: mostly gone * TODO: make toplevel documentation - * TODO: investigate whether we can just use ints as keys + * TODO: investigate whether we can just use ints as keys: will occur for linear, not nonlinear */ using namespace std; diff --git a/slam/planarSLAM.cpp b/slam/planarSLAM.cpp index 73a155c1b..0c090132a 100644 --- a/slam/planarSLAM.cpp +++ b/slam/planarSLAM.cpp @@ -52,6 +52,12 @@ namespace gtsam { push_back(factor); } + void Graph::addBearingRange(const PoseKey& i, const PointKey& j, const Rot2& z1, + double z2, const SharedGaussian& model) { + sharedFactor factor(new BearingRange(i, j, z1, z2, model)); + push_back(factor); + } + } // planarSLAM } // gtsam diff --git a/slam/planarSLAM.h b/slam/planarSLAM.h index e70eca85c..02921039f 100644 --- a/slam/planarSLAM.h +++ b/slam/planarSLAM.h @@ -47,6 +47,8 @@ namespace gtsam { const SharedGaussian& model); void addRange(const PoseKey& i, const PointKey& j, double z, const SharedGaussian& model); + void addBearingRange(const PoseKey& i, const PointKey& j, + const Rot2& z1, double z2, const SharedGaussian& model); }; // Optimizer