diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp new file mode 100644 index 000000000..fdf522f73 --- /dev/null +++ b/examples/LocalizationExample.cpp @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file LocalizationExample.cpp + * @brief Simple robot localization example + * @author Frank Dellaert + */ + +// pull in the 2D PoseSLAM domain with all typedefs and helper functions defined +#include + +using namespace std; +using namespace gtsam; + +/** + * Example of a simple 2D localization example + * - Robot poses are facing along the X axis (horizontal, to the right in 2D) + * - The robot moves 2 meters each step + * - We have full odometry between poses + */ +int main(int argc, char** argv) { + + // create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) + pose2SLAM::Graph graph; + + // add a Gaussian prior on pose x_1 + Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin + SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta + graph.addPrior(1, priorMean, priorNoise); // add directly to graph + + // add two odometry factors + Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) + SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta + graph.addOdometry(1, 2, odometry, odometryNoise); + graph.addOdometry(2, 3, odometry, odometryNoise); + + // print + graph.print("\nFactor graph:\n"); + + // create (deliberatly inaccurate) initial estimate + pose2SLAM::Values initialEstimate; + initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2)); + initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); + + initialEstimate.print("\nInitial estimate:\n "); + + // optimize using Levenberg-Marquardt optimization with an ordering from colamd + pose2SLAM::Values result = graph.optimize(initialEstimate); + result.print("\nFinal result:\n "); + + return 0; +} + diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp index 912589080..5039b1145 100644 --- a/examples/PlanarSLAMExample_easy.cpp +++ b/examples/PlanarSLAMExample_easy.cpp @@ -15,64 +15,57 @@ * @author Alex Cunningham */ -#include -#include - // pull in the planar SLAM domain with all typedefs and helper functions defined #include -#include using namespace std; using namespace gtsam; -using namespace 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) + * Example of a simple 2D planar slam example with landmarls + * - The robot and landmarks are on a 2 meter grid + * - Robot poses are facing along the X axis (horizontal, to the right in 2D) + * - The robot moves 2 meters each step + * - We have full odometry between poses * - 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 graph container and add factors to it - Graph graph; + // create the graph (defined in planarSlam.h, derived from NonlinearFactorGraph) + planarSLAM::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(1, prior_measurement, prior_model); // add directly to graph + // add a Gaussian prior on pose x_1 + Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin + SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta + graph.addPrior(1, priorMean, priorNoise); // 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(1, 2, odom_measurement, odom_model); - graph.addOdometry(2, 3, odom_measurement, odom_model); + // add two odometry factors + Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) + SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta + graph.addOdometry(1, 2, odometry, odometryNoise); + graph.addOdometry(2, 3, odometry, odometryNoise); - /* add measurements */ - // general noisemodel for measurements - SharedDiagonal meas_model = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); + // create a noise model for the landmark measurements + SharedDiagonal measurementNoise(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range // create the measurement values - indices are (pose id, landmark id) Rot2 bearing11 = Rot2::fromDegrees(45), - bearing21 = Rot2::fromDegrees(90), - bearing32 = Rot2::fromDegrees(90); + bearing21 = Rot2::fromDegrees(90), + bearing32 = Rot2::fromDegrees(90); double range11 = sqrt(4+4), - range21 = 2.0, - range32 = 2.0; + range21 = 2.0, + range32 = 2.0; - // create bearing/range factors and add them - graph.addBearingRange(1, 1, bearing11, range11, meas_model); - graph.addBearingRange(2, 1, bearing21, range21, meas_model); - graph.addBearingRange(3, 2, bearing32, range32, meas_model); + // add bearing/range factors (created by "addBearingRange") + graph.addBearingRange(1, 1, bearing11, range11, measurementNoise); + graph.addBearingRange(2, 1, bearing21, range21, measurementNoise); + graph.addBearingRange(3, 2, bearing32, range32, measurementNoise); - graph.print("full graph"); + // print + graph.print("Factor graph"); - // initialize to noisy points + // create (deliberatly inaccurate) initial estimate planarSLAM::Values initialEstimate; initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2)); @@ -80,11 +73,11 @@ int main(int argc, char** argv) { initialEstimate.insertPoint(1, Point2(1.8, 2.1)); initialEstimate.insertPoint(2, Point2(4.1, 1.8)); - initialEstimate.print("initial estimate"); + initialEstimate.print("Initial estimate:\n "); // optimize using Levenberg-Marquardt optimization with an ordering from colamd - planarSLAM::Values result = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); - result.print("final result"); + planarSLAM::Values result = graph.optimize(initialEstimate); + result.print("Final result:\n "); return 0; }