From 564ab1dd63b8df7ae5c40727954f2bd844b3e9b7 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 22 Jul 2012 00:57:24 +0000 Subject: [PATCH 02/68] Wrapped Values::keys() function --- gtsam.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam.h b/gtsam.h index 276a272a6..50743c239 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1056,6 +1056,7 @@ class Values { void insert(size_t j, const gtsam::Value& value); bool exists(size_t j) const; gtsam::Value at(size_t j) const; + gtsam::KeyList keys() const; }; // Actually a FastList From f97869cf202eaf4a5e28c1d90b1f9fe994ae4a15 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 22 Jul 2012 00:57:39 +0000 Subject: [PATCH 03/68] Added plot2DTrajectory matlab function to find all Pose2s in a Values and plot them as a trajectory with optional covariance ellipses --- matlab/plot2DTrajectory.m | 43 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 matlab/plot2DTrajectory.m diff --git a/matlab/plot2DTrajectory.m b/matlab/plot2DTrajectory.m new file mode 100644 index 000000000..82c62bb40 --- /dev/null +++ b/matlab/plot2DTrajectory.m @@ -0,0 +1,43 @@ +function plot2DTrajectory(values, marginals) +%PLOT2DTRAJECTORY Plots the Pose2's in a values, with optional covariances +% Finds all the Pose2 objects in the given Values object and plots them +% in increasing key order, connecting consecutive poses with a line. If +% a Marginals object is given, this function will also plot marginal +% covariance ellipses for each pose. + +import gtsam.* + +haveMarginals = exist('marginals', 'var'); +keys = KeyVector(values.keys); + +% Store poses and covariance matrices +lastIndex = []; +for i = 0:keys.size-1 + key = keys.at(i); + x = values.at(key); + if isa(x, 'gtsam.Pose2') + if ~isempty(lastIndex) + % Draw line from last pose then covariance ellipse on top of + % last pose. + lastKey = keys.at(lastIndex); + lastPose = values.at(lastKey); + plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], 'k*-'); + if haveMarginals + P = marginals.marginalCovariance(lastKey); + plotPose2(lastPose, 'g', P); + end + end + lastIndex = i; + end +end + +% Draw final covariance ellipse +if ~isempty(lastIndex) && haveMarginals + lastKey = keys.at(lastIndex); + lastPose = values.at(lastKey); + P = marginals.marginalCovariance(lastKey); + plotPose2(lastPose, 'g', P); +end + +end + From 9e278b394a0177a79da5070197af76ca5cb221e7 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 22 Jul 2012 00:57:54 +0000 Subject: [PATCH 04/68] Converted first 2 matlab examples to not use slam namespaces --- matlab/examples/LocalizationExample.m | 38 ++++++++++++--------------- matlab/examples/OdometryExample.m | 34 +++++++++++------------- 2 files changed, 32 insertions(+), 40 deletions(-) diff --git a/matlab/examples/LocalizationExample.m b/matlab/examples/LocalizationExample.m index 0a54111b0..1c38bc979 100644 --- a/matlab/examples/LocalizationExample.m +++ b/matlab/examples/LocalizationExample.m @@ -16,51 +16,47 @@ % - The robot is on a grid, moving 2 meters each step %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) -graph = pose2SLAM.Graph; +graph = gtsam.NonlinearFactorGraph; %% Add two odometry factors import gtsam.* odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta -graph.addRelativePose(1, 2, odometry, odometryNoise); -graph.addRelativePose(2, 3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); %% Add three "GPS" measurements import gtsam.* % We use Pose2 Priors here with high variance on theta priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]); -graph.addPosePrior(1, Pose2(0.0, 0.0, 0.0), priorNoise); -graph.addPosePrior(2, Pose2(2.0, 0.0, 0.0), priorNoise); -graph.addPosePrior(3, Pose2(4.0, 0.0, 0.0), priorNoise); +graph.add(PriorFactorPose2(1, Pose2(0.0, 0.0, 0.0), priorNoise)); +graph.add(PriorFactorPose2(2, Pose2(2.0, 0.0, 0.0), priorNoise)); +graph.add(PriorFactorPose2(3, Pose2(4.0, 0.0, 0.0), priorNoise)); %% print graph.print(sprintf('\nFactor graph:\n')); %% Initialize to noisy points import gtsam.* -initialEstimate = pose2SLAM.Values; -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 = Values; +initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); +initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)); initialEstimate.print(sprintf('\nInitial estimate:\n ')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd import gtsam.* -result = graph.optimize(initialEstimate,1); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); result.print(sprintf('\nFinal result:\n ')); -%% Plot Covariance Ellipses +%% Plot trajectory and covariance ellipses import gtsam.* cla; -X=result.poses(); -plot(X(:,1),X(:,2),'k*-'); hold on -marginals = graph.marginals(result); -P={}; -for i=1:result.size() - pose_i = result.pose(i); - P{i}=marginals.marginalCovariance(i); - plotPose2(pose_i,'g',P{i}) -end +hold on; + +plot2DTrajectory(result, Marginals(graph, result)); + axis([-0.6 4.8 -1 1]) axis equal view(2) diff --git a/matlab/examples/OdometryExample.m b/matlab/examples/OdometryExample.m index 297702b51..16323a616 100644 --- a/matlab/examples/OdometryExample.m +++ b/matlab/examples/OdometryExample.m @@ -16,48 +16,44 @@ % - The robot is on a grid, moving 2 meters each step %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) -graph = pose2SLAM.Graph; +graph = gtsam.NonlinearFactorGraph; %% Add a Gaussian prior on pose x_1 import gtsam.* priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta -graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph %% Add two odometry factors import gtsam.* odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta -graph.addRelativePose(1, 2, odometry, odometryNoise); -graph.addRelativePose(2, 3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); %% print graph.print(sprintf('\nFactor graph:\n')); %% Initialize to noisy points import gtsam.* -initialEstimate = pose2SLAM.Values; -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 = Values; +initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); +initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)); initialEstimate.print(sprintf('\nInitial estimate:\n ')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,1); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); result.print(sprintf('\nFinal result:\n ')); -%% Plot Covariance Ellipses +%% Plot trajectory and covariance ellipses import gtsam.* cla; -X=result.poses(); -plot(X(:,1),X(:,2),'k*-'); hold on -marginals = graph.marginals(result); -P={}; -for i=1:result.size() - pose_i = result.pose(i); - P{i}=marginals.marginalCovariance(i); - plotPose2(pose_i,'g',P{i}) -end +hold on; + +plot2DTrajectory(result, Marginals(graph, result)); + axis([-0.6 4.8 -1 1]) axis equal view(2) From d259320aed71112eaff6712f9ed7ad3ab35736f5 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 22 Jul 2012 04:35:12 +0000 Subject: [PATCH 05/68] Combined the two versions of Pose2SLAMExample into a single example without SLAM namespaces --- examples/Pose2SLAMExample.cpp | 133 +++++++++++++++++++------ examples/Pose2SLAMExample_advanced.cpp | 82 --------------- 2 files changed, 102 insertions(+), 113 deletions(-) delete mode 100644 examples/Pose2SLAMExample_advanced.cpp diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 0777301ea..4e660a768 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -11,55 +11,126 @@ /** * @file Pose2SLAMExample.cpp - * @brief A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h + * @brief A 2D Pose SLAM example * @date Oct 21, 2010 * @author Yong Dian Jian */ -// pull in the Pose2 SLAM domain with all typedefs and helper functions defined -#include -#include +/** + * A simple 2D pose slam example + * - The robot moves in a 2 meter square + * - The robot moves 2 meters each step, turning 90 degrees after each step + * - The robot initially faces along the X axis (horizontal, to the right in 2D) + * - We have full odometry between pose + * - We have a loop closure constraint when the robot returns to the first position + */ + +// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent +// the robot positions +#include +#include + +// Each variable in the system (poses) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use simple integer keys +#include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use Between factors for the relative motion described by odometry measurements. +// We will also use a Between Factor to encode the loop closure constraint +// Also, we will initialize the robot at the origin using a Prior factor. +#include +#include + +// When the factors are created, we will add them to a Factor Graph. As the factors we are using +// are nonlinear factors, we will need a Nonlinear Factor Graph. +#include + +// Finally, once all of the factors have been added to our factor graph, we will want to +// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. +// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the +// a Gauss-Newton solver +#include + +// Once the optimized values have been calculated, we can also calculate the marginal covariance +// of desired variables +#include + +// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the +// nonlinear functions around an initial linearization point, then solve the linear system +// to update the linearization point. This happens repeatedly until the solver converges +// to a consistent set of variable values. This requires us to specify an initial guess +// for each variable, held in a Values container. +#include + using namespace std; using namespace gtsam; -using namespace gtsam::noiseModel; int main(int argc, char** argv) { - // 1. Create graph container and add factors to it - pose2SLAM::Graph graph; + // 1. Create a factor graph container and add factors to it + NonlinearFactorGraph graph; - // 2a. Add Gaussian prior - Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - graph.addPosePrior(1, priorMean, priorNoise); + // 2a. Add a prior on the first pose, setting it to the origin + // A prior factor consists of a mean and a noise model (covariance matrix) + Pose2 prior(0.0, 0.0, 0.0); // prior at origin + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + graph.add(PriorFactor(1, prior, priorNoise)); // 2b. Add odometry factors - SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); - graph.addRelativePose(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addRelativePose(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addRelativePose(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + // For simplicity, we will use the same noise model for each odometry factor + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + // Create odometry (Between) factors between consecutive poses + graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - // 2c. Add pose constraint - SharedDiagonal model = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addRelativePose(5, 2, Pose2(2.0, 0.0, M_PI_2), model); + // 2c. Add the loop closure constraint + // This factor encodes the fact that we have returned to the same pose. In real systems, + // these constraints may be identified in many ways, such as appearance-based techniques + // with camera images. + // We will use another Between Factor to enforce this constraint, with the distance set to zero, + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + graph.add(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); + graph.print("\nFactor Graph:\n"); // print - // print - graph.print("\nFactor graph:\n"); // 3. Create the data structure to hold the initialEstimate estimate to the solution - 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, M_PI_2)); - initialEstimate.insertPose(4, Pose2(4.0, 2.0, M_PI)); - initialEstimate.insertPose(5, Pose2(2.1, 2.1, -M_PI_2)); - initialEstimate.print("\nInitial estimate:\n"); + // For illustrative purposes, these have been deliberately set to incorrect values + Values initialEstimate; + initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1)); + initialEstimate.insert(3, Pose2(2.1, 1.9, 2.8)); + initialEstimate.insert(4, Pose2(-.3, 2.5, 4.2)); + initialEstimate.insert(5, Pose2(0.1,-0.7, 5.8)); + initialEstimate.print("\nInitial Estimate:\n"); // print - // 4. Single Step Optimization using Levenberg-Marquardt - pose2SLAM::Values result = graph.optimize(initialEstimate); - result.print("\nFinal result:\n"); + // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer + // The optimizer accepts an optional set of configuration parameters, + // controlling things like convergence criteria, the type of linear + // system solver to use, and the amount of information displayed during + // optimization. We will set a few parameters as a demonstration. + GaussNewtonParams parameters; + // Stop iterating once the change in error between steps is less than this value + parameters.relativeErrorTol = 1e-5; + // Do not perform more than N iteration steps + parameters.maxIterations = 100; + // Create the optimizer ... + GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters); + // ... and optimize + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + + // 5. Calculate and print marginal covariances for all variables + Marginals marginals(graph, result); + cout << "Pose 1 covariance:\n" << marginals.marginalCovariance(1) << endl; + cout << "Pose 2 covariance:\n" << marginals.marginalCovariance(2) << endl; + cout << "Pose 3 covariance:\n" << marginals.marginalCovariance(3) << endl; + cout << "Pose 4 covariance:\n" << marginals.marginalCovariance(4) << endl; + cout << "Pose 5 covariance:\n" << marginals.marginalCovariance(5) << endl; return 0; } diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp deleted file mode 100644 index b5fcddfcf..000000000 --- a/examples/Pose2SLAMExample_advanced.cpp +++ /dev/null @@ -1,82 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Pose2SLAMExample_advanced.cpp - * @brief Simple Pose2SLAM Example using - * pre-built pose2SLAM domain - * @author Chris Beall - */ - -// pull in the Pose2 SLAM domain with all typedefs and helper functions defined -#include -#include -#include -#include -#include - -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace gtsam::noiseModel; - -int main(int argc, char** argv) { - /* 1. create graph container and add factors to it */ - pose2SLAM::Graph graph; - - /* 2.a add prior */ - Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.addPosePrior(1, priorMean, priorNoise); // add directly to graph - - /* 2.b add odometry */ - SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - graph.addRelativePose(1, 2, odometry, odometryNoise); - graph.addRelativePose(2, 3, odometry, odometryNoise); - graph.print("full graph"); - - /* 3. Create the data structure to hold the initial estimate to the solution - * initialize to noisy points */ - pose2SLAM::Values initial; - initial.insertPose(1, Pose2(0.5, 0.0, 0.2)); - initial.insertPose(2, Pose2(2.3, 0.1, -0.2)); - initial.insertPose(3, Pose2(4.1, 0.1, 0.1)); - initial.print("initial estimate"); - - /* 4.2.1 Alternatively, you can go through the process step by step - * Choose an ordering */ - Ordering ordering = *graph.orderingCOLAMD(initial); - - /* 4.2.2 set up solver and optimize */ - LevenbergMarquardtParams params; - params.absoluteErrorTol = 1e-15; - params.relativeErrorTol = 1e-15; - params.ordering = ordering; - LevenbergMarquardtOptimizer optimizer(graph, initial, params); - - pose2SLAM::Values result = optimizer.optimize(); - result.print("final result"); - - /* Get covariances */ - Marginals marginals(graph, result, Marginals::CHOLESKY); - Matrix covariance1 = marginals.marginalCovariance(1); - Matrix covariance2 = marginals.marginalCovariance(2); - - print(covariance1, "Covariance1"); - print(covariance2, "Covariance2"); - - return 0; -} - From 5da5adb2f1ced42d8010756c4bc1a5c21d7a43f6 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 22 Jul 2012 04:36:40 +0000 Subject: [PATCH 06/68] Combined the PlanarSLAM examples into a single example without SLAM namespaces --- examples/PlanarSLAMExample.cpp | 138 +++++++++++++------ examples/PlanarSLAMExample_selfcontained.cpp | 137 ------------------ 2 files changed, 94 insertions(+), 181 deletions(-) delete mode 100644 examples/PlanarSLAMExample_selfcontained.cpp diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index f05ffa9b2..c6f6b636f 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -11,22 +11,12 @@ /** * @file PlanarSLAMExample.cpp - * @brief Simple robotics example using the pre-built planar SLAM domain + * @brief Simple robotics example using odometry measurements and bearing-range (laser) measurements * @author Alex Cunningham */ -// pull in the planar SLAM domain with all typedefs and helper functions defined -#include - -// we will use Symbol keys -#include - -using namespace std; -using namespace gtsam; -using namespace gtsam::noiseModel; - /** - * Example of a simple 2D planar slam example with landmarls + * A simple 2D planar slam example with landmarks * - 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 @@ -34,29 +24,74 @@ using namespace gtsam::noiseModel; * - We have bearing and range information for measurements * - Landmarks are 2 meters away from the robot trajectory */ + +// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent +// the robot positions and Point2 variables (x, y) to represent the landmark coordinates. +#include +#include + +// Each variable in the system (poses and landmarks) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use Symbols +#include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use a RangeBearing factor for the range-bearing measurements to identified +// landmarks, and Between factors for the relative motion described by odometry measurements. +// Also, we will initialize the robot at the origin using a Prior factor. +#include +#include +#include + +// When the factors are created, we will add them to a Factor Graph. As the factors we are using +// are nonlinear factors, we will need a Nonlinear Factor Graph. +#include + +// Finally, once all of the factors have been added to our factor graph, we will want to +// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. +// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the +// common Levenberg-Marquardt solver +#include + +// Once the optimized values have been calculated, we can also calculate the marginal covariance +// of desired variables +#include + +// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the +// nonlinear functions around an initial linearization point, then solve the linear system +// to update the linearization point. This happens repeatedly until the solver converges +// to a consistent set of variable values. This requires us to specify an initial guess +// for each variable, held in a Values container. +#include + + +using namespace std; +using namespace gtsam; + int main(int argc, char** argv) { - // create the graph (defined in planarSlam.h, derived from NonlinearFactorGraph) - planarSLAM::Graph graph; + // Create a factor graph + NonlinearFactorGraph graph; - // Create some keys - static Symbol i1('x',1), i2('x',2), i3('x',3); - static Symbol j1('l',1), j2('l',2); + // Create the keys we need for this simple example + static Symbol x1('x',1), x2('x',2), x3('x',3); + static Symbol l1('l',1), l2('l',2); - // add a Gaussian prior on pose x_1 - Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin - SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.addPosePrior(i1, priorMean, priorNoise); // add directly to graph + // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) + Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta + graph.add(PriorFactor(x1, prior, priorNoise)); // add directly to graph - // add two odometry factors + // 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 = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - graph.addRelativePose(i1, i2, odometry, odometryNoise); - graph.addRelativePose(i2, i3, odometry, odometryNoise); + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta + graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); + graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); + // Add Range-Bearing measurements to two different landmarks // create a noise model for the landmark measurements - SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range - + noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(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), @@ -65,27 +100,42 @@ int main(int argc, char** argv) { range21 = 2.0, range32 = 2.0; - // add bearing/range factors (created by "addBearingRange") - graph.addBearingRange(i1, j1, bearing11, range11, measurementNoise); - graph.addBearingRange(i2, j1, bearing21, range21, measurementNoise); - graph.addBearingRange(i3, j2, bearing32, range32, measurementNoise); + // Add Bearing-Range factors + graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); + graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); + graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); - // print - graph.print("Factor graph"); + // Print + graph.print("Factor Graph:\n"); - // create (deliberatly inaccurate) initial estimate - planarSLAM::Values initialEstimate; - initialEstimate.insertPose(i1, Pose2(0.5, 0.0, 0.2)); - initialEstimate.insertPose(i2, Pose2(2.3, 0.1,-0.2)); - initialEstimate.insertPose(i3, Pose2(4.1, 0.1, 0.1)); - initialEstimate.insertPoint(j1, Point2(1.8, 2.1)); - initialEstimate.insertPoint(j2, Point2(4.1, 1.8)); + // Create (deliberately inaccurate) initial estimate + Values 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:\n "); + // Print + initialEstimate.print("Initial Estimate:\n"); - // optimize using Levenberg-Marquardt optimization with an ordering from colamd - planarSLAM::Values result = graph.optimize(initialEstimate); - result.print("Final result:\n "); + // Optimize using Levenberg-Marquardt optimization. The optimizer + // accepts an optional set of configuration parameters, controlling + // things like convergence criteria, the type of linear system solver + // to use, and the amount of information displayed during optimization. + // Here we will use the default set of parameters. See the + // documentation for the full set of parameters. + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + + // Calculate and print marginal covariances for all variables + Marginals marginals(graph, result); + print(marginals.marginalCovariance(x1), "x1 covariance"); + print(marginals.marginalCovariance(x2), "x2 covariance"); + print(marginals.marginalCovariance(x3), "x3 covariance"); + print(marginals.marginalCovariance(l1), "l1 covariance"); + print(marginals.marginalCovariance(l2), "l2 covariance"); return 0; } diff --git a/examples/PlanarSLAMExample_selfcontained.cpp b/examples/PlanarSLAMExample_selfcontained.cpp deleted file mode 100644 index 7c8ccc134..000000000 --- a/examples/PlanarSLAMExample_selfcontained.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 PlanarSLAMExample_selfcontained.cpp - * @brief Simple robotics example with all typedefs internal to this script. - * @author Alex Cunningham - */ - -// add in headers for specific factors -#include -#include -#include - -// for all nonlinear keys -#include - -// implementations for structures - needed if self-contained, and these should be included last -#include -#include -#include - -// for modeling measurement uncertainty - all models included here -#include - -// for points and poses -#include -#include - -#include -#include - -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 - Symbol i1('x',1), i2('x',2), i3('x',3); - Symbol j1('l',1), j2('l',2); - - // create graph container and add factors to it - NonlinearFactorGraph graph; - - /* add prior */ - // gaussian for prior - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - PriorFactor posePrior(i1, priorMean, priorNoise); // create the factor - graph.add(posePrior); // add the factor to the graph - - /* add odometry */ - // general noisemodel for odometry - SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - Pose2 odometry(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(i1, i2, odometry, odometryNoise); - BetweenFactor odom23(i2, i3, odometry, odometryNoise); - graph.add(odom12); // add both to graph - graph.add(odom23); - - /* add measurements */ - // general noisemodel for measurements - SharedDiagonal measurementNoise = 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.0+4.0), - range21 = 2.0, - range32 = 2.0; - - // create bearing/range factors - BearingRangeFactor meas11(i1, j1, bearing11, range11, measurementNoise); - BearingRangeFactor meas21(i2, j1, bearing21, range21, measurementNoise); - BearingRangeFactor meas32(i3, j2, bearing32, range32, measurementNoise); - - // add the factors - graph.add(meas11); - graph.add(meas21); - graph.add(meas32); - - graph.print("Full Graph"); - - // initialize to noisy points - Values initial; - initial.insert(i1, Pose2(0.5, 0.0, 0.2)); - initial.insert(i2, Pose2(2.3, 0.1,-0.2)); - initial.insert(i3, Pose2(4.1, 0.1, 0.1)); - initial.insert(j1, Point2(1.8, 2.1)); - initial.insert(j2, Point2(4.1, 1.8)); - - initial.print("initial estimate"); - - // optimize using Levenberg-Marquardt optimization with an ordering from colamd - - // first using sequential elimination - LevenbergMarquardtParams lmParams; - lmParams.linearSolverType = LevenbergMarquardtParams::SEQUENTIAL_CHOLESKY; - Values resultSequential = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize(); - resultSequential.print("final result (solved with a sequential solver)"); - - // then using multifrontal, advanced interface - // Note that we keep the original optimizer object so we can use the COLAMD - // ordering it computes. - LevenbergMarquardtOptimizer optimizer(graph, initial); - Values resultMultifrontal = optimizer.optimize(); - resultMultifrontal.print("final result (solved with a multifrontal solver)"); - - // Print marginals covariances for all variables - Marginals marginals(graph, resultMultifrontal, Marginals::CHOLESKY); - print(marginals.marginalCovariance(i1), "i1 covariance"); - print(marginals.marginalCovariance(i2), "i2 covariance"); - print(marginals.marginalCovariance(i3), "i3 covariance"); - print(marginals.marginalCovariance(j1), "j1 covariance"); - print(marginals.marginalCovariance(j2), "j2 covariance"); - - return 0; -} - From 67e2d832feafea9413c5f6d783b6f3d04cf87203 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 22 Jul 2012 04:52:01 +0000 Subject: [PATCH 07/68] Updated the VisualSLAM examples, removing the SLAM namespaces --- examples/VisualISAMExample.cpp | 184 ++++++++++++++++++++------------- examples/VisualSLAMData.h | 89 ---------------- examples/VisualSLAMExample.cpp | 134 ++++++++++++++++++------ 3 files changed, 216 insertions(+), 191 deletions(-) delete mode 100644 examples/VisualSLAMData.h diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index 25e993bca..eab860db0 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -11,99 +11,139 @@ /** * @file VisualISAMExample.cpp - * @brief An ISAM example for synthesis sequence, single camera + * @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset + * This version uses iSAM to solve the problem incrementally * @author Duy-Nguyen Ta */ +/** + * A structure-from-motion example with landmarks + * - The landmarks form a 10 meter cube + * - The robot rotates around the landmarks, always facing towards the cube + */ + +// As this is a full 3D problem, we will use Pose3 variables to represent the camera +// positions and Point3 variables (x, y, z) to represent the landmark coordinates. +// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +// We will also need a camera object to hold calibration information and perform projections. +#include +#include +#include +#include + +// Each variable in the system (poses and landmarks) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use Symbols #include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use Projection factors to model the camera's landmark observations. +// Also, we will initialize the robot at some location using a Prior factor. +#include +#include + +// We want to use iSAM to solve the structure-from-motion problem incrementally, so +// include iSAM here #include -#include -#include -#include "VisualSLAMData.h" + +// iSAM requires as input a set set of new factors to be added stored in a factor graph, +// and initial guesses for any new variables used in the added factors +#include +#include + +#include using namespace std; using namespace gtsam; -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::L; - /* ************************************************************************* */ int main(int argc, char* argv[]) { - VisualSLAMExampleData data = VisualSLAMExampleData::generate(); + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - /* 1. Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates */ - int relinearizeInterval = 3; - NonlinearISAM isam(relinearizeInterval); + // Define the camera observation noise model + noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v - /* 2. At each frame (poseId) with new camera pose and set of associated measurements, - * create a graph of new factors and update ISAM */ + // Create the set of ground-truth landmarks + std::vector points; + points.push_back(gtsam::Point3(10.0,10.0,10.0)); + points.push_back(gtsam::Point3(-10.0,10.0,10.0)); + points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); + points.push_back(gtsam::Point3(10.0,-10.0,10.0)); + points.push_back(gtsam::Point3(10.0,10.0,-10.0)); + points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); + points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); + points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); - // Store the current best estimate from ISAM - Values currentEstimate; - - // First two frames: - // Add factors and initial values for the first two poses and landmarks then update ISAM. - // Note: measurements from the first pose only are not enough to update ISAM: - // the system is underconstrained. - { - visualSLAM::Graph newFactors; - - // First pose with prior factor - newFactors.addPosePrior(X(0), data.poses[0], data.noiseX); - - // Second pose with odometry measurement - newFactors.addRelativePose(X(0), X(1), data.odometry, data.noiseX); - - // Visual measurements at both poses - for (size_t i=0; i<2; ++i) { - for (size_t j=0; j poses; + double radius = 30.0; + int i = 0; + double theta = 0.0; + gtsam::Point3 up(0,0,1); + gtsam::Point3 target(0,0,0); + for(; i < 8; ++i, theta += 2*M_PI/8) { + gtsam::Point3 position = Point3(radius*cos(theta), radius*sin(theta), 0.0); + gtsam::SimpleCamera camera = SimpleCamera::Lookat(position, target, up); + poses.push_back(camera.pose()); } - // Subsequent frames: Add new odometry and measurement factors and initial values, - // then update ISAM at each frame - for (size_t i=2; i(X(i-1))*data.odometry); + // Create a Factor Graph and Values to hold the new data + NonlinearFactorGraph graph; + Values initialEstimate; - // update ISAM - isam.update(newFactors, initials); - currentEstimate = isam.estimate(); - cout << "****************************************************" << endl; - cout << "Frame " << i << ": " << endl; - currentEstimate.print("Current estimate: "); + // Loop over the different poses, adding the observations to iSAM incrementally + for (size_t i = 0; i < poses.size(); ++i) { + + // Add factors for each landmark observation + for (size_t j = 0; j < points.size(); ++j) { + SimpleCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); + graph.add(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + } + + // Add an initial guess for the current pose + // Intentionally initialize the variables off from the ground truth + initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + + // If this is the first iteration, add a prior on the first pose to set the coordinate frame + // and a prior on the first landmark to set the scale + // Also, as iSAM solves incrementally, we must wait until each is observed at least twice before + // adding it to iSAM. + if( i == 0) { + // Add a prior on pose x0 + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + + // Add a prior on landmark l0 + noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); + graph.add(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + + // Add initial guesses to all observed landmarks + // Intentionally initialize the variables off from the ground truth + for (size_t j = 0; j < points.size(); ++j) + initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + + } else { + // Update iSAM with the new factors + isam.update(graph, initialEstimate); + Values currentEstimate = isam.estimate(); + cout << "****************************************************" << endl; + cout << "Frame " << i << ": " << endl; + currentEstimate.print("Current estimate: "); + + // Clear the factor graph and values for the next iteration + graph.resize(0); + initialEstimate.clear(); + } } return 0; } /* ************************************************************************* */ - diff --git a/examples/VisualSLAMData.h b/examples/VisualSLAMData.h deleted file mode 100644 index 28fa3040c..000000000 --- a/examples/VisualSLAMData.h +++ /dev/null @@ -1,89 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 VisualSLAMData.cpp - * @brief Generate ground-truth simulated data for VisualSLAM examples - * @author Duy-Nguyen Ta - */ - -#pragma once - -#include -#include -#include - -/* ************************************************************************* */ -/** - * Simulated data for the visual SLAM examples: - * - 8 Landmarks: (10,10,10) (-10,10,10) (-10,-10,10) (10,-10,10) - * (10,10,-10) (-10,10,-10) (-10,-10,-10) (10,-10,-10) - * - n 90-deg-FoV cameras with the same calibration parameters: - * f = 50.0, Image: 100x100, center: 50.0, 50.0 - * and ground-truth poses on a circle around the landmarks looking at the world's origin: - * Rot3(-sin(theta), 0, -cos(theta), - * cos(theta), 0, -sin(theta), - * 0, -1, 0 ), - * Point3(r*cos(theta), r*sin(theta), 0.0) - * (theta += 2*pi/N) - * - Measurement noise: 1 pix sigma - */ -struct VisualSLAMExampleData { - gtsam::shared_ptrK sK; // camera calibration parameters - std::vector poses; // ground-truth camera poses - gtsam::Pose3 odometry; // ground-truth odometry between 2 consecutive poses (simulated data for iSAM) - std::vector points; // ground-truth landmarks - std::map > z; // 2D measurements of landmarks in each camera frame - gtsam::SharedDiagonal noiseZ; // measurement noise (noiseModel::Isotropic::Sigma(2, 5.0f)); - gtsam::SharedDiagonal noiseX; // noise for camera poses - gtsam::SharedDiagonal noiseL; // noise for landmarks - - static const VisualSLAMExampleData generate() { - VisualSLAMExampleData data; - // Landmarks (ground truth) - data.points.push_back(gtsam::Point3(10.0,10.0,10.0)); - data.points.push_back(gtsam::Point3(-10.0,10.0,10.0)); - data.points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); - data.points.push_back(gtsam::Point3(10.0,-10.0,10.0)); - data.points.push_back(gtsam::Point3(10.0,10.0,-10.0)); - data.points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); - data.points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); - data.points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); - - // Camera calibration parameters - data.sK = gtsam::shared_ptrK(new gtsam::Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - - // n camera poses - int n = 8; - double theta = 0.0; - double r = 30.0; - for (int i=0; isample()))*/); // you can add noise as desired - } - } - data.noiseX = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector_(6, 0.001, 0.001, 0.001, 0.1, 0.1, 0.1)); - data.noiseL = gtsam::noiseModel::Isotropic::Sigma(3, 0.1); - - return data; - } -}; diff --git a/examples/VisualSLAMExample.cpp b/examples/VisualSLAMExample.cpp index 1fc343203..8aed051eb 100644 --- a/examples/VisualSLAMExample.cpp +++ b/examples/VisualSLAMExample.cpp @@ -15,49 +15,123 @@ * @author Duy-Nguyen Ta */ -#include +/** + * A structure-from-motion example with landmarks + * - The landmarks form a 10 meter cube + * - The robot rotates around the landmarks, always facing towards the cube + */ + +// As this is a full 3D problem, we will use Pose3 variables to represent the camera +// positions and Point3 variables (x, y, z) to represent the landmark coordinates. +// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +// We will also need a camera object to hold calibration information and perform projections. +#include +#include +#include +#include + +// Each variable in the system (poses and landmarks) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use Symbols #include -#include -#include -#include "VisualSLAMData.h" + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use Projection factors to model the camera's landmark observations. +// Also, we will initialize the robot at some location using a Prior factor. +#include +#include + +// When the factors are created, we will add them to a Factor Graph. As the factors we are using +// are nonlinear factors, we will need a Nonlinear Factor Graph. +#include + +// Finally, once all of the factors have been added to our factor graph, we will want to +// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. +// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a +// trust-region method known as Powell's Degleg +#include + +// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the +// nonlinear functions around an initial linearization point, then solve the linear system +// to update the linearization point. This happens repeatedly until the solver converges +// to a consistent set of variable values. This requires us to specify an initial guess +// for each variable, held in a Values container. +#include + +#include using namespace std; using namespace gtsam; -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::L; - /* ************************************************************************* */ int main(int argc, char* argv[]) { - VisualSLAMExampleData data = VisualSLAMExampleData::generate(); + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - /* 1. Create graph *///using the 2D measurements (features) and the calibration data - visualSLAM::Graph graph; + // Define the camera observation noise model + noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v - /* 2. Add factors to the graph */ - // 2a. Measurement factors - for (size_t i=0; i points; + points.push_back(gtsam::Point3(10.0,10.0,10.0)); + points.push_back(gtsam::Point3(-10.0,10.0,10.0)); + points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); + points.push_back(gtsam::Point3(10.0,-10.0,10.0)); + points.push_back(gtsam::Point3(10.0,10.0,-10.0)); + points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); + points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); + points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); + + // Create the set of ground-truth poses + std::vector poses; + double radius = 30.0; + int i = 0; + double theta = 0.0; + gtsam::Point3 up(0,0,1); + gtsam::Point3 target(0,0,0); + for(; i < 8; ++i, theta += 2*M_PI/8) { + gtsam::Point3 position = Point3(radius*cos(theta), radius*sin(theta), 0.0); + gtsam::SimpleCamera camera = SimpleCamera::Lookat(position, target, up); + poses.push_back(camera.pose()); } - // 2b. Prior factor for the first pose and point to constraint the system - graph.addPosePrior(X(0), data.poses[0], data.noiseX); - graph.addPointPrior(L(0), data.points[0], data.noiseL); - /* 3. Initial estimates for variable nodes, simulated by Gaussian noises */ - Values initial; - for (size_t i=0; isample())*/); // you can add noise if you want - for (size_t j=0; jsample())*/); // you can add noise if you want - initial.print("Intial Estimates: "); + // Create a factor graph + NonlinearFactorGraph graph; - /* 4. Optimize the graph and print results */ - visualSLAM::Values result = GaussNewtonOptimizer(graph, initial).optimize(); -// visualSLAM::Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); - result.print("Final results: "); + // Add a prior on pose x1. This indirectly specifies where the origin is. + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph + + // Simulated measurements from each camera pose, adding them to the factor graph + for (size_t i = 0; i < poses.size(); ++i) { + for (size_t j = 0; j < points.size(); ++j) { + SimpleCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); + graph.add(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + } + } + + // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained + // Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance + // between the first camera and the first landmark. All other landmark positions are interpreted using this scale. + noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); + graph.add(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.print("Factor Graph:\n"); + + // Create the data structure to hold the initialEstimate estimate to the solution + // Intentionally initialize the variables off from the ground truth + Values initialEstimate; + for (size_t i = 0; i < poses.size(); ++i) + initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + for (size_t j = 0; j < points.size(); ++j) + initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + initialEstimate.print("Initial Estimates:\n"); + + /* Optimize the graph and print results */ + Values result = DoglegOptimizer(graph, initialEstimate).optimize(); + result.print("Final results:\n"); return 0; } From 45d1c4f0ed2a73b685ae3a174d6d241fb7e2a565 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 22 Jul 2012 05:21:32 +0000 Subject: [PATCH 08/68] Removed SLAM namespaces from OdometryExample --- examples/OdometryExample.cpp | 117 ++++++++++++++++++++++------------- 1 file changed, 73 insertions(+), 44 deletions(-) diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index 36ad5f228..f0874bedb 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -15,62 +15,91 @@ * @author Frank Dellaert */ -// pull in the 2D PoseSLAM domain with all typedefs and helper functions defined -#include - -// include this for marginals -#include - -#include -#include - -using namespace std; -using namespace gtsam; -using namespace gtsam::noiseModel; - /** * 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 */ + +// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent +// the robot positions +#include +#include + +// Each variable in the system (poses) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use symbols +#include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use Between factors for the relative motion described by odometry measurements. +// Also, we will initialize the robot at the origin using a Prior factor. +#include +#include + +// When the factors are created, we will add them to a Factor Graph. As the factors we are using +// are nonlinear factors, we will need a Nonlinear Factor Graph. +#include + +// Finally, once all of the factors have been added to our factor graph, we will want to +// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. +// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the +// Levenberg-Marquardt solver +#include + +// Once the optimized values have been calculated, we can also calculate the marginal covariance +// of desired variables +#include + +// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the +// nonlinear functions around an initial linearization point, then solve the linear system +// to update the linearization point. This happens repeatedly until the solver converges +// to a consistent set of variable values. This requires us to specify an initial guess +// for each variable, held in a Values container. +#include + + +using namespace std; +using namespace gtsam; + int main(int argc, char** argv) { - // create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) - pose2SLAM::Graph graph; + // Create a factor graph container + NonlinearFactorGraph graph; - // add a Gaussian prior on pose x_1 - Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin - SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.addPosePrior(1, priorMean, priorNoise); // add directly to graph + // Add a prior on the first pose, setting it to the origin + // A prior factor consists of a mean and a noise model (covariance matrix) + Pose2 prior(0.0, 0.0, 0.0); // prior at origin + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + graph.add(PriorFactor(Symbol('x', 1), prior, priorNoise)); - // 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 = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - graph.addRelativePose(1, 2, odometry, odometryNoise); - graph.addRelativePose(2, 3, odometry, odometryNoise); + // Add odometry factors + // For simplicity, we will use the same noise model for each odometry factor + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + // Create odometry (Between) factors between consecutive poses + graph.add(BetweenFactor(Symbol('x', 1), Symbol('x', 2), Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.add(BetweenFactor(Symbol('x', 2), Symbol('x', 3), Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.print("\nFactor Graph:\n"); // print - // print - graph.print("\nFactor graph:\n"); + // Create the data structure to hold the initialEstimate estimate to the solution + // For illustrative purposes, these have been deliberately set to incorrect values + Values initialEstimate; + initialEstimate.insert(Symbol('x', 1), Pose2(0.5, 0.0, 0.2)); + initialEstimate.insert(Symbol('x', 2), Pose2(2.3, 0.1, -0.2)); + initialEstimate.insert(Symbol('x', 3), Pose2(4.1, 0.1, 0.1)); + initialEstimate.print("\nInitial Estimate:\n"); // print - // 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 + Values result = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); + result.print("Final Result:\n"); - // optimize using Levenberg-Marquardt optimization with an ordering from colamd - pose2SLAM::Values result = graph.optimize(initialEstimate); - result.print("\nFinal result:\n "); + // Calculate and print marginal covariances for all variables + Marginals marginals(graph, result); + cout << "Pose 1 covariance:\n" << marginals.marginalCovariance(Symbol('x', 1)) << endl; + cout << "Pose 2 covariance:\n" << marginals.marginalCovariance(Symbol('x', 2)) << endl; + cout << "Pose 3 covariance:\n" << marginals.marginalCovariance(Symbol('x', 3)) << endl; - // Query the marginals - cout.precision(2); - Marginals marginals = graph.marginals(result); - cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl; - cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl; - cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl; - - return 0; + return 0; } - From e3a6282ff8170f4435dc7d6ed3a5de3f65141a3c Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 22 Jul 2012 05:34:22 +0000 Subject: [PATCH 09/68] Removed SLAM namespaces from SPCG example. Still needs better documentation by someone who knows what SPCG is. --- examples/Pose2SLAMwSPCG.cpp | 130 +++++++++++++++++++++++------------- 1 file changed, 83 insertions(+), 47 deletions(-) diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index e2f3801f7..38a2e18e4 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -16,79 +16,115 @@ * @date June 2, 2012 */ +/** + * A simple 2D pose slam example solved using a Conjugate-Gradient method + * - The robot moves in a 2 meter square + * - The robot moves 2 meters each step, turning 90 degrees after each step + * - The robot initially faces along the X axis (horizontal, to the right in 2D) + * - We have full odometry between pose + * - We have a loop closure constraint when the robot returns to the first position + */ + +// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent +// the robot positions +#include +#include + +// Each variable in the system (poses) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use simple integer keys +#include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use Between factors for the relative motion described by odometry measurements. +// We will also use a Between Factor to encode the loop closure constraint +// Also, we will initialize the robot at the origin using a Prior factor. +#include +#include + +// When the factors are created, we will add them to a Factor Graph. As the factors we are using +// are nonlinear factors, we will need a Nonlinear Factor Graph. +#include + +// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the +// nonlinear functions around an initial linearization point, then solve the linear system +// to update the linearization point. This happens repeatedly until the solver converges +// to a consistent set of variable values. This requires us to specify an initial guess +// for each variable, held in a Values container. +#include + +// ??? #include #include #include -#include -#include -#include + using namespace std; using namespace gtsam; -using namespace gtsam::noiseModel; -/* ************************************************************************* */ -int main(void) { +int main(int argc, char** argv) { - // 1. Create graph container and add factors to it - pose2SLAM::Graph graph ; + // 1. Create a factor graph container and add factors to it + NonlinearFactorGraph graph; - // 2a. Add Gaussian prior - Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - graph.addPosePrior(1, priorMean, priorNoise); + // 2a. Add a prior on the first pose, setting it to the origin + // A prior factor consists of a mean and a noise model (covariance matrix) + Pose2 prior(0.0, 0.0, 0.0); // prior at origin + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + graph.add(PriorFactor(1, prior, priorNoise)); // 2b. Add odometry factors - SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); - graph.addRelativePose(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addRelativePose(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addRelativePose(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + // For simplicity, we will use the same noise model for each odometry factor + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + // Create odometry (Between) factors between consecutive poses + graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - // 2c. Add pose constraint - SharedDiagonal constraintUncertainty = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addRelativePose(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); + // 2c. Add the loop closure constraint + // This factor encodes the fact that we have returned to the same pose. In real systems, + // these constraints may be identified in many ways, such as appearance-based techniques + // with camera images. + // We will use another Between Factor to enforce this constraint, with the distance set to zero, + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + graph.add(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); + graph.print("\nFactor Graph:\n"); // print - // print - graph.print("\nFactor graph:\n"); - // 3. Create the data structure to hold the initialEstimate estinmate to the solution - pose2SLAM::Values initialEstimate; - Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1); - Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2); - Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insertPose(3, x3); - Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insertPose(4, x4); - Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insertPose(5, x5); - initialEstimate.print("\nInitial estimate:\n "); - cout << "initial error = " << graph.error(initialEstimate) << endl ; + // 3. Create the data structure to hold the initialEstimate estimate to the solution + // For illustrative purposes, these have been deliberately set to incorrect values + Values initialEstimate; + initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1)); + initialEstimate.insert(3, Pose2(2.1, 1.9, 2.8)); + initialEstimate.insert(4, Pose2(-.3, 2.5, 4.2)); + initialEstimate.insert(5, Pose2(0.1,-0.7, 5.8)); + initialEstimate.print("\nInitial Estimate:\n"); // print // 4. Single Step Optimization using Levenberg-Marquardt - LevenbergMarquardtParams param; - param.verbosity = NonlinearOptimizerParams::ERROR; - param.verbosityLM = LevenbergMarquardtParams::LAMBDA; - param.linearSolverType = SuccessiveLinearizationParams::CG; + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::ERROR; + parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; + parameters.linearSolverType = SuccessiveLinearizationParams::CG; { - param.iterativeParams = boost::make_shared(); - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, param); + parameters.iterativeParams = boost::make_shared(); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); Values result = optimizer.optimize(); - result.print("\nFinal result:\n"); + result.print("Final Result:\n"); cout << "simple spcg solver final error = " << graph.error(result) << endl; } { - param.iterativeParams = boost::make_shared(); - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, param); + parameters.iterativeParams = boost::make_shared(); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); Values result = optimizer.optimize(); - result.print("\nFinal result:\n"); + result.print("Final Result:\n"); cout << "subgraph solver final error = " << graph.error(result) << endl; } - { - Values result = graph.optimizeSPCG(initialEstimate); - result.print("\nFinal result:\n"); - } - - return 0 ; + return 0; } From ff522a73c05c1f9a76b7bde2c3017e9a1e1eecb0 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 22 Jul 2012 05:43:44 +0000 Subject: [PATCH 10/68] Updated documentation on SimpleRotation example --- examples/SimpleRotation.cpp | 54 ++++++++++++++++++++++++------------- 1 file changed, 35 insertions(+), 19 deletions(-) diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 603d345bf..cff9d754e 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -17,20 +17,41 @@ * @author Alex Cunningham */ -#include -#include -#include + /** + * This example will perform a relatively trivial optimization on + * a single variable with a single factor. + */ + +// In this example, a 2D rotation will be used as the variable of interest #include -#include + +// Each variable in the system (poses) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use symbols #include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// We will apply a simple prior on the rotation +#include + +// When the factors are created, we will add them to a Factor Graph. As the factors we are using +// are nonlinear factors, we will need a Nonlinear Factor Graph. #include + +// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the +// nonlinear functions around an initial linearization point, then solve the linear system +// to update the linearization point. This happens repeatedly until the solver converges +// to a consistent set of variable values. This requires us to specify an initial guess +// for each variable, held in a Values container. +#include + +// Finally, once all of the factors have been added to our factor graph, we will want to +// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. +// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the +// standard Levenberg-Marquardt solver #include -/* - * TODO: make factors independent of RotValues - * TODO: make toplevel documentation - * TODO: Clean up nonlinear optimization API - */ using namespace std; using namespace gtsam; @@ -40,12 +61,7 @@ const double degree = M_PI / 180; int main() { /** - * This example will perform a relatively trivial optimization on - * a single variable with a single factor. - */ - - /** - * Step 1: create a factor on to express a unary constraint + * Step 1: Create a factor to express a unary constraint * The "prior" in this case is the measurement from a sensor, * with a model of the noise on the measurement. * @@ -60,12 +76,12 @@ int main() { */ Rot2 prior = Rot2::fromAngle(30 * degree); prior.print("goal angle"); - SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree); Symbol key('x',1); PriorFactor factor(key, prior, model); /** - * Step 2: create a graph container and add the factor to it + * Step 2: Create a graph container and add the factor to it * Before optimizing, all factors need to be added to a Graph container, * which provides the necessary top-level functionality for defining a * system of constraints. @@ -78,7 +94,7 @@ int main() { graph.print("full graph"); /** - * Step 3: create an initial estimate + * Step 3: Create an initial estimate * An initial estimate of the solution for the system is necessary to * start optimization. This system state is the "RotValues" structure, * which is similar in structure to a STL map, in that it maps @@ -98,7 +114,7 @@ int main() { initial.print("initial estimate"); /** - * Step 4: optimize + * Step 4: Optimize * After formulating the problem with a graph of constraints * and an initial estimate, executing optimization is as simple * as calling a general optimization function with the graph and From 71c6458e961e49a9b6032bb804ceddcb0f9647d9 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 22 Jul 2012 14:57:34 +0000 Subject: [PATCH 11/68] Updated ExtendedKalmanFilter and relkated example, removing the need for the 'inl' header --- examples/easyPoint2KalmanFilter.cpp | 2 +- gtsam/nonlinear/ExtendedKalmanFilter.h | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index 2b4d2476a..6d75e9149 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -21,7 +21,7 @@ * @author Stephen Williams */ -#include +#include #include #include #include diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index 353439d98..ebca4899c 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -90,3 +90,5 @@ namespace gtsam { }; } // namespace + +#include From 5d46beed8653961440416b584fc4304fe7709e74 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 22 Jul 2012 14:59:22 +0000 Subject: [PATCH 12/68] Put the relinearization interval back to 3 for the iSAM example --- examples/VisualISAMExample.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index eab860db0..832a2ab83 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -91,7 +91,7 @@ int main(int argc, char* argv[]) { } // Create a NonlinearISAM object which will relinearize and reorder the variables every "relinearizeInterval" updates - int relinearizeInterval = 1; + int relinearizeInterval = 3; NonlinearISAM isam(relinearizeInterval); // Create a Factor Graph and Values to hold the new data From 412ffa38c7c3328ada14dffa0beee70741ebf98d Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 22 Jul 2012 15:15:12 +0000 Subject: [PATCH 13/68] Created a iSAM2 version of the Visual SLAM example --- examples/VisualISAM2Example.cpp | 159 ++++++++++++++++++++++++++++++++ 1 file changed, 159 insertions(+) create mode 100644 examples/VisualISAM2Example.cpp diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp new file mode 100644 index 000000000..1485da1e4 --- /dev/null +++ b/examples/VisualISAM2Example.cpp @@ -0,0 +1,159 @@ +/* ---------------------------------------------------------------------------- + + * 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 VisualISAM2Example.cpp + * @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset + * This version uses iSAM2 to solve the problem incrementally + * @author Duy-Nguyen Ta + */ + +/** + * A structure-from-motion example with landmarks + * - The landmarks form a 10 meter cube + * - The robot rotates around the landmarks, always facing towards the cube + */ + +// As this is a full 3D problem, we will use Pose3 variables to represent the camera +// positions and Point3 variables (x, y, z) to represent the landmark coordinates. +// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +// We will also need a camera object to hold calibration information and perform projections. +#include +#include +#include +#include + +// Each variable in the system (poses and landmarks) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use Symbols +#include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use Projection factors to model the camera's landmark observations. +// Also, we will initialize the robot at some location using a Prior factor. +#include +#include + +// We want to use iSAM2 to solve the structure-from-motion problem incrementally, so +// include iSAM2 here +#include + +// iSAM2 requires as input a set set of new factors to be added stored in a factor graph, +// and initial guesses for any new variables used in the added factors +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + // Define the camera observation noise model + noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Create the set of ground-truth landmarks + std::vector points; + points.push_back(gtsam::Point3(10.0,10.0,10.0)); + points.push_back(gtsam::Point3(-10.0,10.0,10.0)); + points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); + points.push_back(gtsam::Point3(10.0,-10.0,10.0)); + points.push_back(gtsam::Point3(10.0,10.0,-10.0)); + points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); + points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); + points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); + + // Create the set of ground-truth poses + std::vector poses; + double radius = 30.0; + int i = 0; + double theta = 0.0; + gtsam::Point3 up(0,0,1); + gtsam::Point3 target(0,0,0); + for(; i < 8; ++i, theta += 2*M_PI/8) { + gtsam::Point3 position = Point3(radius*cos(theta), radius*sin(theta), 0.0); + gtsam::SimpleCamera camera = SimpleCamera::Lookat(position, target, up); + poses.push_back(camera.pose()); + } + + // Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization + // and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter + // structure is available that allows the user to set various properties, such as the relinearization threshold + // and type of linear solver. For this example, we we set the relinearization threshold small so the iSAM2 result + // will approach the batch result. + ISAM2Params parameters; + parameters.relinearizeThreshold = 0.01; + parameters.relinearizeSkip = 1; + ISAM2 isam(parameters); + + // Create a Factor Graph and Values to hold the new data + NonlinearFactorGraph graph; + Values initialEstimate; + + // Loop over the different poses, adding the observations to iSAM incrementally + for (size_t i = 0; i < poses.size(); ++i) { + + // Add factors for each landmark observation + for (size_t j = 0; j < points.size(); ++j) { + SimpleCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); + graph.add(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + } + + // Add an initial guess for the current pose + // Intentionally initialize the variables off from the ground truth + initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + + // If this is the first iteration, add a prior on the first pose to set the coordinate frame + // and a prior on the first landmark to set the scale + // Also, as iSAM solves incrementally, we must wait until each is observed at least twice before + // adding it to iSAM. + if( i == 0) { + // Add a prior on pose x0 + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + + // Add a prior on landmark l0 + noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); + graph.add(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + + // Add initial guesses to all observed landmarks + // Intentionally initialize the variables off from the ground truth + for (size_t j = 0; j < points.size(); ++j) + initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + + } else { + // Update iSAM with the new factors + isam.update(graph, initialEstimate); + // Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver. + // If accuracy is desired at the expense of time, update(*) can be called additional times + // to perform multiple optimizer iterations every step. + isam.update(); + Values currentEstimate = isam.calculateEstimate(); + cout << "****************************************************" << endl; + cout << "Frame " << i << ": " << endl; + currentEstimate.print("Current estimate: "); + + // Clear the factor graph and values for the next iteration + graph.resize(0); + initialEstimate.clear(); + } + } + + return 0; +} +/* ************************************************************************* */ From 61b82c9109aa2ad5fe8db43cd1866586b6607f84 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 22 Jul 2012 15:47:07 +0000 Subject: [PATCH 14/68] Tiny code formatting change --- examples/Pose2SLAMExample.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 4e660a768..bf05cc729 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -83,7 +83,7 @@ int main(int argc, char** argv) { // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.add(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.add(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); From f865a9e5513238132bd08fb2faa777195a53b41a Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 22 Jul 2012 16:03:42 +0000 Subject: [PATCH 15/68] Removed SLAM namespaces from Localization Example --- examples/LocalizationExample.cpp | 191 ++++++++++++++++++++++--------- 1 file changed, 135 insertions(+), 56 deletions(-) diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 21fb36860..c9d232df8 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -15,87 +15,166 @@ * @author Frank Dellaert */ -// pull in the 2D PoseSLAM domain with all typedefs and helper functions defined -#include +/** + * A simple 2D pose slam example with "GPS" measurements + * - The robot moves forward 2 meter each iteration + * - The robot initially faces along the X axis (horizontal, to the right in 2D) + * - We have full odometry between pose + * - We have "GPS-like" measurements implemented with a custom factor + */ -// include this for marginals +// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent +// the robot positions +#include + +// Each variable in the system (poses) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use simple integer keys +#include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use Between factors for the relative motion described by odometry measurements. +// Because we have global measurements in the form of "GPS-like" measurements, we don't +// actually need to provide an initial position prior in this example. We will create our +// custom factor shortly. +#include + +// When the factors are created, we will add them to a Factor Graph. As the factors we are using +// are nonlinear factors, we will need a Nonlinear Factor Graph. +#include + +// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the +// nonlinear functions around an initial linearization point, then solve the linear system +// to update the linearization point. This happens repeatedly until the solver converges +// to a consistent set of variable values. This requires us to specify an initial guess +// for each variable, held in a Values container. +#include + +// Finally, once all of the factors have been added to our factor graph, we will want to +// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. +// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the +// standard Levenberg-Marquardt solver #include + +// Once the optimized values have been calculated, we can also calculate the marginal covariance +// of desired variables #include -#include -#include using namespace std; using namespace gtsam; -using namespace gtsam::noiseModel; -/** - * UnaryFactor - * Example on how to create a GPS-like factor on position alone. - */ + +// Before we begin the example, we must create a custom unary factor to implement a +// "GPS-like" functionality. Because standard GPS measurements provide information +// only on the position, and not on the orientation, we cannot use a simple prior to +// properly model this measurement. +// +// The factor will be a unary factor, affect only a single system variable. It will +// also use a standard Gaussian noise model. Hence, we will derive our new factor from +// the NoiseModelFactor1. +#include class UnaryFactor: public NoiseModelFactor1 { - double mx_, my_; ///< X and Y measurements + + // The factor will hold a measurement consisting of an (X,Y) location + Point2 measurement_; public: + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + // The constructor requires the variable key, the (X, Y) measurement value, and the noise model UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model): - NoiseModelFactor1(model, j), mx_(x), my_(y) {} + NoiseModelFactor1(model, j), measurement_(x, y) {} virtual ~UnaryFactor() {} - Vector evaluateError(const Pose2& q, - boost::optional H = boost::none) const + // By using the NoiseModelFactor base classes, the only two function that must be overridden. + // The first is the 'evaluateError' function. This function implements the desired measurement + // function, returning a vector of errors when evaluated at the provided variable value. It + // must also calculate the Jacobians for this measurement function, if requested. + Vector evaluateError(const Pose2& pose, boost::optional H = boost::none) const { - if (H) (*H) = Matrix_(2,3, 1.0,0.0,0.0, 0.0,1.0,0.0); - return Vector_(2, q.x() - mx_, q.y() - my_); + // The measurement function for a GPS-like measurement is simple: + // error_x = pose.x - measurement.x + // error_y = pose.y - measurement.y + // Consequently, the Jacobians are: + // [ derror_x/dx derror_x/dy derror_x/dtheta ] = [1 0 0] + // [ derror_y/dx derror_y/dy derror_y/dtheta ] = [0 1 0] + if (H) + (*H) = Matrix_(2,3, 1.0,0.0,0.0, 0.0,1.0,0.0); + + return Vector_(2, pose.x() - measurement_.x(), pose.y() - measurement_.y()); } + + // The second is a 'clone' function that allows the factor to be copied. Under most + // circumstances, the following code that employs the default copy constructor should + // work fine. + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); } + + // Additionally, custom factors should really provide specific implementations of + // 'equals' to ensure proper operation will all GTSAM functionality, and a custom + // 'print' function, if desired. + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const UnaryFactor* e = dynamic_cast (&expected); + return e != NULL && NoiseModelFactor1::equals(*e, tol) && this->measurement_.equals(e->measurement_, tol); + } + + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "UnaryFactor(" << keyFormatter(this->key()) << ")\n"; + measurement_.print(" measurement: "); + this->noiseModel_->print(" noise model: "); + } + }; -/** - * Example of a more complex 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 - * - We have unary measurement factors at eacht time step - */ + + int main(int argc, char** argv) { - // create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) - pose2SLAM::Graph graph; + // 1. Create a factor graph container and add factors to it + NonlinearFactorGraph 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 = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - graph.addRelativePose(1, 2, odometry, odometryNoise); - graph.addRelativePose(2, 3, odometry, odometryNoise); + // 2a. Add odometry factors + // For simplicity, we will use the same noise model for each odometry factor + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + // Create odometry (Between) factors between consecutive poses + graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)); - // add unary measurement factors, like GPS, on all three poses - SharedDiagonal noiseModel = Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // 10cm std on x,y - graph.push_back(boost::make_shared(1, 0, 0, noiseModel)); - graph.push_back(boost::make_shared(2, 2, 0, noiseModel)); - graph.push_back(boost::make_shared(3, 4, 0, noiseModel)); + // 2b. Add "GPS-like" measurements + // We will use our custom UnaryFactor for this. + noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // 10cm std on x,y + graph.add(UnaryFactor(1, 0.0, 0.0, unaryNoise)); + graph.add(UnaryFactor(3, 4.0, 0.0, unaryNoise)); + graph.print("\nFactor Graph:\n"); // print - // print - graph.print("\nFactor graph:\n"); + // 3. Create the data structure to hold the initialEstimate estimate to the solution + // For illustrative purposes, these have been deliberately set to incorrect values + Values initialEstimate; + initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)); + initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)); + initialEstimate.print("\nInitial Estimate:\n"); // print - // 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 "); + // 4. Optimize using Levenberg-Marquardt optimization. The optimizer + // accepts an optional set of configuration parameters, controlling + // things like convergence criteria, the type of linear system solver + // to use, and the amount of information displayed during optimization. + // Here we will use the default set of parameters. See the + // documentation for the full set of parameters. + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); - // use an explicit Optimizer object - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); - pose2SLAM::Values result = optimizer.optimize(); - result.print("\nFinal result:\n "); + // 5. Calculate and print marginal covariances for all variables + Marginals marginals(graph, result); + cout << "Pose 1 covariance:\n" << marginals.marginalCovariance(1) << endl; + cout << "Pose 2 covariance:\n" << marginals.marginalCovariance(2) << endl; + cout << "Pose 3 covariance:\n" << marginals.marginalCovariance(3) << endl; - // Query the marginals - Marginals marginals(graph, result); - cout.precision(2); - cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl; - cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl; - cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl; - - return 0; + return 0; } - From f4c022c35a8860485be2fac7000f61adf6884169 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 23 Jul 2012 15:33:39 +0000 Subject: [PATCH 16/68] Added required header files to gtsam.h --- gtsam.h | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/gtsam.h b/gtsam.h index 50743c239..13364b210 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1124,6 +1124,7 @@ class KeyVector { void push_back(size_t key) const; }; +#include class Marginals { Marginals(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& solution); @@ -1302,18 +1303,28 @@ class ISAM2 { //************************************************************************* // Nonlinear factor types //************************************************************************* +#include +#include +#include +#include +#include +#include + +#include template virtual class PriorFactor : gtsam::NonlinearFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); }; +#include template virtual class BetweenFactor : gtsam::NonlinearFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); }; +#include template virtual class NonlinearEquality : gtsam::NonlinearFactor { // Constructor - forces exact evaluation @@ -1323,6 +1334,7 @@ virtual class NonlinearEquality : gtsam::NonlinearFactor { }; +#include template virtual class RangeFactor : gtsam::NonlinearFactor { RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); @@ -1337,6 +1349,8 @@ typedef gtsam::RangeFactor RangeFactorSimple typedef gtsam::RangeFactor RangeFactorCalibratedCamera; typedef gtsam::RangeFactor RangeFactorSimpleCamera; + +#include template virtual class BearingFactor : gtsam::NonlinearFactor { BearingFactor(size_t key1, size_t key2, const ROT& measured, const gtsam::noiseModel::Base* noiseModel); From d397139fa9b77a2b6e2b233ef133b114e38d8156 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 23 Jul 2012 19:20:53 +0000 Subject: [PATCH 17/68] Fixed dimensions bug in Marginals and added unit test --- gtsam/inference/GenericSequentialSolver-inl.h | 2 +- gtsam/nonlinear/Marginals.cpp | 13 +++-- tests/testMarginals.cpp | 55 +++++++++++++++++++ 3 files changed, 64 insertions(+), 6 deletions(-) diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index 53dee3dcd..368aae831 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -88,7 +88,7 @@ namespace gtsam { GenericSequentialSolver::jointFactorGraph( const std::vector& js, Eliminate function) const { - // Compute a COLAMD permutation with the marginal variable constrained to the end. + // Compute a COLAMD permutation with the marginal variables constrained to the end. Permutation::shared_ptr permutation(inference::PermutationCOLAMD(*structure_, js)); Permutation::shared_ptr permutationInverse(permutation->inverse()); diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index f7f1aa00f..52c507374 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -110,11 +110,11 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector& variab return JointMarginal(info, dims, indices); } else { - // Convert keys to linear indices + // Obtain requested variables as ordered indices vector indices(variables.size()); for(size_t i=0; i& variab jointFG = *GaussianSequentialSolver(graph_, true).jointFactorGraph(indices); } - // Conversion from variable keys to position in factor graph variables, + // Build map from variable keys to position in factor graph variables, // which are sorted in index order. Ordering variableConversion; { + // First build map from index to key FastMap usedIndices; for(size_t i=0; i Index_Key; BOOST_FOREACH(const Index_Key& index_key, usedIndices) { @@ -145,8 +147,9 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector& variab // Get dimensions from factor graph std::vector dims(indices.size(), 0); - for(size_t i = 0; i < variables.size(); ++i) - dims[i] = values_.at(variables[i]).dim(); + BOOST_FOREACH(Key key, variables) { + dims[variableConversion[key]] = values_.at(key).dim(); + } // Get information matrix Matrix augmentedInfo = jointFG.denseHessian(); diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index da7157a1e..c9bd43b27 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -180,7 +180,62 @@ TEST(Marginals, planarSLAMmarginals) { EXPECT(assert_equal(expectedx1, Matrix(joint_l2x1(x1,x1)), 1e-6)); } +/* ************************************************************************* */ +TEST(Marginals, order) { + NonlinearFactorGraph fg; + fg.add(PriorFactor(0, Pose2(), noiseModel::Unit::Create(3))); + fg.add(BetweenFactor(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3))); + fg.add(BetweenFactor(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3))); + fg.add(BetweenFactor(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3))); + Values vals; + vals.insert(0, Pose2()); + vals.insert(1, Pose2(1,0,0)); + vals.insert(2, Pose2(2,0,0)); + vals.insert(3, Pose2(3,0,0)); + + vals.insert(100, Point2(0,1)); + vals.insert(101, Point2(1,1)); + + fg.add(BearingRangeFactor(0, 100, + vals.at(0).bearing(vals.at(100)), + vals.at(0).range(vals.at(100)), noiseModel::Unit::Create(2))); + fg.add(BearingRangeFactor(0, 101, + vals.at(0).bearing(vals.at(101)), + vals.at(0).range(vals.at(101)), noiseModel::Unit::Create(2))); + + fg.add(BearingRangeFactor(1, 100, + vals.at(1).bearing(vals.at(100)), + vals.at(1).range(vals.at(100)), noiseModel::Unit::Create(2))); + fg.add(BearingRangeFactor(1, 101, + vals.at(1).bearing(vals.at(101)), + vals.at(1).range(vals.at(101)), noiseModel::Unit::Create(2))); + + fg.add(BearingRangeFactor(2, 100, + vals.at(2).bearing(vals.at(100)), + vals.at(2).range(vals.at(100)), noiseModel::Unit::Create(2))); + fg.add(BearingRangeFactor(2, 101, + vals.at(2).bearing(vals.at(101)), + vals.at(2).range(vals.at(101)), noiseModel::Unit::Create(2))); + + fg.add(BearingRangeFactor(3, 100, + vals.at(3).bearing(vals.at(100)), + vals.at(3).range(vals.at(100)), noiseModel::Unit::Create(2))); + fg.add(BearingRangeFactor(3, 101, + vals.at(3).bearing(vals.at(101)), + vals.at(3).range(vals.at(101)), noiseModel::Unit::Create(2))); + + Marginals marginals(fg, vals); + FastVector keys(fg.keys()); + JointMarginal joint = marginals.jointMarginalCovariance(keys); + + LONGS_EQUAL(3, joint(0,0).rows()); + LONGS_EQUAL(3, joint(1,1).rows()); + LONGS_EQUAL(3, joint(2,2).rows()); + LONGS_EQUAL(3, joint(3,3).rows()); + LONGS_EQUAL(2, joint(100,100).rows()); + LONGS_EQUAL(2, joint(101,101).rows()); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} From 5b584c3b73b0413e7b6082bb6a0aa1a373ca6029 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 23 Jul 2012 19:20:56 +0000 Subject: [PATCH 18/68] Added SuccessiveLinearizationParams.ordering and BearingRangeFactor in matlab interface --- gtsam.h | 15 +++++++++++---- .../nonlinear/SuccessiveLinearizationOptimizer.h | 16 ++++++---------- gtsam/slam/BearingFactor.h | 4 ++-- gtsam/slam/BearingRangeFactor.h | 4 ++-- 4 files changed, 21 insertions(+), 18 deletions(-) diff --git a/gtsam.h b/gtsam.h index 08c4ed9d3..7cb46fc55 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1167,6 +1167,7 @@ virtual class NonlinearOptimizerParams { virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams { SuccessiveLinearizationParams(); + void setOrdering(const gtsam::Ordering& ordering); bool isMultifrontal() const; bool isSequential() const; bool isCholmod() const; @@ -1357,16 +1358,22 @@ typedef gtsam::RangeFactor RangeFactorSimple typedef gtsam::RangeFactor RangeFactorCalibratedCamera; typedef gtsam::RangeFactor RangeFactorSimpleCamera; - -#include -template +template virtual class BearingFactor : gtsam::NonlinearFactor { - BearingFactor(size_t key1, size_t key2, const ROT& measured, const gtsam::noiseModel::Base* noiseModel); + BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel); }; typedef gtsam::BearingFactor BearingFactor2D; +template +virtual class BearingRangeFactor : gtsam::NonlinearFactor { + BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel); +}; + +typedef gtsam::BearingRangeFactor BearingRangeFactor2D; + + #include template virtual class GenericProjectionFactor : gtsam::NonlinearFactor { diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h index 7033613ac..2556134c0 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h @@ -78,20 +78,16 @@ public: } inline bool isMultifrontal() const { - return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR); - } + return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR); } inline bool isSequential() const { - return (linearSolverType == SEQUENTIAL_CHOLESKY) || (linearSolverType == SEQUENTIAL_QR); - } + return (linearSolverType == SEQUENTIAL_CHOLESKY) || (linearSolverType == SEQUENTIAL_QR); } - inline bool isCholmod() const { - return (linearSolverType == CHOLMOD); - } + inline bool isCholmod() const { return (linearSolverType == CHOLMOD); } - inline bool isCG() const { - return (linearSolverType == CG); - } + inline bool isCG() const { return (linearSolverType == CG); } + + void setOrdering(const Ordering& ordering) { this->ordering = ordering; } GaussianFactorGraph::Eliminate getEliminationFunction() { switch (linearSolverType) { diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index 62e46d53e..326491e8c 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -25,12 +25,12 @@ namespace gtsam { /** * Binary factor for a bearing measurement */ - template + template class BearingFactor: public NoiseModelFactor2 { private: typedef POSE Pose; - typedef ROT Rot; + typedef ROTATION Rot; typedef POINT Point; typedef BearingFactor This; diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 933333fdf..bf5b6b3a5 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -27,12 +27,12 @@ namespace gtsam { /** * Binary factor for a bearing measurement */ - template + template class BearingRangeFactor: public NoiseModelFactor2 { private: typedef POSE Pose; - typedef typename POSE::Rotation Rot; + typedef ROTATION Rot; typedef POINT Point; typedef BearingRangeFactor This; From 080dd7d57ccc4890af3171e0a7d50cf2d99cbe17 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 23 Jul 2012 19:21:00 +0000 Subject: [PATCH 19/68] Updated some of the matlab utility functions --- matlab/covarianceEllipse3D.m | 2 +- matlab/examples/findExampleDataFile.m | 19 ++++++++ matlab/load2D.m | 10 +++-- matlab/plot2DPoints.m | 34 +++++++++++++++ matlab/plot2DTrajectory.m | 17 ++++++-- matlab/plot3DTrajectory.m | 63 +++++++++++++++++++++------ matlab/plotPose3.m | 26 ++++++----- 7 files changed, 137 insertions(+), 34 deletions(-) create mode 100644 matlab/examples/findExampleDataFile.m create mode 100644 matlab/plot2DPoints.m diff --git a/matlab/covarianceEllipse3D.m b/matlab/covarianceEllipse3D.m index 6d6ed9127..d3ef1f8a7 100644 --- a/matlab/covarianceEllipse3D.m +++ b/matlab/covarianceEllipse3D.m @@ -11,7 +11,7 @@ k = 11.82; radii = k*sqrt(diag(s)); % generate data for "unrotated" ellipsoid -[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3)); +[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8); % rotate data with orientation matrix U and center M data = kron(e(:,1),xc) + kron(e(:,2),yc) + kron(e(:,3),zc); diff --git a/matlab/examples/findExampleDataFile.m b/matlab/examples/findExampleDataFile.m new file mode 100644 index 000000000..7719dfcfa --- /dev/null +++ b/matlab/examples/findExampleDataFile.m @@ -0,0 +1,19 @@ +function datafile = findExampleDataFile(datasetName) +%FINDEXAMPLEDATAFILE Find a dataset in the examples folder + +[ myPath, ~, ~ ] = fileparts(mfilename('fullpath')); +searchPath = { ... + fullfile(myPath, [ '../../examples/Data/' datasetName ]) ... + fullfile(myPath, [ 'Data/' datasetName ]) }; +datafile = []; +for path = searchPath + if exist(path{:}, 'file') + datafile = path{:}; + end +end +if isempty(datafile) + error([ 'Could not find example data file ' datasetName ]); +end + +end + diff --git a/matlab/load2D.m b/matlab/load2D.m index 31e96818f..c4b2e105d 100644 --- a/matlab/load2D.m +++ b/matlab/load2D.m @@ -2,6 +2,8 @@ function [graph,initial] = load2D(filename,model) % load2D: read TORO pose graph % cannot read noise model from file yet, uses specified model +import gtsam.* + fid = fopen(filename); if fid < 0 error(['load2D: Cannot open file ' filename]); @@ -13,17 +15,17 @@ fclose(fid); lines=columns{1}; % loop over lines and add vertices -graph = pose2SLAM.Graph; -initial = pose2SLAM.Values; +graph = NonlinearFactorGraph; +initial = Values; n=size(lines,1); for i=1:n line_i=lines{i}; if strcmp('VERTEX2',line_i(1:7)) v = textscan(line_i,'%s %d %f %f %f',1); - initial.insertPose(v{2}, gtsam.Pose2(v{3}, v{4}, v{5})); + initial.insert(v{2}, Pose2(v{3}, v{4}, v{5})); elseif strcmp('EDGE2',line_i(1:5)) e = textscan(line_i,'%s %d %d %f %f %f',1); - graph.addRelativePose(e{2}, e{3}, gtsam.Pose2(e{4}, e{5}, e{6}), model); + graph.add(BetweenFactorPose2(e{2}, e{3}, Pose2(e{4}, e{5}, e{6}), model)); end end diff --git a/matlab/plot2DPoints.m b/matlab/plot2DPoints.m new file mode 100644 index 000000000..0065b55c4 --- /dev/null +++ b/matlab/plot2DPoints.m @@ -0,0 +1,34 @@ +function plot2DPoints(values, marginals) +%PLOT2DPOINTS Plots the Point2's in a values, with optional covariances +% Finds all the Point2 objects in the given Values object and plots them. +% If a Marginals object is given, this function will also plot marginal +% covariance ellipses for each point. + +import gtsam.* + +haveMarginals = exist('marginals', 'var'); +keys = KeyVector(values.keys); + +holdstate = ishold; +hold on + +% Plot points and covariance matrices +for i = 0:keys.size-1 + key = keys.at(i); + p = values.at(key); + if isa(p, 'gtsam.Point2') + if haveMarginals + P = marginals.marginalCovariance(key); + plotPoint2(p, 'g', P); + else + plotPoint2(p, 'g'); + end + end +end + +if ~holdstate + hold off +end + +end + diff --git a/matlab/plot2DTrajectory.m b/matlab/plot2DTrajectory.m index 82c62bb40..fcbfe4b92 100644 --- a/matlab/plot2DTrajectory.m +++ b/matlab/plot2DTrajectory.m @@ -1,4 +1,4 @@ -function plot2DTrajectory(values, marginals) +function plot2DTrajectory(values, linespec, marginals) %PLOT2DTRAJECTORY Plots the Pose2's in a values, with optional covariances % Finds all the Pose2 objects in the given Values object and plots them % in increasing key order, connecting consecutive poses with a line. If @@ -7,10 +7,17 @@ function plot2DTrajectory(values, marginals) import gtsam.* +if ~exist('linespec', 'var') || isempty(linespec) + linespec = 'k*-'; +end + haveMarginals = exist('marginals', 'var'); keys = KeyVector(values.keys); -% Store poses and covariance matrices +holdstate = ishold; +hold on + +% Plot poses and covariance matrices lastIndex = []; for i = 0:keys.size-1 key = keys.at(i); @@ -21,7 +28,7 @@ for i = 0:keys.size-1 % last pose. lastKey = keys.at(lastIndex); lastPose = values.at(lastKey); - plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], 'k*-'); + plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], linespec); if haveMarginals P = marginals.marginalCovariance(lastKey); plotPose2(lastPose, 'g', P); @@ -39,5 +46,9 @@ if ~isempty(lastIndex) && haveMarginals plotPose2(lastPose, 'g', P); end +if ~holdstate + hold off +end + end diff --git a/matlab/plot3DTrajectory.m b/matlab/plot3DTrajectory.m index 83d3d76e2..771d567fa 100644 --- a/matlab/plot3DTrajectory.m +++ b/matlab/plot3DTrajectory.m @@ -1,18 +1,53 @@ -function plot3DTrajectory(values,style,frames,scale) +function plot3DTrajectory(values,linespec,frames,scale,marginals) % plot3DTrajectory -if nargin<3,frames=false;end -if nargin<4,scale=0;end +if ~exist('scale','var') || isempty(scale), scale=1; end +if ~exist('frames','var'), scale=[]; end -T=values.translations() -plot3(T(:,1),T(:,2),T(:,3),style); hold on -if frames - N=values.size; - for i=0:N-1 - pose = values.pose(i); - t = pose.translation; - R = pose.rotation.matrix; - quiver3(t.x,t.y,t.z,R(1,1),R(2,1),R(3,1),scale,'r'); - quiver3(t.x,t.y,t.z,R(1,2),R(2,2),R(3,2),scale,'g'); - quiver3(t.x,t.y,t.z,R(1,3),R(2,3),R(3,3),scale,'b'); +import gtsam.* + +haveMarginals = exist('marginals', 'var'); +keys = KeyVector(values.keys); + +holdstate = ishold; +hold on + +% Plot poses and covariance matrices +lastIndex = []; +for i = 0:keys.size-1 + key = keys.at(i); + x = values.at(key); + if isa(x, 'gtsam.Pose3') + if ~isempty(lastIndex) + % Draw line from last pose then covariance ellipse on top of + % last pose. + lastKey = keys.at(lastIndex); + lastPose = values.at(lastKey); + plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec); + if haveMarginals + P = marginals.marginalCovariance(lastKey); + else + P = []; + end + plotPose3(lastPose, P, scale); + end + lastIndex = i; end +end + +% Draw final pose +if ~isempty(lastIndex) + lastKey = keys.at(lastIndex); + lastPose = values.at(lastKey); + if haveMarginals + P = marginals.marginalCovariance(lastKey); + else + P = []; + end + plotPose3(lastPose, P, scale); +end + +if ~holdstate + hold off +end + end \ No newline at end of file diff --git a/matlab/plotPose3.m b/matlab/plotPose3.m index 262d1dfc1..956b231be 100644 --- a/matlab/plotPose3.m +++ b/matlab/plotPose3.m @@ -6,18 +6,20 @@ if nargin<3,axisLength=0.1;end gRp = pose.rotation().matrix(); % rotation from pose to global C = pose.translation().vector(); -% draw the camera axes -xAxis = C+gRp(:,1)*axisLength; -L = [C xAxis]'; -line(L(:,1),L(:,2),L(:,3),'Color','r'); - -yAxis = C+gRp(:,2)*axisLength; -L = [C yAxis]'; -line(L(:,1),L(:,2),L(:,3),'Color','g'); - -zAxis = C+gRp(:,3)*axisLength; -L = [C zAxis]'; -line(L(:,1),L(:,2),L(:,3),'Color','b'); +if ~isempty(axisLength) + % draw the camera axes + xAxis = C+gRp(:,1)*axisLength; + L = [C xAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','r'); + + yAxis = C+gRp(:,2)*axisLength; + L = [C yAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','g'); + + zAxis = C+gRp(:,3)*axisLength; + L = [C zAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','b'); +end % plot the covariance if (nargin>2) && (~isempty(P)) From 62f28bb798dca3845eb843bda045c855b29919f7 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 23 Jul 2012 19:21:05 +0000 Subject: [PATCH 20/68] Updated more matlab examples --- matlab/examples/LocalizationExample.m | 2 +- matlab/examples/OdometryExample.m | 2 +- matlab/examples/PlanarSLAMExample.m | 59 +++++++----------- matlab/examples/PlanarSLAMExample_sampling.m | 61 ++++++++----------- matlab/examples/Pose2SLAMExample.m | 43 ++++++------- matlab/examples/Pose2SLAMExample_advanced.m | 33 +++++----- matlab/examples/Pose2SLAMExample_circle.m | 44 +++++++------ matlab/examples/Pose2SLAMExample_graph.m | 22 ++++--- matlab/examples/Pose2SLAMwSPCG.m | 29 ++++----- matlab/examples/Pose3SLAMExample.m | 41 +++++++------ matlab/examples/Pose3SLAMExample_graph.m | 30 +++++---- matlab/examples/gtsamExamples.fig | Bin 9270 -> 10455 bytes matlab/examples/gtsamExamples.m | 34 +++++++++-- 13 files changed, 211 insertions(+), 189 deletions(-) diff --git a/matlab/examples/LocalizationExample.m b/matlab/examples/LocalizationExample.m index 1c38bc979..e4ee76d7c 100644 --- a/matlab/examples/LocalizationExample.m +++ b/matlab/examples/LocalizationExample.m @@ -55,7 +55,7 @@ import gtsam.* cla; hold on; -plot2DTrajectory(result, Marginals(graph, result)); +plot2DTrajectory(result, [], Marginals(graph, result)); axis([-0.6 4.8 -1 1]) axis equal diff --git a/matlab/examples/OdometryExample.m b/matlab/examples/OdometryExample.m index 16323a616..81733cfb0 100644 --- a/matlab/examples/OdometryExample.m +++ b/matlab/examples/OdometryExample.m @@ -52,7 +52,7 @@ import gtsam.* cla; hold on; -plot2DTrajectory(result, Marginals(graph, result)); +plot2DTrajectory(result, [], Marginals(graph, result)); axis([-0.6 4.8 -1 1]) axis equal diff --git a/matlab/examples/PlanarSLAMExample.m b/matlab/examples/PlanarSLAMExample.m index e664d66fc..f82b3aea4 100644 --- a/matlab/examples/PlanarSLAMExample.m +++ b/matlab/examples/PlanarSLAMExample.m @@ -25,72 +25,59 @@ i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3); j1 = symbol('l',1); j2 = symbol('l',2); %% Create graph container and add factors to it -graph = planarSLAM.Graph; +graph = gtsam.NonlinearFactorGraph; %% Add prior import gtsam.* priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); -graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph %% Add odometry import gtsam.* odometry = Pose2(2.0, 0.0, 0.0); odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(i1, i2, odometry, odometryNoise); -graph.addRelativePose(i2, i3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); %% Add bearing/range measurement factors import gtsam.* degrees = pi/180; brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); -graph.addBearingRange(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise); -graph.addBearingRange(i2, j1, Rot2(90*degrees), 2, brNoise); -graph.addBearingRange(i3, j2, Rot2(90*degrees), 2, brNoise); +graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); +graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); +graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); % print graph.print(sprintf('\nFull graph:\n')); %% Initialize to noisy points import gtsam.* -initialEstimate = planarSLAM.Values; -initialEstimate.insertPose(i1, Pose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(i2, Pose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(i3, Pose2(4.1, 0.1, 0.1)); -initialEstimate.insertPoint(j1, Point2(1.8, 2.1)); -initialEstimate.insertPoint(j2, Point2(4.1, 1.8)); +initialEstimate = Values; +initialEstimate.insert(i1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insert(i2, Pose2(2.3, 0.1,-0.2)); +initialEstimate.insert(i3, Pose2(4.1, 0.1, 0.1)); +initialEstimate.insert(j1, Point2(1.8, 2.1)); +initialEstimate.insert(j2, Point2(4.1, 1.8)); initialEstimate.print(sprintf('\nInitial estimate:\n')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,1); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses import gtsam.* cla;hold on -marginals = graph.marginals(result); -for i=1:3 - key = symbol('x',i); - pose{i} = result.pose(key); - P{i}=marginals.marginalCovariance(key); - if i>1 - plot([pose{i-1}.x;pose{i}.x],[pose{i-1}.y;pose{i}.y],'r-'); - end -end -for i=1:3 - plotPose2(pose{i},'g',P{i}) -end -point = {}; -for j=1:2 - key = symbol('l',j); - point{j} = result.point(key); - Q{j}=marginals.marginalCovariance(key); - plotPoint2(point{j},'b',Q{j}) -end -plot([pose{1}.x;point{1}.x],[pose{1}.y;point{1}.y],'c-'); -plot([pose{2}.x;point{1}.x],[pose{2}.y;point{1}.y],'c-'); -plot([pose{3}.x;point{2}.x],[pose{3}.y;point{2}.y],'c-'); + +marginals = Marginals(graph, result); +plot2DTrajectory(result, [], marginals); +plot2DPoints(result, marginals); + +plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-'); +plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-'); +plot([result.at(i3).x; result.at(j2).x],[result.at(i3).y; result.at(j2).y], 'c-'); axis([-0.6 4.8 -1 1]) axis equal view(2) diff --git a/matlab/examples/PlanarSLAMExample_sampling.m b/matlab/examples/PlanarSLAMExample_sampling.m index ec68d81e6..35d3232a1 100644 --- a/matlab/examples/PlanarSLAMExample_sampling.m +++ b/matlab/examples/PlanarSLAMExample_sampling.m @@ -14,14 +14,14 @@ %% Create the same factor graph as in PlanarSLAMExample import gtsam.* i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3); -graph = planarSLAM.Graph; +graph = NonlinearFactorGraph; priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); -graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph odometry = Pose2(2.0, 0.0, 0.0); odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(i1, i2, odometry, odometryNoise); -graph.addRelativePose(i2, i3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); %% Except, for measurements we offer a choice import gtsam.* @@ -29,48 +29,41 @@ j1 = symbol('l',1); j2 = symbol('l',2); degrees = pi/180; brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); if 1 - graph.addBearingRange(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise); - graph.addBearingRange(i2, j1, Rot2(90*degrees), 2, brNoise); + graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); + graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); else bearingModel = noiseModel.Diagonal.Sigmas(0.1); - graph.addBearing(i1, j1, Rot2(45*degrees), bearingModel); - graph.addBearing(i2, j1, Rot2(90*degrees), bearingModel); + graph.add(BearingFactor2D(i1, j1, Rot2(45*degrees), bearingModel)); + graph.add(BearingFactor2D(i2, j1, Rot2(90*degrees), bearingModel)); end -graph.addBearingRange(i3, j2, Rot2(90*degrees), 2, brNoise); +graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); %% Initialize MCMC sampler with ground truth -sample = planarSLAM.Values; -sample.insertPose(i1, Pose2(0,0,0)); -sample.insertPose(i2, Pose2(2,0,0)); -sample.insertPose(i3, Pose2(4,0,0)); -sample.insertPoint(j1, Point2(2,2)); -sample.insertPoint(j2, Point2(4,2)); +sample = Values; +sample.insert(i1, Pose2(0,0,0)); +sample.insert(i2, Pose2(2,0,0)); +sample.insert(i3, Pose2(4,0,0)); +sample.insert(j1, Point2(2,2)); +sample.insert(j2, Point2(4,2)); %% Calculate and plot Covariance Ellipses -figure(1);clf;hold on -marginals = graph.marginals(sample); -for i=1:3 - key = symbol('x',i); - pose{i} = sample.pose(key); - P{i}=marginals.marginalCovariance(key); - if i>1 - plot([pose{i-1}.x;pose{i}.x],[pose{i-1}.y;pose{i}.y],'r-'); - end -end -for i=1:3 - plotPose2(pose{i},'g',P{i}) -end +cla;hold on +marginals = Marginals(graph, sample); + +plot2DTrajectory(sample, [], marginals); +plot2DPoints(sample, marginals); + for j=1:2 key = symbol('l',j); - point{j} = sample.point(key); + point{j} = sample.at(key); Q{j}=marginals.marginalCovariance(key); S{j}=chol(Q{j}); % for sampling - plotPoint2(point{j},'b',Q{j}) end -plot([pose{1}.x;point{1}.x],[pose{1}.y;point{1}.y],'c-'); -plot([pose{2}.x;point{1}.x],[pose{2}.y;point{1}.y],'c-'); -plot([pose{3}.x;point{2}.x],[pose{3}.y;point{2}.y],'c-'); -axis equal + +plot([sample.at(i1).x; sample.at(j1).x],[sample.at(i1).y; sample.at(j1).y], 'c-'); +plot([sample.at(i2).x; sample.at(j1).x],[sample.at(i2).y; sample.at(j1).y], 'c-'); +plot([sample.at(i3).x; sample.at(j2).x],[sample.at(i3).y; sample.at(j2).y], 'c-'); +view(2); axis auto; axis equal %% Do Sampling on point 2 N=1000; diff --git a/matlab/examples/Pose2SLAMExample.m b/matlab/examples/Pose2SLAMExample.m index 4bb072fa8..1e26ce33d 100644 --- a/matlab/examples/Pose2SLAMExample.m +++ b/matlab/examples/Pose2SLAMExample.m @@ -19,58 +19,55 @@ % - The robot is on a grid, moving 2 meters each step %% Create graph container and add factors to it -graph = pose2SLAM.Graph; +graph = NonlinearFactorGraph; %% Add prior import gtsam.* % gaussian for prior priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); -graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph %% Add odometry import gtsam.* % general noisemodel for odometry odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); -graph.addRelativePose(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise); -graph.addRelativePose(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise); -graph.addRelativePose(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise); +graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise)); +graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise)); +graph.add(BetweenFactorPose2(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise)); %% Add pose constraint import gtsam.* model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(5, 2, Pose2(2.0, 0.0, pi/2), model); +graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model)); % print graph.print(sprintf('\nFactor graph:\n')); %% Initialize to noisy points import gtsam.* -initialEstimate = pose2SLAM.Values; -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, pi/2)); -initialEstimate.insertPose(4, Pose2(4.0, 2.0, pi )); -initialEstimate.insertPose(5, Pose2(2.1, 2.1,-pi/2)); +initialEstimate = Values; +initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); +initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2 )); +initialEstimate.insert(3, Pose2(4.1, 0.1, pi/2)); +initialEstimate.insert(4, Pose2(4.0, 2.0, pi )); +initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2)); initialEstimate.print(sprintf('\nInitial estimate:\n')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,1); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses cla; -X=result.poses(); -plot(X(:,1),X(:,2),'k*-'); hold on -plot([result.pose(5).x;result.pose(2).x],[result.pose(5).y;result.pose(2).y],'r-'); -marginals = graph.marginals(result); +hold on +plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-','LineWidth',2); +marginals = Marginals(graph, result); + +plot2DTrajectory(result, [], marginals); -for i=1:result.size() - pose_i = result.pose(i); - P = marginals.marginalCovariance(i) - plotPose2(pose_i,'g',P); -end axis([-0.6 4.8 -1 1]) axis equal view(2) diff --git a/matlab/examples/Pose2SLAMExample_advanced.m b/matlab/examples/Pose2SLAMExample_advanced.m index fc23d67f1..b496c44e7 100644 --- a/matlab/examples/Pose2SLAMExample_advanced.m +++ b/matlab/examples/Pose2SLAMExample_advanced.m @@ -21,22 +21,22 @@ % - The robot is on a grid, moving 2 meters each step %% Create graph container and add factors to it -graph = pose2SLAM.Graph; +graph = NonlinearFactorGraph; %% Add prior import gtsam.* % gaussian for prior priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin -graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph %% Add odometry import gtsam.* % general noisemodel for odometry odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) -graph.addRelativePose(1, 2, odometry, odometryNoise); -graph.addRelativePose(2, 3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); %% Add measurements import gtsam.* @@ -48,23 +48,23 @@ graph.print('full graph'); %% Initialize to noisy points import gtsam.* -initialEstimate = pose2SLAM.Values; -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 = Values; +initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); +initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)); initialEstimate.print('initial estimate'); %% set up solver, choose ordering and optimize -%params = NonlinearOptimizationParameters_newDecreaseThresholds(1e-15, 1e-15); -% -%ord = graph.orderingCOLAMD(initialEstimate); -% -%result = pose2SLAMOptimizer(graph,initialEstimate,ord,params); -%result.print('final result'); +params = DoglegParams; +params.setAbsoluteErrorTol(1e-15); +params.setRelativeErrorTol(1e-15); +params.setVerbosity('ERROR'); +params.setVerbosityDL('VERBOSE'); +params.setOrdering(graph.orderingCOLAMD(initialEstimate)); +optimizer = DoglegOptimizer(graph, initialEstimate, params); -%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,1); +result = optimizer.optimizeSafely(); result.print('final result'); %% Get the corresponding dense matrix @@ -78,3 +78,4 @@ Ab=sparse(IJS(1,:),IJS(2,:),IJS(3,:)); A = Ab(:,1:end-1); b = full(Ab(:,end)); spy(A); +title('Non-zero entries in measurement Jacobian'); diff --git a/matlab/examples/Pose2SLAMExample_circle.m b/matlab/examples/Pose2SLAMExample_circle.m index 355f1a10e..e16edf9cb 100644 --- a/matlab/examples/Pose2SLAMExample_circle.m +++ b/matlab/examples/Pose2SLAMExample_circle.m @@ -16,33 +16,37 @@ p0 = hexagon.pose(0); p1 = hexagon.pose(1); %% create a Pose graph with one equality constraint and one measurement -fg = pose2SLAM.Graph; -fg.addPoseConstraint(0, p0); +import gtsam.* +fg = NonlinearFactorGraph; +fg.add(NonlinearEqualityPose2(0, p0)); delta = p0.between(p1); -covariance = gtsam.noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); -fg.addRelativePose(0,1, delta, covariance); -fg.addRelativePose(1,2, delta, covariance); -fg.addRelativePose(2,3, delta, covariance); -fg.addRelativePose(3,4, delta, covariance); -fg.addRelativePose(4,5, delta, covariance); -fg.addRelativePose(5,0, delta, covariance); +covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); +fg.add(BetweenFactorPose2(0,1, delta, covariance)); +fg.add(BetweenFactorPose2(1,2, delta, covariance)); +fg.add(BetweenFactorPose2(2,3, delta, covariance)); +fg.add(BetweenFactorPose2(3,4, delta, covariance)); +fg.add(BetweenFactorPose2(4,5, delta, covariance)); +fg.add(BetweenFactorPose2(5,0, delta, covariance)); %% Create initial config -initial = pose2SLAM.Values; -initial.insertPose(0, p0); -initial.insertPose(1, hexagon.pose(1).retract([-0.1, 0.1,-0.1]')); -initial.insertPose(2, hexagon.pose(2).retract([ 0.1,-0.1, 0.1]')); -initial.insertPose(3, hexagon.pose(3).retract([-0.1, 0.1,-0.1]')); -initial.insertPose(4, hexagon.pose(4).retract([ 0.1,-0.1, 0.1]')); -initial.insertPose(5, hexagon.pose(5).retract([-0.1, 0.1,-0.1]')); +initial = Values; +initial.insert(0, p0); +initial.insert(1, hexagon.pose(1).retract([-0.1, 0.1,-0.1]')); +initial.insert(2, hexagon.pose(2).retract([ 0.1,-0.1, 0.1]')); +initial.insert(3, hexagon.pose(3).retract([-0.1, 0.1,-0.1]')); +initial.insert(4, hexagon.pose(4).retract([ 0.1,-0.1, 0.1]')); +initial.insert(5, hexagon.pose(5).retract([-0.1, 0.1,-0.1]')); %% Plot Initial Estimate -figure(1);clf -plot(initial.xs(),initial.ys(),'g-*'); axis equal +cla +plot2DTrajectory(initial, 'g*-'); axis equal %% optimize -result = fg.optimize(initial); +optimizer = DoglegOptimizer(fg, initial); +result = optimizer.optimizeSafely; %% Show Result -hold on; plot(result.xs(),result.ys(),'b-*') +hold on; plot2DTrajectory(result, 'b*-'); +view(2); +axis([-1.5 1.5 -1.5 1.5]); result.print(sprintf('\nFinal result:\n')); diff --git a/matlab/examples/Pose2SLAMExample_graph.m b/matlab/examples/Pose2SLAMExample_graph.m index ac6d1e317..2cfe900a0 100644 --- a/matlab/examples/Pose2SLAMExample_graph.m +++ b/matlab/examples/Pose2SLAMExample_graph.m @@ -10,33 +10,39 @@ % @author Frank Dellaert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% Find data file +datafile = findExampleDataFile('w100-odom.graph'); + %% Initialize graph, initial estimate, and odometry noise import gtsam.* model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); -[graph,initial]=load2D('../../examples/Data/w100-odom.graph',model); +[graph,initial] = load2D(datafile, model); initial.print(sprintf('Initial estimate:\n')); %% Add a Gaussian prior on pose x_1 import gtsam.* priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]); -graph.addPosePrior(0, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(0, priorMean, priorNoise)); % add directly to graph %% Plot Initial Estimate -figure(1);clf -plot(initial.xs(),initial.ys(),'g-*'); axis equal +cla +plot2DTrajectory(initial, 'g-*'); axis equal %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initial); -hold on; plot(result.xs(),result.ys(),'b-*') +optimizer = LevenbergMarquardtOptimizer(graph, initial); +result = optimizer.optimizeSafely; +hold on; plot2DTrajectory(result, 'b-*'); result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses -marginals = graph.marginals(result); +marginals = Marginals(graph, result); P={}; for i=1:result.size()-1 - pose_i = result.pose(i); + pose_i = result.at(i); P{i}=marginals.marginalCovariance(i); plotPose2(pose_i,'b',P{i}) end +view(2) +axis([-15 10 -15 10]); axis equal; fprintf(1,'%.5f %.5f %.5f\n',P{99}) \ No newline at end of file diff --git a/matlab/examples/Pose2SLAMwSPCG.m b/matlab/examples/Pose2SLAMwSPCG.m index 5db736fab..dca31ac94 100644 --- a/matlab/examples/Pose2SLAMwSPCG.m +++ b/matlab/examples/Pose2SLAMwSPCG.m @@ -17,41 +17,42 @@ % - The robot is on a grid, moving 2 meters each step %% Create graph container and add factors to it -graph = pose2SLAM.Graph; +graph = NonlinearFactorGraph; %% Add prior import gtsam.* % gaussian for prior priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); -graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph %% Add odometry import gtsam.* % general noisemodel for odometry odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); -graph.addRelativePose(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise); -graph.addRelativePose(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise); -graph.addRelativePose(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise); +graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise)); +graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise)); +graph.add(BetweenFactorPose2(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise)); %% Add pose constraint import gtsam.* model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(5, 2, Pose2(2.0, 0.0, pi/2), model); +graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model)); % print graph.print(sprintf('\nFactor graph:\n')); %% Initialize to noisy points -initialEstimate = pose2SLAM.Values; -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, pi/2)); -initialEstimate.insertPose(4, Pose2(4.0, 2.0, pi )); -initialEstimate.insertPose(5, Pose2(2.1, 2.1,-pi/2)); +initialEstimate = Values; +initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); +initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2 )); +initialEstimate.insert(3, Pose2(4.1, 0.1, pi/2)); +initialEstimate.insert(4, Pose2(4.0, 2.0, pi )); +initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2)); initialEstimate.print(sprintf('\nInitial estimate:\n')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimizeSPCG(initialEstimate); +optimizer = DoglegOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); result.print(sprintf('\nFinal result:\n')); \ No newline at end of file diff --git a/matlab/examples/Pose3SLAMExample.m b/matlab/examples/Pose3SLAMExample.m index d4a8baf99..86c5c23c8 100644 --- a/matlab/examples/Pose3SLAMExample.m +++ b/matlab/examples/Pose3SLAMExample.m @@ -16,37 +16,38 @@ p0 = hexagon.pose(0); p1 = hexagon.pose(1); %% create a Pose graph with one equality constraint and one measurement -fg = pose3SLAM.Graph; -fg.addPoseConstraint(0, p0); +import gtsam.* +fg = NonlinearFactorGraph; +fg.add(NonlinearEqualityPose3(0, p0)); delta = p0.between(p1); -covariance = gtsam.noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); -fg.addRelativePose(0,1, delta, covariance); -fg.addRelativePose(1,2, delta, covariance); -fg.addRelativePose(2,3, delta, covariance); -fg.addRelativePose(3,4, delta, covariance); -fg.addRelativePose(4,5, delta, covariance); -fg.addRelativePose(5,0, delta, covariance); +covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); +fg.add(BetweenFactorPose3(0,1, delta, covariance)); +fg.add(BetweenFactorPose3(1,2, delta, covariance)); +fg.add(BetweenFactorPose3(2,3, delta, covariance)); +fg.add(BetweenFactorPose3(3,4, delta, covariance)); +fg.add(BetweenFactorPose3(4,5, delta, covariance)); +fg.add(BetweenFactorPose3(5,0, delta, covariance)); %% Create initial config -initial = pose3SLAM.Values; +initial = Values; s = 0.10; -initial.insertPose(0, p0); -initial.insertPose(1, hexagon.pose(1).retract(s*randn(6,1))); -initial.insertPose(2, hexagon.pose(2).retract(s*randn(6,1))); -initial.insertPose(3, hexagon.pose(3).retract(s*randn(6,1))); -initial.insertPose(4, hexagon.pose(4).retract(s*randn(6,1))); -initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1))); +initial.insert(0, p0); +initial.insert(1, hexagon.pose(1).retract(s*randn(6,1))); +initial.insert(2, hexagon.pose(2).retract(s*randn(6,1))); +initial.insert(3, hexagon.pose(3).retract(s*randn(6,1))); +initial.insert(4, hexagon.pose(4).retract(s*randn(6,1))); +initial.insert(5, hexagon.pose(5).retract(s*randn(6,1))); %% Plot Initial Estimate cla -T=initial.translations(); -plot3(T(:,1),T(:,2),T(:,3),'g-*'); +plot3DTrajectory(initial, 'g-*'); %% optimize -result = fg.optimize(initial,1); +optimizer = DoglegOptimizer(fg, initial); +result = optimizer.optimizeSafely(); %% Show Result -hold on; plot3DTrajectory(result,'b-*', true, 0.3); +hold on; plot3DTrajectory(result, 'b-*', true, 0.3); axis([-2 2 -2 2 -1 1]); axis equal view(-37,40) diff --git a/matlab/examples/Pose3SLAMExample_graph.m b/matlab/examples/Pose3SLAMExample_graph.m index cace357ae..b6a5444a2 100644 --- a/matlab/examples/Pose3SLAMExample_graph.m +++ b/matlab/examples/Pose3SLAMExample_graph.m @@ -10,23 +10,31 @@ % @author Frank Dellaert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% Find data file N = 2500; -% filename = '../../examples/Data/sphere_smallnoise.graph'; -% filename = '../../examples/Data/sphere2500_groundtruth.txt'; -filename = '../../examples/Data/sphere2500.txt'; +% dataset = 'sphere_smallnoise.graph'; +% dataset = 'sphere2500_groundtruth.txt'; +dataset = 'sphere2500.txt'; + +datafile = findExampleDataFile(dataset); %% Initialize graph, initial estimate, and odometry noise -model = gtsam.noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); -[graph,initial]=load3D(filename,model,true,N); +import gtsam.* +model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); +[graph,initial]=load3D(datafile,model,true,N); %% Plot Initial Estimate -figure(1);clf -first = initial.pose(0); +cla +first = initial.at(0); plot3(first.x(),first.y(),first.z(),'r*'); hold on plot3DTrajectory(initial,'g-',false); %% Read again, now with all constraints, and optimize -graph = load3D(filename,model,false,N); -graph.addPoseConstraint(0, first); -result = graph.optimize(initial); -plot3DTrajectory(result,'r-',false); axis equal; +import gtsam.* +graph = load3D(datafile, model, false, N); +graph.add(NonlinearEqualityPose3(0, first)); +optimizer = DoglegOptimizer(graph, initial); +result = optimizer.optimizeSafely(); +plot3DTrajectory(result, 'r-', false); axis equal; + +view(0); axis equal; \ No newline at end of file diff --git a/matlab/examples/gtsamExamples.fig b/matlab/examples/gtsamExamples.fig index be72daf9c4674e0533e2c13c720f7edb58c385cf..d0037027882c4ee90461dea6d8bd52368f58de0a 100644 GIT binary patch literal 10455 zcma*NcQ71o^e(PT5TXmBw~*+)Z4e}S3Bt0X1<||JmWbYaCwfHhy+rSX=-pzmi>$s_ zdq3ZMf4`ag=Y8g#_q>0ddFPyYX3lfY(Ny@Tp`gej#xKaCsqpd57kfvW*DN}Y7M@>R z+?}LZbd-!fXo-uyW>IptvGBC9W^r+rX3=zUW>NEUWDyo&5fYIW5|$Q{Vi6V;5@z{- zga$X({{i*~O|t)9gIrixzPX4`Q`(FYR1y>6T(3B0K`nCc{3~!M6mv`|#C{2X4+y5h zAxfM#9F&3Qd1VQ#h=^YPwepxqQm=e`dQu8w^tUTz@_YN0PuYOlKsU?}Q6FUGZ#(=N zfl&Mr=WFSEA$x&WgeqNZ3-X1LchqGu&m>}OMxY*_p2B)l7A$V8E2DFKFlYpW?1jd#zGR7oXxVdNbn zEyV7RRwd0O9F_HF8B=UDzF z^L5**gRG7{p=;lIU+LtGRM`{mq}zSs2nry!?U+JwKdWD2aZFPo0yo9tpHE)8RCGeC#JD#&6u6FBphCcDx%zUPlu}asGbW*uI7M8R(phyV2kR69n zI(EjrXkqX7U7eAYmaHA%DOiebxfm*NLoj|Ei+T%T-%qLFL{KbW@{iOcPeVuzWM7|~nxg=?4 zDyl^<5m!8H0IhNZRORjpTz%MqxZ`-?s@*7DA=p!EZ9mTheu_9AgU2e`J1jC_dmL*G z)*Icp?(M4N$^Hf%Qt$)McpOv&8N{5cN7@G3-6C{nzfzy`?LVkJkyIe>b)T+{edyy> zK1So?GS{6|*G%#oUX_qZY}t4UYL5;-^XiUnI`jgzl+^H=)f1fDj4*$Xm2c@;1>fyY z&kIf1KW%paO7sL8Jxl>3vTOzN18DZ$2-b#c*7~l)rJP$)KVlbU=^k2C@1P@A<3Z-8 zH;XjRTgF5oLhgH(OmhAFPGwC(J6pDQaP!*ovq4Tt_6-iW6H2t7^a>c_C-MAs_Mft% zw%9g2k0~GH_y@~1t}Z42ZW=FrP!)OVV5+lLZ6(XCzaS$|ZHQ(H(kYrZ-l6G61e zp}Bn@g)h;=cG`VjSPGQ)4kwrx!A%X47_$ek9<{2|U3jy{@BQs(iB-}L@ajv+R8e3N z{JiQ)@dT%}Pj=E*ZMR1BhdKCd@3_zicskklyE;R3HUMc;g+iJ7e zCc54Z7~qpVh3`srFa7}zw|$-nM2sihLXZ=W_eaX?+T4Ms)cWmw%*X5ScG;U>m&QIL zX--iNTIl!%D^F&L$QV5#vhc|ib~?<8tMsF90&)jEG7vh#x5P&L?Y%sVV~|8OuUJ~^ zA@c5s%B0>!sSeIt$;FIFuT|$M5U3kEv&zCJ9RN{}18N1PYc>nI4IsYF-ut}#Ef~`M zyO)YyqL~Gy54@$1u76=(DKAVL_yDEq1q6)6w3E-x?@^sWJ?d%`P$^4#ynI^KU?Wuwsf&922{{K znlzy8dQW1vz9X6Ha);VWsq40s{1; ztRry)(=inBtgDaz6K%TU?!b%>D;sF58M|=)uFgDg_9pbSGBNXba#DjlK=m0$cfXdh z5|*}4tEwvy&&#i0upAU`G8W|QwK9Xj$HKy35XRctlnPXkRGQy9`m#J+!=ge&?-YYm z{tWU{MM2b9Xe13RR{3pge9BkycwN~*dzRn0qnP_r2<3xS$u_Sni-ee1hkdV$WHw&< zNWXUMq~fr!70C+R0QcS;Oy%5cI$L4QI$zP$%+rMOa4?!Q_|aE|#_=r~0R#hxoq`>uhr|zf98r_%#ndBOcUqAHFMknDbN-Nn($ z-Kl%#?JE95s5j2zmRES}wO(2gWoC8-?GvaM^SU^8#vHYB<6EDLR#a6Zt|n+vfZ5y_{%n?!gAC`4!^t#_d%r z;(mf^Kl!)-VVF#~un2Ng3qr^jJ&cZU3!s^Zu8TbLP;L#8Vp(*2&&r1d1^i&j>F_}> zaGnRq`~aW;J%??_gTKow@3RNfh+R=4>IW2sxIw zw51EZA+M%nc;*Fu$|c+d_HJH@600(zUCA*1ziW?sYQwzx>wi-ApW$=g`~PPD4N9%K zf#^Gq53Ed%4uX{X&Ht-={$uo~8HfKEonO}5oK_wA@%hupmLaCqJvJgP`>#K<=PXzC zMCrK4#LYTajJ+kAPZa#-K;MbJDL+ukQq}rhe(omfj{K;Qcs&~wDDDQiIm}c)tc~YrF^y#A3w7!gF)^-Tmx_OUG`iY(3b& zQ!1G&Q}|X1ABfHzHUwz=+VX2zEwB(Zek7PAk%iFx15evzGv{< z6=#FXvP`_^ABLE4m5M+uX#4mf`1R$ouc#HVjAmA>@ZY#sJnBbJGYfsaA3yEOxecr4 zRS1&;q<>F+cvV72pZtQ68~om6miQdpwr*m)j;{|I3Dip>F=2nt@U5afHr!oJI@Y=* zL@;%qBS-}{Fwpi^=uEXvLdZVK+2whfCwmkPSE5Ox8zZM4*wNYndTy z>6Y!LY}?#v|980O9fYYCr$OQo%X+^Yz4u|rt7bp~h@ZXS68j$8-^Kq+CzJpr#LE|6 z5-B=$+tF1^YCMD_p;cvz5EvQ}Oa1eCks--iHz6UMtEjN>R&%TG2iaX2HhPg!QcvsSJQu5>GpJ{O)dEwLkm_2znb>D+ch@u?_m?^$$7^YXum^G2dbo zg0^cZ0NZZ2f1Wyfp%ii^$X-8qp+Y9ej(?L;n07^)@&ghJal{Dme(w`5J_O&HIs^%t z|L4QvW-8;bQd9L|n}^5DoQ;;kBZ29`;GyrBk;l2I$ZVm1M^$D$^nehgFbdm=oFF5` z2YjkXWY*|>_`&<|_9r>uQ+>Tlb_{LO#2#V&`>?^jef-6);2NiDaEPfb=-=JF5@xvi znL6lmYwfZ$_vfeFQ05_*7av=gPki5sA!^?x1ZBi4Fyw{;VA^dM?En6Z!F@1?Jy<-F z{UiP#X0CtIjHObhs>ws|xUk~VD=Uq@`&LKJ2H@hM@3gfw_OZ?|^GV;~j?tMnlhL1> zU+`Y)BPE0mz2DXT$KiKwsrVOHRhyntLy|5O`GXc0$zez0_lGXl@M=cGOY@D$JaNsA zrnK0ZSFcg6dAHwL<@JI1*=(yJm%0FJsIyXeqNe8i9i1${x(*j6!B9fHj!Ru%|xj>%9=-qlZ3wT==a#OvV( zdKZdPt&LD|Z#IgPcYZMZ?&diUxyiYA&JBI2h-ERO#%Iw{fn7LG^NJ(dls6@?x42KQ zhw=O?IRONUYe|9AwPcTvi4>>RM5?H60xBdqTJvd;?23x^rIj*|1ZJn{!)BsL7e<+}q`m5n#EYC8|tPVgigZ~QI`v2t()D2W)M%GY|XwjWgpp{rXKJuG#yQXyq==`nV#|6u7(ke;(b6sZ^(7 z*V9_ z{m6Pm)V~m3?QesVRNP^09C0w?*$sK82(rN|-R^`#ZRx$@IB)JtcECqB-4%Gu3LMv! z2^<5*Fg(cmH}`h7ZPK1Yf)^jjOn~CWj8$y3!_I@sl!NbB-jWENE2q>%WxKx+5~SxI zR06LlHL*&se?`2nJhZEPPeAg6THjSInmv}tp^-rSo1oo?8~FX1+uW78z{}hHxe(wg zWvA53`QE)Mv0vOn+}LLGMaW^Of6t5DppsH1uharDHR+YJBIZ}aSC^N!Zm36bzX_^G zb1dRNns25tB|{^8b7%)*S%HhBd`Czjn60CTbECEO`(q-9i~OzkXP2u%tw5*)p!eTV z4E%DcfcX*qOtb`orY`j&cep2&XR-Q?Seh57Hfy-AGKsMjOVKJsXUmq8TeBLAn_e6n zv!7077Az|xb|_i|tM7%ar9=jQ4+nj8k&QSaIsd+`EZ->>v8%@p!WXXv4AI3jQxA3q zn@-N4>QI?P6iop>+a>)^ds+=;B}fyXUpK|8vJ*vAsTzujjI~%Nc&%9hagO16{a)k7 z%WfqLgNQ-9kBP5fO3594WEpvXe`7zd4bAm=(t;Y+6c&()E|OV*bV#*wU5R`M_;#lK zbml25DG<@+#}K%HN;sL(k2DI$-(4JQ_vSpT>TS6IMD>I?_D*u5dYa63d?W&sp|c!F zwUE6nZjn{K*2jmFU)~~(N8_q%E?n-9p`1IHC_}BMBF5E~?4-X$NIenpSs_l(K&>y%czgaaz+vgVbI}v8}-6Jup z`Q-Tx^$nhPZOAF2h8Qd#Uo0HT2=Oe}>O!`&ES47tneK?bD&+mZbRoUSOm%~VGt>zp_8n8y>i0@BX=6nc*;-kr?z zJ`$Ja)$)ga3rseLvi1%q1=}hjojGsQlq`C=&{ABrX;he@%isd-ZQ7Cz_58=TJ^$$0 z`&BHAcQxK2@vG#7U!Mwg_rGd2b^Irq$Xl-&-Ff3mc(+{S{MHju6z25lKpL;T+7Ist zrT2v(>+vnL7cmeh#1{{jl$j8Iv3GS`+h z!LxskeVPY#<$NaB7~^LmQL2=rzk9A>vO&wt~4PXk!y^5cj-=d{U4S*>G_|O z%OW{K%Z4O|QNPE9GRteR=6|0rRwzc8kEe#)ud%d2usy}^L0SAZEfXSwTvwor4ILi zz;3#y%ijLHRDzElX8xbp=Tjk{+%}lv17mgq(m&ImqHw@6+jn2ClY5LojiL5H+`M;% z?BLnJUC3+!x~B-0JgKgS6m{e}o%mT0)FVs;nv>`M;VPT%!?QKY{Mx}h3+SaqDQI4tpX4b>UwGB;wgX6ZW^?6gNLF0MWSG1#U@VvWKbP#W z-|fIerLc!@E96lhdXh0iIF#~t0B9$A<(load2n8H99z~ib-de{R&ML%W=Lz>)jJ|H z)p6@{laoCED+cnvh12>cxC@m5Re?N9@OvUE6Gd>qA2Jn6QI+36TTAiRo)Fl*BGVN? zY&yGSPLnh?hB@sL_M$Z3f|Nrb3QWpEILvWRljnTEf9KNYiaz7={^0VR*^p}N^zq5v zz_z&`$W(Dyl@_iw1hvyI(=^0JGF6F+4p4O5n!A0hudAr|bMu$$_}CCn z>Emfd;;$F`uYMU8)+cVh z^8y+Vd73z=rUV2#;}WmNJ?&WdMQ!PK9`3!IHXbH#xfTwxLChKvxX!A@{1T6w2TwGyz^+N@2oatmk z-&m}uD%LnHXuNGk?{zQK86QrW93ccGpZ@~f2v~6M?N=6?X{MR*dDEQvtDZ(|Q?o+9 z6wkGaOutgCdEuQ|nEh8-LcyZ@_wOn>mxhN8swz~-?2^7`ucWH9O2yR($JMaKO(!ks z%DsPMo5w5mC425`Ljv!@TlQz;S6KHRQ50JRVe#^A5E@=2+IX;(r9gD~%~gF)t~G6n zX6R9!n+LLzK|1YZE>9KzsW(dT{3hakti;m8=iK%^Rf#GtgNdnH`uez$o!0e{pkLNx z!dnGXR9XZHU8rt}bCmf!W2b7|)=W^cHCe+x<3QG)oWPj8Hu#n1`brl!wN2t)a&5qf zOLKzRZV*;)>4T5$?fnc*QoP3r;a1n>cFI$|OD!vuunk+eWGcyd_^1tpnq!5UM|>m8 zBTuyc^YeK7Uhp__w|l?0ZlW~5Rn_i$88YWXP+WL8cSkC-(1?whZ+z5D@@a0=)5(onKa=ZP4M0k<7B@3lCMXAIIZj9Tfgbq2l6ir;VlM3 z0>4z-C8#vPg|ZnJg`-Kdd=*===dGGt7r9(h9wxi8=J7Kg8Odh3Gt$CM+4L=Nlmd+> zPEf8}T80hnl1Thyg6>d!wy%m<^o$|0-2Meu0MC3Kg`Ks@Y>}&cxF-E)*AG*Ik4JhB z^Le*QtdB3JY(t=XAb>#hov|>W_0G6B(q*booWAO#SpMHW@lw6JPc=}t)k#l`KWmEL zMd#V)7%(=foyAFqJ^KwpeI91jG0z-?A1^N?u)F=d4Wr|n;e->PVgwIn-y4nS!*=(f zam^e>15Ju^p?jAS!9wRS+3l&+Ab0;>JV_Oe5Cy$CnT*DWNy=N+QgjE53T8LnCWAjb zS|Q%&<3M5ZkMi`X*n-Ig4uwHjJUm-ppw=m}vM6FJw^E;hk; zq!6qwwQ;L!4PVLZ?aGl$@l_;u|AqRTe7 z(@lIk?I=n(@yIKhL$-z<#CX8;M2*~H`5VFAgT@n%&x2-Xgr$9M67q-y=yt~hI1#?# z0NH610{7x~;TM-Ec?yVd5InAKXR-UzG?rO)YwhUb53bZR2WGJ7+oK~cXcy&UyWPS zn~T|#9Xi>Qowyk67=aPv1zpOzh&UxQLx!Xd{o|pJeI8^>WH-G_J&C}{a#?fPrH5&u zSlk&xDd$*1I32L|ad}rN#p!WYs|MIh13mv$yyyLG^i964@3aLYw>DO|{+-ok5uuNJ ze3FYc?y&yd*a_M{n8CWUPt4s;+2lUp)FgWi!e)!l~FSd#- z8tQx71E^!3RZ_#hdkDtZFc0{6?E0?f^fj{Ow7$;>Gk?Z+Vxf>--wci^vzTbSxReEthLUX}d z%f3i`z)1_?mR zoDoj~7WuAIF3$Fu;p8^8(ihp75uDc1HJsM5fv`SvIaq?%oBI?fitzum5`m6X26Xx9 zF+WFNg6=f_U=H?GYcI?^77zTfIfJm4AP{DA;iv@denvozufyB*hhWQ z>A7ZQo1k~SvN&>&u72X1HhGk-2@aw1rqao07m*=;B{LcY4aQzV2^Ku~*$R6mAIiry zY`Mt}>khj9p^+H&*p}PY{sO<0YXQ>T%}q1M=LA~N?1NP?0Woe!X|-l28#>mTGwks9 zsTRSn*RY!I*ESc`q!iOdViRixTJ+=Frsp@kvfVS+-{6r_IGxdq%X<_cPv9g2*Bvw1 z@rm>0SVp75;jHcunpxxCL$aHtytQDE$Qd2kwG8U_RdLPP20lr0DGBc0ZM#(}`U?n! z%v^e+B+#BhrZH(gy8tTJ~e2i-a?wndJoWYpa0XNv!J}uYTWp=A{_7A3d{Vi&>BauidLC>HdJ4 z0A}AjSwC#w&<816-<0)yQbh9+DSR3hveY^M*zZ*s2Zvk|ZcBqhceqx@1iu0S=CerkUV}?pM{?#7=ET9c74U-J`2yu$MW^-5BQkld6)m+3xP)di zZtKjz>vueqcfZkRO#-(x7h|^n7ktsu zRO|l5n(W<@dO^kISmDf`Z-n4bUE`t1lpQuIyc@KjY+U=OuF89Q)Em7X9nL6__vfCtFmGW&$r9!Shy{I=fN*^f{EkADY%$@oP zMVj|5OPKBQZ|@TLdpU5Fd@bUy9N+cjr+D-xJ{l@;S)RR9Y=AUU_4Q@-?(-w}pIAY0 z{e#|qrLkfSl!dcNHaS109C35q8z?XRO&m(_%F{PH7oYMa`?r_sYY}iSlC_V15-85o zz(h^)NjZi2@83*{e-%x$Wyy70MsJRb_V2|!#k|f*pPn?ke{|6U!WZ7J*BBHFN8;bZ zJv9x#MIk>HWG~nla44qrT@HbO91aH)e6mt!GelMwq%B2XEjsYM--mo>u6#_{4@?#m zKU2PZT7@vaK|VU{9jn_&nNM}AYzFb)rzLzbw}E|FEBW60!0}k+6?_KdvN4qS1o`w= z$ugzvmtPlvSNUDX8~dSILr>!*oiB(T$Gin%Lv4T z5XS5gC3Z@h2I$pa2G8IV>J#Hbw;gI`4`1StIh>+Tvf?i2p|UGS`{Y-;mSecPi;7CH&hI}6p8 zl8Pyx8iJiEE4rMW4C1sGA6P5MTQ|Nbw-m;F6MA9d+2LBqVjaX375&SRtWro8cl|iH z?(VnS;=|;2&qPuF*8uo&Ul6G7s9q@P=sN)ZIJv)@Ile%RD7u{~XnYgsJA46GYuTzs z7R3w875D%Mx(+pMK=4~fC~YqwB`#-j3SKx9`*j;M=UZGU=Q97?o)0UZm0{@tgqluJ zU6!II&#y|M?q9z9X7;BjLMe?ph*$AL!N4F7C{=<-ie5Azx7F#EV zQ~xuiDk#x(99m5X<%z_5w`Tdn{F|~;gZ)=>Cp$qWoErt!*{O?Pd<7dbb~K)BwWlXux6Fku%2-I zcGLK(p*3#^$6*;8fs$WP{#a`rt5eK*7KL9)$4U|F-$5+I@dP*IrKNPo5bLLE8D62J zM!nGZFGr7eoU@;LJBRLQORfkd`Aut61%(Bh^gYT-{nIRi+~fiJ?J`pGLQg{j<5ZX9 z#8%=$hz>Qhcl;`DcYI~XkdcGLSk?mzQbwLuzt@z$FUivW!ZrtzKfYk?p}RH|h~r~# z4m%DslZAf<7GrZg<{6uY(^SR#aVdPBuxVap z!@9dpM*8@^{N3#m!+CZSeWnttS`jZ)Lw9- z&PTmriT0fRg|t!l3_-0qbnX-2Y~Wt4Wn1;yXpnq9Cw?{seYn3>@bKAP*WT957%;un zf76%-^0C{Xatdukfb%r?QT#9rK2lwC8g zQaH6o8pgWRKNzvq}wQSqC!vWu=7I>{^$6qPY(Ehjx zxNKdb-JO0oz1#i6Tq6)Iu*J36i<;97bbBURATTg>HT2gpTG>%QWQWkXtxJ;PY^HDL z)azql;X6bpv*=mQ&g_t^p-jI(JLc)yDckJ-?6QSo|IaSl$tM}Dfw#|J)&inZmg@_5 zc#e{Ec-V-D4qx-pak9C;##LMoW_QI?Q;Y8-w`;>y{VV|*Nl2oMoYO(PFK#^pf4Kpp zUx{-Ronj}I>y&95v}FT@0lP#7Po~DC^LUuX8HY4`?T%_2=kgZB)R#2kXQtuco}w@L zpO%0Qo`%iQgu>bfF^D3K7&g9?cQ?I?kVeoDSr(ZA#@VIk%=X^4dp3I(T0faJsYv5l z%Cxd@B|Z0$RC9K5v=NZS-6+~?Vm&s{(AEBC&R*q&)kL%hvY1X4yA~@G+ZpR0>q7^d znYZm4@hw1NL$RT((1B<7AJ1@^ff}^IKi7hP$^`%H4F2gK{L=?Z_Y9}#6t`#-_gyzx z-YtN+gP=U1#O$K(STDqjJYCKMoe>%AEu{4RLGzY2sb{(3tG`?a-0esT@i3yOt6 zw{9gKBMBbL}OZ*{#;*OpFqAnwp?VW|D?@^qKHp1@-PA39&C9jUF81)q)jAi literal 9270 zcma)h^-~lM&^I8ZD2Jrv(W#V_G}2O1B3;srbQ~dgbV;X3gM=UT8ozKo{DHv!fyk!>U7i88_FnDF}lka@pnT1|63ra}77Lxo=5f&5@X8!-B0al#4a`diF|$6dbi1aj?BELv={EMG>`B$af- zHs3I(|3~P`j))n{^TW{K7)Wmm9X<&oz?%Mz9P+@i9MU+@$xI-Ma8ufWF%;+94 zXvzIhu`57@)qoqtVXuID^-tw2>AX4xcNe^p-#OgsphX=Ye3^VKdp=Sy|Pf6>d>h?Ts`IRpA66u`yJ5aYxBDsTHehBGYpvo#| zl0L^x-5ZbA0OmeXQk5>Q=)MbhdK!dgxwfD=Baa@JRfUvvXI5sn!Q?6ZIx1Km9V8RV z#~(qv%2X=?osSX8HbO3Tbj#1O)bPa6>w(GZ;O^OYJp(N~=e-uKQ~57wj-S4>8p~Ps zbS$+JfxyK62H%#{N_Kbg6>7*l^@~55rQ5}{M2Q#Tot1>NYD!}oyOIhMpBZIIk%h^P zUI1rM^qh`x`(~<9vPW``1a+vpUanWvx7{PkmC-6vRNarjVLh1L(b$fSl%QJ{#FmV; zoQ?_L>q6eq5Al%)Y&();-f~la%RnaFwQbLqt)3f@C}lPxeSZ8`3bQn8I@HNM9_kAJ zcpWdt&{d>O=PP-7RZsn`H*B= z0_hsDy@5`lPUxGt#Ia32=-}WH-s4D3^!dlJvlr>i>)mcg#_v4D#6L12{%o4D3XEm| z`^yFHJ+p`gGH)dh_8<-_-S77i$zw}qpG)rEG?$F~=%kl%E{%scUuAbhr91(s@&z$k zw1=p{yh{dCTwJcMQcm+PJ9lWtltj8hmpz<17X+}mX@0-A_M*0w_hgjb9FNnRa`zB{ zm#jIaJY59>EhOzw#Yi$YAk;VzZAv{}`uY95)?N!Yun z*x189x-&gZJR%`#6fl#5c3%YacF#y3lMK(%#BnXqq@gXvu`#Iv>92316T^d0SLX4R zQl1_-62|~l1W~BmNq1Z{5C6-TpGMD1i{+}WeXV`X)I+=&FuQH{!nakYFr9xOcaEE@ zFkBoQEcV2>$75cevb)sDQCFYfu>xO8Wz_fD$~hnfxnde2*>|NXH4=)_tN|@ex1wJH zGk!1HF-CC|6*&nN~rmQSbiasLPbYGows+lGnBs6S|KHTzTBF&;Z;?2+Q+DmN@K<` zEC-?>(^DK~*J&C=c9FEDbs<2^ss8CNd=k31EA90pybH`Kbl)2zl?RRgni=wt+g?#s zrnC3uhdq(F0-Np24NU->p}O`vcaEHI3beKN1#}V;lWA<;8)|I`0XF0r-Ek7uLPTn^ z8W4n$lkhf3CisN~PH%j*)n|@(H;ZnilgGR-O+speR*be$tc|k%HZO$?MycJ2m8DrX zgj>o!p~gLFkKfC-;@BCIUl;fBXc>5_ysxJD#SE)@O`7ETo`J_mSy*pm$J#~K#`14} zxBHYVt0DnUym0kVZ{Mmb(9zbbl4*Pz_v}x2M7p}&XQeYD`g@^s!P9MXs|(hAM(g)I z^-5<={Bndj!HhR6ymI?PPX}y-hWG{YiNO@b5hPy8Sf`2yA-y@o!Q_W>c^?|b{`RwpXzB*W2+u?Aa+?0oHzK*&Y z&AkNVK>~=UosWz&D%fa`adp>x)JfK5RRAI*t9KTCSbBrMA`{p*$$ybNO9Gh$*4RAf z`KU<#W!}3mzyTUN*1fcQk~`}_`Fy@fVXf5bcuQwUhk2B=1B)Ra^l_XT>HP*AULDy`M$E^)w*bk6W%0 zK04X$!eFW7E+}WOp+?=%vf|@>ef1;QqV!LaW!Cx|htsl$d1o`wW!~?-Se1P{#FrbY zF#gT!x8FfqxFs5Lg^Zxveck&&`%o%b40Txi7S}nVept@3OCNm;_3cc(3x~2fk1q(d z{@r`xcoq@_Mby^W*4!Ku9h za(n_V=C8G*>1xCKu@Ng;EJ>y&I4eXr#Az1FLC+4Um^XQ!{Ykhsu`Qi+8F{ZTmXM&= z4$t5HjU@z&GJy|ecCT;pw)oi8Fgp>%8T9Hv9UiZjtQ?ZK8By#?9xQHpM%HHA7eQ}2 zgT65u5{mU#cq<0A@9r#BC(fQ2WhjRP1k2Agm?oedo!!IPx?KwN zT~FryLq+Orm#1Hj1MoEB!M1&$Eg#j-cFc4Q=|nWU_%DTp1XGQ0=p4!4Jm~!?Sr?D- z>7zL$LA*xi4}Ff9zxsFphgY&wFVPD#a#Zd;=Z=kK$^I1J_wW%a<6HjajlLDb)d9XZ z$53jdD7%-0_GL=TBb|Sf()l%00%sPAAoaTjR+|rOj>nPk(bBJa?^qMqOCto+OJ4*I zE%k4f%6pw7$vjwhUrKF#*IDp-ss&&Xef3Qu?l)5df_O=ZBWD)4M34x6T2;+H#A3J_ z;#%7OVVvVESl(}=>L;@n=b`So^A$8k&?6OrpW_t@C(U(?7rC9i_dM2gnyX|O9A>&#{jdvCQ2waA}_zR z^2>SNDg=@D@Sn0-u<>lF7Uej-pc6W%dPZnU*$O9$zMRH@-ESp z*iB;Yt}>M2R-u$j$7cHy&&8tmre&p5p8vT(O1GTswa=mTwpJI!|9Z?p!@|nH4kZcW zAXgufi-RzXoJ+64!{4qHB{QM<>A#3cNP(sw4PpRRJN0PbHntt+zZVv6`M*QD@lnfl zLy5A`6}0x-DT7+c^sxdyua*+aGlsFE-&?MxjT|g)Vw>euDHChoHjB4tZKqpS3njw2 z->P*Pc)pFHz+>md*Yl`sWsNY{{!1WAPUP5Tz_17vbC%@pzm~=2fAwiW~iq|hXS|=N#7mTe|2pjcUQL2&U0X0QV?i}N$;cxQZ+e1 z4D2)RW?^7NQnHxKw|7~49%fkdGH@>I06MYWQK$A0?;{o1NUOALbfI8FYF(J)i%lE4 z>im&4cTfdCfHJwKcD`^s_=WfTmnexwan}b(6VHuI-Vx**i5#qDEjGFKvML$EK@ zUR+DY|NP3>OnSJx_3Vcijqga%&DwRxd2MOEoNx=g32ih7Celmp_J#O{VGGXPSLhd3 z^p_Pndu9q;HwTbL%uUv_W$6UdGXLahgu886bY5_BYc>U#yH_i=OUS;6FWFYjAh=Yi z5N`UG1HaATx=Bo=0)ms~8>a_)Tv8y3kxLl$3J0~Wf~u+A7yH8vkDd4sPT7&y|5#13&&U|r>lOW_WMAMMe*xGp(E@*dl2f%rgnc6h9$d1k zO%(tAO`cT>mnM>2uc*CxK)=2Icr=RN`a+gZbjr6~6AP#=l9 zgflG2YQMNVXyF6s1Ff4wPYtM4VtH{9It;BaJ#d>rVTC>>x|f#RdmJ7Fh~9(*W+3-M zDrs7K)}&d?wYWB0vD_FI^{o(;E#!4Zw9sCJc$i$!VaDmuSv~9_fGF_=By`;AdrxgQ zg{6SPaq)8X!%gJD14UBt~^gaEBxqpl`(4vu3-j(}8>XVr$IgH>cZ>SdOq zljggkFSyEAhTgDDWXOF3+rRo`%x->dTis)pJ)>`FJH%ap0F?TxY!QI)^hH1sWc`043K57BH8s^ z)n+_-G%Lv+0JdQjb59Gr)ur65dzc^nBU)9YzDsc*_AWp>MZ&fAitg35FHEjmPKQmG z_Cu_x6l_iFd)LF-7?%BxlNFSsaT#_fln5!fR*z8pHo!-ahAq0@flj&{jsssYbV-D; zbzR*y!bOH#U2cJ16*oryO7*7OX#>Y*rJY8GCNPH$O9XZxtTqeZqx*xhVQEfl0cvh= zP^!JKD#qeTjJr?kK{I_;(o#>&>-$c{OPi-&q>aK+Y8yr?z!h`Cg4Ip_Kq2iPt}7Opn(kW$^fF26p7#DZzD}1-%lj9 zZSvlWpXr$Fq>3HiHbSNX{JNyRDZHZ1bTep4cXg^a^0si!XA$8qsvj5IO&q@mcNXJ# z5W^P@vy86cDy#vvjap+Z$73IEuGXRGBL4GFTpr66?!^TsZQ@f9)!U56s0;->3*x~J z+8v`O1Q7Q?x9{y5CyzreJ zjN4#!As-e|jqRiB#pk6Uq?&!Y@_vPNR(TUVX+6&T64i}t)571u^(iD_7~=iQIzjBM zp&4#x!W<^`@!3lSksg2Hne zABZ`A9NNfF@`}g$5#<(mDH-(L5Y(SGL9-)&`s^B}`k7NG)4Ht1`4ha1W$%{{2^8?CcI=n8wHbKUKVGk=~{5>_D_m{_*aygHKWk6KY4f+X8 zLs%eoaK3@|k3c`DQhK@X3(=p$?Px9oiLI@^TFO$Z3>3f1M@Ie^SvvZuIwOxr zZ}VHr)Z_C{%l*NdF7F6U06k8N{q*WG`3wErRp1PEgr^>W>wg%aooOpD6pCCl0*cjy zAUBrJO;^7x^lLZ@B0TebEp{&JoS&9=YaQ**XH; zv-o81ZDxQE3&5DjY~lj|g=o|T$2*+b_p5`n&&1} zy7t8(AIn`d_XR$rxy+ZlyHvH+9hLWwW3s+*EC?Hvof~r~N^i{>i2MXBO{AM)&G zow}w<7+czym~fB_Ooxvjf2Su2s3F!uHZs&3iyx{?87*YLn>YFD__SFs(WjwXn!eXm zS^wwqu#5hL$l@ie<83Q74I<|6a@`vR;U-ECWr4T8m;5;F7 z(;xNAnH{=nC-l>`02WXL1pxKPR=(Q!b;=*l^%3R>R$Hm}A{1kLCecOb8k< zJ(@Y_^6oUe`n-4hA=ha(t@?C4XHhvofO2Or{=!B%fPYi_H>J>~Z(PlSVCiD1xfjQ( zhMu=o}EEa&@Tp zbMTWEuHJKx6%?^t|A6ZJWWtZHO*Jc;K+=v`oZHzn>6wtQIyO(?^cd5buID@0L@(&q zqp*~FVs$3Rd6}={vB8FgAyh$aF_4UnZ^Vc_L3mu7U*4mC_h{Ucr7IXX*>8F!;7QWq_rS4-xOQky3dy(W z`qcqW<&#Vb%dQGoQk?_44u9mf*j>*^27OlCQcrc`U0|F>Klc$sTAWj^DbiQAaB)4E zcit2>C)koEBJjrKH#TYBr_BJ?9X&Mn(t3*T5zNHt}G#RnDM|hlo)2Et&`pP zP6mE_*$V0UN(6MIL#X$Q-e0ebDCThjBmqoSctg+4_QS zE2cT7IQz*JsJ|h=MO&10?kQp=cOK6lAvxJ>8>TGJ;GpJ(AJpv;@$=U0my(OCmk~O0 zsJ#44m9n4MP;Odv=jg1G&j?ucGKp1gAGI4vcKmwdukAB-jrT&HVc%j+nfW+>H?>^#1-!9;#{2L1&&7)-`%6V^Z$V> z52HcfkDIWC$*8Tw&%@q#v^_m#M`zyC(EW8Bq$+q$wU#*L3W>4u=M4MVO6cz1-b^9) z2ex;dwIvY_IDian<=-)Ow;dt+OpR`K8!&oeLSX9QcgG69`~<7x1|@I%3cLj(ce01y zL!*H<34{lH7DR&Pu;9(>!^h@C%rbQ4I%LREO#{?=MF;@g<=+)32HiiDQw6gDzmC^C zD%ycIkLNlIE;pG*x~y@Z>Wgm^eYPq7Bw(n|8S!>RSS^j$dCEN;Ys-NhZhxql=Z!EX zmMOk+uonxXtB4*4O4RR4n=YlL8}X0%+C-v9D|a%&p#Mx~WD!MD&HOTa%0ZN-c1a)-1|BLPPrFERJV;>#wi>`p1N-bl$J6*N74edVF8pL zp`;fQpo&Kq>|1D_UJtu|y?|>dVf^DHJ*)LpDo@-a1kEz3L z2|#i8$)Z1A}+1fBoEKoC^Er8wbq>G)Ib>r*H8D!n~InY(XZ z@k9KtGY`gI*yglL((XkeR(dRkCf-Ne5zSq9o@&Hbs1apHvPyl5;4c z0{;NK3}5X2%&ju$w4U~uMP;b4vBmL#yUpydD1yOkHrXnFB+EW4l@2A3MFnr7jnsgKEx<;06f52&p@0G|=N3wzS9}KN z^sK_)-FhabdLLBXrsou=UNr3DH9Jnse?cdd&dOeAXj1cOzt<6Gdkdx0UkJ_=vYXDk zK3V!$DwasO*)gs*v-GifmjO}WFtHYxflgh1-8>QGrbS~lig z$`(9Q*qR;AV=IfZ<}KpYPtY4bv>#NOx?xEL7^_}WT_(duUhfMHg*de-dCj#0?GMZZ zY;%KTtyQWF*jyoO7mqLmGL`iqNs^-v422AOnrZ%i@w?{>Tbt$Cjj*1jHYYR~aVb42 zJpvLh5I7io0q=h>1?Px6B@Z>4d*~Nv-FSh_PEF5A{sA#@zQR~e^nq%ER;Ya(mOjyL zt>)L;hT;a9qS<|scLB+3}@i+}qbopJ0cH=`itdTU-QW%ziIUnhXvD`Nv zitQej?&i`&y487aoAlUhAee@4dxJ?ZU#>hPY4li&ge^8)+>tNtdoj$IuW|>rYV$#A z8?SHo0q9SVc6F4UZ@zoXDbB5XY7zk-0<8#!V z&z$uV)?u1s;-Yg??WT~>61D`&)47k|oAt{7gcSo|$5Xs)_Atf=tF6-iR#H8D^OvG+ zAz?j$oAab|3^T&l-E>AN+;r3G^6%l{^uWnTbAB6#O=0n~#h`Hs(@G!m9hN{Pp(S)A zVf}nB1WkOK;;UrIf(=NLwq>%N=4rHN1l zj^5=Jj!SN1A^B*_{u}}Q{Da^L+n&SjNy9VgUd{HcJqe&zL1NtML*K1<0SQm44(&N0 zG~vy1@Kew>C8 zY5YA(*HaGqSy4Qddwp_6fTtRwnjGf&qh;{?G0NmEQK<7>cs&zez&V;9MDw^O@ZLhz zmU-obL=<1A^Ff5hI200edDSyR*R(<7C=s+_Ni^KtO-F+j3o1k;57#f#Y-KKWlv%&t&seM3F(cjthwB#C=Z z5Gh~3NfbFVRpqK`^|&6^*UW7qURz7g&&Q7k71(}+&7cMeH=B*F-Y>4&3*_H z%YLYDIP|!wLE(-UV}8wto^@Z%3nX$*hWwbWJyN2z$hhFFfg#kc9NI%C902i#g%k7l z_Bk9nHJ4}IY7Vr`fYl#xBv(uNrw!{3yA9h7ht6RMtn{NHq67df&^NzBmZpfLTtj|= zQGUT(enCclAyj@LIJ^>xs|&!<1>*dW#a9Z&j|m#N5KzHKDQxyg-D@vBJVq(*BXcaT zN}TUTm&xtBvi;E5oj_Mm9%R4u*6sfU*7O0%c!g~N&fZ{dPQxM|TF^(LsIr{^%Ryw% Obf{sn$7Pzb-~R)BkGysO diff --git a/matlab/examples/gtsamExamples.m b/matlab/examples/gtsamExamples.m index b3862088a..014885eea 100644 --- a/matlab/examples/gtsamExamples.m +++ b/matlab/examples/gtsamExamples.m @@ -22,7 +22,7 @@ function varargout = gtsamExamples(varargin) % Edit the above text to modify the response to help gtsamExamples -% Last Modified by GUIDE v2.5 13-Jun-2012 08:13:23 +% Last Modified by GUIDE v2.5 23-Jul-2012 13:12:19 % Begin initialization code - DO NOT EDIT gui_Singleton = 1; @@ -104,6 +104,20 @@ echo on Pose2SLAMExample echo off +% --- Executes on button press in Pose2SLAMCircle. +function Pose2SLAMCircle_Callback(hObject, eventdata, handles) +axes(handles.axes3); +echo on +Pose2SLAMExample_circle +echo off + +% --- Executes on button press in Pose2SLAMManhattan. +function Pose2SLAMManhattan_Callback(hObject, eventdata, handles) +axes(handles.axes3); +echo on +Pose2SLAMExample_graph +echo off + % --- Executes on button press in Pose3SLAM. function Pose3SLAM_Callback(hObject, eventdata, handles) axes(handles.axes3); @@ -111,6 +125,13 @@ echo on Pose3SLAMExample echo off +% --- Executes on button press in Pose3SLAMSphere. +function Pose3SLAMSphere_Callback(hObject, eventdata, handles) +axes(handles.axes3); +echo on +Pose3SLAMExample_graph +echo off + % --- Executes on button press in PlanarSLAM. function PlanarSLAM_Callback(hObject, eventdata, handles) axes(handles.axes3); @@ -118,6 +139,13 @@ echo on PlanarSLAMExample echo off +% --- Executes on button press in PlanarSLAMSampling. +function PlanarSLAMSampling_Callback(hObject, eventdata, handles) +axes(handles.axes3); +echo on +PlanarSLAMExample_sampling +echo off + % --- Executes on button press in SFM. function SFM_Callback(hObject, eventdata, handles) axes(handles.axes3); @@ -138,7 +166,3 @@ axes(handles.axes3); echo on StereoVOExample echo off - -% --- Executes on button press in Future. -function Future_Callback(hObject, eventdata, handles) -fprintf(1,'Future demo not implemented yet :-)\n'); From ec07bf400bd717db12af7be97a94eb6ac75b3d02 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 23 Jul 2012 19:21:08 +0000 Subject: [PATCH 21/68] In wrap tests, read file in text mode to translate line endings --- wrap/utilities.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 6eb47c55d..9c5d93c76 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -29,7 +29,7 @@ using namespace std; /* ************************************************************************* */ string file_contents(const string& filename, bool skipheader) { - ifstream ifs(filename.c_str(), ios::binary); // Do not do LF/CRLF translation - we always write in binary mode too + ifstream ifs(filename.c_str()); if(!ifs) throw CantOpenFile(filename); // read file into stringstream From 87d1e0a488603d8b1d87965fd63991c8222f5afc Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 23 Jul 2012 21:27:36 +0000 Subject: [PATCH 22/68] Added 'This' keyword in wrap templates to substitute instantiated class --- gtsam.h | 3 +++ wrap/Class.cpp | 32 ++++++++++++++++++--------- wrap/Class.h | 2 +- wrap/TemplateInstantiationTypedef.cpp | 2 +- 4 files changed, 26 insertions(+), 13 deletions(-) diff --git a/gtsam.h b/gtsam.h index b13bcb092..4e1437f06 100644 --- a/gtsam.h +++ b/gtsam.h @@ -66,6 +66,9 @@ * or with typedefs, e.g. * template class Class2 { ... }; * typedef Class2 MyInstantiatedClass; + * - In the class definition, appearances of the template argument(s) will be replaced with their + * instantiated types, e.g. 'void setValue(const T& value);'. + * - To refer to the instantiation of the template class itself, use 'This', i.e. 'static This Create();' * - To create new instantiations in other modules, you must copy-and-paste the whole class definition * into the new module, but use only your new instantiation types. * - When forward-declaring template instantiations, use the generated/typedefed name, e.g. diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 00fdba011..c1fac2c0b 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -217,7 +217,7 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wra } /* ************************************************************************* */ -vector expandArgumentListsTemplate(const vector& argLists, const string& templateArg, const vector& instName) { +vector expandArgumentListsTemplate(const vector& argLists, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { vector result; BOOST_FOREACH(const ArgumentList& argList, argLists) { ArgumentList instArgList; @@ -226,6 +226,9 @@ vector expandArgumentListsTemplate(const vector& arg if(arg.type == templateArg) { instArg.namespaces.assign(instName.begin(), instName.end()-1); instArg.type = instName.back(); + } else if(arg.type == "This") { + instArg.namespaces.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); + instArg.type = expandedClassName; } instArgList.push_back(instArg); } @@ -236,23 +239,29 @@ vector expandArgumentListsTemplate(const vector& arg /* ************************************************************************* */ template -map expandMethodTemplate(const map& methods, const string& templateArg, const vector& instName) { +map expandMethodTemplate(const map& methods, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { map result; typedef pair Name_Method; BOOST_FOREACH(const Name_Method& name_method, methods) { const METHOD& method = name_method.second; METHOD instMethod = method; - instMethod.argLists = expandArgumentListsTemplate(method.argLists, templateArg, instName); + instMethod.argLists = expandArgumentListsTemplate(method.argLists, templateArg, instName, expandedClassNamespace, expandedClassName); instMethod.returnVals.clear(); BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) { ReturnValue instRetVal = retVal; if(retVal.type1 == templateArg) { instRetVal.namespaces1.assign(instName.begin(), instName.end()-1); instRetVal.type1 = instName.back(); + } else if(retVal.type1 == "This") { + instRetVal.namespaces1.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); + instRetVal.type1 = expandedClassName; } if(retVal.type2 == templateArg) { instRetVal.namespaces2.assign(instName.begin(), instName.end()-1); instRetVal.type2 = instName.back(); + } else if(retVal.type1 == "This") { + instRetVal.namespaces2.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); + instRetVal.type2 = expandedClassName; } instMethod.returnVals.push_back(instRetVal); } @@ -262,18 +271,18 @@ map expandMethodTemplate(const map& methods, con } /* ************************************************************************* */ -Class expandClassTemplate(const Class& cls, const string& templateArg, const vector& instName) { +Class expandClassTemplate(const Class& cls, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { Class inst; inst.name = cls.name; inst.templateArgs = cls.templateArgs; inst.typedefName = cls.typedefName; inst.isVirtual = cls.isVirtual; inst.qualifiedParent = cls.qualifiedParent; - inst.methods = expandMethodTemplate(cls.methods, templateArg, instName); - inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg, instName); + inst.methods = expandMethodTemplate(cls.methods, templateArg, instName, expandedClassNamespace, expandedClassName); + inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg, instName, expandedClassNamespace, expandedClassName); inst.namespaces = cls.namespaces; inst.constructor = cls.constructor; - inst.constructor.args_list = expandArgumentListsTemplate(cls.constructor.args_list, templateArg, instName); + inst.constructor.args_list = expandArgumentListsTemplate(cls.constructor.args_list, templateArg, instName, expandedClassNamespace, expandedClassName); inst.constructor.name = inst.name; inst.deconstructor = cls.deconstructor; inst.deconstructor.name = inst.name; @@ -285,8 +294,9 @@ Class expandClassTemplate(const Class& cls, const string& templateArg, const vec vector Class::expandTemplate(const string& templateArg, const vector >& instantiations) const { vector result; BOOST_FOREACH(const vector& instName, instantiations) { - Class inst = expandClassTemplate(*this, templateArg, instName); - inst.name = name + instName.back(); + const string expandedName = name + instName.back(); + Class inst = expandClassTemplate(*this, templateArg, instName, this->namespaces, expandedName); + inst.name = expandedName; inst.templateArgs.clear(); inst.typedefName = qualifiedName("::") + "<" + wrap::qualifiedName("::", instName) + ">"; result.push_back(inst); @@ -295,8 +305,8 @@ vector Class::expandTemplate(const string& templateArg, const vector& instantiation) const { - return expandClassTemplate(*this, templateArg, instantiation); +Class Class::expandTemplate(const string& templateArg, const vector& instantiation, const std::vector& expandedClassNamespace, const string& expandedClassName) const { + return expandClassTemplate(*this, templateArg, instantiation, expandedClassNamespace, expandedClassName); } /* ************************************************************************* */ diff --git a/wrap/Class.h b/wrap/Class.h index 2d10d691a..6b60f8219 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -57,7 +57,7 @@ struct Class { std::string qualifiedName(const std::string& delim = "") const; ///< creates a namespace-qualified name, optional delimiter std::vector expandTemplate(const std::string& templateArg, const std::vector >& instantiations) const; - Class expandTemplate(const std::string& templateArg, const std::vector& instantiation) const; + Class expandTemplate(const std::string& templateArg, const std::vector& instantiation, const std::vector& expandedClassNamespace, const std::string& expandedClassName) const; // The typedef line for this class, if this class is a typedef, otherwise returns an empty string. std::string getTypedef() const; diff --git a/wrap/TemplateInstantiationTypedef.cpp b/wrap/TemplateInstantiationTypedef.cpp index da1e8f616..b822f732b 100644 --- a/wrap/TemplateInstantiationTypedef.cpp +++ b/wrap/TemplateInstantiationTypedef.cpp @@ -42,7 +42,7 @@ namespace wrap { // Instantiate it Class classInst = *clsIt; for(size_t i = 0; i < typeList.size(); ++i) - classInst = classInst.expandTemplate(classInst.templateArgs[i], typeList[i]); + classInst = classInst.expandTemplate(classInst.templateArgs[i], typeList[i], namespaces, name); // Fix class properties classInst.name = name; From 0b08923c41a3f4fe1cae7f58e1df915c3fed05f3 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 23 Jul 2012 21:27:38 +0000 Subject: [PATCH 23/68] Generated code formatting fix --- wrap/StaticMethod.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 3def66b0f..61b54434c 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -131,8 +131,6 @@ string StaticMethod::wrapper_fragment(FileWriter& file, // unwrap arguments, see Argument.cpp args.matlab_unwrap(file,0); // We start at 0 because there is no self object - file.oss << " "; - // call method with default type and wrap result if (returnVal.type1!="void") returnVal.wrap_result(cppClassName+"::"+name+"("+args.names()+")", file, typeAttributes); From bcad0b661cbb676b85ab421787b1f1a51561e196 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 23 Jul 2012 21:27:40 +0000 Subject: [PATCH 24/68] Added workaround for MSVC limitation on number of consecutive loops --- wrap/Module.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 8c564e92f..229a9810f 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -558,13 +558,15 @@ void Module::WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::st const string cppName = cls.qualifiedName("::"); const string collectorType = "Collector_" + matlabUniqueName; const string collectorName = "collector_" + matlabUniqueName; + // The extra curly-braces around the for loops work around a limitation in MSVC (existing + // since 2005!) preventing more than 248 blocks. wrapperFile.oss << - " for(" << collectorType << "::iterator iter = " << collectorName << ".begin();\n" + " { for(" << collectorType << "::iterator iter = " << collectorName << ".begin();\n" " iter != " << collectorName << ".end(); ) {\n" " delete *iter;\n" " " << collectorName << ".erase(iter++);\n" " anyDeleted = true;\n" - " }\n"; + " } }\n"; } wrapperFile.oss << " if(anyDeleted)\n" From cd69779754ee0bacc339ddb122be7ef11d7af296 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 23 Jul 2012 21:27:42 +0000 Subject: [PATCH 25/68] Wrapped GeneralSFMFactor and added placeholders for Cal3DS2 once it has a 'calibrate' function --- gtsam.h | 58 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 57 insertions(+), 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index 4e1437f06..af2dc3a3b 100644 --- a/gtsam.h +++ b/gtsam.h @@ -593,6 +593,43 @@ virtual class SimpleCamera : gtsam::Value { double range(const gtsam::Pose3& point); // FIXME, overload }; +// TODO: Add this back in when Cal3DS2 has a calibrate function +//template +//virtual class PinholeCamera : gtsam::Value { +// // Standard Constructors and Named Constructors +// PinholeCamera(); +// PinholeCamera(const gtsam::Pose3& pose); +// PinholeCamera(const gtsam::Pose3& pose, const gtsam::Cal3DS2& K); +// static This Level(const gtsam::Cal3DS2& K, +// const gtsam::Pose2& pose, double height); +// static This Level(const gtsam::Pose2& pose, double height); // FIXME overload +// static This Lookat(const gtsam::Point3& eye, +// const gtsam::Point3& target, const gtsam::Point3& upVector, +// const gtsam::Cal3DS2& K); +// +// // Testable +// void print(string s) const; +// bool equals(const This& camera, double tol) const; +// +// // Standard Interface +// gtsam::Pose3 pose() const; +// CALIBRATION calibration() const; +// +// // Manifold +// This retract(const Vector& d) const; +// Vector localCoordinates(const This& T2) const; +// size_t dim() const; +// static size_t Dim(); +// +// // Transformations and measurement functions +// static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); +// pair projectSafe(const gtsam::Point3& pw) const; +// gtsam::Point2 project(const gtsam::Point3& point); +// gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; +// double range(const gtsam::Point3& point); +// double range(const gtsam::Pose3& point); // FIXME, overload +//}; + //************************************************************************* // inference //************************************************************************* @@ -1388,7 +1425,26 @@ virtual class GenericProjectionFactor : gtsam::NonlinearFactor { CALIBRATION* calibration() const; }; typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; +// FIXME: Add Cal3DS2 when it has a 'calibrate' function +//typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; + + +#include +template +virtual class GeneralSFMFactor : gtsam::NonlinearFactor { + GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); + gtsam::Point2 measured() const; +}; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +// FIXME: Add Cal3DS2 when it has a 'calibrate' function +//typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; + +// FIXME: Add Cal3DS2 when it has a 'calibrate' function +template +virtual class GeneralSFMFactor2 : gtsam::NonlinearFactor { + GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); + gtsam::Point2 measured() const; +}; } //\namespace gtsam From 1db1663800943cc44e4069e89f8cf1bafcfd7f47 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 23 Jul 2012 21:27:44 +0000 Subject: [PATCH 26/68] Updated MATLAB SBAExample --- matlab/examples/SBAExample.m | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/matlab/examples/SBAExample.m b/matlab/examples/SBAExample.m index a0f003eeb..9d5d22047 100644 --- a/matlab/examples/SBAExample.m +++ b/matlab/examples/SBAExample.m @@ -30,7 +30,8 @@ cameraNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1 ... 0.001*ones(1,5)]'; %% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph) -graph = sparseBA.Graph; +import gtsam.* +graph = NonlinearFactorGraph; %% Add factors for all measurements @@ -39,7 +40,7 @@ measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma); for i=1:length(data.Z) for k=1:length(data.Z{i}) j = data.J{i}{k}; - graph.addSimpleCameraMeasurement(data.Z{i}{k}, measurementNoise, symbol('c',i), symbol('p',j)); + graph.add(GeneralSFMFactorCal3_S2(data.Z{i}{k}, measurementNoise, symbol('c',i), symbol('p',j))); end end @@ -47,10 +48,10 @@ end import gtsam.* cameraPriorNoise = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas); firstCamera = SimpleCamera(truth.cameras{1}.pose, truth.K); -graph.addSimpleCameraPrior(symbol('c',1), firstCamera, cameraPriorNoise); +graph.add(PriorFactorSimpleCamera(symbol('c',1), firstCamera, cameraPriorNoise)); pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); -graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise); +graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise)); %% Print the graph graph.print(sprintf('\nFactor graph:\n')); @@ -58,15 +59,15 @@ graph.print(sprintf('\nFactor graph:\n')); %% Initialize cameras and points close to ground truth in this example import gtsam.* -initialEstimate = sparseBA.Values; +initialEstimate = Values; for i=1:size(truth.cameras,2) pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1)); camera_i = SimpleCamera(pose_i, truth.K); - initialEstimate.insertSimpleCamera(symbol('c',i), camera_i); + initialEstimate.insert(symbol('c',i), camera_i); end for j=1:size(truth.points,2) point_j = truth.points{j}.retract(0.1*randn(3,1)); - initialEstimate.insertPoint(symbol('p',j), point_j); + initialEstimate.insert(symbol('p',j), point_j); end initialEstimate.print(sprintf('\nInitial estimate:\n ')); @@ -77,7 +78,7 @@ parameters = LevenbergMarquardtParams; parameters.setlambdaInitial(1.0); parameters.setVerbosityLM('trylambda'); -optimizer = graph.optimizer(initialEstimate, parameters); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, parameters); for i=1:5 optimizer.iterate(); From a99595dda8608027319c7c82c4fb3014d077304d Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 23 Jul 2012 22:15:05 +0000 Subject: [PATCH 27/68] Updated some MATLAB utility plotting functions --- matlab/examples/PlanarSLAMExample.m | 2 +- matlab/examples/PlanarSLAMExample_sampling.m | 2 +- matlab/examples/Pose3SLAMExample_graph.m | 5 +++-- matlab/load3D.m | 16 +++++++++------- matlab/plot2DPoints.m | 9 ++++++--- matlab/plotPoint2.m | 7 +++---- 6 files changed, 23 insertions(+), 18 deletions(-) diff --git a/matlab/examples/PlanarSLAMExample.m b/matlab/examples/PlanarSLAMExample.m index f82b3aea4..9b4600578 100644 --- a/matlab/examples/PlanarSLAMExample.m +++ b/matlab/examples/PlanarSLAMExample.m @@ -73,7 +73,7 @@ cla;hold on marginals = Marginals(graph, result); plot2DTrajectory(result, [], marginals); -plot2DPoints(result, marginals); +plot2DPoints(result, [], marginals); plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-'); plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-'); diff --git a/matlab/examples/PlanarSLAMExample_sampling.m b/matlab/examples/PlanarSLAMExample_sampling.m index 35d3232a1..58686c362 100644 --- a/matlab/examples/PlanarSLAMExample_sampling.m +++ b/matlab/examples/PlanarSLAMExample_sampling.m @@ -51,7 +51,7 @@ cla;hold on marginals = Marginals(graph, sample); plot2DTrajectory(sample, [], marginals); -plot2DPoints(sample, marginals); +plot2DPoints(sample, [], marginals); for j=1:2 key = symbol('l',j); diff --git a/matlab/examples/Pose3SLAMExample_graph.m b/matlab/examples/Pose3SLAMExample_graph.m index b6a5444a2..6efb58199 100644 --- a/matlab/examples/Pose3SLAMExample_graph.m +++ b/matlab/examples/Pose3SLAMExample_graph.m @@ -28,13 +28,14 @@ cla first = initial.at(0); plot3(first.x(),first.y(),first.z(),'r*'); hold on plot3DTrajectory(initial,'g-',false); +drawnow; %% Read again, now with all constraints, and optimize import gtsam.* graph = load3D(datafile, model, false, N); graph.add(NonlinearEqualityPose3(0, first)); -optimizer = DoglegOptimizer(graph, initial); +optimizer = LevenbergMarquardtOptimizer(graph, initial); result = optimizer.optimizeSafely(); plot3DTrajectory(result, 'r-', false); axis equal; -view(0); axis equal; \ No newline at end of file +view(3); axis equal; \ No newline at end of file diff --git a/matlab/load3D.m b/matlab/load3D.m index 5c9d08146..c0ccd545f 100644 --- a/matlab/load3D.m +++ b/matlab/load3D.m @@ -3,6 +3,8 @@ function [graph,initial] = load3D(filename,model,successive,N) % cannot read noise model from file yet, uses specified model % if [successive] is tru, constructs initial estimate from odometry +import gtsam.* + if nargin<3, successive=false; end fid = fopen(filename); if fid < 0 @@ -15,10 +17,10 @@ fclose(fid); lines=columns{1}; % loop over lines and add vertices -graph = pose3SLAM.Graph; -initial = pose3SLAM.Values; +graph = NonlinearFactorGraph; +initial = Values; origin=gtsam.Pose3; -initial.insertPose(0,origin); +initial.insert(0,origin); n=size(lines,1); if nargin<4, N=n;end @@ -30,7 +32,7 @@ for i=1:n if (~successive && i1i1 - initial.insertPose(i2,initial.pose(i1).compose(dpose)); + initial.insert(i2,initial.at(i1).compose(dpose)); else - initial.insertPose(i1,initial.pose(i2).compose(dpose.inverse)); + initial.insert(i1,initial.at(i2).compose(dpose.inverse)); end end end diff --git a/matlab/plot2DPoints.m b/matlab/plot2DPoints.m index 0065b55c4..d03676699 100644 --- a/matlab/plot2DPoints.m +++ b/matlab/plot2DPoints.m @@ -1,4 +1,4 @@ -function plot2DPoints(values, marginals) +function plot2DPoints(values, linespec, marginals) %PLOT2DPOINTS Plots the Point2's in a values, with optional covariances % Finds all the Point2 objects in the given Values object and plots them. % If a Marginals object is given, this function will also plot marginal @@ -6,6 +6,9 @@ function plot2DPoints(values, marginals) import gtsam.* +if ~exist('linespec', 'var') || isempty(linespec) + linespec = 'g'; +end haveMarginals = exist('marginals', 'var'); keys = KeyVector(values.keys); @@ -19,9 +22,9 @@ for i = 0:keys.size-1 if isa(p, 'gtsam.Point2') if haveMarginals P = marginals.marginalCovariance(key); - plotPoint2(p, 'g', P); + plotPoint2(p, linespec, P); else - plotPoint2(p, 'g'); + plotPoint2(p, linespec); end end end diff --git a/matlab/plotPoint2.m b/matlab/plotPoint2.m index ae45455f6..fcc274d90 100644 --- a/matlab/plotPoint2.m +++ b/matlab/plotPoint2.m @@ -1,11 +1,10 @@ function plotPoint2(p,color,P) -% plotPose2: show a Pose2, possibly with covariance matrix +% plotPoint2: show a Point2, possibly with covariance matrix if size(color,2)==1 plot(p.x,p.y,[color '*']); else plot(p.x,p.y,color); end -if nargin>2 - pPp = P(1:2,1:2); % covariance matrix in pose coordinate frame - covarianceEllipse([p.x;p.y],pPp,color(1)); +if exist('P', 'var') + covarianceEllipse([p.x;p.y],P,color(1)); end \ No newline at end of file From da598b428d10fe905f1d5cafb6ba0651dba25768 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 23 Jul 2012 22:15:08 +0000 Subject: [PATCH 28/68] Updated matlab SFMExample --- matlab/examples/SFMExample.m | 35 +++++++++++++++-------------------- 1 file changed, 15 insertions(+), 20 deletions(-) diff --git a/matlab/examples/SFMExample.m b/matlab/examples/SFMExample.m index 7c4baa75b..1416fb7e3 100644 --- a/matlab/examples/SFMExample.m +++ b/matlab/examples/SFMExample.m @@ -29,7 +29,8 @@ pointNoiseSigma = 0.1; poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; %% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph) -graph = visualSLAM.Graph; +import gtsam.* +graph = NonlinearFactorGraph; %% Add factors for all measurements import gtsam.* @@ -37,29 +38,30 @@ measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma); for i=1:length(data.Z) for k=1:length(data.Z{i}) j = data.J{i}{k}; - graph.addMeasurement(data.Z{i}{k}, measurementNoise, symbol('x',i), symbol('p',j), data.K); + graph.add(GenericProjectionFactorCal3_S2(data.Z{i}{k}, measurementNoise, symbol('x',i), symbol('p',j), data.K)); end end %% Add Gaussian priors for a pose and a landmark to constrain the system import gtsam.* posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); -graph.addPosePrior(symbol('x',1), truth.cameras{1}.pose, posePriorNoise); +graph.add(PriorFactorPose3(symbol('x',1), truth.cameras{1}.pose, posePriorNoise)); pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); -graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise); +graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise)); %% Print the graph graph.print(sprintf('\nFactor graph:\n')); %% Initialize cameras and points close to ground truth in this example -initialEstimate = visualSLAM.Values; +import gtsam.* +initialEstimate = Values; for i=1:size(truth.cameras,2) pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1)); - initialEstimate.insertPose(symbol('x',i), pose_i); + initialEstimate.insert(symbol('x',i), pose_i); end for j=1:size(truth.points,2) point_j = truth.points{j}.retract(0.1*randn(3,1)); - initialEstimate.insertPoint(symbol('p',j), point_j); + initialEstimate.insert(symbol('p',j), point_j); end initialEstimate.print(sprintf('\nInitial estimate:\n ')); @@ -70,7 +72,7 @@ parameters = LevenbergMarquardtParams; parameters.setlambdaInitial(1.0); parameters.setVerbosityLM('trylambda'); -optimizer = graph.optimizer(initialEstimate, parameters); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, parameters); for i=1:5 optimizer.iterate(); @@ -79,21 +81,14 @@ result = optimizer.values(); result.print(sprintf('\nFinal result:\n ')); %% Plot results with covariance ellipses -marginals = graph.marginals(result); +import gtsam.* +marginals = Marginals(graph, result); cla hold on; -for j=1:result.nrPoints - P = marginals.marginalCovariance(symbol('p',j)); - point_j = result.point(symbol('p',j)); - plot3(point_j.x, point_j.y, point_j.z,'marker','o'); - covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); -end -for i=1:result.nrPoses - P = marginals.marginalCovariance(symbol('x',i)); - pose_i = result.pose(symbol('x',i)); - plotPose3(pose_i,P,10); -end +plot3DPoints(result, [], marginals); +plot3DTrajectory(result, '*', 1, 8, marginals); + axis([-40 40 -40 40 -10 20]);axis equal view(3) colormap('hot') From f56d9c18e5442d3fbe0b4b0d6679fdec284fe986 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 23 Jul 2012 22:41:53 +0000 Subject: [PATCH 29/68] Removed SLAM namespace from testDoglegOptimizer --- tests/testDoglegOptimizer.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 9a4f6bf25..3c5409af7 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -15,14 +15,13 @@ * @author Richard Roberts */ -#include #include #include #include #include #include #include -#include +#include #include #include From 0863b4148dc1b6779859a4783f56a2527a69e521 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 23 Jul 2012 22:42:42 +0000 Subject: [PATCH 30/68] Removed SLAM namespace from testGaussianISAM2 --- tests/testGaussianISAM2.cpp | 106 +++++++++++++++++++----------------- 1 file changed, 56 insertions(+), 50 deletions(-) diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 2c932680d..67fa287d3 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -6,15 +6,21 @@ #include #include +#include +#include #include -#include #include #include #include #include +#include +#include +#include #include +#include +#include +#include #include -#include #include @@ -39,21 +45,21 @@ SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas(Vector_(2, M_PI/100.0, 0.1 ISAM2 createSlamlikeISAM2( boost::optional init_values = boost::none, - boost::optional full_graph = boost::none, + boost::optional full_graph = boost::none, const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)) { // These variables will be reused and accumulate factors and values ISAM2 isam(params); Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; // i keeps track of the time step size_t i = 0; // Add a prior at time 0 and update isam { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + NonlinearFactorGraph newfactors; + newfactors.add(PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -65,8 +71,8 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 0 to time 5 for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -78,10 +84,10 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 5 to 6 and landmark measurement at time 5 { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); + newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); fullgraph.push_back(newfactors); Values init; @@ -98,8 +104,8 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 6 to time 10 for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -111,10 +117,10 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 10 to 11 and landmark measurement at time 10 { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); + newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); fullgraph.push_back(newfactors); Values init; @@ -298,8 +304,8 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) { // typedef GaussianISAM2::Impl Impl; // // Ordering ordering; ordering += (0), (0), (1); -// planarSLAM::Graph graph; -// graph.addPosePrior((0), Pose2(), noiseModel::Unit::Create(Pose2::dimension)); +// NonlinearFactorGraph graph; +// graph.add(PriorFactor((0), Pose2(), noiseModel::Unit::Create(Pose2::dimension)); // graph.addRange((0), (0), 1.0, noiseModel::Unit::Create(1)); // // FastSet expected; @@ -378,7 +384,7 @@ TEST(ISAM2, optimize2) { } /* ************************************************************************* */ -bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) { +bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) { TestResult& result_ = result; const std::string name_ = test.getName(); @@ -439,7 +445,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) { // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); // Compare solutions @@ -451,7 +457,7 @@ TEST(ISAM2, slamlike_solution_dogleg) { // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); // Compare solutions @@ -463,7 +469,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr) { // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR)); // Compare solutions @@ -475,7 +481,7 @@ TEST(ISAM2, slamlike_solution_dogleg_qr) { // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR)); // Compare solutions @@ -584,13 +590,13 @@ TEST(ISAM2, removeFactors) // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); // Remove the 2nd measurement on landmark 0 (Key 100) FastVector toRemove; toRemove.push_back(12); - isam.update(planarSLAM::Graph(), Values(), toRemove); + isam.update(NonlinearFactorGraph(), Values(), toRemove); // Remove the factor from the full system fullgraph.remove(12); @@ -604,14 +610,14 @@ TEST_UNSAFE(ISAM2, removeVariables) { // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); // Remove the measurement on landmark 0 (Key 100) FastVector toRemove; toRemove.push_back(7); toRemove.push_back(14); - isam.update(planarSLAM::Graph(), Values(), toRemove); + isam.update(NonlinearFactorGraph(), Values(), toRemove); // Remove the factors and variable from the full system fullgraph.remove(7); @@ -629,7 +635,7 @@ TEST_UNSAFE(ISAM2, swapFactors) // then swaps the 2nd-to-last landmark measurement with a different one Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph); // Remove the measurement on landmark 0 and replace with a different one @@ -639,15 +645,15 @@ TEST_UNSAFE(ISAM2, swapFactors) toRemove.push_back(swap_idx); fullgraph.remove(swap_idx); - planarSLAM::Graph swapfactors; -// swapfactors.addBearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); // original factor - swapfactors.addBearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise); + NonlinearFactorGraph swapfactors; +// swapfactors.add(BearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); // original factor + swapfactors.add(BearingRangeFactor(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise)); fullgraph.push_back(swapfactors); isam.update(swapfactors, Values(), toRemove); } // Compare solutions - EXPECT(assert_equal(fullgraph, planarSLAM::Graph(isam.getFactorsUnsafe()))); + EXPECT(assert_equal(fullgraph, NonlinearFactorGraph(isam.getFactorsUnsafe()))); EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); // Check gradient at each node @@ -685,7 +691,7 @@ TEST(ISAM2, constrained_ordering) // These variables will be reused and accumulate factors and values ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; // We will constrain x3 and x4 to the end FastMap constrained; @@ -697,8 +703,8 @@ TEST(ISAM2, constrained_ordering) // Add a prior at time 0 and update isam { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + NonlinearFactorGraph newfactors; + newfactors.add(PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -712,8 +718,8 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 0 to time 5 for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -728,10 +734,10 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 5 to 6 and landmark measurement at time 5 { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); + newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); fullgraph.push_back(newfactors); Values init; @@ -748,8 +754,8 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 6 to time 10 for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -761,10 +767,10 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 10 to 11 and landmark measurement at time 10 { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); + newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); fullgraph.push_back(newfactors); Values init; @@ -816,7 +822,7 @@ TEST(ISAM2, slamlike_solution_partial_relinearization_check) // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false); params.enablePartialRelinearizationCheck = true; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, params); From fb33b8a609d1ca81302d3cf6347d12fcc4eeee59 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 23 Jul 2012 22:43:17 +0000 Subject: [PATCH 31/68] removed SLAM namespace from testGaussianJunctionTreeB --- tests/testGaussianJunctionTreeB.cpp | 48 ++++++++++++++++------------- 1 file changed, 26 insertions(+), 22 deletions(-) diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 883011da2..422e52377 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -16,14 +16,18 @@ */ #include -#include -#include +#include +#include +#include +#include +#include #include #include #include #include #include -#include +#include +#include #include #include #include @@ -128,29 +132,29 @@ TEST( GaussianJunctionTreeB, optimizeMultiFrontal2) /* ************************************************************************* */ TEST(GaussianJunctionTreeB, slamlike) { Values init; - planarSLAM::Graph newfactors; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph newfactors; + NonlinearFactorGraph fullgraph; SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas(Vector_(2, M_PI/100.0, 0.1)); size_t i = 0; - newfactors = planarSLAM::Graph(); - newfactors.addPosePrior(X(0), Pose2(0.0, 0.0, 0.0), odoNoise); + newfactors = NonlinearFactorGraph(); + newfactors.add(PriorFactor(X(0), Pose2(0.0, 0.0, 0.0), odoNoise)); init.insert(X(0), Pose2(0.01, 0.01, 0.01)); fullgraph.push_back(newfactors); for( ; i<5; ++i) { - newfactors = planarSLAM::Graph(); - newfactors.addRelativePose(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors = NonlinearFactorGraph(); + newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullgraph.push_back(newfactors); } - newfactors = planarSLAM::Graph(); - newfactors.addRelativePose(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(X(i), L(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(X(i), L(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + newfactors = NonlinearFactorGraph(); + newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors.add(BearingRangeFactor(X(i), L(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); + newfactors.add(BearingRangeFactor(X(i), L(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); init.insert(X(i+1), Pose2(1.01, 0.01, 0.01)); init.insert(L(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); init.insert(L(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); @@ -158,16 +162,16 @@ TEST(GaussianJunctionTreeB, slamlike) { ++ i; for( ; i<5; ++i) { - newfactors = planarSLAM::Graph(); - newfactors.addRelativePose(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors = NonlinearFactorGraph(); + newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullgraph.push_back(newfactors); } - newfactors = planarSLAM::Graph(); - newfactors.addRelativePose(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(X(i), L(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(X(i), L(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors = NonlinearFactorGraph(); + newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors.add(BearingRangeFactor(X(i), L(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); + newfactors.add(BearingRangeFactor(X(i), L(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); init.insert(X(i+1), Pose2(6.9, 0.1, 0.01)); fullgraph.push_back(newfactors); ++ i; @@ -193,9 +197,9 @@ TEST(GaussianJunctionTreeB, simpleMarginal) { typedef BayesTree GaussianBayesTree; // Create a simple graph - pose2SLAM::Graph fg; - fg.addPosePrior(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0)); - fg.addRelativePose(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector_(3, 10.0, 1.0, 1.0))); + NonlinearFactorGraph fg; + fg.add(PriorFactor(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0))); + fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector_(3, 10.0, 1.0, 1.0)))); Values init; init.insert(X(0), Pose2()); From a641f599f67e3c7ae635c335ea15e3eed7693a9c Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 23 Jul 2012 22:44:02 +0000 Subject: [PATCH 32/68] Removed SLAM namespace from testGradientDescentOptimizer --- tests/testGradientDescentOptimizer.cpp | 46 ++++++++++++++------------ 1 file changed, 25 insertions(+), 21 deletions(-) diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index fefa2a05f..c72c6f716 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -5,8 +5,12 @@ * @date Jun 11, 2012 */ -#include +#include +#include #include +#include +#include +#include #include @@ -19,34 +23,34 @@ using namespace std; using namespace gtsam; -boost::tuple generateProblem() { +boost::tuple generateProblem() { // 1. Create graph container and add factors to it - pose2SLAM::Graph graph ; + NonlinearFactorGraph graph ; // 2a. Add Gaussian prior Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - graph.addPosePrior(1, priorMean, priorNoise); + graph.add(PriorFactor(1, priorMean, priorNoise)); // 2b. Add odometry factors SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); - graph.addRelativePose(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addRelativePose(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addRelativePose(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise)); + graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); // 2c. Add pose constraint SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addRelativePose(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); + graph.add(BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty)); // 3. Create the data structure to hold the initialEstimate estinmate to the solution - pose2SLAM::Values initialEstimate; - Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1); - Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2); - Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insertPose(3, x3); - Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insertPose(4, x4); - Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insertPose(5, x5); + Values initialEstimate; + Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insert(1, x1); + Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insert(2, x2); + Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insert(3, x3); + Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insert(4, x4); + Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insert(5, x5); return boost::tie(graph, initialEstimate); } @@ -55,8 +59,8 @@ boost::tuple generateProblem() { /* ************************************************************************* */ TEST(optimize, GradientDescentOptimizer) { - pose2SLAM::Graph graph ; - pose2SLAM::Values initialEstimate; + NonlinearFactorGraph graph; + Values initialEstimate; boost::tie(graph, initialEstimate) = generateProblem(); // cout << "initial error = " << graph.error(initialEstimate) << endl ; @@ -79,8 +83,8 @@ TEST(optimize, GradientDescentOptimizer) { /* ************************************************************************* */ TEST(optimize, ConjugateGradientOptimizer) { - pose2SLAM::Graph graph ; - pose2SLAM::Values initialEstimate; + NonlinearFactorGraph graph; + Values initialEstimate; boost::tie(graph, initialEstimate) = generateProblem(); // cout << "initial error = " << graph.error(initialEstimate) << endl ; @@ -102,8 +106,8 @@ TEST(optimize, ConjugateGradientOptimizer) { /* ************************************************************************* */ TEST(optimize, GradientDescentOptimizer2) { - pose2SLAM::Graph graph ; - pose2SLAM::Values initialEstimate; + NonlinearFactorGraph graph; + Values initialEstimate; boost::tie(graph, initialEstimate) = generateProblem(); // cout << "initial error = " << graph.error(initialEstimate) << endl ; From 2de44904adf3c2f864d9114367d820f5161a484a Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 23 Jul 2012 22:45:07 +0000 Subject: [PATCH 33/68] Removed SLAM namespace from testGraph --- gtsam/inference/graph.h | 2 ++ tests/testGraph.cpp | 17 +++++++++-------- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/gtsam/inference/graph.h b/gtsam/inference/graph.h index 98d41940b..88ab5d0e8 100644 --- a/gtsam/inference/graph.h +++ b/gtsam/inference/graph.h @@ -108,3 +108,5 @@ namespace gtsam { } // namespace gtsam + +#include diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 5f99d60c6..93ecd6dbe 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -16,8 +16,10 @@ * @brief unit test for graph-inl.h */ -#include -#include +#include +#include +#include +#include #include @@ -75,13 +77,13 @@ TEST( Graph, predecessorMap2Graph ) /* ************************************************************************* */ TEST( Graph, composePoses ) { - pose2SLAM::Graph graph; + NonlinearFactorGraph graph; SharedNoiseModel cov = noiseModel::Unit::Create(3); Pose2 p1(1.0, 2.0, 0.3), p2(4.0, 5.0, 0.6), p3(7.0, 8.0, 0.9), p4(2.0, 2.0, 2.9); Pose2 p12=p1.between(p2), p23=p2.between(p3), p43=p4.between(p3); - graph.addRelativePose(1,2, p12, cov); - graph.addRelativePose(2,3, p23, cov); - graph.addRelativePose(4,3, p43, cov); + graph.add(BetweenFactor(1,2, p12, cov)); + graph.add(BetweenFactor(2,3, p23, cov)); + graph.add(BetweenFactor(4,3, p43, cov)); PredecessorMap tree; tree.insert(1,2); @@ -91,8 +93,7 @@ TEST( Graph, composePoses ) Pose2 rootPose = p2; - boost::shared_ptr actual = composePoses (graph, tree, rootPose); + boost::shared_ptr actual = composePoses, Pose2, Key> (graph, tree, rootPose); Values expected; expected.insert(1, p1); From 0e6762e5c9eae8269783a5753cecf0f2a6b3309a Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 23 Jul 2012 22:45:46 +0000 Subject: [PATCH 34/68] Removed SLAM namespace from testInferenceB --- tests/testInferenceB.cpp | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/tests/testInferenceB.cpp b/tests/testInferenceB.cpp index 3e68780c4..9b8127473 100644 --- a/tests/testInferenceB.cpp +++ b/tests/testInferenceB.cpp @@ -17,10 +17,17 @@ #include +#include +#include +#include +#include +#include #include #include #include -#include +#include +#include +#include #include @@ -54,16 +61,16 @@ TEST( inference, marginals ) /* ************************************************************************* */ TEST( inference, marginals2) { - planarSLAM::Graph fg; + NonlinearFactorGraph fg; SharedDiagonal poseModel(noiseModel::Isotropic::Sigma(3, 0.1)); SharedDiagonal pointModel(noiseModel::Isotropic::Sigma(3, 0.1)); - fg.addPosePrior(X(0), Pose2(), poseModel); - fg.addRelativePose(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel); - fg.addRelativePose(X(1), X(2), Pose2(1.0,0.0,0.0), poseModel); - fg.addBearingRange(X(0), L(0), Rot2(), 1.0, pointModel); - fg.addBearingRange(X(1), L(0), Rot2(), 1.0, pointModel); - fg.addBearingRange(X(2), L(0), Rot2(), 1.0, pointModel); + fg.add(PriorFactor(X(0), Pose2(), poseModel)); + fg.add(BetweenFactor(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel)); + fg.add(BetweenFactor(X(1), X(2), Pose2(1.0,0.0,0.0), poseModel)); + fg.add(BearingRangeFactor(X(0), L(0), Rot2(), 1.0, pointModel)); + fg.add(BearingRangeFactor(X(1), L(0), Rot2(), 1.0, pointModel)); + fg.add(BearingRangeFactor(X(2), L(0), Rot2(), 1.0, pointModel)); Values init; init.insert(X(0), Pose2(0.0,0.0,0.0)); From c8f3356af5698ad5dbf0fdc9cef482960ef81dec Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 23 Jul 2012 22:46:21 +0000 Subject: [PATCH 35/68] Removed SLAM namespace from testNonlinearEquality --- tests/testNonlinearEquality.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 1efc40cad..2b580560f 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -17,12 +17,17 @@ #include #include -#include +#include #include #include #include #include +#include #include +#include +#include +#include +#include #include @@ -510,7 +515,7 @@ static Cal3_S2 K(fov,w,h); static boost::shared_ptr shK(new Cal3_S2(K)); // typedefs for visual SLAM example -typedef visualSLAM::Graph VGraph; +typedef NonlinearFactorGraph VGraph; // factors for visual slam typedef NonlinearEquality2 Point3Equality; @@ -537,13 +542,13 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { VGraph graph; // create equality constraints for poses - graph.addPoseConstraint(x1, camera1.pose()); - graph.addPoseConstraint(x2, camera2.pose()); + graph.add(NonlinearEquality(x1, camera1.pose())); + graph.add(NonlinearEquality(x2, camera2.pose())); // create factors SharedDiagonal vmodel = noiseModel::Unit::Create(3); - graph.addMeasurement(camera1.project(landmark), vmodel, x1, l1, shK); - graph.addMeasurement(camera2.project(landmark), vmodel, x2, l2, shK); + graph.add(GenericProjectionFactor(camera1.project(landmark), vmodel, x1, l1, shK)); + graph.add(GenericProjectionFactor(camera2.project(landmark), vmodel, x2, l2, shK)); // add equality constraint graph.add(Point3Equality(l1, l2)); From 7a0030070c7008d3a6b81b3e486c7dd1e40bf7b3 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 23 Jul 2012 22:46:55 +0000 Subject: [PATCH 36/68] Removed SLAM namespace from testNonlinearISAM --- tests/testNonlinearISAM.cpp | 36 ++++++++++++++++++++---------------- 1 file changed, 20 insertions(+), 16 deletions(-) diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 980ea8033..142fba7eb 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -5,13 +5,17 @@ #include -#include +#include +#include #include +#include +#include #include -#include +#include +#include + using namespace gtsam; -using namespace planarSLAM; typedef NonlinearISAM PlanarISAM; @@ -27,22 +31,22 @@ TEST(testNonlinearISAM, markov_chain ) { // create initial graph Pose2 cur_pose; // start at origin - Graph start_factors; - start_factors.addPoseConstraint(0, cur_pose); + NonlinearFactorGraph start_factors; + start_factors.add(NonlinearEquality(0, cur_pose)); - planarSLAM::Values init; - planarSLAM::Values expected; - init.insertPose(0, cur_pose); - expected.insertPose(0, cur_pose); + Values init; + Values expected; + init.insert(0, cur_pose); + expected.insert(0, cur_pose); isam.update(start_factors, init); // loop for a period of time to verify memory usage size_t nrPoses = 21; Pose2 z(1.0, 2.0, 0.1); for (size_t i=1; i<=nrPoses; ++i) { - Graph new_factors; - new_factors.addRelativePose(i-1, i, z, model); - planarSLAM::Values new_init; + NonlinearFactorGraph new_factors; + new_factors.add(BetweenFactor(i-1, i, z, model)); + Values new_init; // perform a check on changing orderings if (i == 5) { @@ -60,15 +64,15 @@ TEST(testNonlinearISAM, markov_chain ) { } cur_pose = cur_pose.compose(z); - new_init.insertPose(i, cur_pose.retract(sampler.sample())); - expected.insertPose(i, cur_pose); + new_init.insert(i, cur_pose.retract(sampler.sample())); + expected.insert(i, cur_pose); isam.update(new_factors, new_init); } // verify values - all but the last one should be very close - planarSLAM::Values actual = isam.estimate(); + Values actual = isam.estimate(); for (size_t i=0; i(i), actual.at(i), tol)); } } From 01bcd9e9395ab346ff655a5be2ee9525e7cff51a Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 23 Jul 2012 22:47:31 +0000 Subject: [PATCH 37/68] Removed SLAM namespace from testNonlinearOptimizer --- tests/testNonlinearOptimizer.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index eb7585420..cb5e8f482 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -16,7 +16,10 @@ */ #include -#include +#include +#include +#include +#include #include #include #include @@ -24,6 +27,7 @@ #include #include #include +#include #include #include @@ -174,9 +178,9 @@ TEST( NonlinearOptimizer, Factorization ) config.insert(X(1), Pose2(0.,0.,0.)); config.insert(X(2), Pose2(1.5,0.,0.)); - pose2SLAM::Graph graph; - graph.addPosePrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); - graph.addRelativePose(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); + NonlinearFactorGraph graph; + graph.add(PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10))); + graph.add(BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1))); Ordering ordering; ordering.push_back(X(1)); From c2daf40c5df71258abb6f498af2ac37be044035f Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 23 Jul 2012 22:50:21 +0000 Subject: [PATCH 38/68] Removed SLAM namespaces from testSerializationSLAM. Instead of testing each SLAM namespace individually, a single test was created that instanciates all variable-factor combinations. However, there is currently an issue with LieVector that should be resolved. --- tests/testSerializationSLAM.cpp | 525 +++++++++++++++++++++++++------- 1 file changed, 422 insertions(+), 103 deletions(-) diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index d9094111e..27f74c222 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -18,11 +18,31 @@ #include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include #include @@ -30,17 +50,70 @@ using namespace std; using namespace gtsam; using namespace gtsam::serializationTestHelpers; +// Creating as many permutations of factors as possible +typedef PriorFactor PriorFactorLieVector; +typedef PriorFactor PriorFactorLieMatrix; +typedef PriorFactor PriorFactorPoint2; +typedef PriorFactor PriorFactorStereoPoint2; +typedef PriorFactor PriorFactorPoint3; +typedef PriorFactor PriorFactorRot2; +typedef PriorFactor PriorFactorRot3; +typedef PriorFactor PriorFactorPose2; +typedef PriorFactor PriorFactorPose3; +typedef PriorFactor PriorFactorCal3_S2; +typedef PriorFactor PriorFactorCal3DS2; +typedef PriorFactor PriorFactorCalibratedCamera; +typedef PriorFactor PriorFactorSimpleCamera; + +typedef BetweenFactor BetweenFactorLieVector; +typedef BetweenFactor BetweenFactorLieMatrix; +typedef BetweenFactor BetweenFactorPoint2; +typedef BetweenFactor BetweenFactorPoint3; +typedef BetweenFactor BetweenFactorRot2; +typedef BetweenFactor BetweenFactorRot3; +typedef BetweenFactor BetweenFactorPose2; +typedef BetweenFactor BetweenFactorPose3; + +typedef NonlinearEquality NonlinearEqualityLieVector; +typedef NonlinearEquality NonlinearEqualityLieMatrix; +typedef NonlinearEquality NonlinearEqualityPoint2; +typedef NonlinearEquality NonlinearEqualityStereoPoint2; +typedef NonlinearEquality NonlinearEqualityPoint3; +typedef NonlinearEquality NonlinearEqualityRot2; +typedef NonlinearEquality NonlinearEqualityRot3; +typedef NonlinearEquality NonlinearEqualityPose2; +typedef NonlinearEquality NonlinearEqualityPose3; +typedef NonlinearEquality NonlinearEqualityCal3_S2; +typedef NonlinearEquality NonlinearEqualityCal3DS2; +typedef NonlinearEquality NonlinearEqualityCalibratedCamera; +typedef NonlinearEquality NonlinearEqualitySimpleCamera; + +typedef RangeFactor RangeFactorPosePoint2; +typedef RangeFactor RangeFactorPosePoint3; +typedef RangeFactor RangeFactorPose2; +typedef RangeFactor RangeFactorPose3; +typedef RangeFactor RangeFactorCalibratedCameraPoint; +typedef RangeFactor RangeFactorSimpleCameraPoint; +typedef RangeFactor RangeFactorCalibratedCamera; +typedef RangeFactor RangeFactorSimpleCamera; + +typedef BearingFactor BearingFactor2D; +typedef BearingFactor BearingFactor3D; + +typedef BearingRangeFactor BearingRangeFactor2D; +typedef BearingRangeFactor BearingRangeFactor3D; + +typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; +typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; + + // Convenience for named keys using symbol_shorthand::X; using symbol_shorthand::L; -/* Create GUIDs for factors */ -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); +/* Create GUIDs for Noisemodels */ /* ************************************************************************* */ -// Export Noisemodels BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); @@ -50,6 +123,82 @@ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropi BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +/* Create GUIDs for geometry */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT(gtsam::LieVector); +BOOST_CLASS_EXPORT(gtsam::LieMatrix); +BOOST_CLASS_EXPORT(gtsam::Point2); +BOOST_CLASS_EXPORT(gtsam::StereoPoint2); +BOOST_CLASS_EXPORT(gtsam::Point3); +BOOST_CLASS_EXPORT(gtsam::Rot2); +BOOST_CLASS_EXPORT(gtsam::Rot3); +BOOST_CLASS_EXPORT(gtsam::Pose2); +BOOST_CLASS_EXPORT(gtsam::Pose3); +BOOST_CLASS_EXPORT(gtsam::Cal3_S2); +BOOST_CLASS_EXPORT(gtsam::Cal3DS2); +BOOST_CLASS_EXPORT(gtsam::CalibratedCamera); +BOOST_CLASS_EXPORT(gtsam::SimpleCamera); + + +/* Create GUIDs for factors */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); + +BOOST_CLASS_EXPORT_GUID(PriorFactorLieVector, "gtsam::PriorFactorLieVector"); +BOOST_CLASS_EXPORT_GUID(PriorFactorLieMatrix, "gtsam::PriorFactorLieMatrix"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3"); +BOOST_CLASS_EXPORT_GUID(PriorFactorRot2, "gtsam::PriorFactorRot2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorRot3, "gtsam::PriorFactorRot3"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPose2, "gtsam::PriorFactorPose2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3"); +BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera"); +BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera"); + +BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorLieMatrix, "gtsam::BetweenFactorLieMatrix"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3"); + +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieVector, "gtsam::NonlinearEqualityLieVector"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieMatrix, "gtsam::NonlinearEqualityLieMatrix"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot2, "gtsam::NonlinearEqualityRot2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot3, "gtsam::NonlinearEqualityRot3"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose2, "gtsam::NonlinearEqualityPose2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera"); + +BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint2, "gtsam::RangeFactorPosePoint2"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint3, "gtsam::RangeFactorPosePoint3"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3"); +BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint"); +BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint"); +BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); +BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); + +BOOST_CLASS_EXPORT_GUID(BearingFactor2D, "gtsam::BearingFactor2D"); + +BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); + +BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2"); +BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2"); + + /* ************************************************************************* */ TEST (Serialization, smallExample_linear) { using namespace example; @@ -85,12 +234,11 @@ TEST (Serialization, gaussianISAM) { BOOST_CLASS_EXPORT_GUID(simulated2D::Prior, "gtsam::simulated2D::Prior" ) BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry" ) BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement") -BOOST_CLASS_EXPORT(gtsam::Point2) /* ************************************************************************* */ -TEST (Serialization, smallExample) { +TEST (Serialization, smallExample_nonlinear) { using namespace example; - Graph nfg = createNonlinearFactorGraph(); + NonlinearFactorGraph nfg = createNonlinearFactorGraph(); Values c1 = createValues(); EXPECT(equalsObj(nfg)); EXPECT(equalsXML(nfg)); @@ -100,113 +248,284 @@ TEST (Serialization, smallExample) { } /* ************************************************************************* */ -/* Create GUIDs for factors */ -BOOST_CLASS_EXPORT_GUID(planarSLAM::Prior, "gtsam::planarSLAM::Prior"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Bearing, "gtsam::planarSLAM::Bearing"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Range, "gtsam::planarSLAM::Range"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::BearingRange,"gtsam::planarSLAM::BearingRange"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Odometry, "gtsam::planarSLAM::Odometry"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint, "gtsam::planarSLAM::Constraint"); +TEST (Serialization, factors) { -BOOST_CLASS_EXPORT(gtsam::Pose2) + LieVector lieVector(4, 1.0, 2.0, 3.0, 4.0); + LieMatrix lieMatrix(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0); + Point2 point2(1.0, 2.0); + StereoPoint2 stereoPoint2(1.0, 2.0, 3.0); + Point3 point3(1.0, 2.0, 3.0); + Rot2 rot2(1.0); + Rot3 rot3(Rot3::RzRyRx(1.0, 2.0, 3.0)); + Pose2 pose2(rot2, point2); + Pose3 pose3(rot3, point3); + Cal3_S2 cal3_s2(1.0, 2.0, 3.0, 4.0, 5.0); + Cal3DS2 cal3ds2(1.0, 2.0, 3.0, 4.0, 5.0,6.0, 7.0, 8.0, 9.0); + CalibratedCamera calibratedCamera(pose3); + SimpleCamera simpleCamera(pose3, cal3_s2); + + + Symbol a01('a',1), a02('a',2), a03('a',3), a04('a',4), a05('a',5), a06('a',6), a07('a',7), a08('a',8), a09('a',9), a10('a',10), a11('a',11), a12('a',12), a13('a',13); + Symbol b01('b',1), b02('b',2), b03('b',3), b04('b',4), b05('b',5), b06('b',6), b07('b',7), b08('b',8), b09('b',9), b10('b',10), b11('b',11), b12('b',12), b13('b',13); + + Values values; +// values.insert(a01, lieVector); +// values.insert(a02, lieMatrix); + values.insert(a03, point2); + values.insert(a04, stereoPoint2); + values.insert(a05, point3); + values.insert(a06, rot2); + values.insert(a07, rot3); + values.insert(a08, pose2); + values.insert(a09, pose3); + values.insert(a10, cal3_s2); + values.insert(a11, cal3ds2); + values.insert(a12, calibratedCamera); + values.insert(a13, simpleCamera); -/* ************************************************************************* */ -TEST (Serialization, planar_system) { - using namespace planarSLAM; - planarSLAM::Values values; - Symbol i2('x',2), i3('x',3), i4('x',4), i9('x',9), j3('l',3), j5('l',5), j9('l',9); - values.insert(j3, Point2(1.0, 2.0)); - values.insert(i4, Pose2(1.0, 2.0, 0.3)); SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3); SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); + SharedNoiseModel model4 = noiseModel::Isotropic::Sigma(4, 0.3); + SharedNoiseModel model5 = noiseModel::Isotropic::Sigma(5, 0.3); + SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3); + SharedNoiseModel model9 = noiseModel::Isotropic::Sigma(9, 0.3); + SharedNoiseModel model11 = noiseModel::Isotropic::Sigma(11, 0.3); - Prior prior(i3, Pose2(0.1,-0.3, 0.2), model1); - Bearing bearing(i3, j5, Rot2::fromDegrees(0.5), model1); - Range range(i2, j9, 7.0, model1); - BearingRange bearingRange(i2, j3, Rot2::fromDegrees(0.6), 2.0, model2); - Odometry odometry(i2, i3, Pose2(1.0, 2.0, 0.3), model3); - Constraint constraint(i9, Pose2(2.0,-1.0, 0.2)); - Graph graph; - graph.add(prior); - graph.add(bearing); - graph.add(range); - graph.add(bearingRange); - graph.add(odometry); - graph.add(constraint); + + PriorFactorLieVector priorFactorLieVector(a01, lieVector, model4); + PriorFactorLieMatrix priorFactorLieMatrix(a02, lieMatrix, model6); + PriorFactorPoint2 priorFactorPoint2(a03, point2, model2); + PriorFactorStereoPoint2 priorFactorStereoPoint2(a04, stereoPoint2, model3); + PriorFactorPoint3 priorFactorPoint3(a05, point3, model3); + PriorFactorRot2 priorFactorRot2(a06, rot2, model1); + PriorFactorRot3 priorFactorRot3(a07, rot3, model3); + PriorFactorPose2 priorFactorPose2(a08, pose2, model3); + PriorFactorPose3 priorFactorPose3(a09, pose3, model6); + PriorFactorCal3_S2 priorFactorCal3_S2(a10, cal3_s2, model5); + PriorFactorCal3DS2 priorFactorCal3DS2(a11, cal3ds2, model9); + PriorFactorCalibratedCamera priorFactorCalibratedCamera(a12, calibratedCamera, model6); + PriorFactorSimpleCamera priorFactorSimpleCamera(a13, simpleCamera, model11); + + BetweenFactorLieVector betweenFactorLieVector(a01, b01, lieVector, model4); + BetweenFactorLieMatrix betweenFactorLieMatrix(a02, b02, lieMatrix, model6); + BetweenFactorPoint2 betweenFactorPoint2(a03, b03, point2, model2); + BetweenFactorPoint3 betweenFactorPoint3(a05, b05, point3, model3); + BetweenFactorRot2 betweenFactorRot2(a06, b06, rot2, model1); + BetweenFactorRot3 betweenFactorRot3(a07, b07, rot3, model3); + BetweenFactorPose2 betweenFactorPose2(a08, b08, pose2, model3); + BetweenFactorPose3 betweenFactorPose3(a09, b09, pose3, model6); + + NonlinearEqualityLieVector nonlinearEqualityLieVector(a01, lieVector); + NonlinearEqualityLieMatrix nonlinearEqualityLieMatrix(a02, lieMatrix); + NonlinearEqualityPoint2 nonlinearEqualityPoint2(a03, point2); + NonlinearEqualityStereoPoint2 nonlinearEqualityStereoPoint2(a04, stereoPoint2); + NonlinearEqualityPoint3 nonlinearEqualityPoint3(a05, point3); + NonlinearEqualityRot2 nonlinearEqualityRot2(a06, rot2); + NonlinearEqualityRot3 nonlinearEqualityRot3(a07, rot3); + NonlinearEqualityPose2 nonlinearEqualityPose2(a08, pose2); + NonlinearEqualityPose3 nonlinearEqualityPose3(a09, pose3); + NonlinearEqualityCal3_S2 nonlinearEqualityCal3_S2(a10, cal3_s2); + NonlinearEqualityCal3DS2 nonlinearEqualityCal3DS2(a11, cal3ds2); + NonlinearEqualityCalibratedCamera nonlinearEqualityCalibratedCamera(a12, calibratedCamera); + NonlinearEqualitySimpleCamera nonlinearEqualitySimpleCamera(a13, simpleCamera); + + RangeFactorPosePoint2 rangeFactorPosePoint2(a08, a03, 2.0, model1); + RangeFactorPosePoint3 rangeFactorPosePoint3(a09, a05, 2.0, model1); + RangeFactorPose2 rangeFactorPose2(a08, b08, 2.0, model1); + RangeFactorPose3 rangeFactorPose3(a09, b09, 2.0, model1); + RangeFactorCalibratedCameraPoint rangeFactorCalibratedCameraPoint(a12, a05, 2.0, model1); + RangeFactorSimpleCameraPoint rangeFactorSimpleCameraPoint(a13, a05, 2.0, model1); + RangeFactorCalibratedCamera rangeFactorCalibratedCamera(a12, b12, 2.0, model1); + RangeFactorSimpleCamera rangeFactorSimpleCamera(a13, b13, 2.0, model1); + + BearingFactor2D bearingFactor2D(a08, a03, rot2, model1); + + BearingRangeFactor2D bearingRangeFactor2D(a08, a03, rot2, 2.0, model2); + + GenericProjectionFactorCal3_S2 genericProjectionFactorCal3_S2(point2, model2, a09, a05, boost::make_shared(cal3_s2)); + GenericProjectionFactorCal3DS2 genericProjectionFactorCal3DS2(point2, model2, a09, a05, boost::make_shared(cal3ds2)); + + + + + NonlinearFactorGraph graph; +// graph.add(priorFactorLieVector); +// graph.add(priorFactorLieMatrix); + graph.add(priorFactorPoint2); + graph.add(priorFactorStereoPoint2); + graph.add(priorFactorPoint3); + graph.add(priorFactorRot2); + graph.add(priorFactorRot3); + graph.add(priorFactorPose2); + graph.add(priorFactorPose3); + graph.add(priorFactorCal3_S2); + graph.add(priorFactorCal3_S2); + graph.add(priorFactorCalibratedCamera); + graph.add(priorFactorSimpleCamera); + +// graph.add(betweenFactorLieVector); +// graph.add(betweenFactorLieMatrix); + graph.add(betweenFactorPoint2); + graph.add(betweenFactorPoint3); + graph.add(betweenFactorRot2); + graph.add(betweenFactorRot3); + graph.add(betweenFactorPose2); + graph.add(betweenFactorPose3); + +// graph.add(nonlinearEqualityLieVector); +// graph.add(nonlinearEqualityLieMatrix); + graph.add(nonlinearEqualityPoint2); + graph.add(nonlinearEqualityStereoPoint2); + graph.add(nonlinearEqualityPoint3); + graph.add(nonlinearEqualityRot2); + graph.add(nonlinearEqualityRot3); + graph.add(nonlinearEqualityPose2); + graph.add(nonlinearEqualityPose3); + graph.add(nonlinearEqualityCal3_S2); + graph.add(nonlinearEqualityCal3DS2); + graph.add(nonlinearEqualityCalibratedCamera); + graph.add(nonlinearEqualitySimpleCamera); + + graph.add(rangeFactorPosePoint2); + graph.add(rangeFactorPosePoint3); + graph.add(rangeFactorPose2); + graph.add(rangeFactorPose3); + graph.add(rangeFactorCalibratedCameraPoint); + graph.add(rangeFactorSimpleCameraPoint); + graph.add(rangeFactorCalibratedCamera); + graph.add(rangeFactorSimpleCamera); + + graph.add(bearingFactor2D); + + graph.add(bearingRangeFactor2D); + + graph.add(genericProjectionFactorCal3_S2); + graph.add(genericProjectionFactorCal3DS2); + // text - EXPECT(equalsObj(i2)); - EXPECT(equalsObj(j3)); - EXPECT(equalsObj(values)); - EXPECT(equalsObj(prior)); - EXPECT(equalsObj(bearing)); - EXPECT(equalsObj(bearingRange)); - EXPECT(equalsObj(range)); - EXPECT(equalsObj(odometry)); - EXPECT(equalsObj(constraint)); - EXPECT(equalsObj(graph)); + EXPECT(equalsObj(a01)); + EXPECT(equalsObj(b02)); + EXPECT(equalsObj(values)); + EXPECT(equalsObj(graph)); + +// EXPECT(equalsObj(priorFactorLieVector)); +// EXPECT(equalsObj(priorFactorLieMatrix)); + EXPECT(equalsObj(priorFactorPoint2)); + EXPECT(equalsObj(priorFactorStereoPoint2)); + EXPECT(equalsObj(priorFactorPoint3)); + EXPECT(equalsObj(priorFactorRot2)); + EXPECT(equalsObj(priorFactorRot3)); + EXPECT(equalsObj(priorFactorPose2)); + EXPECT(equalsObj(priorFactorPose3)); + EXPECT(equalsObj(priorFactorCal3_S2)); + EXPECT(equalsObj(priorFactorCal3DS2)); + EXPECT(equalsObj(priorFactorCalibratedCamera)); + EXPECT(equalsObj(priorFactorSimpleCamera)); + +// EXPECT(equalsObj(betweenFactorLieVector)); +// EXPECT(equalsObj(betweenFactorLieMatrix)); + EXPECT(equalsObj(betweenFactorPoint2)); + EXPECT(equalsObj(betweenFactorPoint3)); + EXPECT(equalsObj(betweenFactorRot2)); + EXPECT(equalsObj(betweenFactorRot3)); + EXPECT(equalsObj(betweenFactorPose2)); + EXPECT(equalsObj(betweenFactorPose3)); + +// EXPECT(equalsObj(nonlinearEqualityLieVector)); +// EXPECT(equalsObj(nonlinearEqualityLieMatrix)); + EXPECT(equalsObj(nonlinearEqualityPoint2)); + EXPECT(equalsObj(nonlinearEqualityStereoPoint2)); + EXPECT(equalsObj(nonlinearEqualityPoint3)); + EXPECT(equalsObj(nonlinearEqualityRot2)); + EXPECT(equalsObj(nonlinearEqualityRot3)); + EXPECT(equalsObj(nonlinearEqualityPose2)); + EXPECT(equalsObj(nonlinearEqualityPose3)); + EXPECT(equalsObj(nonlinearEqualityCal3_S2)); + EXPECT(equalsObj(nonlinearEqualityCal3DS2)); + EXPECT(equalsObj(nonlinearEqualityCalibratedCamera)); + EXPECT(equalsObj(nonlinearEqualitySimpleCamera)); + + EXPECT(equalsObj(rangeFactorPosePoint2)); + EXPECT(equalsObj(rangeFactorPosePoint3)); + EXPECT(equalsObj(rangeFactorPose2)); + EXPECT(equalsObj(rangeFactorPose3)); + EXPECT(equalsObj(rangeFactorCalibratedCameraPoint)); + EXPECT(equalsObj(rangeFactorSimpleCameraPoint)); + EXPECT(equalsObj(rangeFactorCalibratedCamera)); + EXPECT(equalsObj(rangeFactorSimpleCamera)); + + EXPECT(equalsObj(bearingFactor2D)); + + EXPECT(equalsObj(bearingRangeFactor2D)); + + EXPECT(equalsObj(genericProjectionFactorCal3_S2)); + EXPECT(equalsObj(genericProjectionFactorCal3DS2)); + // xml - EXPECT(equalsXML(i2)); - EXPECT(equalsXML(j3)); - EXPECT(equalsXML(values)); - EXPECT(equalsXML(prior)); - EXPECT(equalsXML(bearing)); - EXPECT(equalsXML(bearingRange)); - EXPECT(equalsXML(range)); - EXPECT(equalsXML(odometry)); - EXPECT(equalsXML(constraint)); - EXPECT(equalsXML(graph)); + EXPECT(equalsXML(a01)); + EXPECT(equalsXML(b02)); + EXPECT(equalsXML(values)); + EXPECT(equalsXML(graph)); + +// EXPECT(equalsXML(priorFactorLieVector)); +// EXPECT(equalsXML(priorFactorLieMatrix)); + EXPECT(equalsXML(priorFactorPoint2)); + EXPECT(equalsXML(priorFactorStereoPoint2)); + EXPECT(equalsXML(priorFactorPoint3)); + EXPECT(equalsXML(priorFactorRot2)); + EXPECT(equalsXML(priorFactorRot3)); + EXPECT(equalsXML(priorFactorPose2)); + EXPECT(equalsXML(priorFactorPose3)); + EXPECT(equalsXML(priorFactorCal3_S2)); + EXPECT(equalsXML(priorFactorCal3DS2)); + EXPECT(equalsXML(priorFactorCalibratedCamera)); + EXPECT(equalsXML(priorFactorSimpleCamera)); + +// EXPECT(equalsXML(betweenFactorLieVector)); +// EXPECT(equalsXML(betweenFactorLieMatrix)); + EXPECT(equalsXML(betweenFactorPoint2)); + EXPECT(equalsXML(betweenFactorPoint3)); + EXPECT(equalsXML(betweenFactorRot2)); + EXPECT(equalsXML(betweenFactorRot3)); + EXPECT(equalsXML(betweenFactorPose2)); + EXPECT(equalsXML(betweenFactorPose3)); + +// EXPECT(equalsXML(nonlinearEqualityLieVector)); +// EXPECT(equalsXML(nonlinearEqualityLieMatrix)); + EXPECT(equalsXML(nonlinearEqualityPoint2)); + EXPECT(equalsXML(nonlinearEqualityStereoPoint2)); + EXPECT(equalsXML(nonlinearEqualityPoint3)); + EXPECT(equalsXML(nonlinearEqualityRot2)); + EXPECT(equalsXML(nonlinearEqualityRot3)); + EXPECT(equalsXML(nonlinearEqualityPose2)); + EXPECT(equalsXML(nonlinearEqualityPose3)); + EXPECT(equalsXML(nonlinearEqualityCal3_S2)); + EXPECT(equalsXML(nonlinearEqualityCal3DS2)); + EXPECT(equalsXML(nonlinearEqualityCalibratedCamera)); + EXPECT(equalsXML(nonlinearEqualitySimpleCamera)); + + EXPECT(equalsXML(rangeFactorPosePoint2)); + EXPECT(equalsXML(rangeFactorPosePoint3)); + EXPECT(equalsXML(rangeFactorPose2)); + EXPECT(equalsXML(rangeFactorPose3)); + EXPECT(equalsXML(rangeFactorCalibratedCameraPoint)); + EXPECT(equalsXML(rangeFactorSimpleCameraPoint)); + EXPECT(equalsXML(rangeFactorCalibratedCamera)); + EXPECT(equalsXML(rangeFactorSimpleCamera)); + + EXPECT(equalsXML(bearingFactor2D)); + + EXPECT(equalsXML(bearingRangeFactor2D)); + + EXPECT(equalsXML(genericProjectionFactorCal3_S2)); + EXPECT(equalsXML(genericProjectionFactorCal3DS2)); } -/* ************************************************************************* */ -/* Create GUIDs for factors */ -BOOST_CLASS_EXPORT_GUID(visualSLAM::PoseConstraint, "gtsam::visualSLAM::PoseConstraint"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PointConstraint, "gtsam::visualSLAM::PointConstraint"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PosePrior, "gtsam::visualSLAM::PosePrior"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PointPrior, "gtsam::visualSLAM::PointPrior"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::ProjectionFactor,"gtsam::visualSLAM::ProjectionFactor"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoFactor"); - -BOOST_CLASS_EXPORT(gtsam::Pose3) -BOOST_CLASS_EXPORT(gtsam::Point3) - -static Point3 pt3(1.0, 2.0, 3.0); -static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); -static Pose3 pose3(rt3, pt3); -static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); - -/* ************************************************************************* */ -TEST (Serialization, visual_system) { - using namespace visualSLAM; - visualSLAM::Values values; - Symbol x1('x',1), x2('x',2); - Symbol l1('l',1), l2('l',2); - Pose3 pose1 = pose3, pose2 = pose3.inverse(); - Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0); - values.insert(x1, pose1); - values.insert(l1, pt1); - SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); - SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); - SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3); - boost::shared_ptr K(new Cal3_S2(cal1)); - - Graph graph; - graph.addMeasurement(Point2(1.0, 2.0), model2, x1, l1, K); - graph.addPointConstraint(l1, pt1); - graph.addPointPrior(l1, pt2, model3); - graph.addPoseConstraint(x1, pose1); - graph.addPosePrior(x1, pose2, model6); - - EXPECT(equalsObj(values)); - EXPECT(equalsObj(graph)); - - EXPECT(equalsXML(values)); - EXPECT(equalsXML(graph)); -} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } From e2e780de4d1633a812887e67be19e6cf269115ff Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 23 Jul 2012 22:51:18 +0000 Subject: [PATCH 39/68] Removed SLAM namespaces from testLinearizedFactor --- gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp index d15bcd150..9c0438e49 100644 --- a/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp @@ -6,7 +6,8 @@ #include #include #include -#include +#include +#include #include From 1b935dbdc5aed9279d2da7efd5b27fb981dec5ab Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 24 Jul 2012 13:35:04 +0000 Subject: [PATCH 40/68] Added matlab utility functions for plotting 3D points --- matlab/plot3DPoints.m | 36 ++++++++++++++++++++++++++++++++++++ matlab/plotPoint3.m | 13 +++++++++++++ 2 files changed, 49 insertions(+) create mode 100644 matlab/plot3DPoints.m create mode 100644 matlab/plotPoint3.m diff --git a/matlab/plot3DPoints.m b/matlab/plot3DPoints.m new file mode 100644 index 000000000..264298ef4 --- /dev/null +++ b/matlab/plot3DPoints.m @@ -0,0 +1,36 @@ +function plot3DPoints(values, linespec, marginals) +%PLOT3DPOINTS Plots the Point3's in a values, with optional covariances +% Finds all the Point3 objects in the given Values object and plots them. +% If a Marginals object is given, this function will also plot marginal +% covariance ellipses for each point. + +import gtsam.* + +if ~exist('linespec', 'var') || isempty(linespec) + linespec = 'g'; +end +haveMarginals = exist('marginals', 'var'); +keys = KeyVector(values.keys); + +holdstate = ishold; +hold on + +% Plot points and covariance matrices +for i = 0:keys.size-1 + key = keys.at(i); + p = values.at(key); + if isa(p, 'gtsam.Point3') + if haveMarginals + P = marginals.marginalCovariance(key); + plotPoint3(p, linespec, P); + else + plotPoint3(p, linespec); + end + end +end + +if ~holdstate + hold off +end + +end diff --git a/matlab/plotPoint3.m b/matlab/plotPoint3.m new file mode 100644 index 000000000..89d5613d5 --- /dev/null +++ b/matlab/plotPoint3.m @@ -0,0 +1,13 @@ +function plotPoint3(p, color, P) +%PLOTPOINT3 Plot a Point3 with an optional covariance matrix +if size(color,2)==1 + plot3(p.x,p.y,p.z,[color '*']); +else + plot3(p.x,p.y,p.z,color); +end +if exist('P', 'var') + covarianceEllipse3D([p.x;p.y;p.z],P); +end + +end + From cceebbf41c434d554067cabb00788343e58ce41f Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 24 Jul 2012 13:35:11 +0000 Subject: [PATCH 41/68] Moved utility functions that are example-specific to a 'support' namespace inside the examples folder --- matlab/CMakeLists.txt | 6 ++---- matlab/VisualISAM_gui.fig | Bin 9761 -> 0 bytes .../+support}/VisualISAMGenerateData.m | 0 .../+support}/VisualISAMInitialize.m | 3 ++- matlab/{ => examples/+support}/VisualISAMPlot.m | 0 matlab/{ => examples/+support}/VisualISAMStep.m | 0 .../{ => +support}/findExampleDataFile.m | 4 ++-- matlab/examples/Pose2SLAMExample_graph.m | 2 +- matlab/examples/Pose3SLAMExample_graph.m | 2 +- matlab/examples/SBAExample.m | 2 +- matlab/examples/VisualISAMExample.m | 10 +++++----- matlab/examples/VisualISAM_gui.fig | Bin 0 -> 10226 bytes matlab/{ => examples}/VisualISAM_gui.m | 14 +++++++------- 13 files changed, 21 insertions(+), 22 deletions(-) delete mode 100644 matlab/VisualISAM_gui.fig rename matlab/{ => examples/+support}/VisualISAMGenerateData.m (100%) rename matlab/{ => examples/+support}/VisualISAMInitialize.m (93%) rename matlab/{ => examples/+support}/VisualISAMPlot.m (100%) rename matlab/{ => examples/+support}/VisualISAMStep.m (100%) rename matlab/examples/{ => +support}/findExampleDataFile.m (76%) create mode 100644 matlab/examples/VisualISAM_gui.fig rename matlab/{ => examples}/VisualISAM_gui.m (94%) diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 3e77de602..0b07b51b1 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -16,10 +16,8 @@ install(FILES ${matlab_tests} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_te # Examples message(STATUS "Installing Matlab Toolbox Examples") # Matlab files: *.m and *.fig -file(GLOB matlab_examples_m "${GTSAM_SOURCE_ROOT_DIR}/matlab/examples/*.m") -file(GLOB matlab_examples_fig "${GTSAM_SOURCE_ROOT_DIR}/matlab/examples/*.fig") -set(matlab_examples ${matlab_examples_m} ${matlab_examples_fig}) -install(FILES ${matlab_examples} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples) +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/examples/" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples" FILES_MATCHING PATTERN "*.m") +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/examples/" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples" FILES_MATCHING PATTERN "*.fig") message(STATUS "Installing Matlab Toolbox Examples (Data)") # Data files: *.graph and *.txt diff --git a/matlab/VisualISAM_gui.fig b/matlab/VisualISAM_gui.fig deleted file mode 100644 index 7140cbf7333b38b5712208c676d6da9caadbbf26..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 9761 zcma)hXEYpK)UFai^d6lMy#^uL=p|Z^=rsu{=56fIs5Fr*Ewgcy`JZkZJ_#6Usa7uMofasK=q}lv#Yz42-gdDM?YsT z9}oF|Hg(;n(jr{yK2DB)PH(xqJmtB}|Mky-o?MbrTu-FrCFSI$|0xoZPq_Yn#hDlH ze+6IHfa;&N#)F3k%11C8c(KXMEC#n4^lJxwOI0C?lIyD=sbW(p7&i~lRM&f|oAE#p zT&)y&AHZVIm!T0l`B?!{uvai1R0$& zLoKuSjF&H1D`Y@U3%|iFM~)mE!FoM1l&Yarp%J7lT|2B3&GF`e9>{F!6(X9v1)9R{ z5EJz#o^w2e%+dtAgZjz?c-L8EdJcuV4E8JHk>->~H9gg0zcm;l;M*g#&^FK--5k;U zzHn|m+9}p$7B$vbwN9O=x%Ej(Lel-Y^6RLJ2d|+y!mvo;7a;5d3W?#FgoklB8j)f= zFMe&yiqB=vaWYne_;n5uV8ZYBV#ie}^OPPyj2bAm+a<%8JGjWoibOx0%5xI$1GAcK zBgvQ88o$aI(y(={^>cNvxoF&7vXWAXpLovn_8}1-smU(s6HscFEp`nOdP!yb;<52g#qf(@ zb{_&^Fpa0W&hzbG@Mu=JtY%5+Zl3a6$i_@?f4Er<`{=fb&oeIHD8nr+6Ca-5QjP=O z2_xpvG{2<-ETWr$a)K&75DE%qd^dRq9Ee9+eu7zaCwD&DfB_E^NbJlP8XsY)G;25& zh`-3Xj2b;WOK;`oW_ah78|ZRF$lEYcQ@%0)3b?t{)$HL~8qEB&f_Ad!%g5)E_*;|e zP?-tTI@z`e*gV8sx4e5aKQKR~B^vs>IH6nURY=vyZ)X3OzsIOmB_?mA^d3)7wi-{4 z3d^S_SkWr59!hPdojT9dqrQaVpwhf&}fOKuxBh=)K`S34X z4I5s;wyqa*Xr|vh*o56ras4Z*ssD1kU9zZb6Lq$tvc=zvy#fs<0-3kEZQD-1kducD zR&`%PJYAYQ<$%wp!agS6btEQ8;yG)+bdtE>8n-HYMDeckz%)A~`%9!1n&<%Ob6avm zTaGUT#)lisS=~7t9({K^`kqME)~feRV&Sf>5Or8^B_i`YA2$)kw)tW0;KEKky4zVP z5DnP_!tjohKm?0;l|@ zb%z*w8Gr@U?xjRaZ5q-u>tho~9@}cE!(7=fn7!|hZS?Xtq}@_VpR|u46kr-jESfb@ zi1(v^2Ri8A3cf!bqcUKg5`o)lF4VUCViquoZX@DMC+Mr=e*mBqbEL`y^T@<(*$z_H zKL#bF#x*H;cxk2H*ZZuVVZQU-QqnDTeLe?9ZKSdN>gPD3%>UPwak;!U0LkdCv?rpT zTFNNuX!sJ#=fy|PLZnb@u)HfZsy<*}q1?Kzo#YG{%QjB^P&oJHlk#w8MbGcoYwzMq zPG#aSwp#jKEFjj_4yVvaw>ux=CRCJ;b^02sTlR5wHER<_xts!eWvHM zXBx+4bFNtq>#}BaUSn#V(QLkeb00CeUwd{ZQ^D~{H}FeSFUm*Q9F(ihLEhu5|L7fu zLzo+RO-s-RK;e`ZN`Ec}CP9*>yvG66O zH_524K)mmm#FZ1j6SzF^JL~DPgF4t`3u2E#1%)Q#(9V;+&kugq6-ER)>(_Xwf)k~3 z%zHSiT;{rDGkWuqkoT20OA-Xta(nWBwQa&*oS|Z^4L4wx!AJ8SA%wbY?jICzi&3Eu z(n8}8<|+CW+_09{loF(`n>v_YqX{~SQUjJiyX49ZrI{iZLb>f1^lLK0$0oeTXOS># zz%hHhh|>ON&eBbNFxO|hwqnggWm4Egm@1f7`EB8Dh}h$kim)Po$ROPrhPA8NJL|jm zdz=ReH}60}8Ki3!GI=911kxN>#P#Pji+e+bMX}Gn{zLqoH*z09ct8OjU`vsz3slKgiXj^;Wjee^z=R@aM!S zcQ-!(+m&%yj8a~2)?9Wyu{=PhxjZ}BJ!ysJ;=DsUfKBhv=I?tu6G!j{Jqn~;H)|Ha zo1`31+I|Z%g}9^L+?~gj`d6pY)!pidFFV}`h#N+2V|}bC$e6cG965-{5{M)$Og)UA zwh|8;muW#_QO z*9L+lDELY=MCg{_mPzZ4DD~|n>!{(Z^2-1Uv(1ym4k<*l5T4${yQTLkeM4sbU(LPY zi!+kGg3UTAm!yxI=7+;?6;}8sO!t!3czlQ>bdb@{5+<|XhP|w>h+n2MHgBnj45WgQ zDTY!UIn?0&_9vv45%H$4xmJE9GpUq}J#f+6TpvPF{PB>8iB9v{(ImKhf?vbuka=(H zdifxgz;UPQdEANh4_!O+#%cU{EJqW9td7NRE~pAdI!z|4w^F>O-`S>pHTrtJb0H|G zF2UC}c-9ZuHfUu3yeG8o^9g!r0CS5GI)J-^DKCAMf}e-@DCHk+*ih|u)qIaJQkZYX zm@3(D>}~Y_GX*}}4z#eYi}C$01X>w``(LbB7na~95e6SKMxL)1BNeCkz-8?%kaX$t%Wx!`8W4;*ltPXdfC&&!|%~*^X4oT zTpTBR_;-7Un*dy8?h3CkUu31c)?j=kY|rpS@bOv)eh3sw!395tGt%3~Dy3%)E-P?N zc5+Nx>rW}vY~i!7Phu+eS>fX^Lfr%dHOEh?L4}^SSY#)eea|(fZ>lYC7_e@EX)wp) zp>u_k@3@q_xHZ9K0(y`H((Xrp_n>LPDbv<7XkEy-oD0Ud=T`9olthZe(gnJrY$kUE zZ^RY1iX}sTY-I!GD?19c)L?jt*s$}a9jEFGagD&}iprv4BATgq`1XPZW%NCOs@W%# zA@1Qs4hZLh(*5bh?lRD`<6^NAvjYE|>8qq6IOhHHP9_HN;f~QgN`DUmVgqF^k^uxZ zdk$PD8`P}}d^dNMS9YDlXf8-&uDb?6^Il>8^cyY$`1Pg>GUn-VRXL~Z@juDZ{W6T} zpl!wA8DNfftq{)`(rL;_4WSl ze$!lk@Wy6ti?U+1%+t0tzBLjZZ9K*8BlmQ3DsCmHWd5rHLbBU6TC0XKf=#olj9nvD zZ259Ubx>c+G;-$A9(8AX66p@1eROO{ue*C_)cyVw34n^H2-tBf*~NE-W2Z(CeJl3< z(LC=+0LZ>~PwE3+%RQ2g%||Z8J`b~Bx#z{Q=_K?Qq2KBHcm+4b+yURyinqEPC#9`& z^3)`!y&4*!?-3Uh-1-KrrgdLldJi{a$W?l~f5&ZP+b~dux-5U%|B_=xF;uWrKyRI+ zJ|h}d;JL0WIH7n`+yvvgL93)<#oAvUbSG;=zr>+*O!@)0)?NR2JGMPyE-`gz7ImTL4U{VT4Mw&_^2w9lg68ycf>pQwM>dqiU7nzV3%;@YI7V%xn_P zDn&8Z@Z`a*{76d1SG<@=Wz;XJfb`Hug+&QV0Yr{f#@(}@(T5Ii0j(wa+Lv(PWf!CN zf?mw!;~thS?A!3I4ki_BhuNL!A8RK}SK_p|>#ce8qP!_~sB+e)*WPTCljoX`tgbGB zMew5VxlwaH)#Oal+Z)jRLc~TD@G}^Ct?s1y_dj;Gg!|*B{pJVJ_W& z-yJzd-yMnnUtU`^q*J9YG{G`_kCU7oh?V@EqG2WTQK40ZgqNCx$P4PHP|gEAejC6z zAS~9smF{vX7)(m%r5}8+dwy^1*gZi0yI<#LHGTdsV}*-K_AapXfL6i3Daw;%PuvX} zs~W+0Rrt_WCu1BWlBAit#hrSDZ;1wYX$) z)H+ziO!~7wL$9cy*YWi#PTd|phqLlmsgWUIhKaH@2+z-#lD1F2sCO@C* zBPOOk{Un7-WS-`39KlJ>%s;7+aegcWD}VUs7Os^BrRx1DC-RAr5^2WUMYGL@i3j}5 zzYy=a(7B3mOP1x|xX@nE9%7}v6ArruRab#&%hzl6Qf6x}Z0@#_zPD~BY3KIf+>{Nw zpzX|j43>%CydUzfX-AEj6Uv{9N`G)-Jtn%G%gulDQ2Mq2>ezLAcQjx;@7!*TJbJzQ zq{@St)-YY}@8;XTF}w9Yv2zHH8Mj|M$#Jf!Zo&6-IYyv}W=%A=4q=1qvxRy+tbhq? zUQjXQWpuWMD&DVxi?kWez@({I!?dF)xypTNe!7o5J+fh;vIS#?=q_8kPH>l*31V=& zz00}Nnbt}#{|cv2U*30dwv;>krv9*NXFWt<4%C%(zo~oL0V)Fn9*%Wne|(0ZINyo?EBq zP1c7RIdxqXu^MKrcTu)=+Kc|#j1Fvg$D=dV#3A6eeDNffZ$fzbR|bQoO0`qip4~BlC6PvxHF2CMf0uoU)%NA`@SK9TPcSWa zp?5puK_{}~#Ud=KBu>{e*DFI~y};q7hiB1x?g$6xX9B%^rF@e2G+0GVU1pxR8Mr?s zkBGhrfClf}u>F-1E`*YjpZw%Ed&omzlS%^UKYJ16bgYU1p-fuQM7hP8UJV*j3WOhSgZIzj(?hwR!n zkx$K#&N*R@NJ#Mj>4#l#%t;$l@IkfaY0S2E!a>$38#EP^ejf7BUMipqg8K>Wptvm> z-_6yvOs$dFX%BdH`&5Uu+5P8Cy<6DJFY4ptm#9~*Ueu#@@7=^fw%p!b+B}9U5fIt| ziiWi<56j(3c%g`A_mtk&;nqOTB$Tu`d))<&ILU1S^fHK_Jb1}#_XzX$Eo>S_^{ovk zIQ(oZCBKQY_b>DZ$0EX@9V5FOgKgJ>GDd?8rcK>@2kSn;2#hl7MiF~ zPXtPi|7Hp37-Y0Z$Z&kXuuwOmxZlH!^nmUDuplfyTS5 z1;w4y=|@cX>HmMnW?3I?JjM6N{4#cU%JQV}1 z-x42bGVYvcjd9NHRBEnYSv?90#Xs_%8y2>tvP0VvuDOQ?v{Rh)<9R)YQ$!p6(VUZ* z?o4*;5IuAoma0JFJw2%Kw(wW}g|7d2qfT?180(dj{!ojo6OCtGW-=hddvWY!%g`uh zk=0g>zEuIgv?&;ptwR!cQujAL^ZkxzBn~Dzk`3J6p1pY&&{4^Y6}g2Nv?t_>rij)g zCaHgt)odm@8+~c9rz+>|{)sG~rgJXe{%>X0Vw8Y(l_oj4=~Ibe9nTY|Lk(@<{mkU> zBzfnBzkI%ViY}vkMnY=DAV?v?KsNS@#Fs4l%jO3hu1;4|WyMj)F^_Kf<#J**#WEhv zbD+C3TrdTnvX`A@hAPPkHPW52wsOx)v zHphXXc1w{IzAZJp@+F^P<~MqI?}cK$#*ITDBTZ59KG8xg#Aa+_Cg3-xex%FTn?_NF zw3r{n10D{Cw%l#D)iNVv%?T;v1}1J2az~GW4=SWYrKI5A4;gk5->*Wt*N7MV5z{=C z#S;!Q%HJ5bj8PG{ihm0ANGgudhWMwZGwCVi6@LsMwRr2F8Hc~M)`u5ehee}(305g_ z=YRslD1cw48=px7Qs(qap!+yC%&_=_Vz~OQL$4yq>Qh{P97gIAwg7l+Z#hl&K^A+H zuKWh4afC4i!wl|r3uUpEhl2qA7ZzTh(od6KQU544AIx zh#;1(9E;(3D8f*LeW)H8Y$wWF^Dp#yuwa~*e0vX;Y{{1h(Z++2N)IW#P^vHgEN&OX zl2{*s+!Z<%3PZ}Cu4bYsK+BK##=R5UZq0A@ zlwubOTQzNfe)<5%r0aOeM45Gs!rmK6DDP z1pL~Gc^roCpC=ywytt*Dm#8()lVaSpS)Zp@Xcs&_ z1ohp0hX$g6bAzkg7k!53j=Z-o+WA8Ew8m0&ump2EuX`W-i+Z)UQi`j+k2e>UY4}QtqsjuYq#Y`AG_y>lo>W4A*%06 z+TQsELR$_h&gU{_vo#0jS5Imn6Uq-xQ)X4+fFzBuO5-ZIg`cD6i%zK%5yGS9x)Ax# zXrk-qH$pcl(oMmGZi9!_L1BddgGNiJ=d=oaK^fIM@V8iVOPVNZ(du^~orW4cypI~M zG#)6B(7F7fl)D_^Aat623>CBVkF!9MqVLhNmSG9VvPgE8q(Dz(nhMIwX_-fJ^Lejs zCqg**-;fkX!iC}v+gBV7bP;qgFB%*K8i#wJ5McFLyNHuvUdcO2@5WeAb}mNQx9!Qf z(ZgH(n3+qP6JRN9m zVP5GK_lv4K+~?rugZZCEtFv2oK*%w*0c3}6Y}D48ROHFd3)aZ^<(xOo5i%S3q50Bi zk_EedweClEQMvaBAQD+^h>{as9AXk2cJH#1|qs6QbPCF$lIO zenO*aB%=;{mtAN)Fv?2lW6cw{@-o`~@11*DTX2N0zeK=?>8q}G_ZKQTB=lFoWew0O zy_dJ8{(DQQY)LYRRt>%{-Z~1qmi_kOyDb`IJK2op93pG)6+5iDeagYiQqgDE^)uyd zQ9I>;_}`W$o)tD@DMBz8=^n0u`7a_3PhXW^E}`u2lP@>+v_3%uIbM@}n1sA&ah}V{ z6OSF=9TBF<%K8Rdm!(^{ge#ggHb#Oesvdd6#4zA~w7M;yH42OMX2QTU<7Vb^<UPwy=U0%d#905 zFQ=m)bUw02&+5)hNhwBvJJi7QUDCV7&&1s*o%HkCcNEu*P*Tw3{%H2_jnqsh#+D;#g+J$P z(&u_5Cl*qQZnT=_C4uDE&2VFN;)-k1i z2iRx4NQyEe9$x;ZB0E8e*ZJuxyo^9A<&XC$9ySUlYhxgPq3>kmahGqsF1kPY&jnB@ z7}70^GaP3F5?08r$colPMp%oez?&+d&2tXq&^Y;BAq$ouKU?-!9zKnY@tn3@wcJ+R zz9N$MISAeZnn;?zRO{l*@ZIiU*lcoAj^VxevemdK9{8oEB;<9#EO)me)v{y8d9MA~ z8}37D*%`$ANfAR#6Q~?6i>Sc`*oQd(%i#vZ>XbQn!F>AeK=AYVYUEi%({(OCFujMW50?DxJ$?|RC6)BCpdTk9R1Wx zQf5L;{JrYLN7v?9F91SN@KC}3?ORu3Y|(RDp~irNnR5Hff&!_XC9;ios=v~=(Ud)$ zCrUjB5 zwR6l4-Gl~B-?kE1505mmvcHoxC~Z6hDL}}8>VK^nuZMU6XNh?zQ$=PBT9FIwkzV`x zq@Y(eHI*~XE9Y8aNW#`&Z`0#l$qt|URqlzkNufY1VGU)ii$T<;+z)u#y!2XGAW7=+ z!h)~<$KA%>45QIyf!M>hHm<@m*tdU!!Qv8#^q)>VV{RGAkKqOJYo|}}50g{gv3q}i zO|^Ig>WRNHP;aRJSr_N0V^F9@j;}CrymzH|PI>+{nF7{hv6dA!CCR?yp)?q((Jj~b-=h2?8YC`J1tyVsbSiijc_8e^(fty60` z27~!;Bt-@_B`=(~d4lljaqynIK469ICDZ225-7t4&vu-HdG_w4PjHgvD8V1yK=P!E*uG80)*~J_8 z`Jpl|A&EMQ^oQ)~>r?sNxT&7CG<31WjOv_XnAP;PIEG!Reg6$CuTA{R-V^eVfql0N zLGBoNz1eFs0-O3wRzV6%D_MV z{Rn~bkG?<+f2lnDRH<%Vw;h^7#@XK;*GJ^Np%$P+O_-(<$%3Q^rswF(M%udXzf9&Q zQeQ|5O9xc0?*pdQ`I#9-h04ur%U4vSRGnT)v-O+3==Lnzn2gQ@u2TJcIMeZ?pq_4L znBnGfr_d?zHe0q0DNog4ICyT|^WG~^{W78>cVZ|2T6~g{9r^J~7$vF=mT;Zu6NM3W zxjH$17IxQy^b2O68CW?zau8MM;7r*-t1ESnYu8T?21_tcaJp>v-Mzj+!>P32WGeba z44=T3OmpQl-RTqt#JqEJuB7zxc_!$XVwK>JyyM#SZMf@b=VK2CDByhF3GD?9fOex` zkF>-E` zWuV8f2;2f+oU?x2lU1Ga`dG65&!HX|Pk42!cck2g)K9nznmjS?(Zg`(U7^R_23;mT z$s=$HD4ozz@temzTiNJz`!l`&XMmla=uwuyg~3eU<50;3I8sywZa2o7|o{y&Z&$iEGxwa)kk?h0f6pMy%g3N0ugha=?_|Tx@X5*2%huJyS%Tq{ zf`O{0h!8h}f`^Tzm(3RjR~HEeU2hwPkKRrU0zwRY;t~QP68xeJ0(|@e4F5k(lnvwm z8m6iS;eV$(RtyZkyfaD-R~q5Pwco0;^Ph@T$cXhY8Qn3p!*nA#CG5ZZ7_c~xvmM9i z<8DzxtM#CM`&SuGnx?7icuz&K)OnwPvDSnPcNp5KVjp4^iuCl}lj`mEzd54}^c|H{ zy=O*Y+^KH0L6ZtmNGA~D=NpXvX>xdfQGcM~R2aoQM<-zJK0)6*A)$m_3HvAnYv)@) z$a^7kSQ=F^u5`?BB&a-vVvGaBhm0hK5$J|Rzee%Bd@S}WH2%*W#rUlCC(Bncq-Fyy zro-Fh}#hQH|JlU=a#8+k5SWZ1W0jNcPo`WNo`1x?U~?i z>-#DGD%!gEdFUNH#dYQAuDFa~$9JU)sHHw_x2x8v>oV|jcxpD! zU$+3ugB8K5U=6SqSPyIj?)e`Fg)A>ZRwgw>q4V|}xrSgk2eC+_%~d7liiYdGoHZ-d zvrRnwF&s;&_MiVy`}%4*C_MbTTQ?(1)b8qSG4-yf;y4LNvU8{#RpF1otkGTnK2^H! zAvcP9KK}am{)=zQ(u>J`^!#Vc$G16wCQ$rU8BLWF9u%CvNWKt^$u&AgDr4m>c_HbF z_i+0C4%LwaopJ7VK|Mr#tBAjQJh`K;TDj2&oNi1%h^vLvF#_zViM6Be$#kIHf#04l zU$taN3N0FK1b>O#?7Z>dig>k*|wsHYFz}EYRni?dOT>eICLJ`22|K`#1 z6HkmO?^IdH(aHxpEDR4m16ncT`OtID#=4B-?+?^g^Z~KWO>{bSc@J5E!yXDLe!c&A z4+@U8FXXa3u*@dIcn=6{$Gvy!=+a`O=?5#NsKqLmQPfM7W!89e}3r>97cHx`sff6?+ATGFTs#f0=M@F0u zV>ees1&;l)5ZOODwPPUJ(~KaF@S|P%SiO|cI^$A6cw_cgsme;cGS`qD=_?WG4_l(g>t-6G9*UrfpFoE;4Xwt47nfMA}`&r+J4 zZYH~=zeW+rtaff+gw$$;B*IUPYaSrzVNnm5=IZkNvIZp&+P$!m8xD(GiTDWwQv0uF0B6pp^S54IM)+7)ZA z8s`Toh(fWIxjVrDIlu;XfD7Oe;_^W6Df3!2m=%eKf${WoW&m_u;3M_L%cY2MVEFC6 zkH6aomVo7D23N!h(c6w&+lUFnNi zZQwGAf)R?kSg|g#Zq-wPr3doS?&6Uq$X?>)*MSH4)^mA2*1#QoialbrL8DVN!v$sz z?}hi?DT33lf8&MYskZ@6(tek$&4%OX3M_iaDCxP(NY9@$ExEMzrv))ccRGZ&g&_YH z{f8%@@IJ5fp58wXg_d^65`HqX@AUT=dI0Fgv{9sC#)EO~=0~&Yn@(TR3A9bVBM){$ z3W*3p3U)5OPX4l4!9sv-!mteYct3R1L`UI!RPd#QM~zVN8S3x=MOt4EpN{kEL&DGA z(JbJy(w$cBQ5m!1TO~vaBsr59_%N+fy`{6{usxMxNVBPOrM3-Dw9PCFJMk;ielrJKLD)Fs=LS0=2sB{w|-YZE-pg3P_$J2v3j z@XJSp|KiBPHO(0$-~n(qapzVW<0+kY!y8VgxxUhzUqRO^JUfKwpqshlo4l)UH6?@u zh^-9S)x|vh*DL*p6hq*DeG(}Qt-7f1p!zQSKAed$;{&dCIj2_p7m?iP>TxzL5shE* z83A_PU4P62Fa5Mz3%`+Ywq0Yxu^%vcZU^7Qw=-U25}kxGKb=v6W)gjeW;DOQw=WR5 z?#}WLMIw=B+i&Gmbgr{AQj*H$XujcH2g^^*jF(mS<<15%UPZ#OF9}}W)w18RDIeu8 zU8fSf)#xj^O}MTpN7W|Ja#4l=s@1Gec;EUB_p7x6 zqw6W-8WJ;*)yKM*6sV^oo9QXTQ;XQU_~?QTdiF0LT)&zpy&mRgd64no%n-oDw)KMa z%wAwpfG(zuhxCR zd5?<~0)u`G3ENilfiAQ@2or)0xAk%4{fij7N;82I9I_K5I?7(@I&Xibg)n%6v|MX*`4H(Y(&8 zti&j4k?|`|V0+xFw>@&vv0c-qPN9(jC}{nvk~x#V*53_ZY?u3+rPL5C@+yH9mcPDU z=KT+X^{&jv=1yAY=7#f)<* ze977V9z(htkYD+{tF|G>uMYQ|z~gVg_yReN_;EK(JIk_C#D%UeLo4H8tKcksob zyc69AaREd9kf`fYcnh?E_7!;qzJJ2e_UMS&nOLPp;qI=EX;6Fu(A|9t9;$V6r8qCC zX^I3wa*K!D8ti2|tw#`z__!n5;!x>{vBrLEJC=X4qP@K&yN-??-6)Y#e{%fYQ%;uO z?;5;@bK94O3E5nP8q(q_8u!oh(nLqVwBx^c8uH1({)X}A>aQEp^V;COI+I!KWOYs+ z&qc(osayiaGcn`Pt}hBA2Z_HcPvIKNyk>xyvtu-cJx#y430e5rwp&Gi=Hjt@STETV z{_5K2H1oV=a{gj@DayBQFbK9}mI?&T2P}+ST~pLRoR|O@u?1CCl~E-!p($5g1$gIt z7KF;fF^NI%wj6tUK2Q6IfeY)VDwIeS2;E+bN1kT?(USW^A{#Qila4hOLmDhI)RX-n){AWdk0qg6t_2y`p!eB z#U3~f>rMg3kN2Pd-ki^*-kiJsKeC$!QY(|vTNg$CSkx+xHD^-e7*tUS3*yl7_?+}c z<2Ugqbp=qWKC<6Co@(a77s%9oVSv)~?!YGzUUhwoQ-M8Y!hnC-i z3SaW&QTjXt|NZJsyP^>4=)BCIoU=zDHfit_05~s)`1tHygG)Q@-N0YI=L`8`G$J19 z&AVL^eGXyu7ACt7^F>nC!m0J+3f$foL@}RV$9a(WZbX8ejvZ^0`?f)xHCcswvQi}Mxi@#Q&RGfzyQO)s zxbWe5sIQb+aA<<@X`mLL@1}@ZiLpTT&(Y3&i>aWp3fT;&aB-t$g{Y93Hjk~*N23`q zW0*j8qAb0a7`f;RPj}$qZcyQ&l|MSc#6t0keCM~P`_gC%l!A${jsMh5=>7{I{TOJB z(ST+pC}`(umi1h@=$9yVjTV-8U9jiN4|lG`kA~BTDh{Kb@+Sb3-Xu=2!)%$?h{d;s zyNcA`>7teLh4D}wW{wjb6#4@7;fmz5_)Jf0Ndb5*c>Ct6&dvjhLbNSBwiVu7xL@x2 z(gDytmxFr)l;Mohfv2D-7DTSxGy`GcXJO_qF zu`IC792}`>yqSM<4SC$@x~=hs%l=<3F_;D}%8?dE9jGbF6Jy2kQ)gmCf7oG4!-~?2 z{1DkbZcicrrd|DLpX>935J-1$_=b=pE;J1Nn&vJUhxiGDAOPlQE@>097VWcw&2rD0 ze(dVIb($**?Xej1N#)8~qxf8smSigvA>@|#xugKZ;Em5p{K~+!s`vNG+oCL@+t0!B zh8Ejrt@h$EWfk>X$4(?V}?SI19>gP*GF zL0|e2t}mgkdt$+|l8(7*BIc?ZUnSfd7py%~#zvcfF)v=kxcMW)&)AYzHHw_%HcF1( znF90-jA#IE8(hQKjGdNm=(t+>q4v|wS+san=H0FDkz6WQ&(64g2swJ9vNt@7=R+DN zlUY;VUF5FhF)fz~STEqIDb}bzjR=Uk>g6z)tN?JO8N@c@Qq2gojjt%+`| z_+~>!+|ey6Xw1=Qd#CE3iUsIjke!6DV=E+k3%oUId}<5F+4wuWx<69zN>#GXG;r^Y zS!Jn8J+U{Ar7u=qJw2ZGZ}Ca=)+z?_AYl{R3~i(x=Ha*L-$UY)&P5q~GqWQ$*>Sqs zG6$K)g%8fR4)3=PL30k*Aw-H*S^IB#SeUr0< zy@cv&r6)d_2>$qZD(I!a9*+KLbDR?|e}GxVHJk$bR6INl1~A3)t7XfOn_WE_}xgv5VGKrix~Lgt`L7JD|hpA#^j)Hfwia@lq+hveu^tAg5!a70UZ$-Pf zo-7r!Chd>zro6HHTgV^k5%?jVe5V|l-FCH}XU4+arz%VuV2pgk`Cr{*Gz}z?CoR0z zlKY^ho@lO~L{GYaMNIOF#*jl^eV_TQc$(0{c`L1vk7$#QjFf*y!c{CA#S=CfCpiGJ zJHf?|#y;u3z~9tdxm}P;&;KHT@`j<{hv&Jlz-gxaq*z01tDaE^W1*G|&LfT9MM9BG zbDpsVR`0Z3@0a7mRsBD)HF@@5e4INEBn(A^g4ASs-&_Uv^=l3AJYsHN%Y;C$%(|HL zcXF?<6v=vH2Sr(WE#O#t1uyO$z0VM&2(dyey2UN}d^(!^WCyyud3}(k{)}b_=vm$2 z-Z|b*Pwg=E<+G8lD{o_gUB_0(c3}kGE~Ty?#;Kyh_T+lt=cfU?Z5?(Q6p~!&dGyjS zFSws=qGsg?KMuRo~yB;xYMJrH|^)RplSHDO0C2zeBD}f&Mk_Xx#wfpc_u? z%zMEfwtf9L$@Et-Gi~rs_6YpPaKZ*y$e%fdZ1;-6?Se&9@MHD+5t-?Gjgn_`d3!%U zjD2qWMya%WRgj{z%K<7%Wd7U}`))Z~eiIjD=^DlUwtp`LYs2Eqna5=ZDq_nvqYbv* z@?LRkSNQu!#VX;=RBDP*je$xHjy$5b@o+k^b)_lVIM#ESa>*aczaVx?AmW%0B}#^6ENTW zQ^;JtPT#KwDH@V`X(oaSqByZDFZ>mP+gtr#Ak#+>!T5YD$7{+HcHCuPg*16hp{QN5 z>tC9upLd@<;o^yvd#~H83(W`f#;U-#g5`kA!U)Y9q+<r;qr97L`AJD=Ga{n)1# zS-vIDSyj4YgAz0<6r~;wr~IFn3n$zfLt_8mQ3lBIKS&q+ZfIU|XkmzFz|iQGcx( zw96{ggk#pFK;=Ulrma7Re&`Jzj+RaoO0kce&Z^DC#Vtc=$)|~&KQ%MX5u?SDAH>+! z>$j(_Fh^UhMMd&EPeTc%j<+vWbo_sBx9<9C2LjlBzV%eY?zdAksRFZ=C7V@SbvdQ& zV#PSO3p)`~$6Q&yu>Z9wzxV2_#0f*Gnh<9p}YbZ^N&Bh+qm*R02^iHR#tFF~oJSgX~04^|qdoHP94;@P6C@_+Q3%HzVW|CE0mw?Lbnvdl1lbkc)JD4!N=!i+#N?5FC5Prizb%li~9O* z`9Z9`3*=8lYX3|6pG;9zR(B=ez1D5d#?EBa?1K3rc|BlO{_42r&0y#yTw-&w&6XZo z+E`qm`%|ZFN_>piVa~Nj`fp%G{gXEqF3_~dko)bi&u=hd-dTx8pUr9DZAE)jjfu@9 z^QIDN;aum)&AZDx*jwB==tXvKmmp-*E?=Y@QCsJkzaL6dag|(h>}7{wYZwEB#j4bn zn=9Tqm?eID8~nk`k&Lq)$CDcyNB zZMe-dZBc|%1GKH4glq4;ZFm$~BeJxQzmf5dc){pXy9jx^vd&ne?r`U$_2cj z2kW!3;ecRtK~DTuZX2ETil?ZgImJqUwcl{@?ywIheQybBW|h5HCX7JHLZMp+CW+rI zD!n98a9_Z@9wfB(PDpbj#n4 znHcLj09AW;m)2l;t$y*38~734?EU*+qP)A$=uwgL;X!_T}oyC<=wgG*Q zydUAysCIzzbvOL>B!S()BBT1KH)tmQP(!T`#PXQE-(U{7zHH*?T6LCW0rjjCP_wv( z@TWvoD{Oo+3e`8$gE__n-H}S(cTZ*peRD``c)z!Ie;2a6_Rq>vI!8rSVDOVl7vaYxuRT+4 zlAMZRPx0WVn~NMy$8kjxB5}+wKU?ii6OjM{t{lyMvo;B;zyRIvyl{Z*EC`ma;U9p=~gzxYd=!J|V0M#)M*X8oV#KlIH1}5;{d$p;b31heN zBfzaLPB}K*Yme)MafF_a&z#=Y0^=LzKVC4V9X(%TqMEjj9~1quFgR%?(U8Go?BS%| zw8)|lJ+#Tupn9(J{d;pmoL~rv6aQb{y@&RjP@+N#VhaE{{pafA_1a34KPeC(WGAO^ z?Cu6)muf<8sjfDWDYWy(VkPH=5O1>PK6#hWZ&^IT%J}ktm&E#UVU9h32QeIq;x94b+a!y|((oHvQf1LL zKtIuxZUXWR9CXXR3J;`oRu4^?GeWU86z|tJAMYmo9z$uS*fD$?E5tFhIC0{3C8~?B z2=|Pxm4r2^PI5C;Qjd>NWeV@o2|m%aA_N!VXzsBCJhvKDw$XeWxt=MDt%uBMd{Od? zn$1FNm>u)@DTt~AlE}8Lh6qlKZRi;K)iEUMj#!~|KX^{}m;TCl=A1a|Ki#1Z2sh&^ zd&CX22oR&|*N#)k6%U43+}34v_+cQtXB`T*r!A2XbaMQ;OzT#6u#{aC)D+hm=+W)2sws8mRU*2{%2^EH%>!rgH z=2uTt+mlHRpsWYSdg=u|INk!`?mgrJQhZcPuQx{MN8Kba7he`{&q|2v$1Ou?k~AS4BkeyAsI0NefFdtVhItTed@s;k#Qq;vyWS7uJf zy2A((G2N->T&t~Z#Fv9dy}w(VzIL+ zzmDW^jO_9Lu)rn&o+a9LB|%uPIdW^#=inNka39{8Gc)Qt^$VJk6%m#lN^sK#N_a^0 zh(vMO!Jrr7p@^0%uOQmR;f>4l&peWy^eO+43er8(%5@7PL41@m^mcm#2&+frE}^n@ zrj%Fs7!z?xyCa9LjJVQbrE4|C)9@nmJ%2-Rh5_>xQdY8>Tp*_D| zwjCJcoK`142MjBAjVOkGD~5&_7rs!G#flGkONyf5fct;C#Va}rEIJ~Wp96#yTf9*0 zdlnx;Mv9{1fO~(sr7k)Wm7jABE8hOkp%4&{1%s=d%d}moCm;f6p;~=vD8^hEa@ujk zZYE0N>?z?!C1e8d$lAA`h5T-Dt@i)9pciKPWcZ#!K{^gkH_7==uWr(NVH*m4B8l69 Rmniz19(Eeppa3VL{{w2`zb^m) literal 0 HcmV?d00001 diff --git a/matlab/VisualISAM_gui.m b/matlab/examples/VisualISAM_gui.m similarity index 94% rename from matlab/VisualISAM_gui.m rename to matlab/examples/VisualISAM_gui.m index cdb2ddbaf..765a6f5ac 100644 --- a/matlab/VisualISAM_gui.m +++ b/matlab/examples/VisualISAM_gui.m @@ -230,12 +230,12 @@ global frame_i truth data noiseModels isam result options initOptions(handles) % Generate Data -[data,truth] = VisualISAMGenerateData(options); +[data,truth] = support.VisualISAMGenerateData(options); % Initialize and plot -[noiseModels,isam,result] = VisualISAMInitialize(data,truth,options); +[noiseModels,isam,result] = support.VisualISAMInitialize(data,truth,options); cla -VisualISAMPlot(truth, data, isam, result, options) +support.VisualISAMPlot(truth, data, isam, result, options) frame_i = 2; showFramei(hObject, handles) @@ -246,10 +246,10 @@ global frame_i truth data noiseModels isam result options while (frame_i Date: Tue, 24 Jul 2012 14:23:36 +0000 Subject: [PATCH 42/68] Wrapped GenericStereoFactor --- gtsam.h | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/gtsam.h b/gtsam.h index af2dc3a3b..869ee6a09 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1446,6 +1446,17 @@ virtual class GeneralSFMFactor2 : gtsam::NonlinearFactor { gtsam::Point2 measured() const; }; + +#include +template +virtual class GenericStereoFactor : gtsam::NonlinearFactor { + GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); + gtsam::StereoPoint2 measured() const; + gtsam::Cal3_S2Stereo* calibration() const; +}; +typedef gtsam::GenericStereoFactor GenericStereoFactor3D; + } //\namespace gtsam //************************************************************************* From b63201b20d50a31785f65407033279cf19068796 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 24 Jul 2012 14:23:39 +0000 Subject: [PATCH 43/68] Updated remaining matlab examples --- matlab/examples/SFMExample.m | 2 +- matlab/examples/StereoVOExample.m | 52 +++++++++++------------- matlab/examples/StereoVOExample_large.m | 48 +++++++++------------- matlab/examples/gtsamExamples.fig | Bin 10455 -> 10561 bytes matlab/examples/gtsamExamples.m | 11 ++--- 5 files changed, 50 insertions(+), 63 deletions(-) diff --git a/matlab/examples/SFMExample.m b/matlab/examples/SFMExample.m index 1416fb7e3..2153ba647 100644 --- a/matlab/examples/SFMExample.m +++ b/matlab/examples/SFMExample.m @@ -22,7 +22,7 @@ options.nrCameras = 10; options.showImages = false; %% Generate data -[data,truth] = VisualISAMGenerateData(options); +[data,truth] = support.VisualISAMGenerateData(options); measurementNoiseSigma = 1.0; pointNoiseSigma = 0.1; diff --git a/matlab/examples/StereoVOExample.m b/matlab/examples/StereoVOExample.m index 98e1b5845..468123529 100644 --- a/matlab/examples/StereoVOExample.m +++ b/matlab/examples/StereoVOExample.m @@ -22,12 +22,13 @@ x1 = symbol('x',1); x2 = symbol('x',2); l1 = symbol('l',1); l2 = symbol('l',2); l3 = symbol('l',3); %% Create graph container and add factors to it -graph = visualSLAM.Graph; +import gtsam.* +graph = NonlinearFactorGraph; %% add a constraint on the starting pose import gtsam.* first_pose = Pose3(); -graph.addPoseConstraint(x1, first_pose); +graph.add(NonlinearEqualityPose3(x1, first_pose)); %% Create realistic calibration and measurement noise model % format: fx fy skew cx cy baseline @@ -38,48 +39,41 @@ stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]); %% Add measurements import gtsam.* % pose 1 -graph.addStereoMeasurement(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K); -graph.addStereoMeasurement(StereoPoint2(120, 80, 440), stereo_model, x1, l2, K); -graph.addStereoMeasurement(StereoPoint2(320, 280, 140), stereo_model, x1, l3, K); +graph.add(GenericStereoFactor3D(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K)); +graph.add(GenericStereoFactor3D(StereoPoint2(120, 80, 440), stereo_model, x1, l2, K)); +graph.add(GenericStereoFactor3D(StereoPoint2(320, 280, 140), stereo_model, x1, l3, K)); %pose 2 -graph.addStereoMeasurement(StereoPoint2(570, 520, 490), stereo_model, x2, l1, K); -graph.addStereoMeasurement(StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K); -graph.addStereoMeasurement(StereoPoint2(320, 270, 115), stereo_model, x2, l3, K); +graph.add(GenericStereoFactor3D(StereoPoint2(570, 520, 490), stereo_model, x2, l1, K)); +graph.add(GenericStereoFactor3D(StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K)); +graph.add(GenericStereoFactor3D(StereoPoint2(320, 270, 115), stereo_model, x2, l3, K)); %% Create initial estimate for camera poses and landmarks import gtsam.* -initialEstimate = visualSLAM.Values; -initialEstimate.insertPose(x1, first_pose); +initialEstimate = Values; +initialEstimate.insert(x1, first_pose); % noisy estimate for pose 2 -initialEstimate.insertPose(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1))); -initialEstimate.insertPoint(l1, Point3( 1, 1, 5)); -initialEstimate.insertPoint(l2, Point3(-1, 1, 5)); -initialEstimate.insertPoint(l3, Point3( 0,-.5, 5)); +initialEstimate.insert(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1))); +initialEstimate.insert(l1, Point3( 1, 1, 5)); +initialEstimate.insert(l2, Point3(-1, 1, 5)); +initialEstimate.insert(l3, Point3( 0,-.5, 5)); %% optimize fprintf(1,'Optimizing\n'); tic -result = graph.optimize(initialEstimate,1); +import gtsam.* +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); toc %% visualize initial trajectory, final trajectory, and final points cla; hold on; axis normal -axis([-1 6 -2 2 -1.5 1.5]); +axis([-1.5 1.5 -2 2 -1 6]); axis equal view(-38,12) +camup([0;1;0]); -% initial trajectory in red (rotated so Z is up) -T=initialEstimate.translations; -plot3(T(:,3),-T(:,1),-T(:,2), '-*r','LineWidth',2); - -% final trajectory in green (rotated so Z is up) -T=result.translations; -plot3(T(:,3),-T(:,1),-T(:,2), '-*g','LineWidth',2); -xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); - -% optimized 3D points (rotated so Z is up) -P = result.points(); -plot3(P(:,3),-P(:,1),-P(:,2),'*'); - +plot3DTrajectory(initialEstimate, 'r', 1, 0.3); +plot3DTrajectory(result, 'g', 1, 0.3); +plot3DPoints(result); diff --git a/matlab/examples/StereoVOExample_large.m b/matlab/examples/StereoVOExample_large.m index 282f7162c..fb45dd8a7 100644 --- a/matlab/examples/StereoVOExample_large.m +++ b/matlab/examples/StereoVOExample_large.m @@ -13,73 +13,65 @@ %% Load calibration import gtsam.* % format: fx fy skew cx cy baseline -calib = dlmread('../../examples/Data/VO_calibration.txt'); +calib = dlmread(support.findExampleDataFile('VO_calibration.txt')); K = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6)); stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]); %% create empty graph and values -graph = visualSLAM.Graph; -initial = visualSLAM.Values; +graph = NonlinearFactorGraph; +initial = Values; %% load the initial poses from VO % row format: camera_id 4x4 pose (row, major) import gtsam.* fprintf(1,'Reading data\n'); -cameras = dlmread('../../examples/Data/VO_camera_poses_large.txt'); +cameras = dlmread(support.findExampleDataFile('VO_camera_poses_large.txt')); for i=1:size(cameras,1) pose = Pose3(reshape(cameras(i,2:17),4,4)'); - initial.insertPose(symbol('x',cameras(i,1)),pose); + initial.insert(symbol('x',cameras(i,1)),pose); end %% load stereo measurements and initialize landmarks % camera_id landmark_id uL uR v X Y Z import gtsam.* -measurements = dlmread('../../examples/Data/VO_stereo_factors_large.txt'); +measurements = dlmread(support.findExampleDataFile('VO_stereo_factors_large.txt')); fprintf(1,'Creating Graph\n'); tic for i=1:size(measurements,1) sf = measurements(i,:); - graph.addStereoMeasurement(StereoPoint2(sf(3),sf(4),sf(5)), stereo_model, ... - symbol('x', sf(1)), symbol('l', sf(2)), K); + graph.add(GenericStereoFactor3D(StereoPoint2(sf(3),sf(4),sf(5)), stereo_model, ... + symbol('x', sf(1)), symbol('l', sf(2)), K)); if ~initial.exists(symbol('l',sf(2))) % 3D landmarks are stored in camera coordinates: transform % to world coordinates using the respective initial pose - pose = initial.pose(symbol('x', sf(1))); + pose = initial.at(symbol('x', sf(1))); world_point = pose.transform_from(Point3(sf(6),sf(7),sf(8))); - initial.insertPoint(symbol('l',sf(2)), world_point); + initial.insert(symbol('l',sf(2)), world_point); end end toc %% add a constraint on the starting pose key = symbol('x',1); -first_pose = initial.pose(key); -graph.addPoseConstraint(key, first_pose); +first_pose = initial.at(key); +graph.add(NonlinearEqualityPose3(key, first_pose)); %% optimize fprintf(1,'Optimizing\n'); tic -result = graph.optimize(initial,1); +optimizer = LevenbergMarquardtOptimizer(graph, initial); +result = optimizer.optimizeSafely(); toc %% visualize initial trajectory, final trajectory, and final points -figure(1); clf; hold on; +cla; hold on; -% initial trajectory in red (rotated so Z is up) -plot3(initial.zs(),-initial.xs(),-initial.ys(), '-*r','LineWidth',2); +plot3DTrajectory(initial, 'r', 1, 0.5); +plot3DTrajectory(result, 'g', 1, 0.5); +plot3DPoints(result); -% final trajectory in green (rotated so Z is up) -plot3(result.zs(),-result.xs(),-result.ys(), '-*g','LineWidth',2); -xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); - -% switch to XZ view -view([0 0]); - -% optimized 3D points (rotated so Z is up) -points = result.points(); -plot3(points(:,3),-points(:,1),-points(:,2),'.'); - -axis([0 100 -20 20 -5 20]); +axis([-5 20 -20 20 0 100]); axis equal view(3) +camup([0;1;0]); diff --git a/matlab/examples/gtsamExamples.fig b/matlab/examples/gtsamExamples.fig index d0037027882c4ee90461dea6d8bd52368f58de0a..e51c23468d21978f6cdd6c9f8d5d4650085db138 100644 GIT binary patch delta 10534 zcmV+>DcRQ7QNdD>G#FHMWgtp*Y#=f;ATcmHF*rIfH6SuDF*1=+BavVQf37D000000 z0003=O%DJ70465@0C=42Sj|ryHxxI^hI|wdq!-ddw3jL%A+*UQQKKkB$OlLvpa3~- zMZB}#S(o{eJwrqPg65VZmsb4~(o4@AduXK|df?DXQCme-RH0s4^;o~Py)$E$-Pwge z8}Y>O=6UAX&%fvO`;A8te?mKdBQzxRIdK(`lrzd6NL?4i)o@nU%h$_7^m|40o_c;= zT+voYR2L2G*Ia3YEC>mUDB;L%P`Ad2FIss>Y>M6H8>8a7fs#0iZQ>YG8e~k_b?U^7 z?Drq)Fb|OPsvYF7uZRWy|BUCuLC7j&SRZ*)VjejiDRazKWep7|ncGsB=Cqplm+D zwi}pXJ0grH{mi4ze|a55*}P7O>vB`}ZkrnVD-RvqpVg+n)=U3;d3jsv9i_ij`UjgA(tjwur}UryF}-KR`No*XYh#{z-iMV_{IhZOioeD| z_Ut{P{J`(fo2f2mIj|C1A_8&JAC)w5eDThG9rfBUEc)st5kh0<25R0HNz zoQPwu;^D}})e3orz0f6Gw%$|Lj9@lu+;W$32Q%XiqM%;S#Xs)R`xwwe{WkizkXl*&0B|=XE&aF4rOr$+s3Y2#rB3dZoB!Ql-4{L{I-XB@@a$}>emDHZXj4R zqcufw8F2~bn(wj<`Ng(Pc#HOWb?PR(SR*KjDW|^Uf09ViXuid5p!f1OTXh3_Lp+y$ zFxeHnNov6Nn-<*oinw>nhn9Bv*d6FEACFM--6W^|j3nPKVayxVIFeHfgD?&~@znuO zVj2a3z<_DPX+jXVIp$%;QxJEBKK+V)`dvEaIFH*8_31a##;=!s|84I_X1uzVysLf) z<>MPSe+^v0?w@aXxyOB5Jg3__I%|vzw%o_A9*O_UUk~0-$BaCk^to3|<*t0FbFUqF z?lsY4-PKrF;+j*xWAzw4#8u*=M9hOB^W+G|e=(ShKh|Y@c>n2O|MAMT$buk}&p8P( z!%^#~juStkelmr6?As9Ry2`4_2ixWAGRo!)e{AvlSZ0C4uvdce&nt_ws}E=IR32X| zU%iI&?Y{$=eg{wm_2*|PzllO`IO_Ad$rLW>m7mb1PXo5WLu^aDUUZ4?V*@X+dHut( zHQ89IenRYMnrw=+)Wmw3%IqDJR{%>m6kXTr@XGW)pJ^wipmlFkQz+m0Z- z0cG>uu5RVw1CWe<`G8I&_mQ7?`vE>oY_evazR{oHD^pZ^O00RR8&SP=jyEe27DJG5l= z6jIe3$}c#NA&qjf3T~T1Mc~^=xvVOAL=>!#A%ftP_1k(gK#LVX(&3$$a>`#k+nJIj6MC^82gJAg^moeug{i8^^<(`YZLUNG}5uYq~VGQ zF3xWIz#5Ok_;~wt3iFyQq)cieOX;()%^&AnOS^bCIlK-*j~1xiS4wx=x1S^2vu0-u zYM<)HBnAcsIKBLEIs}{!e}=<0*uhzY#{9N}!&i|>W{|2BoWQi7dVw%r@xg5oa4B6; zGz)Y)e~ZsL+rag+Ux|5%@~^~##Yr09{Nj36h?`-Y599m+#`%1{IOe>Sc#WQHPfbS@Q`BO5%W?#c4zUAwzxy<*w8HZSe6Se4uAf>5gKXxBde&pkmmwxvIdr`X1 z{5Vm-noNwPhErph7rpZ$%XQ+qyv{kVFY}+EarxCB+Ac`-$mRJX&YgHJz4)R~D>q^F z&M|sDPo9Z>{PFhd_vCsPy*?yo#(!VG^L`^)Pkq4KBks`5e=+xB$5m+TeEQ|}y{==g z{7ervN z<{frDu=bzpfv%x%L!Vx3dDa6G?CvM+P`kgB<{gyj7u24I{k~uCdiD!P9Q|+)yiR!X zv%2pqVV!U!f9izrd%^MkUT|Nq7p3d{-xQnja5g=X*{MkQ3jhHB|Lj=LYZE~fo~FOG zl~hD3NQsCx#U9%9B8pAY)L68o#iXTRXq+V5bnRv*>`rQ1P385(2LF_JI!RexXGF(Rd<$Q_U-KZ_PsZ6f8Km-3Su~z&dDhKd2$fHfzL-;!SVn4m392bYir+=rvYv%WiNl-V@VW&%+6-)lPvzd??RytD*KUlW zUu`Dwf8p(Qadup^(BtjsG@j>w(snjb$#;2umvKk+W-RntgztJt?-{4pWB;**Z}|JQ zFaE9L^m^@|3;!6s@oL*&NuG3p^7W;%Z`?D3bjcH$3{Elo?8bAG({q{V@$qOS7Iqsl z9TlkKIjG2~C`>avD}T)Lo;u@W&eRx39PQxdf8-=mv4#{5$4(y7ws8d%@^GWQy12Uy zd7JcERmQAF&^88>%V`be)Kmy5NCP3LC<*e6&4vee2-ajZUDBV`C5?v+X0wW%gjA*Q*hq)HR!Q8CfbzIgSw&igq}RHXD&g>T zf50STzF4u(zCwB__Z?n0C=QhFP*MYu&yV>5l zQU5a#D=?uUgp2dKl!Y3^WW|`QVp)|~nu&FJUmY@wW1kR5Du*t`W;B%6V8#=c6-kdx z1*4O}vDi)D(6A3$pEIIgZgLj`_VfaVe_pSvx;V1@@N#hh0Iy|@EM6pz74J%?mssD- z>{}4xMP+^Q+)#iW=!4PHX!`6Ojlz#L;|C-EAU_zX5kC-d#L*6JZTUfm!w*dSb@+k94;sM_f8s*C zs9Y&OP@oDm8wA1O2j={M`CM;)u+m_D@b%u){Fz5@ZSjMnpnQF)TujV8dhl2A1G96W zw9!g-)e1)(t#A*TAH3{y7xHkz;RlC;ALOg#2lEa;s0lxC*NGqGE$o{Y;zi{O`9UOH zB{6V#fjKW=Yg=z#pfs2ld>VfDf8_bCH>SM6YG3hn&P^nejDHlr4N%>FwBiA^u)i8T z_FXmxEN~9~oIm0G2Qcy1`48}ao%4tJKfo^l00960+*nPF6h#!So&Di&(FKiQLdZmd zun{6Z8WfG)!z}BL?C#*sGW=u_YPxG@N_x85?i$$PpeAZ0F(!BdJ#jR~e;AG&O!R<} zgE1H{UJM)sjX^IOCDE$>nW?U+WoCN8HM~iu=DqIs>h-Hv?|s$Y?8a^S=1RWwDl+I6 zbQ0OrgcJ^!sRyfj=U~;bkhghL5qmXgKtTN=-@~qDf8oe!DCD6{?0S{dyim^VY`cSvow*- z7dZbYyJl;)K{2t9RiJHJ=-4UZp70!KAi>_4U~i@5$3SR1vd{Uo!QUTK!o3yYD8JW$ zB0i$ODZ!6t+USXXEDPo4Kb$t`oy;rhr^0_+Z+{!TK55=GyH<_xf8Ph%r8R#){n=Yz zYv)(<-xaMm`qvBlE??9-$&(czzP_9f5;KiiW;Xmw8qc$WUP_@S#-pEkxH9DVXhDw_ zp@q$9SdQ$qd!Oaf`z&v~PcC|$yJ|-s+u2p^%NjO_O6Oos*~T~C|1RhC|-k=e8()*)5gdqF(z zybhVqUCY0J?xuF*@$E}LU3XJ^X)yJ~yN}%P_utvxroADBe?4&>`18kmldT(n|0<*w zEV+cx;J$|(=t9a?EaGj7Z4{-MT$fwwkSdPrr8sgvyL)E8OUf>+rbjVrZ8H;@>G8~- znW-Jyw(o%L`;7cvL+sxSNT#QUdTUeN#WBi{%|cuNpmitA;#u}$!M|qrlIxqwzoSyT zIPZ0a4+<2ye~1+GU7~~Jb?AP_2ZLa7@WIp~+9e7`Q=NmtkySvap?E=(6IjPH;e|C zb<}s{_2RS`dhP0OKZx%?&hwhN6D&?YY3I+C=jZn)T1(NN=sdm)i0Bi92%VGT5(l{y zBaamxY?&_N#n*F?fuW_0e<%BzqB**dzH|J+f3%-xXCCM-{tt)zOs}%<_?@BMd6bi) z&wvr^ITox>vaZyS?KR6tes-@5J>+*QLARKbB!Mp(yKEsz7MlPlyO}RLh;X^mY-eDM zxcD@aLn~w9vR!4Eo}HC#RG{_P99(t~o3tXxCZJo!Vmk)PCJm+tg|bXOZ*$7otbaK! zf1*NHF7h-pMDJMaYm~B%)QDGs&FfDw#$s&+@h%`2^K(9IKBj^ME#DnD zsc?ettrgFWL*uxbvkj{zp*NuiltmFUKW`*SSPkud24sPuXAb`^0~(mjrLV zRzo$Un75PolG)jNx}K?+UJCGTbie3z8kertzo$F=ye8!N7~vdbW^6meeZ^gY-Rqz` zyQ@QiVmT5>pp}8Uh~n zZIcl9apS-NJ5Adp1hQryWg<^zebUO7eeuF$M}^&LBGbm=f@YW7Zm^1WS^gn;pe2?%bxyFzVHeo!bU+^ z;7o+^IjlV~EbcJ9#3{F2f2{x1mol+Uire>b3wqeRZa{eKyoGI8R^b-emN=$GN(|NlCQywqsEc;MhQd~fE}A}U5nBdf!S{W@ zZ6XK{pi5lfFjf~9VMdT!WdbbI0{Pp+8ej>c`Mr~|cZA@q%%8Ibf2})d$pS2(uPZ47 zg)lbf*zKYg^rnKDPN6|;vPmcrZ5IP-E_0J;_n_SyN5nGNzTkCzwj5KC&0f6f(ynWw zB4bTE@vehKFeTf#KMXD7PynQ?*kF~hz<#V~npDO>&Di_P^_B+r##)WEs!XGfZ_lhp zU>gxwDFQ1-V4l7Ne@q9| zlh|dPL6{eJPG(&#-di2%YTr70#DrUXqBXVKa7LhW+g3E5qQ$?c*E@##8A}9dIDR?jU*SNv-2q$x)kf0LUBQr&-Tom1Y~sE4EOH6_EXhi|y`@J?C}r}Z#O>tQ8nJ)G9VX+50Q z!^5tJKfS&tGlvhm=fBk6fBU+ZKT18kfre8LCt2rBF7K+=aO&H%EGHF#ecislpI%G{0jg8|Nq=r>u(fQ6rVmIf(Qi)e=XtpMPp1%&=>`y-j)_@BN(Km zJVa&Kz1`k0of&3l3){pGe)F4vyov(m*w`Hx#ym9&pr3f9VRM@@)Z9~RAQLm$ax7rz$8Zx*gWmIvZ=iNbZX2F z^!Yyae-o5KrPz@-6)4JDo!Ma}87mCkmi5cjz}ApuMD$>Vp&6FY#s7+sZikNYJfPtq{|bf9Jc}2s4`*N!m4rt;)I@+hTes*|u7@ zsbyM}|Lf}NsAaQ2Vst$B5!*KON@KgeB7lQdSX8xXTn1E9jmo-Zk#Mv;kQ_~wss}Y% z$HHt2)0iCdH-1<2is4-A5u?Ex9(^XfhU!6TIqOuRG1HV*lYLa71{!Ekt_Rqki7c;wCRy^+uVn(Wv;Z2W%q6C`_(E$0}{B=31i_Ib*SY2wc2J*g-I zB$s^UJASKt>=O3__$}qX+e6=fwve=+Z9DMB#rvO(lcKj9%9bkQImMx5kT*mwfBUN- z$*w@L>5iQ1q>%-Ka7_EnQ;rw!E&qjGF7>=ELL)Sd-%Rpr)Ym`O^~_JQ%U9rS^nbC- zG%s1Le_nR?=MqPT7VkMUSgz~Ny|3=I*ti2qcH_=)WN25?5WjL%>-BK%CR`}QeLu@_ zr#mvV)d7B~irvwZuDp+PopY)Ge66VIpy@!2Z=G%dq~4`muw0u4+He_9mI$FNBQ z3@)h|#y>@he~J@-D8#>``7Cgp-0SBBQ9q04=2kze74fYZ*h+&Hs>YKV*s4@WAIr5o zPMewTe=lTGZ+`b3Y%wn&KO3a=YDBYzeQEl9OmX_dufWkK=0zMF0{0%s?a~dvG4e_+ zaF`ta5ZF)d%m5A%xqov@f0T2KocabZO8%S!yphsRZts-hrlQ~*$TKuj<+A<93+uFHs`Qs95~(rf%9bd8elsq?wrZlLH5)FJKg7A zMR7eDjIzc7f~U zi}!(-$oxa|IL~>GgLz+gWC7=SvglFe{3-y@;YK&Vg6B%dB*1` zxty0=d#q1eVA#!P>j}WHyWaOp zfn#ob*dNEib(ixXf7iR${=#u%^0+&H^uJVKjGX=s@QYJ#=RDWh%YW0}2JZ+`x<9#QtHGcW%*-!rEHH_6#EfLosP-@OZPA;q5@k8FqQIalBk`TH-x z&nfSQ<4+-zKI0rNKk1ggyUPr7e%s=We&-ff=e|$cxtC+LXy-yu%0rZjm>wrCE zmOz(#j$j;EUpNk~udK&)lIydz^8_cqkMz&yyh66`YvLSnpEnEKaO1=N@_v%@AlJLs z{=#ul#`&ZF*Ea%hkT(Txy6f$=&R+g;|M>cM-WQmEtXJ$O=ikde)+5SZb>=nyj|Fa$ zw*+pHe+LVIzk1Fe>st0F$MX*W00960>{#n>6jc2_pyW|^IZLLz*W#FzjkQi%co0Aosd)x_5aKkyY=9vY-?T3!W| zhYE`K+?_K!y|csYt|gk--!Id>~H?0WO>0rQ3F#K0?`NTOaTD_afjG ze^PjU9_2Yw(3MYlhRo?MpzI>YjsgZ;=fiyR>z%{UO``iX%6`Z37)KGq1zX%0&%w8V zKbfu5_t~`ID5xW&``pI$^xnkL$%aP+#Z=>&(B$I`R5M9brCKzYV-b9zFxSPX3$^ zyg_cw0`}T;hjr)k!0X*~HyC_G=V57ePqhvS3O~?65Yk|GC`S^JFaeVzN-vN%2@4f}l z`HO)!9qW%e;_L79p0C5N`M`nH_0M3>m(w~?_t=l)ozB}-+n4HrgRbW|?vJf|Ba{jK z;Qn{o7krEP@(g>Us*AehSzc|t zPsL9z=LhVVtgFFbNYYg`RA%_uo0agBoy1R;#m^Kv(@evDo<8754j7$+PYi{cME{o8 zluc1d2#HaBvq-z^#UOqGV_$WNJD-ai?6s|tkgAEgtSU?6xk&iiBY&h9f1Y7O>kG;L zEm8mjXZ}{Y#5YYC8z0yU15&dX3+dxo7r~)*^PfyVG1;#wx~7IKeyr=vamm|s<2~Tr zStVW=-#In2ytmV5lOpR;A%QhC`9V#MDFL5K3vDV1`+e1-E?T}{M^{rP@08G9FGuwz zwY6T2#O+!&DuYY(QpDgc_gt2)qqEBFT&xVfE8LW7XmIIl6z zlbv23Q6Gyfd7&rhj)VHu&ytY*gM`#ikFQVH-Dz95#RpXO2e@JS?ZMUezHnb!dlc7bYOO^;i z=I4>9sLGD88pAWdtqusQ9~@`R&us3yEb6Jox1kQ~rv;lH8xxsM__OZMTrQ3ruC8Dg z#ee>sZRx`^o?ln8{cepHb(HD(yyp?%S#K)yTE}M_zkm4snQYu-Ze9<4W>*9Ekr^N6 zbQV4sB5nD=Q)KhHKPun&p`D#m5#jMVStX3EFW40=dN@>B;in>$w94{o4=C0Hip{NP z(mcy5asF@@eL zR(v^-(Y=BQW9tjH>%F2zR^%5=_1Z2M|0MrbU5&@RB9s58*f(e*zVY_Qw_X?jPx_wG zKDCtT`1^@_#((CX@izbf0RR8(RzGjkKoq~EZGYM#Qko73B!nzg&{S0Fzk#YmiPMw{ zL8?Sen<-FT64x~nK*4NlTR z1msz#^RxZlz4z{S&%XcwV1SHaU`Zd-ecYfjL`JCxfB<{%_xQdK^qM}YE|O7(h6?i- zA%DYquS*q(p@A^4bg6LE3r=YDHBt*H44``n@jeU*y&wW_n^im&QoO zuq-Sos-)+ZkQfgEVhPGok1zqLA^VtSaX3joG;MPjug ziCK5|5#H1*uwd0|&m%tBQ7@|N8<(cUaDOS4zbh4t3F#i$mLfFZxNPXKe0>X+E2N0w zv9t;`h%?Ffa)v%qGzbBR;A_4QbnDRMpvY zuYItE_hZ}f^$9t?^AS(WiByK_=fM#?uGcGF_jiNy7j(oCaO^A9&sn$c{M@@UY=3s< z6>zB!AFqugYB%R$$Hy_qp6`CAGAcH+E^qH=ch2+aX4mUjS0E#_NUx`{Pj6oi-TC|` zwl_kr*RkxXDLBR-=&uL7;@A7F-32*I*=%g~_N5+sGvQ zBr<7Q9H;I)o|-w=L8jALg}RZ2s((^kg|h9B$CF#$BaXbOe)M40<2z}8&RO*E?{=Q6 zN&#sGMk?p09sE+M^-r7lt@XF}pQ@3urYVNqTc|9MjwWMN)rtwElE0PRP7Wt`32IYS+na@N)nb@Bp0Cle)O;N3;WFcX@2pb!~EjwsNw$@ ze%}_qxC$Egm+FoDVs=rWh<|M$7$$I?^x{}`e*#)N*IxWW^vL@RXv~}Hj)Qrke(boK zjqmsKi5U~GyInrfA@1prjiiLZLo<75aKYsvQG3}ub0001s z0RRAaob6aUPQySD9j_fHPGTpHC^!NoWqKqOiim;&I?D=1iHID_fDj!e$Drg093huL z;utAe$GhY)vV)>DNbE?Xx9@po_j&y(HXx$afXJ2DlIc+y4aF^)%Rz}n2*t@d8y7r~ zc?A8UqF-mJJL_wfs(%@uoL}g9KEusCp{lpO(OK6P+?RPenQ{FCeJ|~w^+IVvjYl%4GM{C^vDAlld0j`GKGDrXI=P=M(r6l|*YhNLT;#WR zSv*OoIz)xW_s`mrf^nO1hw%pEF5^wc`SbJ`Z!_Luyvw-Hcz?inkMVxZzUh78V~gnm z5Oo_6bq5gj1|aG#AnHv()LVe4dw{670a5P&qTU5W-3LTH07Sh9hK-GQ17#Pm(>^iupEByx%4|fq}~3X+>h~Z{2TwqzwvMU8~?_?@o)Sa|NqDT o+m8N!TaRe_dK>%pzd(Q0dD9it3&rug3HN`W^QI!c0NN&2%Gg|Dvj6}9 delta 10427 zcmV;sC`8x6QrA(CG#E{9ZXil^Y#=f-ATcvKF)}(aIUq7HF*1=+BavVQe@rI;00000 z0003=O%DJ70K_H$0C=42Sj|ryHxxI^h9)T_&|bDZM0=?M5<;6?5;dwagnWP$0t%4R zR>V8&opqTn*)ufs)MInYkxQ%o3GJol9D8V`9(q7(D@9cmRZ)d{?6H1hduPTh?Ce6I zjd)^s^E~tH=il@C{l*s%e?r^42#pARR$L_{<&3h2GS_)=wVjpq+O?Vx{azWpt)5>K zSF{xp)kPcoO;;Kr3qryoN;tCH)NL~2i&h>In_@S4~ZmL&8HDZ8JP>*fbD7Ue{k4oxJzB~0DGj< zu;53;A#A>b+k(F-P@BiBX$Rq!XjmkEd<$o~&G{z{vDYAMl|~AAw2TGv%K@hm4g91b zbDzjV!s%z(USt4|D5nkT(r62tZd2j}xV%I6bo zyNwyPBf@yv&pbLff3L$RpVx}GE+w*i)6~#kZDjxctS=9~e?G2$@z*$L zTr^G^H;tplRpYF2KdhYMFZD^`e{uqK14{Qn_3R^*uV-M-e|=bo>dEVjLTPK(8v%3b zPQ`uV4>#XsbTCiI#tn77inQo;TehUGDqXP#?f1tYgp5A6zS?>E z<<8SF%V`O@e`{MezI|8x&0B|=XD^<77UgjU+s3Zj!1jhYZnycMjMh9D{I*B>~@HxMkE z(WaufjJO1I&39Rj{9@ZCyhD4v7IjlztPzyNlvCeve@UchG~ePj(0lotZMcEGA)ZS= znCuGPBu(J^i3K;lBJRENp`}|sc7_Ja#{-mpH_2%~A?deE81r@`j^xzBAdEv#e09K+ zm_|V$Fksqn5(omfz&y-&O5(0GpkH}FzsfPkdE9l z7~i;If8YxC;C#a?Jnrk_Ilb1=TgJFx%YE$1k@$b{%l`Z6MI%oqeeM-gxho&)-0QhK z_p0<+cgK}&iEB>%j@4syA2*1LQZWyP%#$M+|JiUl{&nf`zAMBQ|ODLZ&f3U^xW0?gG!(J82Kd&#&uHK)$ReyN3 zcI7HAw*L<0`W-@bG?<^I{3@mXaMb5@lNnsHD?g=6p9XA$huD^QJ?jzQ#|BLwnZYdWvw`U-q1I%!otpA-aFCEAsZY z63XYhTiq(c2Ot@P@&TR5WFJQTtxL^oL7SfzM-fl$o&Dp{-)L`CU3eku!u1ob3)_9x zh4yJ(IFhoY_evVqner|Qy&;JDg0RR8&Sx-+BK@@+K7Qs+c zJSZl{G%*T9LV%Oe#4Z$xe@Y-0I8|ZVE@fqRr`eqtqLfj1Fd>H2sG0vw|Y|3GrZ+@JQf*a@k;#Evo z7p1EkQ-@tBU!;aV0@SO5#@`x;fa8-Fy!3)SMn8Nnf2=44tP=yPe;iKWJR#Y6V)0b4 zIJ;%_8C@alY|u7p9m|q|D|#`FRV&(xc_dxU25uX-R+Cj$nOj3fF-aaTPvf#pPhnHB z}^M%{wU5FQ`3_`hCCN^)w4d9Q|+)|9io&>b|dq zb;7Zz6TB`u4I`$(J9RhSYPyb%Vkle446#HRQ}?k?PZ@-TC)=E(ci zVi6zKe_jt`$3+`G){b`b`N~hy&IZf*F01b`?}*;Gjb0P?T`%rk!{mArcyVp>{m9n0 zz}j(gy$ei5eq6cnc0Eu|o^*iH^(C@z*gKAN(Ho0~MyY*HF6Je<`FG8j=|tCwWL(&d8-l61jD~VbI*eqbfiRTiBz{Kc!^5~hG4DsPH!*36Zf_8kbR&rtCXiZ? zaU3O559oZ%&?F^O)SuKvjfD)Usw|}-QOQ3x(_ycb6Za#aH10%Jke0>iwIrsCIBXrV ze~Nn}D2+R*L+Q&~V~?Nh1h&r)w!J*P6S&^tx$x-Zc7Fe@HWu*&3p{om$oqF2ovj=3 zKMM^RCN+eN!n`i3P=f|uF)AyDq=+=l_`1BW4q3(V3>QZthcCs)HI&g{)*F#zQIC&? zCZeIC_)Xuypby%gGrV5{zKa8AdO=gKf7?@89BF>^GI0UGSX9j{p23b4?uw@uU*D|k zyTiqc$okB=fgnB5`^9;@EaZsq%NRP$UR{7$6$i1mhm;#XDK`na;d=V_DY^Whe11TE zuQfk-P-lMdb@1cU=eIvsjUV{`L4M$`5^42eC+n z#K7eR*1UkOZLN8MTxVYJ`ujuwf5xp1OI~2Nuh=>#Uj-{9*nN@CyI{|Nq=rO^g&p6t129;cp=W8bw3MM1rsp zB0nZ58oOs_S$AZYU3Zq@CyP+iT{~0K)75s@zzzrF#Yo~$@FaTjVnWmde@9Om6HGW5 zgYlpj1Mwti40_QRj8^r}Om$5yGt&#M;Y~Uu~&muzL1?bo`aTEg2u@$+gh}?m1~%fk-ecIum%92 zvul7|%h}Zj0_qR>9(Jt=e@9N~kcT$0>s3?yU+(`Mm}d z@e%z^34T1;Mo;u(MJPA_;j~HbWL{A}75?LTQ*HG6q&y8dF*BHDX2ZXv@jNH!r4)K%Jo=f3t3#fT7W8Nl zTG*U{<;Y&U_gN{u&&tO8y4Xo<|v5GUZL zh=+MSi0&7ti|uk#|DcCl(PY+fENnoo;)}y{Xi(Diw250~>2(H~O*>*8QpLR&#N*EE zkool0$+zCUpr_}h17y2 zmk=7<_mBf!NZE=-yiKu)1vXl z<9ogtoPMOG9(9zTHA*_!@1h?m#`W~{_;LCEHhRkx{$BtK(M>JR|D?soE7JXI3p_9L zk)CXSf1!CIx}qiBx3`4;kR)$D_;z4#^~6qn<7+>^@ac=|^v$0i{{0O!q<{Stqrnv& z^&NS=I4y-ri*y-^&DhiXes00$-bs&o-U^Eq?c#>e>^+8rn~sxAM&%k%D&@whIZ#sPKrJQ zMzH5tuzrAbrG{*uSw`}+`&{TDzgr2q#k?d5e978n3rVur1VGu%eAyv{%avw317pO+ zXP6vXSqqo#D#P^btZbtKt;gozvP0OU6+t!u-7*&2F;F&XFij|wW%7BOQ{HC%%W)AE zf4Xv!rfLzSa`LOwz3KFz@ci^PL z3BI>hJU0%H<7(bEteTKBM-;N73RzAef6FRlAwP|5hMmH7%30WPq}9ai#AsBIDd$6y zy1);Yoz%er5U+!rcL=um#!WIWJBp$^)qlDnV=&ZlC^FN8cfo#9bCHcOzUEy~(eUjtQB=Jgt(%eId82Ee|bLU4F;+Lj2?r$|F?DSR_|W`00960>{rik(=Zfv zyLH+Sng&9G8x!L?i5qZgyLO`$9YX8Y{SXrJlDE{twL{_n-2rjj4e>XC1md6Al_N)P zn}oQJ8wU>9Y0@SkkTn~Wi9E@5-dBHqFVBwsUI8IADO*mqyw@w4ks4p}5ocJAZ*`J>{@op$LqIx0?k z9LPu}zA*_izXL*rIMgbnK#qZCK_y1i7tx1nK!wW(I`_ z7Bb*rO|ttFwhqK-ivd>S7TJ|Jre#VD)&V9^gD_}{Zpenh)`>3K0c;st24Ug%z0YkT z2oIqvT;M3y6c%Adm|JH8EYkw{+s9g938DqPld*S%;H=7@vL(%*f3#u&7BJA2RDnVi zTX5`dSqpnp!%U~pA~xA3l!&g20X0{-NwmAr?T;g373@Ipe4j1H6lA*}ucox?nyATG z(_XxrU=d8oE*=a+t2hz>DQh-ZRV;82E1M>jF;Fx1|8l*h;k~iZAe}nXs1w*Tn=#l{ z3|5K3$}yOyuK?4*fAZF;LWyZcR+BueV66>$bU&my7j*g3(Q_e=Hxc6XbdwXNKHM~R z8D|jXC7qLbS4+0%OVHo6@$vmk6CcK{4yNhsVn^`Puv<#tPeu4jm*3|TP$$DL;+Ax8 zg$?Jrh2(~e<;=c*VZ}XosD`p?p{+e+Isl9TMzGL^>9`XqpTiQlGVdmJ)G6USv@@J zdicZ3t1@$Vw}1XqCVX-sbYI>Wrqo&FbQ_m8WNQaaI>+yuZgg z&ARw^_nVT_s*8UB00960+*s>x6jcuhez_Xh7P5?p9;`Gp!xFmqUvX5iu&Jrh_tYlp z>Q)3pRPMXNQp0q$5u}N>^)-?v7E6U5(x{|Ma^UD{Sxbs;17)E`JFcrXEY`yHs#c+~ zklsXfTUo_)T5Cs>e1L8P3)+ndy49)`VtVp?f7cjcW-}v6yUwsxSyyXYOb;d7*6231 zOpEe=U0pr3Y!*n2j^{pN+lF3c?9f*RaL`JNsy2Rn7b^9^9j)oJGhcR0b(`H-UeARNl!bLUDwHC{FEwz}cu@*Jn ze@&@Vr37%S4^yj3wbevjg=uO$2QZ@hNJOh8*T%+Gtf@v1Xpw}P*_1}NIgPF|jjke% z&XLzqJxDEQohmhEn$l{rk1EwbBMr*+0Q+-^65pSNjy`dJ7Ao=mS;VCvpETGl^|70r zWEYc7gXyqwbMdPiI+g9xoks-;cKJf`fA@z-j5pW(V4S(+M>nh{Rm;t<8nKPX;kVq! zeyU^-GT)!g6ix<@y!pL1^7&uW9J@!2-_L%MWDm0C9K)I9Juk^VPkAX#+}XS*6=jg* zlCONnZ?%tI;(h?XrTlki`1{Wmk&bii2j9H(;FAeb^iE^>GG!vCIFtghAJ%blti4)xRDacTmZ0+*ytc?J64LSB@IJ0nXio3x&AvXF2Xn zM~1d0z%Ny?J9^TU_erjEF7^Mkf7IjCWaqUGkNf=wNNoRYU*a7rzAp=vy!?W~T<=K{ zW*dgKRmCr=N4Yl?O?ja_m-C^>kryke-#qfSO(yf4Ce3qN;yhoD&vTcOU7XK4GPIf+ zw(I#p7IXrBoN<+xWf#{oj?9ly#g3YIMm37hR`I84876!v)36d~WLnUoe{epAO&VZu zNzE|+=~DdDo%ll`{vFL{f#c*}KQD^uH$jq z%yj>IA(MLZyYFC&c>(#kAZ<`1nl0>0)8}Kl(;t2Xjy^Fj;@}Xt|4?q1Z32#ySL=Wy z0djX1aG1#bTUw)>f8*rzH-Itn=RDxelzwu1zXUi&Oo2gC(mR*4hs+cBA*KDg z=M?8?%D6iP#>lBQz;$w><2dIKiR`sGM?B-e@fHePAbZvUJ4kWYY|c)yw+`6lKKB|2 z=l@zgaFo0vFhmZ20K7`d1g;Rwt89;Xoq4f{vz^TDoW8TZjdkD z2VN!%4wrGB_Z$cFzUb&e&I@GsTHrd64Vfe`|l?xG{Ocoj>|tCNNIUd+I#fd0&L{y1)PaQqDfI zzaE&Hf2=n@|5%SGd)1kjf1K~xGk{y<+*!bF&-w4!4Y-)%PmV{n!}XjmaGCu57vSd< z|2Thn{jrYZ_4m^Itpl#Vod4uJ#qH!g%k#v#$L9+Ke)K$Ve=}{5wSUDq=zEUi{=jn& z*AMr<*S?Tj%%?nGJlFirF^mJx2RUwx2kRJHum0HLIg@Sv&`z#fzZT>2s-xs~g2*kN zXL!zG94AizhEw`u-l7M0a*mO2PXoqN`pND6dw?5cyTCPaXfyDVJ72lI_Xl7!Wt^oO zf&Bg7-Nt#He}n~w$?v}bMm+uFc=*0QupiJvN(K5z>p?)D``l|BJb#-7M#kWf52Zo=Z|$Q`;+7O2LJ&7|Lj;>Y!p=(o|cQn*iu?9HIlN4L5d=@MIwRPvt48!)i&jF5*)29Ge$TEf@a(@=kS->oesCU7?>x#La`XtGmz2E=>?0>n00z_65$1!hBkBtCe~<4m z&)APP_G5g%%mDVAb;5YDAJ>aj7jI>LOed>?q7JbD3mgZw!cc$3_o z4(zq*4(rb6f!Dj!{=#*kjq88(2jDt+gW)FG$uMeLKVN63`L}(xmD0Xn`1w&seExC$ zsm_mn(00<9)BL**?xY+gkN*OUT#{W3w;bm$tp)bl=Huhx$MN;A+yopY-+u?7^A`he zIo2O_#Mj^HJzs}k3xETu>z~1%FQ;{)?y(=oJDsgSA~>{e{*&n^Ci_)I*VK^3k9D0>UGg@~cn|R8lz3r$ z`!kv4{Rn+FD6$?E5?Do(AJo*C67Z?C(58~G-&ZZ_qUHNFbTxJI4hikGa#U|pTkF+G z+^$ulGKdBH>wk1jR)UG~RG*sFWmP0XmB94HURxw<=F2NQIq%O3WAhHSqH1Bf1$Ib* zgdeN^ljVJqFgEW+^a-r&S@-e&Yvk%nWjSBXy++=CF8h^_W?yCJN!z4^zx*WrtaX6z zTP9^zH&}mI)I(yErmA|pKPp8;x-DtN&{lL=kx*trbAL?nLsA=CL?sZC*!;CFc`WAg z2)1uUL#?I;H8Jd2BZs7DLrqov+NxCz>&r{a%EkD)N?rVBjq#f$GzgiE^BUtk+3DpG z^-*HU3q3)%M+_A=(}l%zY20{Q4qkD=kZa z>Uv>NkAI5c)vaPULX%jsfNm41yl!n(jZfD6A*m6!-GW87p^Jhs8CrC@aG@Y%ejbU6 zs%#IdF+2m@>VUBN!Ex67%;3JuqMmAe>*~OMUa;x0F_GznKkNR?<>Hvi)fMcb_|KoS zEq!>#^Xp2s->vbYjxs%;cRvQa=uKr_>-cQr7k|G$lZ~6q&Fi7hjB4Os(s-k*J#W(x zX)6GpAsg5HUir?CN7y+P5uU7*6~fs1f?d&~heMSWekwvqt1PeffMPwM*xZUH&9kI3 zF5KMQ+=?Z}M-@xhm_!~k&Ho-Dz5R_fhnBzlq_R(@bL$72+o0p>2jfaLzNdY!$j@2K zbboth;$AUvub8-3{Ok9MAeS$&)4f;Jh)Rp7>!On8)nejak^EkfXKDXW_X^J^m%i-m z?$796L4>jO1>5yrQ6nqztEPHwmy3Urf2*#><6e=;|5NN6G!fr;`{P@$i~py6&v-bw zl=AQ9400030|Ls=IYZE~fpMRuHY{lA-gD8juMB8GZUxGzLvT14&ZD=rU z@enAJ-R)-W?#?nh(Y6P#dJ*(w{{=y?2QMCk3Lde}WgC*_~-7+u){YQBdD9 z%>H)fy?OJSH*a4O0MJ9*00<<(aZGAVPSaNF0wBr9;}P={pxd{}utJ*+i3O_|A%7B# zd%6oTBnSh69b!7V#UWU&J1Q}evr>bOX6gR=CD2&EyT$q!r1b|lE?lpaFfuT-Q*)-J zdzE5-aXvp&xjQ*NF$sfxjYrsxHQ9}U3g}Ev;d-g~;dngB@rc9=0A$0}IahRJ!P`TG z*E|=h0bg4Y{uv>@81_~#jVmLyg@5XOT_tJ#0X-HC5;$rQ585}^q3u!n2ei{h-DJJ6J38w7r$qSI8}g4~=pbjqMs<&Wk2(h-vK^r( z^D`Kl7o(Png@rQ1=fHt@Txl@fuy;$c*BA0589G;n&jznwXall|!B-=F+oa#3k#U0FZQ-@^B^m0ovf-4tzuv+Q}EyLWZx-RIYza$7^}`IuXt z`<}h?VLRvN9q{WonFh`0%W$SfY%7~lX$0yx8-#0$#cN-tic@1*5w@8zJ|#2eYaE4l zIiA|N*L@tb-hv)khL))BPo3gnn z(gK(_JyJ#Mi^vUd33+CazXK@~yw53VlTIQd7|gl`MuojQ}HX0&+Pj5c1x-k{?`}( zUVWj3_+q%-`T}|Sl%I9nnvV5D78juNeDt^17vj$R?)u`~o8i#KJXSlxFS-8-+wLOJ z4*&oFg8={lc%1E6J5Iwu5FM{SiIdo|q2Va$kx(ci3JT~fD;Omrax4Qv+<+r+1WHPd zz!7M;14m#tUdNA-6Nu7K>`0@x?|EkTdHpFiB%94cWoz*o<*G!Jj&g*%;qGq00*IR3JR<();GEe4+dh`&91(A6rZp zfT-Jms5^kDyMU;BfT(u>QSSnx?gOIU14O+Ki248!^#Bm{5D@htAnFmr{}gyHZ?yS8 zfZs@6KZg2%)qAX7@uO<^$>*xy;SufjKjeOlf8*cyH#Gi@f8*cyH~x)( Date: Tue, 24 Jul 2012 14:50:01 +0000 Subject: [PATCH 44/68] Fixed serialization for LieVector and LieMatrix --- gtsam/base/LieMatrix.h | 14 ++++++++++++ gtsam/base/LieVector.h | 12 ++++++++++ tests/testSerializationSLAM.cpp | 40 ++++++++++++++++----------------- 3 files changed, 46 insertions(+), 20 deletions(-) diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 2ab15a054..5844e8201 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -22,6 +22,7 @@ #include #include #include +#include namespace gtsam { @@ -135,5 +136,18 @@ struct LieMatrix : public Matrix, public DerivedValue { static inline Vector Logmap(const LieMatrix& p) { return Eigen::Map(&p(0,0), p.dim()); } +private: + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("LieMatrix", + boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Matrix", + boost::serialization::base_object(*this)); + + } + }; } // \namespace gtsam diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index ef31e93cc..916730fd3 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -119,5 +119,17 @@ struct LieVector : public Vector, public DerivedValue { /** Logmap around identity - just returns with default cast back */ static inline Vector Logmap(const LieVector& p) { return p; } +private: + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("LieVector", + boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Vector", + boost::serialization::base_object(*this)); + } + }; } // \namespace gtsam diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 27f74c222..c0cdb6a93 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -269,8 +269,8 @@ TEST (Serialization, factors) { Symbol b01('b',1), b02('b',2), b03('b',3), b04('b',4), b05('b',5), b06('b',6), b07('b',7), b08('b',8), b09('b',9), b10('b',10), b11('b',11), b12('b',12), b13('b',13); Values values; -// values.insert(a01, lieVector); -// values.insert(a02, lieMatrix); + values.insert(a01, lieVector); + values.insert(a02, lieMatrix); values.insert(a03, point2); values.insert(a04, stereoPoint2); values.insert(a05, point3); @@ -352,8 +352,8 @@ TEST (Serialization, factors) { NonlinearFactorGraph graph; -// graph.add(priorFactorLieVector); -// graph.add(priorFactorLieMatrix); + graph.add(priorFactorLieVector); + graph.add(priorFactorLieMatrix); graph.add(priorFactorPoint2); graph.add(priorFactorStereoPoint2); graph.add(priorFactorPoint3); @@ -366,8 +366,8 @@ TEST (Serialization, factors) { graph.add(priorFactorCalibratedCamera); graph.add(priorFactorSimpleCamera); -// graph.add(betweenFactorLieVector); -// graph.add(betweenFactorLieMatrix); + graph.add(betweenFactorLieVector); + graph.add(betweenFactorLieMatrix); graph.add(betweenFactorPoint2); graph.add(betweenFactorPoint3); graph.add(betweenFactorRot2); @@ -375,8 +375,8 @@ TEST (Serialization, factors) { graph.add(betweenFactorPose2); graph.add(betweenFactorPose3); -// graph.add(nonlinearEqualityLieVector); -// graph.add(nonlinearEqualityLieMatrix); + graph.add(nonlinearEqualityLieVector); + graph.add(nonlinearEqualityLieMatrix); graph.add(nonlinearEqualityPoint2); graph.add(nonlinearEqualityStereoPoint2); graph.add(nonlinearEqualityPoint3); @@ -412,8 +412,8 @@ TEST (Serialization, factors) { EXPECT(equalsObj(values)); EXPECT(equalsObj(graph)); -// EXPECT(equalsObj(priorFactorLieVector)); -// EXPECT(equalsObj(priorFactorLieMatrix)); + EXPECT(equalsObj(priorFactorLieVector)); + EXPECT(equalsObj(priorFactorLieMatrix)); EXPECT(equalsObj(priorFactorPoint2)); EXPECT(equalsObj(priorFactorStereoPoint2)); EXPECT(equalsObj(priorFactorPoint3)); @@ -426,8 +426,8 @@ TEST (Serialization, factors) { EXPECT(equalsObj(priorFactorCalibratedCamera)); EXPECT(equalsObj(priorFactorSimpleCamera)); -// EXPECT(equalsObj(betweenFactorLieVector)); -// EXPECT(equalsObj(betweenFactorLieMatrix)); + EXPECT(equalsObj(betweenFactorLieVector)); + EXPECT(equalsObj(betweenFactorLieMatrix)); EXPECT(equalsObj(betweenFactorPoint2)); EXPECT(equalsObj(betweenFactorPoint3)); EXPECT(equalsObj(betweenFactorRot2)); @@ -435,8 +435,8 @@ TEST (Serialization, factors) { EXPECT(equalsObj(betweenFactorPose2)); EXPECT(equalsObj(betweenFactorPose3)); -// EXPECT(equalsObj(nonlinearEqualityLieVector)); -// EXPECT(equalsObj(nonlinearEqualityLieMatrix)); + EXPECT(equalsObj(nonlinearEqualityLieVector)); + EXPECT(equalsObj(nonlinearEqualityLieMatrix)); EXPECT(equalsObj(nonlinearEqualityPoint2)); EXPECT(equalsObj(nonlinearEqualityStereoPoint2)); EXPECT(equalsObj(nonlinearEqualityPoint3)); @@ -472,8 +472,8 @@ TEST (Serialization, factors) { EXPECT(equalsXML(values)); EXPECT(equalsXML(graph)); -// EXPECT(equalsXML(priorFactorLieVector)); -// EXPECT(equalsXML(priorFactorLieMatrix)); + EXPECT(equalsXML(priorFactorLieVector)); + EXPECT(equalsXML(priorFactorLieMatrix)); EXPECT(equalsXML(priorFactorPoint2)); EXPECT(equalsXML(priorFactorStereoPoint2)); EXPECT(equalsXML(priorFactorPoint3)); @@ -486,8 +486,8 @@ TEST (Serialization, factors) { EXPECT(equalsXML(priorFactorCalibratedCamera)); EXPECT(equalsXML(priorFactorSimpleCamera)); -// EXPECT(equalsXML(betweenFactorLieVector)); -// EXPECT(equalsXML(betweenFactorLieMatrix)); + EXPECT(equalsXML(betweenFactorLieVector)); + EXPECT(equalsXML(betweenFactorLieMatrix)); EXPECT(equalsXML(betweenFactorPoint2)); EXPECT(equalsXML(betweenFactorPoint3)); EXPECT(equalsXML(betweenFactorRot2)); @@ -495,8 +495,8 @@ TEST (Serialization, factors) { EXPECT(equalsXML(betweenFactorPose2)); EXPECT(equalsXML(betweenFactorPose3)); -// EXPECT(equalsXML(nonlinearEqualityLieVector)); -// EXPECT(equalsXML(nonlinearEqualityLieMatrix)); + EXPECT(equalsXML(nonlinearEqualityLieVector)); + EXPECT(equalsXML(nonlinearEqualityLieMatrix)); EXPECT(equalsXML(nonlinearEqualityPoint2)); EXPECT(equalsXML(nonlinearEqualityStereoPoint2)); EXPECT(equalsXML(nonlinearEqualityPoint3)); From 6bee17b60332a19aabcd36dba64b92011c960d9e Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 24 Jul 2012 15:48:33 +0000 Subject: [PATCH 45/68] Added support for SPCG in matlab wrapper --- gtsam.h | 15 ++++++++ gtsam/nonlinear/DoglegOptimizer.h | 1 + gtsam/nonlinear/EasyFactorGraph.cpp | 2 +- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 1 + gtsam/nonlinear/NonlinearOptimizer.h | 1 + .../SuccessiveLinearizationOptimizer.h | 35 ++++++++++++++++--- 6 files changed, 49 insertions(+), 6 deletions(-) diff --git a/gtsam.h b/gtsam.h index 869ee6a09..e48ced5ee 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1209,13 +1209,22 @@ virtual class NonlinearOptimizerParams { virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams { SuccessiveLinearizationParams(); + string getLinearSolverType() const; + + void setLinearSolverType(string solver); void setOrdering(const gtsam::Ordering& ordering); + bool isMultifrontal() const; bool isSequential() const; bool isCholmod() const; bool isCG() const; }; +#include +virtual class GaussNewtonParams : gtsam::SuccessiveLinearizationParams { + GaussNewtonParams(); +}; + #include virtual class LevenbergMarquardtParams : gtsam::SuccessiveLinearizationParams { LevenbergMarquardtParams(); @@ -1243,6 +1252,7 @@ virtual class DoglegParams : gtsam::SuccessiveLinearizationParams { }; virtual class NonlinearOptimizer { + gtsam::Values optimize(); gtsam::Values optimizeSafely(); double error() const; int iterations() const; @@ -1250,6 +1260,11 @@ virtual class NonlinearOptimizer { void iterate() const; }; +virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); +}; + virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h index 8a21dd96e..70d55d192 100644 --- a/gtsam/nonlinear/DoglegOptimizer.h +++ b/gtsam/nonlinear/DoglegOptimizer.h @@ -57,6 +57,7 @@ public: void setDeltaInitial(double deltaInitial) { this->deltaInitial = deltaInitial; } void setVerbosityDL(const std::string& verbosityDL) { this->verbosityDL = verbosityDLTranslator(verbosityDL); } +private: VerbosityDL verbosityDLTranslator(const std::string& verbosityDL) const; std::string verbosityDLTranslator(VerbosityDL verbosityDL) const; }; diff --git a/gtsam/nonlinear/EasyFactorGraph.cpp b/gtsam/nonlinear/EasyFactorGraph.cpp index 28e344efe..5031975f7 100644 --- a/gtsam/nonlinear/EasyFactorGraph.cpp +++ b/gtsam/nonlinear/EasyFactorGraph.cpp @@ -44,7 +44,7 @@ namespace gtsam { size_t verbosity) const { LevenbergMarquardtParams p; p.verbosity = (NonlinearOptimizerParams::Verbosity) verbosity; - p.linearSolverType = SuccessiveLinearizationParams::CG; + p.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT; p.iterativeParams = boost::make_shared(); return optimizer(initialEstimate, p).optimizeSafely(); } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 5677f6c08..4b3f8d391 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -61,6 +61,7 @@ public: inline void setlambdaUpperBound(double value) { lambdaUpperBound = value; } inline void setVerbosityLM(const std::string &s) { verbosityLM = verbosityLMTranslator(s); } +private: VerbosityLM verbosityLMTranslator(const std::string &s) const; std::string verbosityLMTranslator(VerbosityLM value) const; }; diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 1c071faef..03f3a5e34 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -63,6 +63,7 @@ public: void setErrorTol(double value) { errorTol = value ; } void setVerbosity(const std::string &src) { verbosity = verbosityTranslator(src); } +private: Verbosity verbosityTranslator(const std::string &s) const; std::string verbosityTranslator(Verbosity value) const; }; diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h index 2556134c0..b46686fe7 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h @@ -31,7 +31,7 @@ public: MULTIFRONTAL_QR, SEQUENTIAL_CHOLESKY, SEQUENTIAL_QR, - CG, /* Experimental Flag */ + CONJUGATE_GRADIENT, /* Experimental Flag */ CHOLMOD, /* Experimental Flag */ }; @@ -61,7 +61,7 @@ public: case CHOLMOD: std::cout << " linear solver type: CHOLMOD\n"; break; - case CG: + case CONJUGATE_GRADIENT: std::cout << " linear solver type: CG\n"; break; default: @@ -85,9 +85,7 @@ public: inline bool isCholmod() const { return (linearSolverType == CHOLMOD); } - inline bool isCG() const { return (linearSolverType == CG); } - - void setOrdering(const Ordering& ordering) { this->ordering = ordering; } + inline bool isCG() const { return (linearSolverType == CONJUGATE_GRADIENT); } GaussianFactorGraph::Eliminate getEliminationFunction() { switch (linearSolverType) { @@ -105,6 +103,33 @@ public: break; } } + + std::string getLinearSolverType() const { return linearSolverTranslator(linearSolverType); } + + void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); } + void setOrdering(const Ordering& ordering) { this->ordering = ordering; } + +private: + std::string linearSolverTranslator(LinearSolverType linearSolverType) const { + switch(linearSolverType) { + case MULTIFRONTAL_CHOLESKY: return "MULTIFRONTAL_CHOLESKY"; + case MULTIFRONTAL_QR: return "MULTIFRONTAL_QR"; + case SEQUENTIAL_CHOLESKY: return "SEQUENTIAL_CHOLESKY"; + case SEQUENTIAL_QR: return "SEQUENTIAL_QR"; + case CONJUGATE_GRADIENT: return "CONJUGATE_GRADIENT"; + case CHOLMOD: return "CHOLMOD"; + default: throw std::invalid_argument("Unknown linear solver type in SuccessiveLinearizationOptimizer"); + } + } + LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const { + if(linearSolverType == "MULTIFRONTAL_CHOLESKY") return MULTIFRONTAL_CHOLESKY; + if(linearSolverType == "MULTIFRONTAL_QR") return MULTIFRONTAL_QR; + if(linearSolverType == "SEQUENTIAL_CHOLESKY") return SEQUENTIAL_CHOLESKY; + if(linearSolverType == "SEQUENTIAL_QR") return SEQUENTIAL_QR; + if(linearSolverType == "CONJUGATE_GRADIENT") return CONJUGATE_GRADIENT; + if(linearSolverType == "CHOLMOD") return CHOLMOD; + throw std::invalid_argument("Unknown linear solver type in SuccessiveLinearizationOptimizer"); + } }; } /* namespace gtsam */ From 1724267c852523e71d27da6dcc9c857388ab94f2 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 24 Jul 2012 15:48:39 +0000 Subject: [PATCH 46/68] Updating matlab unit tests --- matlab/tests/testLocalizationExample.m | 37 +++++++++++--------- matlab/tests/testOdometryExample.m | 24 +++++++------ matlab/tests/testPlanarSLAMExample.m | 37 +++++++++++--------- matlab/tests/testPose2SLAMExample.m | 48 ++++++++++++++++---------- matlab/tests/testPose3SLAMExample.m | 35 ++++++++++--------- matlab/tests/testSFMExample.m | 36 +++++++++---------- 6 files changed, 118 insertions(+), 99 deletions(-) diff --git a/matlab/tests/testLocalizationExample.m b/matlab/tests/testLocalizationExample.m index 35c16d85f..bbe91641d 100644 --- a/matlab/tests/testLocalizationExample.m +++ b/matlab/tests/testLocalizationExample.m @@ -11,41 +11,46 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) -graph = pose2SLAM.Graph; +import gtsam.* +graph = NonlinearFactorGraph; %% Add two odometry factors import gtsam.* odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta -graph.addRelativePose(1, 2, odometry, odometryNoise); -graph.addRelativePose(2, 3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); %% Add three "GPS" measurements % We use Pose2 Priors here with high variance on theta import gtsam.* -groundTruth = pose2SLAM.Values; -groundTruth.insertPose(1, Pose2(0.0, 0.0, 0.0)); -groundTruth.insertPose(2, Pose2(2.0, 0.0, 0.0)); -groundTruth.insertPose(3, Pose2(4.0, 0.0, 0.0)); +groundTruth = Values; +groundTruth.insert(1, Pose2(0.0, 0.0, 0.0)); +groundTruth.insert(2, Pose2(2.0, 0.0, 0.0)); +groundTruth.insert(3, Pose2(4.0, 0.0, 0.0)); model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]); for i=1:3 - graph.addPosePrior(i, groundTruth.pose(i), model); + graph.add(PriorFactorPose2(i, groundTruth.at(i), model)); end %% Initialize to noisy points -initialEstimate = pose2SLAM.Values; -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)); +import gtsam.* +initialEstimate = Values; +initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); +initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,0); +import gtsam.* +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); %% Plot Covariance Ellipses -marginals = graph.marginals(result); +import gtsam.* +marginals = Marginals(graph, result); P={}; for i=1:result.size() - pose_i = result.pose(i); - CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.pose(i),1e-4)); + pose_i = result.at(i); + CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.at(i),1e-4)); P{i}=marginals.marginalCovariance(i); end diff --git a/matlab/tests/testOdometryExample.m b/matlab/tests/testOdometryExample.m index e793df9a8..17b90157a 100644 --- a/matlab/tests/testOdometryExample.m +++ b/matlab/tests/testOdometryExample.m @@ -11,33 +11,35 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) -graph = pose2SLAM.Graph; +import gtsam.* +graph = NonlinearFactorGraph; %% Add a Gaussian prior on pose x_1 import gtsam.* priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta -graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph %% Add two odometry factors import gtsam.* odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta -graph.addRelativePose(1, 2, odometry, odometryNoise); -graph.addRelativePose(2, 3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); %% Initialize to noisy points import gtsam.* -initialEstimate = pose2SLAM.Values; -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 = Values; +initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); +initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,0); -marginals = graph.marginals(result); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); +marginals = Marginals(graph, result); marginals.marginalCovariance(1); %% Check first pose equality -pose_1 = result.pose(1); +pose_1 = result.at(1); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); diff --git a/matlab/tests/testPlanarSLAMExample.m b/matlab/tests/testPlanarSLAMExample.m index 4c3e6c9da..c6d4cb352 100644 --- a/matlab/tests/testPlanarSLAMExample.m +++ b/matlab/tests/testPlanarSLAMExample.m @@ -24,49 +24,52 @@ i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3); j1 = symbol('l',1); j2 = symbol('l',2); %% Create graph container and add factors to it -graph = planarSLAM.Graph; +import gtsam.* +graph = NonlinearFactorGraph; %% Add prior import gtsam.* priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); -graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph %% Add odometry import gtsam.* odometry = Pose2(2.0, 0.0, 0.0); odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(i1, i2, odometry, odometryNoise); -graph.addRelativePose(i2, i3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); %% Add bearing/range measurement factors import gtsam.* degrees = pi/180; brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); -graph.addBearingRange(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise); -graph.addBearingRange(i2, j1, Rot2(90*degrees), 2, brNoise); -graph.addBearingRange(i3, j2, Rot2(90*degrees), 2, brNoise); +graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); +graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); +graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); %% Initialize to noisy points import gtsam.* -initialEstimate = planarSLAM.Values; -initialEstimate.insertPose(i1, Pose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(i2, Pose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(i3, Pose2(4.1, 0.1, 0.1)); -initialEstimate.insertPoint(j1, Point2(1.8, 2.1)); -initialEstimate.insertPoint(j2, Point2(4.1, 1.8)); +initialEstimate = Values; +initialEstimate.insert(i1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insert(i2, Pose2(2.3, 0.1,-0.2)); +initialEstimate.insert(i3, Pose2(4.1, 0.1, 0.1)); +initialEstimate.insert(j1, Point2(1.8, 2.1)); +initialEstimate.insert(j2, Point2(4.1, 1.8)); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,0); -marginals = graph.marginals(result); +import gtsam.* +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); +marginals = Marginals(graph, result); %% Check first pose and point equality import gtsam.* -pose_1 = result.pose(symbol('x',1)); +pose_1 = result.at(symbol('x',1)); marginals.marginalCovariance(symbol('x',1)); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); -point_1 = result.point(symbol('l',1)); +point_1 = result.at(symbol('l',1)); marginals.marginalCovariance(symbol('l',1)); CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4)); diff --git a/matlab/tests/testPose2SLAMExample.m b/matlab/tests/testPose2SLAMExample.m index cd778cba3..9d934aab9 100644 --- a/matlab/tests/testPose2SLAMExample.m +++ b/matlab/tests/testPose2SLAMExample.m @@ -19,50 +19,60 @@ % - The robot is on a grid, moving 2 meters each step %% Create graph container and add factors to it -graph = pose2SLAM.Graph; +import gtsam.* +graph = NonlinearFactorGraph; %% Add prior import gtsam.* % gaussian for prior priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); -graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph %% Add odometry % general noisemodel for odometry import gtsam.* odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); -graph.addRelativePose(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise); -graph.addRelativePose(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise); -graph.addRelativePose(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise); +graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise)); +graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise)); +graph.add(BetweenFactorPose2(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise)); %% Add pose constraint import gtsam.* model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(5, 2, Pose2(2.0, 0.0, pi/2), model); +graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model)); %% Initialize to noisy points import gtsam.* -initialEstimate = pose2SLAM.Values; -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, pi/2)); -initialEstimate.insertPose(4, Pose2(4.0, 2.0, pi )); -initialEstimate.insertPose(5, Pose2(2.1, 2.1,-pi/2)); +initialEstimate = Values; +initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); +initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2 )); +initialEstimate.insert(3, Pose2(4.1, 0.1, pi/2)); +initialEstimate.insert(4, Pose2(4.0, 2.0, pi )); +initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2)); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,0); -resultSPCG = graph.optimizeSPCG(initialEstimate,0); +import gtsam.* +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); + +%% Optimize using SPCG +%import gtsam.* +%params = LevenbergMarquardtParams; +%params.setLinearSolverType('CONJUGATE_GRADIENT'); +%optimizerSPCG = LevenbergMarquardtOptimizer(graph, initialEstimate, params); +%resultSPCG = optimizerSPCG.optimize(); %% Plot Covariance Ellipses -marginals = graph.marginals(result); +import gtsam.* +marginals = Marginals(graph, result); P = marginals.marginalCovariance(1); -pose_1 = result.pose(1); +pose_1 = result.at(1); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); -poseSPCG_1 = resultSPCG.pose(1); -CHECK('poseSPCG_1.equals(Pose2,1e-4)',poseSPCG_1.equals(Pose2,1e-4)); +%poseSPCG_1 = resultSPCG.at(1); +%CHECK('poseSPCG_1.equals(Pose2,1e-4)',poseSPCG_1.equals(Pose2,1e-4)); diff --git a/matlab/tests/testPose3SLAMExample.m b/matlab/tests/testPose3SLAMExample.m index ac3c0d5bb..0d47990e8 100644 --- a/matlab/tests/testPose3SLAMExample.m +++ b/matlab/tests/testPose3SLAMExample.m @@ -17,31 +17,32 @@ p1 = hexagon.pose(1); %% create a Pose graph with one equality constraint and one measurement import gtsam.* -fg = pose3SLAM.Graph; -fg.addPoseConstraint(0, p0); +fg = NonlinearFactorGraph; +fg.add(NonlinearEqualityPose3(0, p0)); delta = p0.between(p1); covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); -fg.addRelativePose(0,1, delta, covariance); -fg.addRelativePose(1,2, delta, covariance); -fg.addRelativePose(2,3, delta, covariance); -fg.addRelativePose(3,4, delta, covariance); -fg.addRelativePose(4,5, delta, covariance); -fg.addRelativePose(5,0, delta, covariance); +fg.add(BetweenFactorPose3(0,1, delta, covariance)); +fg.add(BetweenFactorPose3(1,2, delta, covariance)); +fg.add(BetweenFactorPose3(2,3, delta, covariance)); +fg.add(BetweenFactorPose3(3,4, delta, covariance)); +fg.add(BetweenFactorPose3(4,5, delta, covariance)); +fg.add(BetweenFactorPose3(5,0, delta, covariance)); %% Create initial config -initial = pose3SLAM.Values; +initial = Values; s = 0.10; -initial.insertPose(0, p0); -initial.insertPose(1, hexagon.pose(1).retract(s*randn(6,1))); -initial.insertPose(2, hexagon.pose(2).retract(s*randn(6,1))); -initial.insertPose(3, hexagon.pose(3).retract(s*randn(6,1))); -initial.insertPose(4, hexagon.pose(4).retract(s*randn(6,1))); -initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1))); +initial.insert(0, p0); +initial.insert(1, hexagon.pose(1).retract(s*randn(6,1))); +initial.insert(2, hexagon.pose(2).retract(s*randn(6,1))); +initial.insert(3, hexagon.pose(3).retract(s*randn(6,1))); +initial.insert(4, hexagon.pose(4).retract(s*randn(6,1))); +initial.insert(5, hexagon.pose(5).retract(s*randn(6,1))); %% optimize -result = fg.optimize(initial,0); +optimizer = LevenbergMarquardtOptimizer(fg, initial); +result = optimizer.optimizeSafely; -pose_1 = result.pose(1); +pose_1 = result.at(1); CHECK('pose_1.equals(Pose3,1e-4)',pose_1.equals(p1,1e-4)); diff --git a/matlab/tests/testSFMExample.m b/matlab/tests/testSFMExample.m index b3595dc18..d83517c7e 100644 --- a/matlab/tests/testSFMExample.m +++ b/matlab/tests/testSFMExample.m @@ -10,17 +10,19 @@ % @author Duy-Nguyen Ta %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +import gtsam.* + options.triangle = false; options.nrCameras = 10; options.showImages = false; -[data,truth] = VisualISAMGenerateData(options); +[data,truth] = support.VisualISAMGenerateData(options); measurementNoiseSigma = 1.0; pointNoiseSigma = 0.1; poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; -graph = visualSLAM.Graph; +graph = NonlinearFactorGraph; %% Add factors for all measurements import gtsam.* @@ -28,47 +30,43 @@ measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma); for i=1:length(data.Z) for k=1:length(data.Z{i}) j = data.J{i}{k}; - graph.addMeasurement(data.Z{i}{k}, measurementNoise, symbol('x',i), symbol('p',j), data.K); + graph.add(GenericProjectionFactorCal3_S2(data.Z{i}{k}, measurementNoise, symbol('x',i), symbol('p',j), data.K)); end end posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); -graph.addPosePrior(symbol('x',1), truth.cameras{1}.pose, posePriorNoise); +graph.add(PriorFactorPose3(symbol('x',1), truth.cameras{1}.pose, posePriorNoise)); pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); -graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise); +graph.add(PriorFactorPose3(symbol('p',1), truth.points{1}, pointPriorNoise)); %% Initial estimate -initialEstimate = visualSLAM.Values; +initialEstimate = Values; for i=1:size(truth.cameras,2) pose_i = truth.cameras{i}.pose; - initialEstimate.insertPose(symbol('x',i), pose_i); + initialEstimate.insert(symbol('x',i), pose_i); end for j=1:size(truth.points,2) point_j = truth.points{j}; - initialEstimate.insertPoint(symbol('p',j), point_j); + initialEstimate.insert(symbol('p',j), point_j); end %% Optimization import gtsam.* -parameters = LevenbergMarquardtParams; -optimizer = graph.optimizer(initialEstimate, parameters); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); for i=1:5 optimizer.iterate(); end result = optimizer.values(); %% Marginalization -marginals = graph.marginals(result); +marginals = Marginals(graph, result); marginals.marginalCovariance(symbol('p',1)); marginals.marginalCovariance(symbol('x',1)); %% Check optimized results, should be equal to ground truth -for i=1:size(truth.cameras,2) - pose_i = result.pose(symbol('x',i)); - CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5)) -end - -for j=1:size(truth.points,2) - point_j = result.point(symbol('p',j)); - CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5)) +keys = truth.keys; +for i=0:keys.size-1 + truth_i = truth.at(keys.at(i)); + result_i = result.at(keys.at(i)); + CHECK('result_i.equals(truth_i,1e-5)',result_i.equals(truth_i,1e-5)) end From a44b602d19367ee546eb1918b61ff9545421434c Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Wed, 25 Jul 2012 21:04:00 +0000 Subject: [PATCH 47/68] Changed parameter name from CG to CONJUGATE_GRADIENT --- examples/Pose2SLAMwSPCG.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 38a2e18e4..240361db7 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -108,7 +108,7 @@ int main(int argc, char** argv) { LevenbergMarquardtParams parameters; parameters.verbosity = NonlinearOptimizerParams::ERROR; parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; - parameters.linearSolverType = SuccessiveLinearizationParams::CG; + parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT; { parameters.iterativeParams = boost::make_shared(); From 7fc6a622bf9aa15204e8303a767ad10e07776277 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Wed, 25 Jul 2012 22:13:22 +0000 Subject: [PATCH 48/68] Added additional factors to the serialization unit test --- gtsam/geometry/StereoCamera.h | 4 +- gtsam/slam/GeneralSFMFactor.h | 5 +++ gtsam/slam/StereoFactor.h | 4 +- tests/testSerializationSLAM.cpp | 76 ++++++++++++++++++++++++++++++--- 4 files changed, 80 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 867844fbb..140ab1bdf 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -141,7 +141,9 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(leftCamPose_); + ar & boost::serialization::make_nvp("StereoCamera", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(leftCamPose_); ar & BOOST_SERIALIZATION_NVP(K_); } diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index d94c162af..a9db7dcbe 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -111,6 +111,8 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } }; @@ -143,6 +145,7 @@ namespace gtsam { */ GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) : Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {} + GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor virtual ~GeneralSFMFactor2() {} ///< destructor @@ -197,6 +200,8 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor3", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } }; diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index d0c0bda6a..5be783aaf 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -74,7 +74,7 @@ public: /** * equals */ - virtual bool equals(const NonlinearFactor& f, double tol) const { + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const GenericStereoFactor* p = dynamic_cast (&f); return p && Base::equals(f) && measured_.equals(p->measured_, tol); } @@ -102,6 +102,8 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); } diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index c0cdb6a93..4f9664e20 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -17,13 +17,17 @@ */ #include - -#include -#include +//#include #include -#include #include +#include +//#include +#include +//#include +#include #include +#include +#include #include #include #include @@ -42,6 +46,7 @@ #include #include #include +#include #include #include @@ -64,6 +69,7 @@ typedef PriorFactor PriorFactorCal3_S2; typedef PriorFactor PriorFactorCal3DS2; typedef PriorFactor PriorFactorCalibratedCamera; typedef PriorFactor PriorFactorSimpleCamera; +typedef PriorFactor PriorFactorStereoCamera; typedef BetweenFactor BetweenFactorLieVector; typedef BetweenFactor BetweenFactorLieMatrix; @@ -87,6 +93,7 @@ typedef NonlinearEquality NonlinearEqualityCal3_S2; typedef NonlinearEquality NonlinearEqualityCal3DS2; typedef NonlinearEquality NonlinearEqualityCalibratedCamera; typedef NonlinearEquality NonlinearEqualitySimpleCamera; +typedef NonlinearEquality NonlinearEqualityStereoCamera; typedef RangeFactor RangeFactorPosePoint2; typedef RangeFactor RangeFactorPosePoint3; @@ -106,6 +113,13 @@ typedef BearingRangeFactor BearingRangeFactor3D; typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +//typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; + +typedef gtsam::GeneralSFMFactor2 GeneralSFMFactor2Cal3_S2; + +typedef gtsam::GenericStereoFactor GenericStereoFactor3D; + // Convenience for named keys using symbol_shorthand::X; @@ -136,8 +150,10 @@ BOOST_CLASS_EXPORT(gtsam::Pose2); BOOST_CLASS_EXPORT(gtsam::Pose3); BOOST_CLASS_EXPORT(gtsam::Cal3_S2); BOOST_CLASS_EXPORT(gtsam::Cal3DS2); +BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo); BOOST_CLASS_EXPORT(gtsam::CalibratedCamera); BOOST_CLASS_EXPORT(gtsam::SimpleCamera); +BOOST_CLASS_EXPORT(gtsam::StereoCamera); /* Create GUIDs for factors */ @@ -158,6 +174,7 @@ BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2"); BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera"); BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera"); +BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera"); BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector"); BOOST_CLASS_EXPORT_GUID(BetweenFactorLieMatrix, "gtsam::BetweenFactorLieMatrix"); @@ -181,6 +198,7 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera"); BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint2, "gtsam::RangeFactorPosePoint2"); BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint3, "gtsam::RangeFactorPosePoint3"); @@ -198,6 +216,13 @@ BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2"); BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2"); +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2"); +//BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2"); + +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2"); + +BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); + /* ************************************************************************* */ TEST (Serialization, smallExample_linear) { @@ -261,12 +286,18 @@ TEST (Serialization, factors) { Pose3 pose3(rot3, point3); Cal3_S2 cal3_s2(1.0, 2.0, 3.0, 4.0, 5.0); Cal3DS2 cal3ds2(1.0, 2.0, 3.0, 4.0, 5.0,6.0, 7.0, 8.0, 9.0); + Cal3_S2Stereo cal3_s2stereo(1.0, 2.0, 3.0, 4.0, 5.0, 1.0); CalibratedCamera calibratedCamera(pose3); SimpleCamera simpleCamera(pose3, cal3_s2); + StereoCamera stereoCamera(pose3, boost::make_shared(cal3_s2stereo)); - Symbol a01('a',1), a02('a',2), a03('a',3), a04('a',4), a05('a',5), a06('a',6), a07('a',7), a08('a',8), a09('a',9), a10('a',10), a11('a',11), a12('a',12), a13('a',13); - Symbol b01('b',1), b02('b',2), b03('b',3), b04('b',4), b05('b',5), b06('b',6), b07('b',7), b08('b',8), b09('b',9), b10('b',10), b11('b',11), b12('b',12), b13('b',13); + Symbol a01('a',1), a02('a',2), a03('a',3), a04('a',4), a05('a',5), + a06('a',6), a07('a',7), a08('a',8), a09('a',9), a10('a',10), + a11('a',11), a12('a',12), a13('a',13), a14('a',14), a15('a',15); + Symbol b01('b',1), b02('b',2), b03('b',3), b04('b',4), b05('b',5), + b06('b',6), b07('b',7), b08('b',8), b09('b',9), b10('b',10), + b11('b',11), b12('b',12), b13('b',13), b14('b',14), b15('b',15); Values values; values.insert(a01, lieVector); @@ -282,6 +313,7 @@ TEST (Serialization, factors) { values.insert(a11, cal3ds2); values.insert(a12, calibratedCamera); values.insert(a13, simpleCamera); + values.insert(a14, stereoCamera); SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3); @@ -308,6 +340,7 @@ TEST (Serialization, factors) { PriorFactorCal3DS2 priorFactorCal3DS2(a11, cal3ds2, model9); PriorFactorCalibratedCamera priorFactorCalibratedCamera(a12, calibratedCamera, model6); PriorFactorSimpleCamera priorFactorSimpleCamera(a13, simpleCamera, model11); + PriorFactorStereoCamera priorFactorStereoCamera(a14, stereoCamera, model11); BetweenFactorLieVector betweenFactorLieVector(a01, b01, lieVector, model4); BetweenFactorLieMatrix betweenFactorLieMatrix(a02, b02, lieMatrix, model6); @@ -331,6 +364,7 @@ TEST (Serialization, factors) { NonlinearEqualityCal3DS2 nonlinearEqualityCal3DS2(a11, cal3ds2); NonlinearEqualityCalibratedCamera nonlinearEqualityCalibratedCamera(a12, calibratedCamera); NonlinearEqualitySimpleCamera nonlinearEqualitySimpleCamera(a13, simpleCamera); + NonlinearEqualityStereoCamera nonlinearEqualityStereoCamera(a14, stereoCamera); RangeFactorPosePoint2 rangeFactorPosePoint2(a08, a03, 2.0, model1); RangeFactorPosePoint3 rangeFactorPosePoint3(a09, a05, 2.0, model1); @@ -348,7 +382,11 @@ TEST (Serialization, factors) { GenericProjectionFactorCal3_S2 genericProjectionFactorCal3_S2(point2, model2, a09, a05, boost::make_shared(cal3_s2)); GenericProjectionFactorCal3DS2 genericProjectionFactorCal3DS2(point2, model2, a09, a05, boost::make_shared(cal3ds2)); + GeneralSFMFactorCal3_S2 generalSFMFactorCal3_S2(point2, model2, a13, a05); + GeneralSFMFactor2Cal3_S2 generalSFMFactor2Cal3_S2(point2, model2, a09, a05, a10); + + GenericStereoFactor3D genericStereoFactor3D(stereoPoint2, model3, a09, a05, boost::make_shared(cal3_s2stereo)); NonlinearFactorGraph graph; @@ -362,9 +400,10 @@ TEST (Serialization, factors) { graph.add(priorFactorPose2); graph.add(priorFactorPose3); graph.add(priorFactorCal3_S2); - graph.add(priorFactorCal3_S2); + graph.add(priorFactorCal3DS2); graph.add(priorFactorCalibratedCamera); graph.add(priorFactorSimpleCamera); + graph.add(priorFactorStereoCamera); graph.add(betweenFactorLieVector); graph.add(betweenFactorLieMatrix); @@ -388,6 +427,7 @@ TEST (Serialization, factors) { graph.add(nonlinearEqualityCal3DS2); graph.add(nonlinearEqualityCalibratedCamera); graph.add(nonlinearEqualitySimpleCamera); + graph.add(nonlinearEqualityStereoCamera); graph.add(rangeFactorPosePoint2); graph.add(rangeFactorPosePoint3); @@ -405,6 +445,12 @@ TEST (Serialization, factors) { graph.add(genericProjectionFactorCal3_S2); graph.add(genericProjectionFactorCal3DS2); + graph.add(generalSFMFactorCal3_S2); + + graph.add(generalSFMFactor2Cal3_S2); + + graph.add(genericStereoFactor3D); + // text EXPECT(equalsObj(a01)); @@ -425,6 +471,7 @@ TEST (Serialization, factors) { EXPECT(equalsObj(priorFactorCal3DS2)); EXPECT(equalsObj(priorFactorCalibratedCamera)); EXPECT(equalsObj(priorFactorSimpleCamera)); + EXPECT(equalsObj(priorFactorStereoCamera)); EXPECT(equalsObj(betweenFactorLieVector)); EXPECT(equalsObj(betweenFactorLieMatrix)); @@ -448,6 +495,7 @@ TEST (Serialization, factors) { EXPECT(equalsObj(nonlinearEqualityCal3DS2)); EXPECT(equalsObj(nonlinearEqualityCalibratedCamera)); EXPECT(equalsObj(nonlinearEqualitySimpleCamera)); + EXPECT(equalsObj(nonlinearEqualityStereoCamera)); EXPECT(equalsObj(rangeFactorPosePoint2)); EXPECT(equalsObj(rangeFactorPosePoint3)); @@ -465,6 +513,12 @@ TEST (Serialization, factors) { EXPECT(equalsObj(genericProjectionFactorCal3_S2)); EXPECT(equalsObj(genericProjectionFactorCal3DS2)); + EXPECT(equalsObj(generalSFMFactorCal3_S2)); + + EXPECT(equalsObj(generalSFMFactor2Cal3_S2)); + + EXPECT(equalsObj(genericStereoFactor3D)); + // xml EXPECT(equalsXML(a01)); @@ -485,6 +539,7 @@ TEST (Serialization, factors) { EXPECT(equalsXML(priorFactorCal3DS2)); EXPECT(equalsXML(priorFactorCalibratedCamera)); EXPECT(equalsXML(priorFactorSimpleCamera)); + EXPECT(equalsXML(priorFactorStereoCamera)); EXPECT(equalsXML(betweenFactorLieVector)); EXPECT(equalsXML(betweenFactorLieMatrix)); @@ -508,6 +563,7 @@ TEST (Serialization, factors) { EXPECT(equalsXML(nonlinearEqualityCal3DS2)); EXPECT(equalsXML(nonlinearEqualityCalibratedCamera)); EXPECT(equalsXML(nonlinearEqualitySimpleCamera)); + EXPECT(equalsXML(nonlinearEqualityStereoCamera)); EXPECT(equalsXML(rangeFactorPosePoint2)); EXPECT(equalsXML(rangeFactorPosePoint3)); @@ -524,6 +580,12 @@ TEST (Serialization, factors) { EXPECT(equalsXML(genericProjectionFactorCal3_S2)); EXPECT(equalsXML(genericProjectionFactorCal3DS2)); + + EXPECT(equalsXML(generalSFMFactorCal3_S2)); + + EXPECT(equalsXML(generalSFMFactor2Cal3_S2)); + + EXPECT(equalsXML(genericStereoFactor3D)); } From f93760327ea1bac8a6ac61850d28ba9ffd5c2b95 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Thu, 26 Jul 2012 13:08:54 +0000 Subject: [PATCH 49/68] Added matlab functions for generating poses in a circle. This replaces the need for the SLAM namespace functions of the same purpose. --- matlab/circlePose2.m | 19 +++++++++++++++++++ matlab/circlePose3.m | 29 +++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+) create mode 100644 matlab/circlePose2.m create mode 100644 matlab/circlePose3.m diff --git a/matlab/circlePose2.m b/matlab/circlePose2.m new file mode 100644 index 000000000..4da70f9bf --- /dev/null +++ b/matlab/circlePose2.m @@ -0,0 +1,19 @@ +function values = circlePose2(numPoses, radius, symbolChar) +% circlePose2: generate a set of poses in a circle. This function +% returns those poses inside a gtsam.Values object, with sequential +% keys starting from 1. An optional character may be provided, which +% will be stored in the msb of each key (i.e. gtsam.Symbol). + +if nargin<3,symbolChar=0x00;end +if nargin<2,radius=1.0;end +if nargin<1,numPoses=8;end + +values = gtsam.Values; +theta = 0.0; +dtheta = 2*pi()/numPoses; +for i = 1:numPoses + key = gtsam.Symbol(symbolChar, i); + pose = gtsam.Pose2(radius*cos(theta), radius*sin(theta), pi()/2 + theta); + values.insert(key, pose); + theta = theta + dtheta; +end diff --git a/matlab/circlePose3.m b/matlab/circlePose3.m new file mode 100644 index 000000000..bd102ad78 --- /dev/null +++ b/matlab/circlePose3.m @@ -0,0 +1,29 @@ +function values = circlePose2(numPoses, radius, symbolChar) +% circlePose3: generate a set of poses in a circle. This function +% returns those poses inside a gtsam.Values object, with sequential +% keys starting from 1. An optional character may be provided, which +% will be stored in the msb of each key (i.e. gtsam.Symbol). +% +% We use aerospace/navlab convention, X forward, Y right, Z down +% First pose will be at (R,0,0) +% ^y ^ X +% | | +% z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) +% Vehicle at p0 is looking towards y axis (X-axis points towards world y) + +if nargin<3,symbolChar=0x00;end +if nargin<2,radius=1.0;end +if nargin<1,numPoses=8;end + +values = gtsam.Values; +theta = 0.0; +dtheta = 2*pi()/numPoses; +gR0 = gtsam.Rot3(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); +for i = 1:numPoses + key = gtsam.Symbol(symbolChar, i); + gti = Point3(radius*cos(theta), radius*sin(theta), 0); + _0Ri = gtsam.Rot3.yaw(-theta); % negative yaw goes counterclockwise, with Z down ! + gTi = gtsam.Pose3(gR0.compose(_0Ri), gti); + values.insert(key, gTi); + theta = theta + dtheta; +end From 078eb1bb4fd8929a07952fd81db339a1a4465de6 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Thu, 26 Jul 2012 13:23:04 +0000 Subject: [PATCH 50/68] Added matlab functions for generating poses in a circle. This replaces the need for the SLAM namespace functions of the same purpose. I mean it this time. Edited the wrong copy before committing. --- matlab/circlePose2.m | 19 +++++++++++++++---- matlab/circlePose3.m | 20 ++++++++++++-------- 2 files changed, 27 insertions(+), 12 deletions(-) diff --git a/matlab/circlePose2.m b/matlab/circlePose2.m index 4da70f9bf..7153fc128 100644 --- a/matlab/circlePose2.m +++ b/matlab/circlePose2.m @@ -1,18 +1,29 @@ function values = circlePose2(numPoses, radius, symbolChar) -% circlePose2: generate a set of poses in a circle. This function +% circlePose3: generate a set of poses in a circle. This function % returns those poses inside a gtsam.Values object, with sequential -% keys starting from 1. An optional character may be provided, which +% keys starting from 0. An optional character may be provided, which % will be stored in the msb of each key (i.e. gtsam.Symbol). +% +% We use aerospace/navlab convention, X forward, Y right, Z down +% First pose will be at (R,0,0) +% ^y ^ X +% | | +% z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) +% Vehicle at p0 is looking towards y axis (X-axis points towards world y) -if nargin<3,symbolChar=0x00;end +if nargin<3,symbolChar=0;end if nargin<2,radius=1.0;end if nargin<1,numPoses=8;end +% Force symbolChar to be a single character +symbolChar = char(symbolChar); +symbolChar = symbolChar(1); + values = gtsam.Values; theta = 0.0; dtheta = 2*pi()/numPoses; for i = 1:numPoses - key = gtsam.Symbol(symbolChar, i); + key = gtsam.Symbol(symbolChar, i-1).key(); pose = gtsam.Pose2(radius*cos(theta), radius*sin(theta), pi()/2 + theta); values.insert(key, pose); theta = theta + dtheta; diff --git a/matlab/circlePose3.m b/matlab/circlePose3.m index bd102ad78..0e74a2f1b 100644 --- a/matlab/circlePose3.m +++ b/matlab/circlePose3.m @@ -1,7 +1,7 @@ -function values = circlePose2(numPoses, radius, symbolChar) +function values = circlePose3(numPoses, radius, symbolChar) % circlePose3: generate a set of poses in a circle. This function % returns those poses inside a gtsam.Values object, with sequential -% keys starting from 1. An optional character may be provided, which +% keys starting from 0. An optional character may be provided, which % will be stored in the msb of each key (i.e. gtsam.Symbol). % % We use aerospace/navlab convention, X forward, Y right, Z down @@ -11,19 +11,23 @@ function values = circlePose2(numPoses, radius, symbolChar) % z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) % Vehicle at p0 is looking towards y axis (X-axis points towards world y) -if nargin<3,symbolChar=0x00;end +if nargin<3,symbolChar=0;end if nargin<2,radius=1.0;end if nargin<1,numPoses=8;end +% Force symbolChar to be a single character +symbolChar = char(symbolChar); +symbolChar = symbolChar(1); + values = gtsam.Values; theta = 0.0; dtheta = 2*pi()/numPoses; -gR0 = gtsam.Rot3(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); +gRo = gtsam.Rot3([0, 1, 0 ; 1, 0, 0 ; 0, 0, -1]); for i = 1:numPoses - key = gtsam.Symbol(symbolChar, i); - gti = Point3(radius*cos(theta), radius*sin(theta), 0); - _0Ri = gtsam.Rot3.yaw(-theta); % negative yaw goes counterclockwise, with Z down ! - gTi = gtsam.Pose3(gR0.compose(_0Ri), gti); + key = gtsam.Symbol(symbolChar, i-1).key(); + gti = gtsam.Point3(radius*cos(theta), radius*sin(theta), 0); + oRi = gtsam.Rot3.Yaw(-theta); % negative yaw goes counterclockwise, with Z down ! + gTi = gtsam.Pose3(gRo.compose(oRi), gti); values.insert(key, gTi); theta = theta + dtheta; end From c2935c5dd19ae0e1c58782a56897d97c1d64a29f Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Thu, 26 Jul 2012 13:33:40 +0000 Subject: [PATCH 51/68] Replaced calls to the namespace Circle functions with the newly added matlab function --- matlab/examples/Pose2SLAMExample_circle.m | 19 ++++++++++--------- matlab/examples/Pose3SLAMExample.m | 19 ++++++++++--------- matlab/tests/testPose3SLAMExample.m | 19 ++++++++++--------- 3 files changed, 30 insertions(+), 27 deletions(-) diff --git a/matlab/examples/Pose2SLAMExample_circle.m b/matlab/examples/Pose2SLAMExample_circle.m index e16edf9cb..3d2265d76 100644 --- a/matlab/examples/Pose2SLAMExample_circle.m +++ b/matlab/examples/Pose2SLAMExample_circle.m @@ -10,13 +10,14 @@ % @author Frank Dellaert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +import gtsam.* + %% Create a hexagon of poses -hexagon = pose2SLAM.Values.Circle(6,1.0); -p0 = hexagon.pose(0); -p1 = hexagon.pose(1); +hexagon = circlePose2(6,1.0); +p0 = hexagon.at(0); +p1 = hexagon.at(1); %% create a Pose graph with one equality constraint and one measurement -import gtsam.* fg = NonlinearFactorGraph; fg.add(NonlinearEqualityPose2(0, p0)); delta = p0.between(p1); @@ -31,11 +32,11 @@ fg.add(BetweenFactorPose2(5,0, delta, covariance)); %% Create initial config initial = Values; initial.insert(0, p0); -initial.insert(1, hexagon.pose(1).retract([-0.1, 0.1,-0.1]')); -initial.insert(2, hexagon.pose(2).retract([ 0.1,-0.1, 0.1]')); -initial.insert(3, hexagon.pose(3).retract([-0.1, 0.1,-0.1]')); -initial.insert(4, hexagon.pose(4).retract([ 0.1,-0.1, 0.1]')); -initial.insert(5, hexagon.pose(5).retract([-0.1, 0.1,-0.1]')); +initial.insert(1, hexagon.at(1).retract([-0.1, 0.1,-0.1]')); +initial.insert(2, hexagon.at(2).retract([ 0.1,-0.1, 0.1]')); +initial.insert(3, hexagon.at(3).retract([-0.1, 0.1,-0.1]')); +initial.insert(4, hexagon.at(4).retract([ 0.1,-0.1, 0.1]')); +initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]')); %% Plot Initial Estimate cla diff --git a/matlab/examples/Pose3SLAMExample.m b/matlab/examples/Pose3SLAMExample.m index 86c5c23c8..0d2bd237f 100644 --- a/matlab/examples/Pose3SLAMExample.m +++ b/matlab/examples/Pose3SLAMExample.m @@ -10,13 +10,14 @@ % @author Frank Dellaert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +import gtsam.* + %% Create a hexagon of poses -hexagon = pose3SLAM.Values.Circle(6,1.0); -p0 = hexagon.pose(0); -p1 = hexagon.pose(1); +hexagon = circlePose3(6,1.0); +p0 = hexagon.at(0); +p1 = hexagon.at(1); %% create a Pose graph with one equality constraint and one measurement -import gtsam.* fg = NonlinearFactorGraph; fg.add(NonlinearEqualityPose3(0, p0)); delta = p0.between(p1); @@ -32,11 +33,11 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance)); initial = Values; s = 0.10; initial.insert(0, p0); -initial.insert(1, hexagon.pose(1).retract(s*randn(6,1))); -initial.insert(2, hexagon.pose(2).retract(s*randn(6,1))); -initial.insert(3, hexagon.pose(3).retract(s*randn(6,1))); -initial.insert(4, hexagon.pose(4).retract(s*randn(6,1))); -initial.insert(5, hexagon.pose(5).retract(s*randn(6,1))); +initial.insert(1, hexagon.at(1).retract(s*randn(6,1))); +initial.insert(2, hexagon.at(2).retract(s*randn(6,1))); +initial.insert(3, hexagon.at(3).retract(s*randn(6,1))); +initial.insert(4, hexagon.at(4).retract(s*randn(6,1))); +initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); %% Plot Initial Estimate cla diff --git a/matlab/tests/testPose3SLAMExample.m b/matlab/tests/testPose3SLAMExample.m index 0d47990e8..dafad4e47 100644 --- a/matlab/tests/testPose3SLAMExample.m +++ b/matlab/tests/testPose3SLAMExample.m @@ -10,13 +10,14 @@ % @author Frank Dellaert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +import gtsam.* + %% Create a hexagon of poses -hexagon = pose3SLAM.Values.Circle(6,1.0); -p0 = hexagon.pose(0); -p1 = hexagon.pose(1); +hexagon = circlePose3(6,1.0); +p0 = hexagon.at(0); +p1 = hexagon.at(1); %% create a Pose graph with one equality constraint and one measurement -import gtsam.* fg = NonlinearFactorGraph; fg.add(NonlinearEqualityPose3(0, p0)); delta = p0.between(p1); @@ -32,11 +33,11 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance)); initial = Values; s = 0.10; initial.insert(0, p0); -initial.insert(1, hexagon.pose(1).retract(s*randn(6,1))); -initial.insert(2, hexagon.pose(2).retract(s*randn(6,1))); -initial.insert(3, hexagon.pose(3).retract(s*randn(6,1))); -initial.insert(4, hexagon.pose(4).retract(s*randn(6,1))); -initial.insert(5, hexagon.pose(5).retract(s*randn(6,1))); +initial.insert(1, hexagon.at(1).retract(s*randn(6,1))); +initial.insert(2, hexagon.at(2).retract(s*randn(6,1))); +initial.insert(3, hexagon.at(3).retract(s*randn(6,1))); +initial.insert(4, hexagon.at(4).retract(s*randn(6,1))); +initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); %% optimize optimizer = LevenbergMarquardtOptimizer(fg, initial); From a3ee762dac5f25f0db98080f0f7a79be44ac4740 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 27 Jul 2012 19:01:51 +0000 Subject: [PATCH 52/68] Fixed svn merging problem in SuccessiveLinearizationOptimizer --- .../SuccessiveLinearizationOptimizer.cpp | 62 +++++++++---------- .../SuccessiveLinearizationOptimizer.h | 34 ---------- 2 files changed, 31 insertions(+), 65 deletions(-) diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp index 0fe0440ae..af3f70ddc 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp @@ -17,37 +17,37 @@ namespace gtsam { void SuccessiveLinearizationParams::print(const std::string& str) const { - NonlinearOptimizerParams::print(str); - switch ( linearSolverType ) { - case MULTIFRONTAL_CHOLESKY: - std::cout << " linear solver type: MULTIFRONTAL CHOLESKY\n"; - break; - case MULTIFRONTAL_QR: - std::cout << " linear solver type: MULTIFRONTAL QR\n"; - break; - case SEQUENTIAL_CHOLESKY: - std::cout << " linear solver type: SEQUENTIAL CHOLESKY\n"; - break; - case SEQUENTIAL_QR: - std::cout << " linear solver type: SEQUENTIAL QR\n"; - break; - case CHOLMOD: - std::cout << " linear solver type: CHOLMOD\n"; - break; - case CG: - std::cout << " linear solver type: CG\n"; - break; - default: - std::cout << " linear solver type: (invalid)\n"; - break; - } - - if(ordering) - std::cout << " ordering: custom\n"; - else - std::cout << " ordering: COLAMD\n"; - - std::cout.flush(); + NonlinearOptimizerParams::print(str); + switch ( linearSolverType ) { + case MULTIFRONTAL_CHOLESKY: + std::cout << " linear solver type: MULTIFRONTAL CHOLESKY\n"; + break; + case MULTIFRONTAL_QR: + std::cout << " linear solver type: MULTIFRONTAL QR\n"; + break; + case SEQUENTIAL_CHOLESKY: + std::cout << " linear solver type: SEQUENTIAL CHOLESKY\n"; + break; + case SEQUENTIAL_QR: + std::cout << " linear solver type: SEQUENTIAL QR\n"; + break; + case CHOLMOD: + std::cout << " linear solver type: CHOLMOD\n"; + break; + case CONJUGATE_GRADIENT: + std::cout << " linear solver type: CONJUGATE GRADIENT\n"; + break; + default: + std::cout << " linear solver type: (invalid)\n"; + break; + } + + if(ordering) + std::cout << " ordering: custom\n"; + else + std::cout << " ordering: COLAMD\n"; + + std::cout.flush(); } VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const SuccessiveLinearizationParams ¶ms) { diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h index fc44621f6..e79a364c5 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h @@ -42,40 +42,6 @@ public: SuccessiveLinearizationParams() : linearSolverType(MULTIFRONTAL_CHOLESKY) {} virtual ~SuccessiveLinearizationParams() {} - virtual void print(const std::string& str = "") const { - NonlinearOptimizerParams::print(str); - switch ( linearSolverType ) { - case MULTIFRONTAL_CHOLESKY: - std::cout << " linear solver type: MULTIFRONTAL CHOLESKY\n"; - break; - case MULTIFRONTAL_QR: - std::cout << " linear solver type: MULTIFRONTAL QR\n"; - break; - case SEQUENTIAL_CHOLESKY: - std::cout << " linear solver type: SEQUENTIAL CHOLESKY\n"; - break; - case SEQUENTIAL_QR: - std::cout << " linear solver type: SEQUENTIAL QR\n"; - break; - case CHOLMOD: - std::cout << " linear solver type: CHOLMOD\n"; - break; - case CONJUGATE_GRADIENT: - std::cout << " linear solver type: CONJUGATE GRADIENT\n"; - break; - default: - std::cout << " linear solver type: (invalid)\n"; - break; - } - - if(ordering) - std::cout << " ordering: custom\n"; - else - std::cout << " ordering: COLAMD\n"; - - std::cout.flush(); - } - inline bool isMultifrontal() const { return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR); } From aef5ae269fd2b75b2d8f24120551e98ba45bbe82 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 27 Jul 2012 19:01:53 +0000 Subject: [PATCH 53/68] Added argument and return type checking for global wrapped functions --- wrap/Module.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 229a9810f..b2f061838 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -416,6 +416,9 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co vector validTypes = GenerateValidTypes(expandedClasses, forward_declarations); // Check that all classes have been defined somewhere + verifyArguments(validTypes, global_functions); + verifyReturnTypes(validTypes, global_functions); + BOOST_FOREACH(const Class& cls, expandedClasses) { // verify all of the function arguments //TODO:verifyArguments(validTypes, cls.constructor.args_list); From 9d2a3bf14edb2e07d91559758692b4aa42ee315c Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 27 Jul 2012 19:02:11 +0000 Subject: [PATCH 54/68] Finished denamespacing and reorganizing matlab code --- gtsam.h | 423 +----------------- gtsam/nonlinear/Symbol.h | 9 + gtsam/slam/dataset.cpp | 20 +- gtsam/slam/dataset.h | 16 +- matlab/{ => +gtsam_utils}/CHECK.m | 0 matlab/{ => +gtsam_utils}/EQUALITY.m | 0 .../VisualISAMGenerateData.m | 0 .../VisualISAMInitialize.m | 43 +- .../VisualISAMPlot.m | 34 +- matlab/+gtsam_utils/VisualISAMStep.m | 44 ++ matlab/{ => +gtsam_utils}/circlePose2.m | 2 +- matlab/{ => +gtsam_utils}/circlePose3.m | 2 +- matlab/{ => +gtsam_utils}/covarianceEllipse.m | 66 +-- .../{ => +gtsam_utils}/covarianceEllipse3D.m | 0 .../findExampleDataFile.m | 4 +- matlab/{ => +gtsam_utils}/load2D.m | 0 matlab/{ => +gtsam_utils}/load3D.m | 0 matlab/{ => +gtsam_utils}/plot2DPoints.m | 4 +- matlab/{ => +gtsam_utils}/plot2DTrajectory.m | 4 +- matlab/{ => +gtsam_utils}/plot3DPoints.m | 4 +- matlab/{ => +gtsam_utils}/plot3DTrajectory.m | 4 +- matlab/{ => +gtsam_utils}/plotCamera.m | 0 matlab/{ => +gtsam_utils}/plotPoint2.m | 2 +- matlab/{ => +gtsam_utils}/plotPoint3.m | 2 +- matlab/{ => +gtsam_utils}/plotPose2.m | 2 +- matlab/{ => +gtsam_utils}/plotPose3.m | 2 +- matlab/CMakeLists.txt | 11 +- matlab/examples/+support/VisualISAMStep.m | 44 -- .../LocalizationExample.m | 2 +- .../OdometryExample.m | 2 +- .../PlanarSLAMExample.m | 4 +- .../PlanarSLAMExample_sampling.m | 9 +- .../Pose2SLAMExample.m | 4 +- .../Pose2SLAMExample_advanced.m | 1 + .../Pose2SLAMExample_circle.m | 14 +- .../Pose2SLAMExample_graph.m | 13 +- .../Pose2SLAMwSPCG.m | 0 .../Pose3SLAMExample.m | 14 +- .../Pose3SLAMExample_graph.m | 11 +- .../{examples => gtsam_examples}/SBAExample.m | 2 +- .../{examples => gtsam_examples}/SFMExample.m | 6 +- .../StereoVOExample.m | 7 +- .../StereoVOExample_large.m | 14 +- .../VisualISAMDemo.m | 0 .../VisualISAMExample.m | 10 +- .../VisualISAM_gui.fig | Bin .../VisualISAM_gui.m | 20 +- .../gtsamExamples.fig | Bin .../gtsamExamples.m | 0 .../testJacobianFactor.m | 1 + .../{tests => gtsam_tests}/testKalmanFilter.m | 4 + .../testLocalizationExample.m | 1 + .../testOdometryExample.m | 2 + .../testPlanarSLAMExample.m | 2 + .../testPose2SLAMExample.m | 1 + .../testPose3SLAMExample.m | 7 +- .../{tests => gtsam_tests}/testSFMExample.m | 22 +- .../testStereoVOExample.m | 40 +- .../testVisualISAMExample.m | 11 +- matlab/{tests => gtsam_tests}/test_gtsam.m | 18 +- matlab/symbol.m | 4 - matlab/symbolChr.m | 4 - matlab/symbolIndex.m | 4 - 63 files changed, 324 insertions(+), 672 deletions(-) rename matlab/{ => +gtsam_utils}/CHECK.m (100%) rename matlab/{ => +gtsam_utils}/EQUALITY.m (100%) rename matlab/{examples/+support => +gtsam_utils}/VisualISAMGenerateData.m (100%) rename matlab/{examples/+support => +gtsam_utils}/VisualISAMInitialize.m (54%) rename matlab/{examples/+support => +gtsam_utils}/VisualISAMPlot.m (55%) create mode 100644 matlab/+gtsam_utils/VisualISAMStep.m rename matlab/{ => +gtsam_utils}/circlePose2.m (95%) rename matlab/{ => +gtsam_utils}/circlePose3.m (96%) rename matlab/{ => +gtsam_utils}/covarianceEllipse.m (96%) rename matlab/{ => +gtsam_utils}/covarianceEllipse3D.m (100%) rename matlab/{examples/+support => +gtsam_utils}/findExampleDataFile.m (74%) rename matlab/{ => +gtsam_utils}/load2D.m (100%) rename matlab/{ => +gtsam_utils}/load3D.m (100%) rename matlab/{ => +gtsam_utils}/plot2DPoints.m (88%) rename matlab/{ => +gtsam_utils}/plot2DTrajectory.m (93%) rename matlab/{ => +gtsam_utils}/plot3DPoints.m (88%) rename matlab/{ => +gtsam_utils}/plot3DTrajectory.m (91%) rename matlab/{ => +gtsam_utils}/plotCamera.m (100%) rename matlab/{ => +gtsam_utils}/plotPoint2.m (76%) rename matlab/{ => +gtsam_utils}/plotPoint3.m (80%) rename matlab/{ => +gtsam_utils}/plotPose2.m (84%) rename matlab/{ => +gtsam_utils}/plotPose3.m (94%) delete mode 100644 matlab/examples/+support/VisualISAMStep.m rename matlab/{examples => gtsam_examples}/LocalizationExample.m (96%) rename matlab/{examples => gtsam_examples}/OdometryExample.m (96%) rename matlab/{examples => gtsam_examples}/PlanarSLAMExample.m (96%) rename matlab/{examples => gtsam_examples}/PlanarSLAMExample_sampling.m (92%) rename matlab/{examples => gtsam_examples}/Pose2SLAMExample.m (96%) rename matlab/{examples => gtsam_examples}/Pose2SLAMExample_advanced.m (99%) rename matlab/{examples => gtsam_examples}/Pose2SLAMExample_circle.m (87%) rename matlab/{examples => gtsam_examples}/Pose2SLAMExample_graph.m (79%) rename matlab/{examples => gtsam_examples}/Pose2SLAMwSPCG.m (100%) rename matlab/{examples => gtsam_examples}/Pose3SLAMExample.m (88%) rename matlab/{examples => gtsam_examples}/Pose3SLAMExample_graph.m (77%) rename matlab/{examples => gtsam_examples}/SBAExample.m (97%) rename matlab/{examples => gtsam_examples}/SFMExample.m (94%) rename matlab/{examples => gtsam_examples}/StereoVOExample.m (94%) rename matlab/{examples => gtsam_examples}/StereoVOExample_large.m (84%) rename matlab/{examples => gtsam_examples}/VisualISAMDemo.m (100%) rename matlab/{examples => gtsam_examples}/VisualISAMExample.m (75%) rename matlab/{examples => gtsam_examples}/VisualISAM_gui.fig (100%) rename matlab/{examples => gtsam_examples}/VisualISAM_gui.m (92%) rename matlab/{examples => gtsam_examples}/gtsamExamples.fig (100%) rename matlab/{examples => gtsam_examples}/gtsamExamples.m (100%) rename matlab/{tests => gtsam_tests}/testJacobianFactor.m (98%) rename matlab/{tests => gtsam_tests}/testKalmanFilter.m (96%) rename matlab/{tests => gtsam_tests}/testLocalizationExample.m (98%) rename matlab/{tests => gtsam_tests}/testOdometryExample.m (98%) rename matlab/{tests => gtsam_tests}/testPlanarSLAMExample.m (98%) rename matlab/{tests => gtsam_tests}/testPose2SLAMExample.m (99%) rename matlab/{tests => gtsam_tests}/testPose3SLAMExample.m (96%) rename matlab/{tests => gtsam_tests}/testSFMExample.m (77%) rename matlab/{tests => gtsam_tests}/testStereoVOExample.m (59%) rename matlab/{tests => gtsam_tests}/testVisualISAMExample.m (82%) rename matlab/{tests => gtsam_tests}/test_gtsam.m (100%) delete mode 100644 matlab/symbol.m delete mode 100644 matlab/symbolChr.m delete mode 100644 matlab/symbolIndex.m diff --git a/gtsam.h b/gtsam.h index 646e44141..6f87670ca 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1023,14 +1023,9 @@ class KalmanFilter { //************************************************************************* #include -class Symbol { - Symbol(char c, size_t j); - Symbol(size_t k); - void print(string s) const; - size_t key() const; - size_t index() const; - char chr() const; -}; +size_t symbol(char chr, size_t index); +char symbolChr(size_t key); +size_t symbolIndex(size_t key); #include class Ordering { @@ -1415,6 +1410,8 @@ typedef gtsam::RangeFactor RangeFactorSimple typedef gtsam::RangeFactor RangeFactorCalibratedCamera; typedef gtsam::RangeFactor RangeFactorSimpleCamera; + +#include template virtual class BearingFactor : gtsam::NonlinearFactor { BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel); @@ -1423,6 +1420,7 @@ virtual class BearingFactor : gtsam::NonlinearFactor { typedef gtsam::BearingFactor BearingFactor2D; +#include template virtual class BearingRangeFactor : gtsam::NonlinearFactor { BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel); @@ -1473,412 +1471,7 @@ virtual class GenericStereoFactor : gtsam::NonlinearFactor { typedef gtsam::GenericStereoFactor GenericStereoFactor3D; #include -pair load2D(string filename, +pair load2D(string filename, gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); -} //\namespace gtsam - -//************************************************************************* -// Pose2SLAM -//************************************************************************* - -namespace pose2SLAM { - -#include -class Values { - Values(); - Values(const pose2SLAM::Values& values); - size_t size() const; - void print(string s) const; - bool exists(size_t key); - gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList - - static pose2SLAM::Values Circle(size_t n, double R); - void insertPose(size_t key, const gtsam::Pose2& pose); - void updatePose(size_t key, const gtsam::Pose2& pose); - gtsam::Pose2 pose(size_t i); - Matrix poses() const; -}; - -#include -class Graph { - Graph(); - Graph(const gtsam::NonlinearFactorGraph& graph); - Graph(const pose2SLAM::Graph& graph); - - // FactorGraph - void print(string s) const; - bool equals(const pose2SLAM::Graph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - - // NonlinearFactorGraph - double error(const pose2SLAM::Values& values) const; - double probPrime(const pose2SLAM::Values& values) const; - gtsam::Ordering* orderingCOLAMD(const pose2SLAM::Values& values) const; - gtsam::GaussianFactorGraph* linearize(const pose2SLAM::Values& values, - const gtsam::Ordering& ordering) const; - - // pose2SLAM-specific - void addPoseConstraint(size_t key, const gtsam::Pose2& pose); - void addPosePrior(size_t key, const gtsam::Pose2& pose, const gtsam::noiseModel::Base* noiseModel); - void addRelativePose(size_t key1, size_t key2, const gtsam::Pose2& relativePoseMeasurement, const gtsam::noiseModel::Base* noiseModel); - pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate, size_t verbosity) const; - pose2SLAM::Values optimizeSPCG(const pose2SLAM::Values& initialEstimate, size_t verbosity) const; - gtsam::Marginals marginals(const pose2SLAM::Values& solution) const; -}; - -} //\namespace pose2SLAM - -//************************************************************************* -// Pose3SLAM -//************************************************************************* - -namespace pose3SLAM { - -#include -class Values { - Values(); - Values(const pose3SLAM::Values& values); - size_t size() const; - void print(string s) const; - bool exists(size_t key); - gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList - - static pose3SLAM::Values Circle(size_t n, double R); - void insertPose(size_t key, const gtsam::Pose3& pose); - void updatePose(size_t key, const gtsam::Pose3& pose); - gtsam::Pose3 pose(size_t i); - Matrix translations() const; -}; - -#include -class Graph { - Graph(); - Graph(const gtsam::NonlinearFactorGraph& graph); - Graph(const pose3SLAM::Graph& graph); - - // FactorGraph - void print(string s) const; - bool equals(const pose3SLAM::Graph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - - // NonlinearFactorGraph - double error(const pose3SLAM::Values& values) const; - double probPrime(const pose3SLAM::Values& values) const; - gtsam::Ordering* orderingCOLAMD(const pose3SLAM::Values& values) const; - gtsam::GaussianFactorGraph* linearize(const pose3SLAM::Values& values, - const gtsam::Ordering& ordering) const; - - // pose3SLAM-specific - void addPoseConstraint(size_t i, const gtsam::Pose3& p); - void addPosePrior(size_t key, const gtsam::Pose3& p, const gtsam::noiseModel::Base* model); - void addRelativePose(size_t key1, size_t key2, const gtsam::Pose3& z, const gtsam::noiseModel::Base* model); - pose3SLAM::Values optimize(const pose3SLAM::Values& initialEstimate, size_t verbosity) const; - // FIXME gtsam::LevenbergMarquardtOptimizer optimizer(const pose3SLAM::Values& initialEstimate, const gtsam::LevenbergMarquardtParams& parameters) const; - gtsam::Marginals marginals(const pose3SLAM::Values& solution) const; -}; - -} //\namespace pose3SLAM - -//************************************************************************* -// planarSLAM -//************************************************************************* - -namespace planarSLAM { - -#include -class Values { - Values(); - Values(const planarSLAM::Values& values); - size_t size() const; - void print(string s) const; - bool exists(size_t key); - gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList - - // inherited from pose2SLAM - static planarSLAM::Values Circle(size_t n, double R); - void insertPose(size_t key, const gtsam::Pose2& pose); - void updatePose(size_t key, const gtsam::Pose2& pose); - gtsam::Pose2 pose(size_t i); - Matrix poses() const; - - // Access to poses - planarSLAM::Values allPoses() const; - size_t nrPoses() const; - gtsam::KeyVector poseKeys() const; // Note the switch to KeyVector, rather than KeyList - - // Access to points - planarSLAM::Values allPoints() const; - size_t nrPoints() const; - gtsam::KeyVector pointKeys() const; // Note the switch to KeyVector, rather than KeyList - - void insertPoint(size_t key, const gtsam::Point2& point); - void updatePoint(size_t key, const gtsam::Point2& point); - gtsam::Point2 point(size_t key) const; - Matrix points() const; -}; - -#include -class Graph { - Graph(); - Graph(const gtsam::NonlinearFactorGraph& graph); - Graph(const pose2SLAM::Graph& graph); - Graph(const planarSLAM::Graph& graph); - - // FactorGraph - void print(string s) const; - bool equals(const planarSLAM::Graph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - - // NonlinearFactorGraph - double error(const planarSLAM::Values& values) const; - double probPrime(const planarSLAM::Values& values) const; - gtsam::Ordering* orderingCOLAMD(const planarSLAM::Values& values) const; - gtsam::GaussianFactorGraph* linearize(const planarSLAM::Values& values, - const gtsam::Ordering& ordering) const; - - // pose2SLAM-inherited - void addPoseConstraint(size_t key, const gtsam::Pose2& pose); - void addPosePrior(size_t key, const gtsam::Pose2& pose, const gtsam::noiseModel::Base* noiseModel); - void addRelativePose(size_t key1, size_t key2, const gtsam::Pose2& relativePoseMeasurement, const gtsam::noiseModel::Base* noiseModel); - planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate, size_t verbosity) const; - planarSLAM::Values optimizeSPCG(const planarSLAM::Values& initialEstimate, size_t verbosity) const; - gtsam::Marginals marginals(const planarSLAM::Values& solution) const; - - // planarSLAM-specific - void addPointConstraint(size_t pointKey, const gtsam::Point2& p); - void addPointPrior(size_t pointKey, const gtsam::Point2& p, const gtsam::noiseModel::Base* model); - void addBearing(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing, const gtsam::noiseModel::Base* noiseModel); - void addRange(size_t poseKey, size_t pointKey, double range, const gtsam::noiseModel::Base* noiseModel); - void addBearingRange(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing,double range, const gtsam::noiseModel::Base* noiseModel); -}; - -#include -class Odometry { - Odometry(size_t key1, size_t key2, const gtsam::Pose2& measured, - const gtsam::noiseModel::Base* model); - void print(string s) const; - gtsam::GaussianFactor* linearize(const planarSLAM::Values& center, - const gtsam::Ordering& ordering) const; -}; - -} //\namespace planarSLAM - -//************************************************************************* -// VisualSLAM -//************************************************************************* - -namespace visualSLAM { - -#include -class Values { - Values(); - Values(const visualSLAM::Values& values); - size_t size() const; - void print(string s) const; - bool exists(size_t key); - gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList - - // pose3SLAM inherited - static visualSLAM::Values Circle(size_t n, double R); - void insertPose(size_t key, const gtsam::Pose3& pose); - void updatePose(size_t key, const gtsam::Pose3& pose); - gtsam::Pose3 pose(size_t i); - Matrix translations() const; - - // Access to poses - visualSLAM::Values allPoses() const; - size_t nrPoses() const; - gtsam::KeyVector poseKeys() const; // Note the switch to KeyVector, rather than KeyList - - // Access to points - visualSLAM::Values allPoints() const; - size_t nrPoints() const; - gtsam::KeyVector pointKeys() const; // Note the switch to KeyVector, rather than KeyList - - void insertPoint(size_t key, const gtsam::Point3& pose); - void updatePoint(size_t key, const gtsam::Point3& pose); - gtsam::Point3 point(size_t j); - void insertBackprojections(const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); - void perturbPoints(double sigma, size_t seed); - Matrix points() const; -}; - -#include -class Graph { - Graph(); - Graph(const gtsam::NonlinearFactorGraph& graph); - Graph(const pose3SLAM::Graph& graph); - Graph(const visualSLAM::Graph& graph); - - // FactorGraph - void print(string s) const; - bool equals(const visualSLAM::Graph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - - double error(const visualSLAM::Values& values) const; - gtsam::Ordering* orderingCOLAMD(const visualSLAM::Values& values) const; - gtsam::GaussianFactorGraph* linearize(const visualSLAM::Values& values, - const gtsam::Ordering& ordering) const; - - // pose3SLAM-inherited - void addPoseConstraint(size_t i, const gtsam::Pose3& p); - void addPosePrior(size_t key, const gtsam::Pose3& p, const gtsam::noiseModel::Base* model); - void addRelativePose(size_t key1, size_t key2, const gtsam::Pose3& z, const gtsam::noiseModel::Base* model); - visualSLAM::Values optimize(const visualSLAM::Values& initialEstimate, size_t verbosity) const; - visualSLAM::LevenbergMarquardtOptimizer optimizer(const visualSLAM::Values& initialEstimate, const gtsam::LevenbergMarquardtParams& parameters) const; - gtsam::Marginals marginals(const visualSLAM::Values& solution) const; - - // Priors and constraints - void addPointConstraint(size_t pointKey, const gtsam::Point3& p); - void addPointPrior(size_t pointKey, const gtsam::Point3& p, const gtsam::noiseModel::Base* model); - void addRangeFactor(size_t poseKey, size_t pointKey, double range, const gtsam::noiseModel::Base* model); - - // Measurements - void addMeasurement(const gtsam::Point2& measured, - const gtsam::noiseModel::Base* model, size_t poseKey, size_t pointKey, - const gtsam::Cal3_S2* K); - void addMeasurements(size_t i, Vector J, Matrix Z, - const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); - void addStereoMeasurement(const gtsam::StereoPoint2& measured, - const gtsam::noiseModel::Base* model, size_t poseKey, size_t pointKey, - const gtsam::Cal3_S2Stereo* K); - - // Information - Matrix reprojectionErrors(const visualSLAM::Values& values) const; - -}; - -#include -class ISAM { - ISAM(); - ISAM(int reorderInterval); - void print(string s) const; - void printStats() const; - void saveGraph(string s) const; - visualSLAM::Values estimate() const; - Matrix marginalCovariance(size_t key) const; - int reorderInterval() const; - int reorderCounter() const; - void update(const visualSLAM::Graph& newFactors, const visualSLAM::Values& initialValues); - void reorder_relinearize(); - void addKey(size_t key); - void setOrdering(const gtsam::Ordering& new_ordering); - - // These might be expensive as instead of a reference the wrapper will make a copy - gtsam::GaussianISAM bayesTree() const; - visualSLAM::Values getLinearizationPoint() const; - gtsam::Ordering getOrdering() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; -}; - -#include -class LevenbergMarquardtOptimizer { - double lambda() const; - void iterate(); - double error() const; - size_t iterations() const; - visualSLAM::Values optimize(); - visualSLAM::Values optimizeSafely(); - visualSLAM::Values values() const; -}; - -} //\namespace visualSLAM - -//************************************************************************ -// sparse BA -//************************************************************************ - -namespace sparseBA { - -#include -class Values { - Values(); - Values(const sparseBA::Values& values); - size_t size() const; - void print(string s) const; - bool exists(size_t key); - gtsam::KeyVector keys() const; - - // Access to cameras - sparseBA::Values allSimpleCameras() const ; - size_t nrSimpleCameras() const ; - gtsam::KeyVector simpleCameraKeys() const ; - void insertSimpleCamera(size_t j, const gtsam::SimpleCamera& camera); - void updateSimpleCamera(size_t j, const gtsam::SimpleCamera& camera); - gtsam::SimpleCamera simpleCamera(size_t j) const; - - // Access to points, inherited from visualSLAM - sparseBA::Values allPoints() const; - size_t nrPoints() const; - gtsam::KeyVector pointKeys() const; // Note the switch to KeyVector, rather than KeyList - void insertPoint(size_t key, const gtsam::Point3& pose); - void updatePoint(size_t key, const gtsam::Point3& pose); - gtsam::Point3 point(size_t j); - Matrix points() const; -}; - -#include -class Graph { - Graph(); - Graph(const gtsam::NonlinearFactorGraph& graph); - Graph(const sparseBA::Graph& graph); - - // Information - Matrix reprojectionErrors(const sparseBA::Values& values) const; - - // inherited from FactorGraph - void print(string s) const; - bool equals(const sparseBA::Graph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - - double error(const sparseBA::Values& values) const; - gtsam::Ordering* orderingCOLAMD(const sparseBA::Values& values) const; - gtsam::GaussianFactorGraph* linearize(const sparseBA::Values& values, const gtsam::Ordering& ordering) const; - - sparseBA::Values optimize(const sparseBA::Values& initialEstimate, size_t verbosity) const; - sparseBA::LevenbergMarquardtOptimizer optimizer(const sparseBA::Values& initialEstimate, const gtsam::LevenbergMarquardtParams& parameters) const; - gtsam::Marginals marginals(const sparseBA::Values& solution) const; - - // inherited from visualSLAM - void addPointConstraint(size_t pointKey, const gtsam::Point3& p); - void addPointPrior(size_t pointKey, const gtsam::Point3& p, const gtsam::noiseModel::Base* model); - - // add factors - void addSimpleCameraPrior(size_t cameraKey, const gtsam::SimpleCamera &camera, gtsam::noiseModel::Base* model); - void addSimpleCameraConstraint(size_t cameraKey, const gtsam::SimpleCamera &camera); - void addSimpleCameraMeasurement(const gtsam::Point2 &z, gtsam::noiseModel::Base* model, size_t cameraKey, size_t pointKey); -}; - -#include -class LevenbergMarquardtOptimizer { - double lambda() const; - void iterate(); - double error() const; - size_t iterations() const; - sparseBA::Values optimize(); - sparseBA::Values optimizeSafely(); - sparseBA::Values values() const; -}; -} //\namespace sparseBA - +} diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h index b4bda4f1a..c128b1105 100644 --- a/gtsam/nonlinear/Symbol.h +++ b/gtsam/nonlinear/Symbol.h @@ -122,6 +122,15 @@ private: } }; +/** Create a symbol key from a character and index, i.e. x5. */ +inline Key symbol(unsigned char c, size_t j) { return (Key)Symbol(c,j); } + +/** Return the character portion of a symbol key. */ +inline unsigned char symbolChr(Key key) { return Symbol(key).chr(); } + +/** Return the index portion of a symbol key. */ +inline size_t symbolIndex(Key key) { return Symbol(key).index(); } + namespace symbol_shorthand { inline Key A(size_t j) { return Symbol('a', j); } inline Key B(size_t j) { return Symbol('b', j); } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 0502bb69b..59df1da67 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -20,8 +20,10 @@ #include #include +#include #include #include +#include using namespace std; @@ -78,23 +80,23 @@ pair > dataset(const string& dataset, } /* ************************************************************************* */ -pair load2D( - pair > dataset, +pair load2D( + pair > dataset, int maxID, bool addNoise, bool smart) { return load2D(dataset.first, dataset.second, maxID, addNoise, smart); } /* ************************************************************************* */ -pair load2D( - const string& filename, boost::optional model, int maxID, +pair load2D( + const string& filename, boost::optional model, int maxID, bool addNoise, bool smart) { cout << "Will try to read " << filename << endl; ifstream is(filename.c_str()); if (!is) throw std::invalid_argument("load2D: can not find the file!"); - pose2SLAM::Values::shared_ptr poses(new pose2SLAM::Values); - pose2SLAM::Graph::shared_ptr graph(new pose2SLAM::Graph); + Values::shared_ptr poses(new Values); + NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); string tag; @@ -155,7 +157,7 @@ pair load2D( if (!poses->exists(id2)) poses->insert(id2, poses->at(id1) * l1Xl2); - pose2SLAM::Graph::sharedFactor factor( + NonlinearFactor::shared_ptr factor( new BetweenFactor(id1, id2, l1Xl2, *model)); graph->push_back(factor); } @@ -169,8 +171,8 @@ pair load2D( } /* ************************************************************************* */ -void save2D(const pose2SLAM::Graph& graph, const Values& config, - const SharedDiagonal model, const string& filename) { +void save2D(const NonlinearFactorGraph& graph, const Values& config, + const noiseModel::Diagonal::shared_ptr model, const string& filename) { fstream stream(filename.c_str(), fstream::out); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index a099fb3d5..d3f00aae2 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -18,7 +18,9 @@ #pragma once -#include +#include +#include + #include namespace gtsam { @@ -39,8 +41,8 @@ namespace gtsam { * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model */ - std::pair load2D( - std::pair > dataset, + std::pair load2D( + std::pair > dataset, int maxID = 0, bool addNoise = false, bool smart = true); /** @@ -51,15 +53,15 @@ namespace gtsam { * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model */ - std::pair load2D( + std::pair load2D( const std::string& filename, boost::optional model = boost::optional< - gtsam::SharedDiagonal>(), int maxID = 0, bool addNoise = false, + noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false, bool smart = true); /** save 2d graph */ - void save2D(const pose2SLAM::Graph& graph, const Values& config, - const gtsam::SharedDiagonal model, const std::string& filename); + void save2D(const NonlinearFactorGraph& graph, const Values& config, + const noiseModel::Diagonal::shared_ptr model, const std::string& filename); /** * Load TORO 3D Graph diff --git a/matlab/CHECK.m b/matlab/+gtsam_utils/CHECK.m similarity index 100% rename from matlab/CHECK.m rename to matlab/+gtsam_utils/CHECK.m diff --git a/matlab/EQUALITY.m b/matlab/+gtsam_utils/EQUALITY.m similarity index 100% rename from matlab/EQUALITY.m rename to matlab/+gtsam_utils/EQUALITY.m diff --git a/matlab/examples/+support/VisualISAMGenerateData.m b/matlab/+gtsam_utils/VisualISAMGenerateData.m similarity index 100% rename from matlab/examples/+support/VisualISAMGenerateData.m rename to matlab/+gtsam_utils/VisualISAMGenerateData.m diff --git a/matlab/examples/+support/VisualISAMInitialize.m b/matlab/+gtsam_utils/VisualISAMInitialize.m similarity index 54% rename from matlab/examples/+support/VisualISAMInitialize.m rename to matlab/+gtsam_utils/VisualISAMInitialize.m index 90ba1e0c5..1e408d3d9 100644 --- a/matlab/examples/+support/VisualISAMInitialize.m +++ b/matlab/+gtsam_utils/VisualISAMInitialize.m @@ -1,66 +1,75 @@ -function [noiseModels,isam,result] = VisualISAMInitialize(data,truth,options) +function [noiseModels,isam,result,nextPoseIndex] = VisualISAMInitialize(data,truth,options) % VisualInitialize: initialize visualSLAM::iSAM object and noise parameters % Authors: Duy Nguyen Ta, Frank Dellaert and Alex Cunningham %% Initialize iSAM -isam = visualSLAM.ISAM(options.reorderInterval); +import gtsam.* +params = gtsam.ISAM2Params; +if options.alwaysRelinearize + params.setRelinearizeSkip(1); +end +isam = ISAM2; %% Set Noise parameters import gtsam.* noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); %noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); -noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.1 0.1 0.1 1.0 1.0 1.0]'); +noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.05 0.05 0.2 0.2 0.2]'); noiseModels.point = noiseModel.Isotropic.Sigma(3, 0.1); noiseModels.measurement = noiseModel.Isotropic.Sigma(2, 1.0); %% Add constraints/priors % TODO: should not be from ground truth! -newFactors = visualSLAM.Graph; -initialEstimates = visualSLAM.Values; +import gtsam.* +newFactors = NonlinearFactorGraph; +initialEstimates = Values; for i=1:2 ii = symbol('x',i); if i==1 if options.hardConstraint % add hard constraint - newFactors.addPoseConstraint(ii,truth.cameras{1}.pose); + newFactors.add(NonlinearEqualityPose3(ii,truth.cameras{1}.pose)); else - newFactors.addPosePrior(ii,truth.cameras{i}.pose, noiseModels.pose); + newFactors.add(PriorFactorPose3(ii,truth.cameras{i}.pose, noiseModels.pose)); end end - initialEstimates.insertPose(ii,truth.cameras{i}.pose); + initialEstimates.insert(ii,truth.cameras{i}.pose); end +nextPoseIndex = 3; + %% Add visual measurement factors from two first poses and initialize observed landmarks +import gtsam.* for i=1:2 ii = symbol('x',i); for k=1:length(data.Z{i}) j = data.J{i}{k}; jj = symbol('l',data.J{i}{k}); - newFactors.addMeasurement(data.Z{i}{k}, noiseModels.measurement, ii, jj, data.K); + newFactors.add(GenericProjectionFactorCal3_S2(data.Z{i}{k}, noiseModels.measurement, ii, jj, data.K)); % TODO: initial estimates should not be from ground truth! if ~initialEstimates.exists(jj) - initialEstimates.insertPoint(jj, truth.points{j}); + initialEstimates.insert(jj, truth.points{j}); end if options.pointPriors % add point priors - newFactors.addPointPrior(jj, truth.points{j}, noiseModels.point); + newFactors.add(priorFactorPoint3(jj, truth.points{j}, noiseModels.point)); end end end %% Add odometry between frames 1 and 2 -newFactors.addRelativePose(symbol('x',1), symbol('x',2), data.odometry{1}, noiseModels.odometry); +import gtsam.* +newFactors.add(BetweenFactorPose3(symbol('x',1), symbol('x',2), data.odometry{1}, noiseModels.odometry)); %% Update ISAM +import gtsam.* if options.batchInitialization % Do a full optimize for first two poses - fullyOptimized = newFactors.optimize(initialEstimates,0); + batchOptimizer = LevenbergMarquardtOptimizer(newFactors, initialEstimates); + fullyOptimized = batchOptimizer.optimize(); isam.update(newFactors, fullyOptimized); else isam.update(newFactors, initialEstimates); end % figure(1);tic; % t=toc; plot(frame_i,t,'r.'); tic -result = isam.estimate(); +result = isam.calculateEstimate(); % t=toc; plot(frame_i,t,'g.'); -if options.alwaysRelinearize % re-linearize - isam.reorder_relinearize(); -end diff --git a/matlab/examples/+support/VisualISAMPlot.m b/matlab/+gtsam_utils/VisualISAMPlot.m similarity index 55% rename from matlab/examples/+support/VisualISAMPlot.m rename to matlab/+gtsam_utils/VisualISAMPlot.m index 1f2b7d710..41aa49ac6 100644 --- a/matlab/examples/+support/VisualISAMPlot.m +++ b/matlab/+gtsam_utils/VisualISAMPlot.m @@ -1,39 +1,33 @@ function VisualISAMPlot(truth, data, isam, result, options) -% VisualISAMPlot: plot current state of visualSLAM::iSAM object +% VisualISAMPlot: plot current state of ISAM2 object % Authors: Duy Nguyen Ta and Frank Dellaert -M = result.nrPoses; -N = result.nrPoints; - h=gca; cla(h); hold on; %% Plot points % Can't use data because current frame might not see all points -pointKeys = result.allPoints().keys(); -for j=0:N-1 % NOTE: uses indexing directly from a C++ vector, so zero-indexed - jj = pointKeys.at(j); - point_j = result.point(jj); - plot3(point_j.x, point_j.y, point_j.z,'marker','o'); - P = isam.marginalCovariance(jj); - covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); -end +marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()); % TODO - this is slow +gtsam_utils.plot3DPoints(result, [], marginals); %% Plot cameras import gtsam.* -for i=1:options.cameraInterval:M - ii = symbol('x',i); - pose_i = result.pose(ii); - if options.hardConstraint && (i==1) - plotPose3(pose_i,[],10); +M = 1; +while result.exists(symbol('x',M)) + ii = symbol('x',M); + pose_i = result.at(ii); + if options.hardConstraint && (M==1) + gtsam_utils.plotPose3(pose_i,[],10); else - P = isam.marginalCovariance(ii); - plotPose3(pose_i,P,10); + P = marginals.marginalCovariance(ii); + gtsam_utils.plotPose3(pose_i,P,10); end if options.drawTruePoses % show ground truth - plotPose3(truth.cameras{i}.pose,[],10); + gtsam_utils.plotPose3(truth.cameras{M}.pose,[],10); end + + M = M + options.cameraInterval; end %% draw diff --git a/matlab/+gtsam_utils/VisualISAMStep.m b/matlab/+gtsam_utils/VisualISAMStep.m new file mode 100644 index 000000000..71b1c849c --- /dev/null +++ b/matlab/+gtsam_utils/VisualISAMStep.m @@ -0,0 +1,44 @@ +function [isam,result,nextPoseIndex] = VisualISAMStep(data,noiseModels,isam,result,truth,nextPoseIndex) +% VisualISAMStep: execute one update step of visualSLAM::iSAM object +% Authors: Duy Nguyen Ta and Frank Dellaert + +% iSAM expects us to give it a new set of factors +% along with initial estimates for any new variables introduced. +import gtsam.* +newFactors = NonlinearFactorGraph; +initialEstimates = Values; + +%% Add odometry +import gtsam.* +odometry = data.odometry{nextPoseIndex-1}; +newFactors.add(BetweenFactorPose3(symbol('x',nextPoseIndex-1), symbol('x',nextPoseIndex), odometry, noiseModels.odometry)); + +%% Add visual measurement factors and initializations as necessary +import gtsam.* +for k=1:length(data.Z{nextPoseIndex}) + zij = data.Z{nextPoseIndex}{k}; + j = data.J{nextPoseIndex}{k}; + jj = symbol('l', j); + newFactors.add(GenericProjectionFactorCal3_S2(zij, noiseModels.measurement, symbol('x',nextPoseIndex), jj, data.K)); + % TODO: initialize with something other than truth + if ~result.exists(jj) && ~initialEstimates.exists(jj) + lmInit = truth.points{j}; + initialEstimates.insert(jj, lmInit); + end +end + +%% Initial estimates for the new pose. +import gtsam.* +prevPose = result.at(symbol('x',nextPoseIndex-1)); +initialEstimates.insert(symbol('x',nextPoseIndex), prevPose.compose(odometry)); + +%% Update ISAM +% figure(1);tic; +isam.update(newFactors, initialEstimates); +% t=toc; plot(frame_i,t,'r.'); tic +result = isam.calculateEstimate(); +% t=toc; plot(frame_i,t,'g.'); + +% Update nextPoseIndex to return +nextPoseIndex = nextPoseIndex + 1; + diff --git a/matlab/circlePose2.m b/matlab/+gtsam_utils/circlePose2.m similarity index 95% rename from matlab/circlePose2.m rename to matlab/+gtsam_utils/circlePose2.m index 7153fc128..0679c16b3 100644 --- a/matlab/circlePose2.m +++ b/matlab/+gtsam_utils/circlePose2.m @@ -23,7 +23,7 @@ values = gtsam.Values; theta = 0.0; dtheta = 2*pi()/numPoses; for i = 1:numPoses - key = gtsam.Symbol(symbolChar, i-1).key(); + key = gtsam.symbol(symbolChar, i-1); pose = gtsam.Pose2(radius*cos(theta), radius*sin(theta), pi()/2 + theta); values.insert(key, pose); theta = theta + dtheta; diff --git a/matlab/circlePose3.m b/matlab/+gtsam_utils/circlePose3.m similarity index 96% rename from matlab/circlePose3.m rename to matlab/+gtsam_utils/circlePose3.m index 0e74a2f1b..9085ab0d0 100644 --- a/matlab/circlePose3.m +++ b/matlab/+gtsam_utils/circlePose3.m @@ -24,7 +24,7 @@ theta = 0.0; dtheta = 2*pi()/numPoses; gRo = gtsam.Rot3([0, 1, 0 ; 1, 0, 0 ; 0, 0, -1]); for i = 1:numPoses - key = gtsam.Symbol(symbolChar, i-1).key(); + key = gtsam.symbol(symbolChar, i-1); gti = gtsam.Point3(radius*cos(theta), radius*sin(theta), 0); oRi = gtsam.Rot3.Yaw(-theta); % negative yaw goes counterclockwise, with Z down ! gTi = gtsam.Pose3(gRo.compose(oRi), gti); diff --git a/matlab/covarianceEllipse.m b/matlab/+gtsam_utils/covarianceEllipse.m similarity index 96% rename from matlab/covarianceEllipse.m rename to matlab/+gtsam_utils/covarianceEllipse.m index 491e099c3..b4579862e 100644 --- a/matlab/covarianceEllipse.m +++ b/matlab/+gtsam_utils/covarianceEllipse.m @@ -1,34 +1,34 @@ -function covarianceEllipse(x,P,color) -% covarianceEllipse: plot a Gaussian as an uncertainty ellipse -% Based on Maybeck Vol 1, page 366 -% k=2.296 corresponds to 1 std, 68.26% of all probability -% k=11.82 corresponds to 3 std, 99.74% of all probability -% -% covarianceEllipse(x,P,color) -% it is assumed x and y are the first two components of state x - -[e,s] = eig(P(1:2,1:2)); -s1 = s(1,1); -s2 = s(2,2); -k = 2.296; -[ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) ); -line(ex,ey,'color',color); - - function [x,y] = ellipse(a,b,c); - % ellipse: return the x and y coordinates for an ellipse - % [x,y] = ellipse(a,b,c); - % a, and b are the axes. c is the center - - global ellipse_x ellipse_y - if ~exist('elipse_x') - q =0:2*pi/25:2*pi; - ellipse_x = cos(q); - ellipse_y = sin(q); - end - - points = a*ellipse_x + b*ellipse_y; - x = c(1) + points(1,:); - y = c(2) + points(2,:); - end - +function covarianceEllipse(x,P,color) +% covarianceEllipse: plot a Gaussian as an uncertainty ellipse +% Based on Maybeck Vol 1, page 366 +% k=2.296 corresponds to 1 std, 68.26% of all probability +% k=11.82 corresponds to 3 std, 99.74% of all probability +% +% covarianceEllipse(x,P,color) +% it is assumed x and y are the first two components of state x + +[e,s] = eig(P(1:2,1:2)); +s1 = s(1,1); +s2 = s(2,2); +k = 2.296; +[ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) ); +line(ex,ey,'color',color); + + function [x,y] = ellipse(a,b,c); + % ellipse: return the x and y coordinates for an ellipse + % [x,y] = ellipse(a,b,c); + % a, and b are the axes. c is the center + + global ellipse_x ellipse_y + if ~exist('elipse_x') + q =0:2*pi/25:2*pi; + ellipse_x = cos(q); + ellipse_y = sin(q); + end + + points = a*ellipse_x + b*ellipse_y; + x = c(1) + points(1,:); + y = c(2) + points(2,:); + end + end \ No newline at end of file diff --git a/matlab/covarianceEllipse3D.m b/matlab/+gtsam_utils/covarianceEllipse3D.m similarity index 100% rename from matlab/covarianceEllipse3D.m rename to matlab/+gtsam_utils/covarianceEllipse3D.m diff --git a/matlab/examples/+support/findExampleDataFile.m b/matlab/+gtsam_utils/findExampleDataFile.m similarity index 74% rename from matlab/examples/+support/findExampleDataFile.m rename to matlab/+gtsam_utils/findExampleDataFile.m index 5fd23352e..9c40960b9 100644 --- a/matlab/examples/+support/findExampleDataFile.m +++ b/matlab/+gtsam_utils/findExampleDataFile.m @@ -3,8 +3,8 @@ function datafile = findExampleDataFile(datasetName) [ myPath, ~, ~ ] = fileparts(mfilename('fullpath')); searchPath = { ... - fullfile(myPath, [ '../../../examples/Data/' datasetName ]) ... - fullfile(myPath, [ '../Data/' datasetName ]) }; + fullfile(myPath, [ '../../examples/Data/' datasetName ]) ... + fullfile(myPath, [ '../gtsam_examples/Data/' datasetName ]) }; datafile = []; for path = searchPath if exist(path{:}, 'file') diff --git a/matlab/load2D.m b/matlab/+gtsam_utils/load2D.m similarity index 100% rename from matlab/load2D.m rename to matlab/+gtsam_utils/load2D.m diff --git a/matlab/load3D.m b/matlab/+gtsam_utils/load3D.m similarity index 100% rename from matlab/load3D.m rename to matlab/+gtsam_utils/load3D.m diff --git a/matlab/plot2DPoints.m b/matlab/+gtsam_utils/plot2DPoints.m similarity index 88% rename from matlab/plot2DPoints.m rename to matlab/+gtsam_utils/plot2DPoints.m index d03676699..b98434627 100644 --- a/matlab/plot2DPoints.m +++ b/matlab/+gtsam_utils/plot2DPoints.m @@ -22,9 +22,9 @@ for i = 0:keys.size-1 if isa(p, 'gtsam.Point2') if haveMarginals P = marginals.marginalCovariance(key); - plotPoint2(p, linespec, P); + gtsam_utils.plotPoint2(p, linespec, P); else - plotPoint2(p, linespec); + gtsam_utils.plotPoint2(p, linespec); end end end diff --git a/matlab/plot2DTrajectory.m b/matlab/+gtsam_utils/plot2DTrajectory.m similarity index 93% rename from matlab/plot2DTrajectory.m rename to matlab/+gtsam_utils/plot2DTrajectory.m index fcbfe4b92..c962c993b 100644 --- a/matlab/plot2DTrajectory.m +++ b/matlab/+gtsam_utils/plot2DTrajectory.m @@ -31,7 +31,7 @@ for i = 0:keys.size-1 plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], linespec); if haveMarginals P = marginals.marginalCovariance(lastKey); - plotPose2(lastPose, 'g', P); + gtsam_utils.plotPose2(lastPose, 'g', P); end end lastIndex = i; @@ -43,7 +43,7 @@ if ~isempty(lastIndex) && haveMarginals lastKey = keys.at(lastIndex); lastPose = values.at(lastKey); P = marginals.marginalCovariance(lastKey); - plotPose2(lastPose, 'g', P); + gtsam_utils.plotPose2(lastPose, 'g', P); end if ~holdstate diff --git a/matlab/plot3DPoints.m b/matlab/+gtsam_utils/plot3DPoints.m similarity index 88% rename from matlab/plot3DPoints.m rename to matlab/+gtsam_utils/plot3DPoints.m index 264298ef4..e7a5ab0a0 100644 --- a/matlab/plot3DPoints.m +++ b/matlab/+gtsam_utils/plot3DPoints.m @@ -22,9 +22,9 @@ for i = 0:keys.size-1 if isa(p, 'gtsam.Point3') if haveMarginals P = marginals.marginalCovariance(key); - plotPoint3(p, linespec, P); + gtsam_utils.plotPoint3(p, linespec, P); else - plotPoint3(p, linespec); + gtsam_utils.plotPoint3(p, linespec); end end end diff --git a/matlab/plot3DTrajectory.m b/matlab/+gtsam_utils/plot3DTrajectory.m similarity index 91% rename from matlab/plot3DTrajectory.m rename to matlab/+gtsam_utils/plot3DTrajectory.m index 771d567fa..08b2f0daa 100644 --- a/matlab/plot3DTrajectory.m +++ b/matlab/+gtsam_utils/plot3DTrajectory.m @@ -28,7 +28,7 @@ for i = 0:keys.size-1 else P = []; end - plotPose3(lastPose, P, scale); + gtsam_utils.plotPose3(lastPose, P, scale); end lastIndex = i; end @@ -43,7 +43,7 @@ if ~isempty(lastIndex) else P = []; end - plotPose3(lastPose, P, scale); + gtsam_utils.plotPose3(lastPose, P, scale); end if ~holdstate diff --git a/matlab/plotCamera.m b/matlab/+gtsam_utils/plotCamera.m similarity index 100% rename from matlab/plotCamera.m rename to matlab/+gtsam_utils/plotCamera.m diff --git a/matlab/plotPoint2.m b/matlab/+gtsam_utils/plotPoint2.m similarity index 76% rename from matlab/plotPoint2.m rename to matlab/+gtsam_utils/plotPoint2.m index fcc274d90..a1bc85a88 100644 --- a/matlab/plotPoint2.m +++ b/matlab/+gtsam_utils/plotPoint2.m @@ -6,5 +6,5 @@ else plot(p.x,p.y,color); end if exist('P', 'var') - covarianceEllipse([p.x;p.y],P,color(1)); + gtsam_utils.covarianceEllipse([p.x;p.y],P,color(1)); end \ No newline at end of file diff --git a/matlab/plotPoint3.m b/matlab/+gtsam_utils/plotPoint3.m similarity index 80% rename from matlab/plotPoint3.m rename to matlab/+gtsam_utils/plotPoint3.m index 89d5613d5..6aafd5f21 100644 --- a/matlab/plotPoint3.m +++ b/matlab/+gtsam_utils/plotPoint3.m @@ -6,7 +6,7 @@ else plot3(p.x,p.y,p.z,color); end if exist('P', 'var') - covarianceEllipse3D([p.x;p.y;p.z],P); + gtsam_utils.covarianceEllipse3D([p.x;p.y;p.z],P); end end diff --git a/matlab/plotPose2.m b/matlab/+gtsam_utils/plotPose2.m similarity index 84% rename from matlab/plotPose2.m rename to matlab/+gtsam_utils/plotPose2.m index 54b1e68f0..a9997dc2c 100644 --- a/matlab/plotPose2.m +++ b/matlab/+gtsam_utils/plotPose2.m @@ -9,5 +9,5 @@ quiver(pose.x,pose.y,c,s,axisLength,color); if nargin>2 pPp = P(1:2,1:2); % covariance matrix in pose coordinate frame gRp = [c -s;s c]; % rotation from pose to global - covarianceEllipse([pose.x;pose.y],gRp*pPp*gRp',color); + gtsam_utils.covarianceEllipse([pose.x;pose.y],gRp*pPp*gRp',color); end \ No newline at end of file diff --git a/matlab/plotPose3.m b/matlab/+gtsam_utils/plotPose3.m similarity index 94% rename from matlab/plotPose3.m rename to matlab/+gtsam_utils/plotPose3.m index 956b231be..d24a998c8 100644 --- a/matlab/plotPose3.m +++ b/matlab/+gtsam_utils/plotPose3.m @@ -25,7 +25,7 @@ end if (nargin>2) && (~isempty(P)) pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame - covarianceEllipse3D(C,gPp); + gtsam_utils.covarianceEllipse3D(C,gPp); end end diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 0b07b51b1..66fa99def 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -10,14 +10,17 @@ install(FILES ${matlab_utils} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/) # Tests message(STATUS "Installing Matlab Toolbox Tests") -file(GLOB matlab_tests "${GTSAM_SOURCE_ROOT_DIR}/matlab/tests/*.m") -install(FILES ${matlab_tests} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_tests) +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_tests" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") # Examples message(STATUS "Installing Matlab Toolbox Examples") # Matlab files: *.m and *.fig -install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/examples/" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples" FILES_MATCHING PATTERN "*.m") -install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/examples/" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples" FILES_MATCHING PATTERN "*.fig") +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.fig") + +# Utilities +message(STATUS "Installing Matlab Toolbox Utilities") +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam_utils" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") message(STATUS "Installing Matlab Toolbox Examples (Data)") # Data files: *.graph and *.txt diff --git a/matlab/examples/+support/VisualISAMStep.m b/matlab/examples/+support/VisualISAMStep.m deleted file mode 100644 index ef056553e..000000000 --- a/matlab/examples/+support/VisualISAMStep.m +++ /dev/null @@ -1,44 +0,0 @@ -function [isam,result] = VisualISAMStep(data,noiseModels,isam,result,truth, options) -% VisualISAMStep: execute one update step of visualSLAM::iSAM object -% Authors: Duy Nguyen Ta and Frank Dellaert - -% iSAM expects us to give it a new set of factors -% along with initial estimates for any new variables introduced. -newFactors = visualSLAM.Graph; -initialEstimates = visualSLAM.Values; - -%% Add odometry -import gtsam.* -i = result.nrPoses+1; -odometry = data.odometry{i-1}; -newFactors.addRelativePose(symbol('x',i-1), symbol('x',i), odometry, noiseModels.odometry); - -%% Add visual measurement factors and initializations as necessary -import gtsam.* -for k=1:length(data.Z{i}) - zij = data.Z{i}{k}; - j = data.J{i}{k}; - jj = symbol('l', j); - newFactors.addMeasurement(zij, noiseModels.measurement, symbol('x',i), jj, data.K); - % TODO: initialize with something other than truth - if ~result.exists(jj) && ~initialEstimates.exists(jj) - lmInit = truth.points{j}; - initialEstimates.insertPoint(jj, lmInit); - end -end - -%% Initial estimates for the new pose. -import gtsam.* -prevPose = result.pose(symbol('x',i-1)); -initialEstimates.insertPose(symbol('x',i), prevPose.compose(odometry)); - -%% Update ISAM -% figure(1);tic; -isam.update(newFactors, initialEstimates); -% t=toc; plot(frame_i,t,'r.'); tic -result = isam.estimate(); -% t=toc; plot(frame_i,t,'g.'); - -if options.alwaysRelinearize % re-linearize - isam.reorder_relinearize(); -end diff --git a/matlab/examples/LocalizationExample.m b/matlab/gtsam_examples/LocalizationExample.m similarity index 96% rename from matlab/examples/LocalizationExample.m rename to matlab/gtsam_examples/LocalizationExample.m index e4ee76d7c..aad3af98c 100644 --- a/matlab/examples/LocalizationExample.m +++ b/matlab/gtsam_examples/LocalizationExample.m @@ -55,7 +55,7 @@ import gtsam.* cla; hold on; -plot2DTrajectory(result, [], Marginals(graph, result)); +gtsam_utils.plot2DTrajectory(result, [], Marginals(graph, result)); axis([-0.6 4.8 -1 1]) axis equal diff --git a/matlab/examples/OdometryExample.m b/matlab/gtsam_examples/OdometryExample.m similarity index 96% rename from matlab/examples/OdometryExample.m rename to matlab/gtsam_examples/OdometryExample.m index 81733cfb0..e47becda5 100644 --- a/matlab/examples/OdometryExample.m +++ b/matlab/gtsam_examples/OdometryExample.m @@ -52,7 +52,7 @@ import gtsam.* cla; hold on; -plot2DTrajectory(result, [], Marginals(graph, result)); +gtsam_utils.plot2DTrajectory(result, [], Marginals(graph, result)); axis([-0.6 4.8 -1 1]) axis equal diff --git a/matlab/examples/PlanarSLAMExample.m b/matlab/gtsam_examples/PlanarSLAMExample.m similarity index 96% rename from matlab/examples/PlanarSLAMExample.m rename to matlab/gtsam_examples/PlanarSLAMExample.m index 9b4600578..056db501e 100644 --- a/matlab/examples/PlanarSLAMExample.m +++ b/matlab/gtsam_examples/PlanarSLAMExample.m @@ -72,8 +72,8 @@ import gtsam.* cla;hold on marginals = Marginals(graph, result); -plot2DTrajectory(result, [], marginals); -plot2DPoints(result, [], marginals); +gtsam_utils.plot2DTrajectory(result, [], marginals); +gtsam_utils.plot2DPoints(result, [], marginals); plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-'); plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-'); diff --git a/matlab/examples/PlanarSLAMExample_sampling.m b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m similarity index 92% rename from matlab/examples/PlanarSLAMExample_sampling.m rename to matlab/gtsam_examples/PlanarSLAMExample_sampling.m index 58686c362..5604d0ddc 100644 --- a/matlab/examples/PlanarSLAMExample_sampling.m +++ b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m @@ -39,6 +39,7 @@ end graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); %% Initialize MCMC sampler with ground truth +import gtsam.* sample = Values; sample.insert(i1, Pose2(0,0,0)); sample.insert(i2, Pose2(2,0,0)); @@ -47,11 +48,12 @@ sample.insert(j1, Point2(2,2)); sample.insert(j2, Point2(4,2)); %% Calculate and plot Covariance Ellipses +import gtsam.* cla;hold on marginals = Marginals(graph, sample); -plot2DTrajectory(sample, [], marginals); -plot2DPoints(sample, [], marginals); +gtsam_utils.plot2DTrajectory(sample, [], marginals); +gtsam_utils.plot2DPoints(sample, [], marginals); for j=1:2 key = symbol('l',j); @@ -66,9 +68,10 @@ plot([sample.at(i3).x; sample.at(j2).x],[sample.at(i3).y; sample.at(j2).y], 'c-' view(2); axis auto; axis equal %% Do Sampling on point 2 +import gtsam.* N=1000; for s=1:N delta = S{2}*randn(2,1); proposedPoint = Point2(point{2}.x+delta(1),point{2}.y+delta(2)); - plotPoint2(proposedPoint,'k.') + gtsam_utils.plotPoint2(proposedPoint,'k.') end \ No newline at end of file diff --git a/matlab/examples/Pose2SLAMExample.m b/matlab/gtsam_examples/Pose2SLAMExample.m similarity index 96% rename from matlab/examples/Pose2SLAMExample.m rename to matlab/gtsam_examples/Pose2SLAMExample.m index 1e26ce33d..a8ad32254 100644 --- a/matlab/examples/Pose2SLAMExample.m +++ b/matlab/gtsam_examples/Pose2SLAMExample.m @@ -56,17 +56,19 @@ initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2)); initialEstimate.print(sprintf('\nInitial estimate:\n')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd +import gtsam.* optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); result = optimizer.optimizeSafely(); result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses +import gtsam.* cla; hold on plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-','LineWidth',2); marginals = Marginals(graph, result); -plot2DTrajectory(result, [], marginals); +gtsam_utils.plot2DTrajectory(result, [], marginals); axis([-0.6 4.8 -1 1]) axis equal diff --git a/matlab/examples/Pose2SLAMExample_advanced.m b/matlab/gtsam_examples/Pose2SLAMExample_advanced.m similarity index 99% rename from matlab/examples/Pose2SLAMExample_advanced.m rename to matlab/gtsam_examples/Pose2SLAMExample_advanced.m index b496c44e7..fab340f2a 100644 --- a/matlab/examples/Pose2SLAMExample_advanced.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_advanced.m @@ -77,5 +77,6 @@ IJS = gfg.sparseJacobian_(); Ab=sparse(IJS(1,:),IJS(2,:),IJS(3,:)); A = Ab(:,1:end-1); b = full(Ab(:,end)); +clf spy(A); title('Non-zero entries in measurement Jacobian'); diff --git a/matlab/examples/Pose2SLAMExample_circle.m b/matlab/gtsam_examples/Pose2SLAMExample_circle.m similarity index 87% rename from matlab/examples/Pose2SLAMExample_circle.m rename to matlab/gtsam_examples/Pose2SLAMExample_circle.m index 3d2265d76..b40c67322 100644 --- a/matlab/examples/Pose2SLAMExample_circle.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_circle.m @@ -10,14 +10,14 @@ % @author Frank Dellaert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -import gtsam.* - %% Create a hexagon of poses -hexagon = circlePose2(6,1.0); +import gtsam.* +hexagon = gtsam_utils.circlePose2(6,1.0); p0 = hexagon.at(0); p1 = hexagon.at(1); %% create a Pose graph with one equality constraint and one measurement +import gtsam.* fg = NonlinearFactorGraph; fg.add(NonlinearEqualityPose2(0, p0)); delta = p0.between(p1); @@ -30,6 +30,7 @@ fg.add(BetweenFactorPose2(4,5, delta, covariance)); fg.add(BetweenFactorPose2(5,0, delta, covariance)); %% Create initial config +import gtsam.* initial = Values; initial.insert(0, p0); initial.insert(1, hexagon.at(1).retract([-0.1, 0.1,-0.1]')); @@ -39,15 +40,18 @@ initial.insert(4, hexagon.at(4).retract([ 0.1,-0.1, 0.1]')); initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]')); %% Plot Initial Estimate +import gtsam.* cla -plot2DTrajectory(initial, 'g*-'); axis equal +gtsam_utils.plot2DTrajectory(initial, 'g*-'); axis equal %% optimize +import gtsam.* optimizer = DoglegOptimizer(fg, initial); result = optimizer.optimizeSafely; %% Show Result -hold on; plot2DTrajectory(result, 'b*-'); +import gtsam.* +hold on; gtsam_utils.plot2DTrajectory(result, 'b*-'); view(2); axis([-1.5 1.5 -1.5 1.5]); result.print(sprintf('\nFinal result:\n')); diff --git a/matlab/examples/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m similarity index 79% rename from matlab/examples/Pose2SLAMExample_graph.m rename to matlab/gtsam_examples/Pose2SLAMExample_graph.m index f5193a7de..980a11cfa 100644 --- a/matlab/examples/Pose2SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_graph.m @@ -11,12 +11,12 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Find data file -datafile = support.findExampleDataFile('w100-odom.graph'); +datafile = gtsam_utils.findExampleDataFile('w100-odom.graph'); %% Initialize graph, initial estimate, and odometry noise import gtsam.* model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); -[graph,initial] = load2D(datafile, model); +[graph,initial] = gtsam_utils.load2D(datafile, model); initial.print(sprintf('Initial estimate:\n')); %% Add a Gaussian prior on pose x_1 @@ -26,22 +26,25 @@ priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]); graph.add(PriorFactorPose2(0, priorMean, priorNoise)); % add directly to graph %% Plot Initial Estimate +import gtsam.* cla -plot2DTrajectory(initial, 'g-*'); axis equal +gtsam_utils.plot2DTrajectory(initial, 'g-*'); axis equal %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd +import gtsam.* optimizer = LevenbergMarquardtOptimizer(graph, initial); result = optimizer.optimizeSafely; -hold on; plot2DTrajectory(result, 'b-*'); +hold on; gtsam_utils.plot2DTrajectory(result, 'b-*'); result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses +import gtsam.* marginals = Marginals(graph, result); P={}; for i=1:result.size()-1 pose_i = result.at(i); P{i}=marginals.marginalCovariance(i); - plotPose2(pose_i,'b',P{i}) + gtsam_utils.plotPose2(pose_i,'b',P{i}) end view(2) axis([-15 10 -15 10]); axis equal; diff --git a/matlab/examples/Pose2SLAMwSPCG.m b/matlab/gtsam_examples/Pose2SLAMwSPCG.m similarity index 100% rename from matlab/examples/Pose2SLAMwSPCG.m rename to matlab/gtsam_examples/Pose2SLAMwSPCG.m diff --git a/matlab/examples/Pose3SLAMExample.m b/matlab/gtsam_examples/Pose3SLAMExample.m similarity index 88% rename from matlab/examples/Pose3SLAMExample.m rename to matlab/gtsam_examples/Pose3SLAMExample.m index 0d2bd237f..bdc101b9d 100644 --- a/matlab/examples/Pose3SLAMExample.m +++ b/matlab/gtsam_examples/Pose3SLAMExample.m @@ -10,14 +10,14 @@ % @author Frank Dellaert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -import gtsam.* - %% Create a hexagon of poses -hexagon = circlePose3(6,1.0); +import gtsam.* +hexagon = gtsam_utils.circlePose3(6,1.0); p0 = hexagon.at(0); p1 = hexagon.at(1); %% create a Pose graph with one equality constraint and one measurement +import gtsam.* fg = NonlinearFactorGraph; fg.add(NonlinearEqualityPose3(0, p0)); delta = p0.between(p1); @@ -30,6 +30,7 @@ fg.add(BetweenFactorPose3(4,5, delta, covariance)); fg.add(BetweenFactorPose3(5,0, delta, covariance)); %% Create initial config +import gtsam.* initial = Values; s = 0.10; initial.insert(0, p0); @@ -40,15 +41,18 @@ initial.insert(4, hexagon.at(4).retract(s*randn(6,1))); initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); %% Plot Initial Estimate +import gtsam.* cla -plot3DTrajectory(initial, 'g-*'); +gtsam_utils.plot3DTrajectory(initial, 'g-*'); %% optimize +import gtsam.* optimizer = DoglegOptimizer(fg, initial); result = optimizer.optimizeSafely(); %% Show Result -hold on; plot3DTrajectory(result, 'b-*', true, 0.3); +import gtsam.* +hold on; gtsam_utils.plot3DTrajectory(result, 'b-*', true, 0.3); axis([-2 2 -2 2 -1 1]); axis equal view(-37,40) diff --git a/matlab/examples/Pose3SLAMExample_graph.m b/matlab/gtsam_examples/Pose3SLAMExample_graph.m similarity index 77% rename from matlab/examples/Pose3SLAMExample_graph.m rename to matlab/gtsam_examples/Pose3SLAMExample_graph.m index 1d33c30e4..574894878 100644 --- a/matlab/examples/Pose3SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose3SLAMExample_graph.m @@ -16,26 +16,27 @@ N = 2500; % dataset = 'sphere2500_groundtruth.txt'; dataset = 'sphere2500.txt'; -datafile = support.findExampleDataFile(dataset); +datafile = gtsam_utils.findExampleDataFile(dataset); %% Initialize graph, initial estimate, and odometry noise import gtsam.* model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); -[graph,initial]=load3D(datafile,model,true,N); +[graph,initial]=gtsam_utils.load3D(datafile,model,true,N); %% Plot Initial Estimate +import gtsam.* cla first = initial.at(0); plot3(first.x(),first.y(),first.z(),'r*'); hold on -plot3DTrajectory(initial,'g-',false); +gtsam_utils.plot3DTrajectory(initial,'g-',false); drawnow; %% Read again, now with all constraints, and optimize import gtsam.* -graph = load3D(datafile, model, false, N); +graph = gtsam_utils.load3D(datafile, model, false, N); graph.add(NonlinearEqualityPose3(0, first)); optimizer = LevenbergMarquardtOptimizer(graph, initial); result = optimizer.optimizeSafely(); -plot3DTrajectory(result, 'r-', false); axis equal; +gtsam_utils.plot3DTrajectory(result, 'r-', false); axis equal; view(3); axis equal; \ No newline at end of file diff --git a/matlab/examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m similarity index 97% rename from matlab/examples/SBAExample.m rename to matlab/gtsam_examples/SBAExample.m index a9f101613..852c83fdc 100644 --- a/matlab/examples/SBAExample.m +++ b/matlab/gtsam_examples/SBAExample.m @@ -22,7 +22,7 @@ options.nrCameras = 10; options.showImages = false; %% Generate data -[data,truth] = support.VisualISAMGenerateData(options); +[data,truth] = gtsam_utils.VisualISAMGenerateData(options); measurementNoiseSigma = 1.0; pointNoiseSigma = 0.1; diff --git a/matlab/examples/SFMExample.m b/matlab/gtsam_examples/SFMExample.m similarity index 94% rename from matlab/examples/SFMExample.m rename to matlab/gtsam_examples/SFMExample.m index 2153ba647..7f583b386 100644 --- a/matlab/examples/SFMExample.m +++ b/matlab/gtsam_examples/SFMExample.m @@ -22,7 +22,7 @@ options.nrCameras = 10; options.showImages = false; %% Generate data -[data,truth] = support.VisualISAMGenerateData(options); +[data,truth] = gtsam_utils.VisualISAMGenerateData(options); measurementNoiseSigma = 1.0; pointNoiseSigma = 0.1; @@ -86,8 +86,8 @@ marginals = Marginals(graph, result); cla hold on; -plot3DPoints(result, [], marginals); -plot3DTrajectory(result, '*', 1, 8, marginals); +gtsam_utils.plot3DPoints(result, [], marginals); +gtsam_utils.plot3DTrajectory(result, '*', 1, 8, marginals); axis([-40 40 -40 40 -10 20]);axis equal view(3) diff --git a/matlab/examples/StereoVOExample.m b/matlab/gtsam_examples/StereoVOExample.m similarity index 94% rename from matlab/examples/StereoVOExample.m rename to matlab/gtsam_examples/StereoVOExample.m index 468123529..e62c4395c 100644 --- a/matlab/examples/StereoVOExample.m +++ b/matlab/gtsam_examples/StereoVOExample.m @@ -18,6 +18,7 @@ % - No noise on measurements %% Create keys for variables +import gtsam.* x1 = symbol('x',1); x2 = symbol('x',2); l1 = symbol('l',1); l2 = symbol('l',2); l3 = symbol('l',3); @@ -74,6 +75,6 @@ axis equal view(-38,12) camup([0;1;0]); -plot3DTrajectory(initialEstimate, 'r', 1, 0.3); -plot3DTrajectory(result, 'g', 1, 0.3); -plot3DPoints(result); +gtsam_utils.plot3DTrajectory(initialEstimate, 'r', 1, 0.3); +gtsam_utils.plot3DTrajectory(result, 'g', 1, 0.3); +gtsam_utils.plot3DPoints(result); diff --git a/matlab/examples/StereoVOExample_large.m b/matlab/gtsam_examples/StereoVOExample_large.m similarity index 84% rename from matlab/examples/StereoVOExample_large.m rename to matlab/gtsam_examples/StereoVOExample_large.m index fb45dd8a7..5ae3a8fd7 100644 --- a/matlab/examples/StereoVOExample_large.m +++ b/matlab/gtsam_examples/StereoVOExample_large.m @@ -13,7 +13,7 @@ %% Load calibration import gtsam.* % format: fx fy skew cx cy baseline -calib = dlmread(support.findExampleDataFile('VO_calibration.txt')); +calib = dlmread(gtsam_utils.findExampleDataFile('VO_calibration.txt')); K = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6)); stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]); @@ -26,7 +26,7 @@ initial = Values; % row format: camera_id 4x4 pose (row, major) import gtsam.* fprintf(1,'Reading data\n'); -cameras = dlmread(support.findExampleDataFile('VO_camera_poses_large.txt')); +cameras = dlmread(gtsam_utils.findExampleDataFile('VO_camera_poses_large.txt')); for i=1:size(cameras,1) pose = Pose3(reshape(cameras(i,2:17),4,4)'); initial.insert(symbol('x',cameras(i,1)),pose); @@ -35,7 +35,7 @@ end %% load stereo measurements and initialize landmarks % camera_id landmark_id uL uR v X Y Z import gtsam.* -measurements = dlmread(support.findExampleDataFile('VO_stereo_factors_large.txt')); +measurements = dlmread(gtsam_utils.findExampleDataFile('VO_stereo_factors_large.txt')); fprintf(1,'Creating Graph\n'); tic for i=1:size(measurements,1) @@ -54,11 +54,13 @@ end toc %% add a constraint on the starting pose +import gtsam.* key = symbol('x',1); first_pose = initial.at(key); graph.add(NonlinearEqualityPose3(key, first_pose)); %% optimize +import gtsam.* fprintf(1,'Optimizing\n'); tic optimizer = LevenbergMarquardtOptimizer(graph, initial); result = optimizer.optimizeSafely(); @@ -67,9 +69,9 @@ toc %% visualize initial trajectory, final trajectory, and final points cla; hold on; -plot3DTrajectory(initial, 'r', 1, 0.5); -plot3DTrajectory(result, 'g', 1, 0.5); -plot3DPoints(result); +gtsam_utils.plot3DTrajectory(initial, 'r', 1, 0.5); +gtsam_utils.plot3DTrajectory(result, 'g', 1, 0.5); +gtsam_utils.plot3DPoints(result); axis([-5 20 -20 20 0 100]); axis equal diff --git a/matlab/examples/VisualISAMDemo.m b/matlab/gtsam_examples/VisualISAMDemo.m similarity index 100% rename from matlab/examples/VisualISAMDemo.m rename to matlab/gtsam_examples/VisualISAMDemo.m diff --git a/matlab/examples/VisualISAMExample.m b/matlab/gtsam_examples/VisualISAMExample.m similarity index 75% rename from matlab/examples/VisualISAMExample.m rename to matlab/gtsam_examples/VisualISAMExample.m index 7553ed2a4..64634d336 100644 --- a/matlab/examples/VisualISAMExample.m +++ b/matlab/gtsam_examples/VisualISAMExample.m @@ -32,17 +32,17 @@ options.saveFigures = false; options.saveDotFiles = false; %% Generate data -[data,truth] = support.VisualISAMGenerateData(options); +[data,truth] = gtsam_utils.VisualISAMGenerateData(options); %% Initialize iSAM with the first pose and points -[noiseModels,isam,result] = support.VisualISAMInitialize(data,truth,options); +[noiseModels,isam,result,nextPose] = gtsam_utils.VisualISAMInitialize(data,truth,options); cla; -support.VisualISAMPlot(truth, data, isam, result, options) +gtsam_utils.VisualISAMPlot(truth, data, isam, result, options) %% Main loop for iSAM: stepping through all poses for frame_i=3:options.nrCameras - [isam,result] = support.VisualISAMStep(data,noiseModels,isam,result,truth,options); + [isam,result,nextPose] = gtsam_utils.VisualISAMStep(data,noiseModels,isam,result,truth,nextPose); if mod(frame_i,options.drawInterval)==0 - support.VisualISAMPlot(truth, data, isam, result, options) + gtsam_utils.VisualISAMPlot(truth, data, isam, result, options) end end diff --git a/matlab/examples/VisualISAM_gui.fig b/matlab/gtsam_examples/VisualISAM_gui.fig similarity index 100% rename from matlab/examples/VisualISAM_gui.fig rename to matlab/gtsam_examples/VisualISAM_gui.fig diff --git a/matlab/examples/VisualISAM_gui.m b/matlab/gtsam_examples/VisualISAM_gui.m similarity index 92% rename from matlab/examples/VisualISAM_gui.m rename to matlab/gtsam_examples/VisualISAM_gui.m index 765a6f5ac..f7990268f 100644 --- a/matlab/examples/VisualISAM_gui.m +++ b/matlab/gtsam_examples/VisualISAM_gui.m @@ -224,32 +224,32 @@ function saveGraphsCB_Callback(hObject, ~, handles) % --- Executes on button press in intializeButton. function intializeButton_Callback(hObject, ~, handles) -global frame_i truth data noiseModels isam result options +global frame_i truth data noiseModels isam result nextPoseIndex options % initialize global options initOptions(handles) % Generate Data -[data,truth] = support.VisualISAMGenerateData(options); +[data,truth] = gtsam_utils.VisualISAMGenerateData(options); % Initialize and plot -[noiseModels,isam,result] = support.VisualISAMInitialize(data,truth,options); +[noiseModels,isam,result,nextPoseIndex] = gtsam_utils.VisualISAMInitialize(data,truth,options); cla -support.VisualISAMPlot(truth, data, isam, result, options) +gtsam_utils.VisualISAMPlot(truth, data, isam, result, options) frame_i = 2; showFramei(hObject, handles) % --- Executes on button press in runButton. function runButton_Callback(hObject, ~, handles) -global frame_i truth data noiseModels isam result options +global frame_i truth data noiseModels isam result nextPoseIndex options while (frame_i Date: Fri, 27 Jul 2012 19:15:58 +0000 Subject: [PATCH 55/68] Fixed missing renames --- tests/testGradientDescentOptimizer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 6f03bc017..4f556eb0f 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -7,7 +7,7 @@ #include #include -#include +#include #include #include #include @@ -69,7 +69,7 @@ TEST(optimize, GradientDescentOptimizer) { param.maxIterations = 500; /* requires a larger number of iterations to converge */ param.verbosity = NonlinearOptimizerParams::SILENT; - GradientDescentOptimizer optimizer(graph, initialEstimate, param); + NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); Values result = optimizer.optimize(); // cout << "gd1 solver final error = " << graph.error(result) << endl; From d95ca7857b0e0cf605ae1dab43a4321749658063 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 27 Jul 2012 19:16:00 +0000 Subject: [PATCH 56/68] Updated wrap unit tests --- wrap/tests/expected/geometry_wrapper.cpp | 36 ++++++++++--------- .../testNamespaces_wrapper.cpp | 26 +++++++------- 2 files changed, 33 insertions(+), 29 deletions(-) diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index 93fde1ca4..b546aa65f 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -19,24 +19,24 @@ void _deleteAllObjects() std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - for(Collector_Point2::iterator iter = collector_Point2.begin(); + { for(Collector_Point2::iterator iter = collector_Point2.begin(); iter != collector_Point2.end(); ) { delete *iter; collector_Point2.erase(iter++); anyDeleted = true; - } - for(Collector_Point3::iterator iter = collector_Point3.begin(); + } } + { for(Collector_Point3::iterator iter = collector_Point3.begin(); iter != collector_Point3.end(); ) { delete *iter; collector_Point3.erase(iter++); anyDeleted = true; - } - for(Collector_Test::iterator iter = collector_Test.begin(); + } } + { for(Collector_Test::iterator iter = collector_Test.begin(); iter != collector_Test.end(); ) { delete *iter; collector_Test.erase(iter++); anyDeleted = true; - } + } } if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -227,14 +227,14 @@ void Point3_StaticFunctionRet_15(int nargout, mxArray *out[], int nargin, const typedef boost::shared_ptr Shared; checkArguments("Point3.StaticFunctionRet",nargout,nargin,1); double z = unwrap< double >(in[0]); - out[0] = wrap_shared_ptr(SharedPoint3(new Point3(Point3::StaticFunctionRet(z))),"Point3", false); + out[0] = wrap_shared_ptr(SharedPoint3(new Point3(Point3::StaticFunctionRet(z))),"Point3", false); } void Point3_staticFunction_16(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; checkArguments("Point3.staticFunction",nargout,nargin,0); - out[0] = wrap< double >(Point3::staticFunction()); + out[0] = wrap< double >(Point3::staticFunction()); } void Test_collectorInsertAndMakeBase_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) @@ -299,8 +299,9 @@ void Test_create_MixedPtrs_22(int nargout, mxArray *out[], int nargin, const mxA typedef boost::shared_ptr Shared; checkArguments("create_MixedPtrs",nargout,nargin-1,0); Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); - out[0] = wrap_shared_ptr(SharedTest(new Test(obj->create_MixedPtrs().first)),"Test", false); - out[0] = wrap_shared_ptr(obj->create_MixedPtrs().second,"Test", false); + pair< Test, SharedTest > pairResult = obj->create_MixedPtrs(); + out[0] = wrap_shared_ptr(SharedTest(new Test(pairResult.first)),"Test", false); + out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } void Test_create_ptrs_23(int nargout, mxArray *out[], int nargin, const mxArray *in[]) @@ -310,8 +311,9 @@ void Test_create_ptrs_23(int nargout, mxArray *out[], int nargin, const mxArray typedef boost::shared_ptr Shared; checkArguments("create_ptrs",nargout,nargin-1,0); Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); - out[0] = wrap_shared_ptr(obj->create_ptrs().first,"Test", false); - out[0] = wrap_shared_ptr(obj->create_ptrs().second,"Test", false); + pair< SharedTest, SharedTest > pairResult = obj->create_ptrs(); + out[0] = wrap_shared_ptr(pairResult.first,"Test", false); + out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } void Test_print_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) @@ -413,8 +415,9 @@ void Test_return_pair_34(int nargout, mxArray *out[], int nargin, const mxArray Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); Vector v = unwrap< Vector >(in[1]); Matrix A = unwrap< Matrix >(in[2]); - out[0] = wrap< Vector >(obj->return_pair(v,A).first); - out[1] = wrap< Matrix >(obj->return_pair(v,A).second); + pair< Vector, Matrix > pairResult = obj->return_pair(v,A); + out[0] = wrap< Vector >(pairResult.first); + out[1] = wrap< Matrix >(pairResult.second); } void Test_return_ptrs_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) @@ -426,8 +429,9 @@ void Test_return_ptrs_35(int nargout, mxArray *out[], int nargin, const mxArray Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); boost::shared_ptr p1 = unwrap_shared_ptr< Test >(in[1], "ptr_Test"); boost::shared_ptr p2 = unwrap_shared_ptr< Test >(in[2], "ptr_Test"); - out[0] = wrap_shared_ptr(obj->return_ptrs(p1,p2).first,"Test", false); - out[0] = wrap_shared_ptr(obj->return_ptrs(p1,p2).second,"Test", false); + pair< SharedTest, SharedTest > pairResult = obj->return_ptrs(p1,p2); + out[0] = wrap_shared_ptr(pairResult.first,"Test", false); + out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } void Test_return_size_t_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) diff --git a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp index c75c6a388..d199e59ab 100644 --- a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp +++ b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp @@ -29,42 +29,42 @@ void _deleteAllObjects() std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - for(Collector_ns1ClassA::iterator iter = collector_ns1ClassA.begin(); + { for(Collector_ns1ClassA::iterator iter = collector_ns1ClassA.begin(); iter != collector_ns1ClassA.end(); ) { delete *iter; collector_ns1ClassA.erase(iter++); anyDeleted = true; - } - for(Collector_ns1ClassB::iterator iter = collector_ns1ClassB.begin(); + } } + { for(Collector_ns1ClassB::iterator iter = collector_ns1ClassB.begin(); iter != collector_ns1ClassB.end(); ) { delete *iter; collector_ns1ClassB.erase(iter++); anyDeleted = true; - } - for(Collector_ns2ClassA::iterator iter = collector_ns2ClassA.begin(); + } } + { for(Collector_ns2ClassA::iterator iter = collector_ns2ClassA.begin(); iter != collector_ns2ClassA.end(); ) { delete *iter; collector_ns2ClassA.erase(iter++); anyDeleted = true; - } - for(Collector_ns2ns3ClassB::iterator iter = collector_ns2ns3ClassB.begin(); + } } + { for(Collector_ns2ns3ClassB::iterator iter = collector_ns2ns3ClassB.begin(); iter != collector_ns2ns3ClassB.end(); ) { delete *iter; collector_ns2ns3ClassB.erase(iter++); anyDeleted = true; - } - for(Collector_ns2ClassC::iterator iter = collector_ns2ClassC.begin(); + } } + { for(Collector_ns2ClassC::iterator iter = collector_ns2ClassC.begin(); iter != collector_ns2ClassC.end(); ) { delete *iter; collector_ns2ClassC.erase(iter++); anyDeleted = true; - } - for(Collector_ClassD::iterator iter = collector_ClassD.begin(); + } } + { for(Collector_ClassD::iterator iter = collector_ClassD.begin(); iter != collector_ClassD.end(); ) { delete *iter; collector_ClassD.erase(iter++); anyDeleted = true; - } + } } if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -230,7 +230,7 @@ void ns2ClassA_afunction_12(int nargout, mxArray *out[], int nargin, const mxArr { typedef boost::shared_ptr Shared; checkArguments("ns2ClassA.afunction",nargout,nargin,0); - out[0] = wrap< double >(ns2::ClassA::afunction()); + out[0] = wrap< double >(ns2::ClassA::afunction()); } void ns2ns3ClassB_collectorInsertAndMakeBase_13(int nargout, mxArray *out[], int nargin, const mxArray *in[]) From 32f9ea526d052bb301d65db71d6f949f8c72bf84 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 28 Jul 2012 16:01:49 +0000 Subject: [PATCH 57/68] Removed dataset function from gtsam (moved to CitySLAM, see previous commit) --- gtsam/slam/dataset.cpp | 48 ------------------------------------------ gtsam/slam/dataset.h | 9 -------- 2 files changed, 57 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 59df1da67..29154f686 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -31,54 +31,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -pair > dataset(const string& dataset, - const string& user_path) { - string path = user_path, set = dataset; - boost::optional null_model; - boost::optional identity(noiseModel::Unit::Create(3)); - boost::optional small( - noiseModel::Diagonal::Variances( - gtsam::Vector_(3, 0.0001, 0.0001, 0.0003))); - - if (path.empty()) - path = string(getenv("HOME")) + "/data"; - if (set.empty()) - set = string(getenv("DATASET")); - - if (set == "intel") - return make_pair(path + "/Intel/intel.graph", null_model); - if (set == "intel-gfs") - return make_pair(path + "/Intel/intel.gfs.graph", null_model); - if (set == "Killian-gfs") - return make_pair(path + "/Killian/Killian.gfs.graph", null_model); - if (set == "Killian") - return make_pair(path + "/Killian/Killian.graph", small); - if (set == "Killian-noised") - return make_pair(path + "/Killian/Killian-noised.graph", null_model); - if (set == "3") - return make_pair(path + "/TORO/w3-odom.graph", identity); - if (set == "100") - return make_pair(path + "/TORO/w100-odom.graph", identity); - if (set == "10K") - return make_pair(path + "/TORO/w10000-odom.graph", identity); - if (set == "10K2") - return make_pair(path + "/hogman/data/2D/w10000.graph", - noiseModel::Diagonal::Variances(gtsam::Vector_(3, 0.1, 0.1, 0.05))); - if (set == "Eiffel100") - return make_pair(path + "/TORO/w100-Eiffel.graph", identity); - if (set == "Eiffel10K") - return make_pair(path + "/TORO/w10000-Eiffel.graph", identity); - if (set == "olson") - return make_pair(path + "/Olson/olson06icra.graph", null_model); - if (set == "victoria") - return make_pair(path + "/VictoriaPark/victoria_park.graph", null_model); - if (set == "beijing") - return make_pair(path + "/Beijing/beijingData_trips.graph", null_model); - - return make_pair("unknown", null_model); -} - /* ************************************************************************* */ pair load2D( pair > dataset, diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index d3f00aae2..c881ce2bd 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -24,15 +24,6 @@ #include namespace gtsam { - /** - * Construct dataset filename from short name - * Currently has "Killian" "intel.gfs", "10K", etc... - * @param filename - * @param optional dataset, if empty will try to getenv $DATASET - * @param optional path, if empty will try to getenv $HOME - */ - std::pair > - dataset(const std::string& dataset = "", const std::string& path = ""); /** * Load TORO 2D Graph From 08c72e2a8dc7888e8d8c3e4aa68f2dfbc80276b0 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 30 Jul 2012 14:59:18 +0000 Subject: [PATCH 58/68] Updated Pose2SLAMExample_graph to remove SLAM namespaces --- examples/Pose2SLAMExample_graph.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index d27e184d8..cc873eb08 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -17,32 +17,35 @@ */ #include -#include +#include #include +#include +#include +#include +#include #include #include using namespace std; using namespace gtsam; -using namespace gtsam::noiseModel; int main(int argc, char** argv) { // Read File and create graph and initial estimate // we are in build/examples, data is in examples/Data - pose2SLAM::Graph::shared_ptr graph ; - pose2SLAM::Values::shared_ptr initial; - SharedDiagonal model = Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 5.0*M_PI/180.0)); + NonlinearFactorGraph::shared_ptr graph ; + Values::shared_ptr initial; + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 5.0*M_PI/180.0)); boost::tie(graph,initial) = load2D("../../examples/Data/w100-odom.graph",model); initial->print("Initial estimate:\n"); // Add a Gaussian prior on first poses Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.01, 0.01, 0.01)); - graph->addPosePrior(0, priorMean, priorNoise); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.01, 0.01, 0.01)); + graph->add(PriorFactor(0, priorMean, priorNoise)); // Single Step Optimization using Levenberg-Marquardt - pose2SLAM::Values result = graph->optimize(*initial); + Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); result.print("\nFinal result:\n"); // Plot the covariance of the last pose From 4cf6f104581e651982901b2e04e0770d23a637af Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 30 Jul 2012 15:34:18 +0000 Subject: [PATCH 59/68] Removed SLAM namespace from testAntiFactor --- gtsam/slam/tests/testAntiFactor.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index d99313767..0dd576aed 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -17,10 +17,12 @@ #include -#include #include -#include +#include +#include #include +#include +#include #include #include @@ -95,12 +97,12 @@ TEST( AntiFactor, EquivalentBayesNet) Pose3 z(Rot3(), Point3(1, 1, 1)); SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); - boost::shared_ptr graph(new pose3SLAM::Graph()); - graph->addPosePrior(1, pose1, sigma); - graph->addRelativePose(1, 2, pose1.between(pose2), sigma); + NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph()); + graph->add(PriorFactor(1, pose1, sigma)); + graph->add(BetweenFactor(1, 2, pose1.between(pose2), sigma)); // Create a configuration corresponding to the ground truth - boost::shared_ptr values(new Values()); + Values::shared_ptr values(new Values()); values->insert(1, pose1); values->insert(2, pose2); From c49a2e5933686f21743d79382bac4be5e591b401 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 30 Jul 2012 15:34:42 +0000 Subject: [PATCH 60/68] Removed SLAM namespaces from testProjectionFactor --- gtsam/slam/tests/testProjectionFactor.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index ac9a6891d..1bda97951 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -16,9 +16,13 @@ * @date Nov 2009 */ -#include +#include +#include +#include #include #include +#include +#include #include using namespace std; @@ -80,7 +84,7 @@ TEST( ProjectionFactor, error ) CHECK(assert_equal(expected,*actual,1e-3)); // linearize graph - visualSLAM::Graph graph; + NonlinearFactorGraph graph; graph.push_back(factor); FactorGraph expected_lfg; expected_lfg.push_back(actual); From 094d3952381941a6c6980321daa3e8244bb64f20 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 30 Jul 2012 15:35:19 +0000 Subject: [PATCH 61/68] Removed SLAM namespaces from testStereoFactor --- gtsam/slam/StereoFactor.h | 1 + gtsam/slam/tests/testStereoFactor.cpp | 20 +++++++++----------- 2 files changed, 10 insertions(+), 11 deletions(-) diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 5be783aaf..94af6e10e 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -18,6 +18,7 @@ #pragma once +#include #include namespace gtsam { diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index b38dae990..f6a861354 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -15,13 +15,16 @@ * @author Chris Beall */ -#include #include -#include +#include #include +#include +#include #include #include +#include #include +#include #include @@ -35,8 +38,7 @@ static Pose3 camera1(Matrix_(3,3, ), Point3(0,0,6.25)); -static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); -static StereoCamera stereoCam(Pose3(), K); +static boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); // point X Y Z in meters static Point3 p(0, 0, 5); @@ -46,20 +48,16 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); using symbol_shorthand::X; using symbol_shorthand::L; -typedef GenericStereoFactor MyStereoFactor; - /* ************************************************************************* */ TEST( StereoFactor, singlePoint) { - //Cal3_S2 K(625, 625, 0, 320, 240, 0.5); - boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); NonlinearFactorGraph graph; - graph.add(NonlinearEquality(X(1),camera1)); + graph.add(NonlinearEquality(X(1), camera1)); - StereoPoint2 z14(320,320.0-50, 240); + StereoPoint2 z14(320, 320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph.add(MyStereoFactor(z14,sigma, X(1), L(1), K)); + graph.add(GenericStereoFactor(z14, sigma, X(1), L(1), K)); // Create a configuration corresponding to the ground truth Values values; From 15cfa7e70239045f3fafa7078d47b0b990c431d1 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 30 Jul 2012 15:38:58 +0000 Subject: [PATCH 62/68] Remove namespace files --- gtsam/slam/planarSLAM.cpp | 63 --- gtsam/slam/planarSLAM.h | 122 ------ gtsam/slam/pose2SLAM.cpp | 66 --- gtsam/slam/pose2SLAM.h | 107 ----- gtsam/slam/pose3SLAM.cpp | 73 ---- gtsam/slam/pose3SLAM.h | 120 ------ gtsam/slam/sparseBA.cpp | 60 --- gtsam/slam/sparseBA.h | 143 ------- gtsam/slam/tests/testPlanarSLAM.cpp | 218 ---------- gtsam/slam/tests/testPose2SLAM.cpp | 608 ---------------------------- gtsam/slam/tests/testPose3SLAM.cpp | 211 ---------- gtsam/slam/tests/testSparseBA.cpp | 193 --------- gtsam/slam/tests/testVisualSLAM.cpp | 344 ---------------- gtsam/slam/visualSLAM.cpp | 116 ------ gtsam/slam/visualSLAM.h | 186 --------- 15 files changed, 2630 deletions(-) delete mode 100644 gtsam/slam/planarSLAM.cpp delete mode 100644 gtsam/slam/planarSLAM.h delete mode 100644 gtsam/slam/pose2SLAM.cpp delete mode 100644 gtsam/slam/pose2SLAM.h delete mode 100644 gtsam/slam/pose3SLAM.cpp delete mode 100644 gtsam/slam/pose3SLAM.h delete mode 100644 gtsam/slam/sparseBA.cpp delete mode 100644 gtsam/slam/sparseBA.h delete mode 100644 gtsam/slam/tests/testPlanarSLAM.cpp delete mode 100644 gtsam/slam/tests/testPose2SLAM.cpp delete mode 100644 gtsam/slam/tests/testPose3SLAM.cpp delete mode 100644 gtsam/slam/tests/testSparseBA.cpp delete mode 100644 gtsam/slam/tests/testVisualSLAM.cpp delete mode 100644 gtsam/slam/visualSLAM.cpp delete mode 100644 gtsam/slam/visualSLAM.h diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp deleted file mode 100644 index 20c149b66..000000000 --- a/gtsam/slam/planarSLAM.cpp +++ /dev/null @@ -1,63 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 planarSLAM.cpp - * @brief: bearing/range measurements in 2D plane - * @author Frank Dellaert - **/ - -#include -#include - -// Use planarSLAM namespace for specific SLAM instance -namespace planarSLAM { - - /* ************************************************************************* */ - Matrix Values::points() const { - size_t j=0; - ConstFiltered points = filter(); - Matrix result(points.size(),2); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, points) - result.row(j++) = keyValue.value.vector(); - return result; - } - - /* ************************************************************************* */ - void Graph::addPointConstraint(Key pointKey, const Point2& p) { - push_back(boost::make_shared >(pointKey, p)); - } - - /* ************************************************************************* */ - void Graph::addPointPrior(Key pointKey, const Point2& p, const SharedNoiseModel& model) { - push_back(boost::make_shared >(pointKey, p, model)); - } - - /* ************************************************************************* */ - void Graph::addBearing(Key i, Key j, const Rot2& z, const SharedNoiseModel& model) { - push_back(boost::make_shared >(i, j, z, model)); - } - - /* ************************************************************************* */ - void Graph::addRange(Key i, Key j, double z, const SharedNoiseModel& model) { - push_back(boost::make_shared >(i, j, z, model)); - } - - /* ************************************************************************* */ - void Graph::addBearingRange(Key i, Key j, const Rot2& z1, - double z2, const SharedNoiseModel& model) { - push_back(boost::make_shared >(i, j, z1, z2, model)); - } - - /* ************************************************************************* */ - -} // planarSLAM - diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h deleted file mode 100644 index 967d91581..000000000 --- a/gtsam/slam/planarSLAM.h +++ /dev/null @@ -1,122 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 planarSLAM.h - * @brief bearing/range measurements in 2D plane - * @author Frank Dellaert - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -// Use planarSLAM namespace for specific SLAM instance -namespace planarSLAM { - - using namespace gtsam; - - /* - * Values class, inherited from pose2SLAM::Values, mainly used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Values: public pose2SLAM::Values { - - typedef boost::shared_ptr shared_ptr; - typedef gtsam::Values::ConstFiltered PoseFiltered; - typedef gtsam::Values::ConstFiltered PointFiltered; - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const gtsam::Values& values) : - pose2SLAM::Values(values) { - } - - /// Constructor from filtered values view of poses - Values(const PoseFiltered& view) : pose2SLAM::Values(view) {} - - /// Constructor from filtered values view of points - Values(const PointFiltered& view) : pose2SLAM::Values(view) {} - - PoseFiltered allPoses() const { return this->filter(); } ///< pose view - size_t nrPoses() const { return allPoses().size(); } ///< get number of poses - KeyList poseKeys() const { return allPoses().keys(); } ///< get keys to poses only - - PointFiltered allPoints() const { return this->filter(); } ///< point view - size_t nrPoints() const { return allPoints().size(); } ///< get number of points - KeyList pointKeys() const { return allPoints().keys(); } ///< get keys to points only - - /// insert a point - void insertPoint(Key j, const Point2& point) { insert(j, point); } - - /// update a point - void updatePoint(Key j, const Point2& point) { update(j, point); } - - /// get a point - Point2 point(Key j) const { return at(j); } - - /// get all [x,y] coordinates in a 2*n matrix - Matrix points() const; -}; - - /** - * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Graph: public pose2SLAM::Graph { - - /// Default constructor - Graph(){} - - /// Copy constructor given any other NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph): - pose2SLAM::Graph(graph) {} - - /// Creates a hard constraint on a point - void addPointConstraint(Key j, const Point2& p); - - /// Adds a prior with mean p and given noise model on point j - void addPointPrior(Key j, const Point2& p, const SharedNoiseModel& model); - - /// Creates a bearing measurement from pose i to point j - void addBearing(Key i, Key j, const Rot2& bearing, const SharedNoiseModel& model); - - /// Creates a range measurement from pose i to point j - void addRange(Key i, Key k, double range, const SharedNoiseModel& model); - - /// Creates a range/bearing measurement from pose i to point j - void addBearingRange(Key i, Key j, const Rot2& bearing, double range, const SharedNoiseModel& model); - }; - -} // planarSLAM - - -/** - * Backwards compatibility and wrap use only, avoid using - */ -namespace planarSLAM { - typedef gtsam::NonlinearEquality Constraint; ///< \deprecated typedef for backwards compatibility - typedef gtsam::PriorFactor Prior; ///< \deprecated typedef for backwards compatibility - typedef gtsam::BetweenFactor Odometry; ///< \deprecated typedef for backwards compatibility - typedef gtsam::BearingFactor Bearing; ///< \deprecated typedef for backwards compatibility - typedef gtsam::RangeFactor Range; ///< \deprecated typedef for backwards compatibility - typedef gtsam::BearingRangeFactor BearingRange; ///< \deprecated typedef for backwards compatibility -} - - diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp deleted file mode 100644 index cc549d84a..000000000 --- a/gtsam/slam/pose2SLAM.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 pose2SLAM.cpp - * @brief: Odometry measurements in 2D plane - * @author Frank Dellaert - **/ - -#include - -// Use pose2SLAM namespace for specific SLAM instance - -namespace pose2SLAM { - - /* ************************************************************************* */ - Values 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; - } - - /* ************************************************************************* */ - Matrix Values::poses() const { - size_t j=0; - ConstFiltered poses = filter(); - Matrix result(poses.size(),3); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) { - const Pose2& r = keyValue.value; - result.row(j++) = Matrix_(1,3, r.x(), r.y(), r.theta()); - } - return result; - } - - /* ************************************************************************* */ - void Graph::addPoseConstraint(Key i, const Pose2& p) { - sharedFactor factor(new NonlinearEquality(i, p)); - push_back(factor); - } - - /* ************************************************************************* */ - void Graph::addPosePrior(Key i, const Pose2& p, const SharedNoiseModel& model) { - sharedFactor factor(new PriorFactor(i, p, model)); - push_back(factor); - } - - /* ************************************************************************* */ - void Graph::addRelativePose(Key i1, Key i2, const Pose2& z, - const SharedNoiseModel& model) { - sharedFactor factor(new BetweenFactor(i1, i2, z, model)); - push_back(factor); - } - - /* ************************************************************************* */ - -} // pose2SLAM diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h deleted file mode 100644 index 018090c6a..000000000 --- a/gtsam/slam/pose2SLAM.h +++ /dev/null @@ -1,107 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 pose2SLAM.h - * @brief: 2D Pose SLAM - * @author Frank Dellaert - **/ - -#pragma once - -#include -#include -#include -#include -#include - -// Use pose2SLAM namespace for specific SLAM instance -namespace pose2SLAM { - - using namespace gtsam; - - /* - * Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Values: public gtsam::Values { - - typedef boost::shared_ptr shared_ptr; - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const gtsam::Values& values) : - gtsam::Values(values) { - } - - /** - * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) - * @param n number of poses - * @param R radius of circle - * @param c character to use for keys - * @return circle of n 2D poses - */ - static Values Circle(size_t n, double R); - - /// insert a pose - void insertPose(Key i, const Pose2& pose) { insert(i, pose); } - - /// update a pose - void updatePose(Key i, const Pose2& pose) { update(i, pose); } - - /// get a pose - Pose2 pose(Key i) const { return at(i); } - - /// get all [x,y,theta] coordinates in a 3*m matrix - Matrix poses() const; - }; - - /** - * List of typedefs for factors - */ - - /** - * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Graph: public EasyFactorGraph { - - typedef boost::shared_ptr shared_ptr; - - /// Default constructor - Graph(){} - - /// Copy constructor given any other NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph): - EasyFactorGraph(graph) {} - - /// Creates a hard constraint for key i with the given Pose2 p. - void addPoseConstraint(Key i, const Pose2& p); - - /// Adds a Pose2 prior with mean p and given noise model on pose i - void addPosePrior(Key i, const Pose2& p, const SharedNoiseModel& model); - - /// Creates an relative pose factor between poses with keys i1 and i2 - void addRelativePose(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model); - }; - -} // pose2SLAM - -/** - * Backwards compatibility and wrap use only, avoid using - */ -namespace pose2SLAM { - typedef gtsam::NonlinearEquality HardConstraint; ///< \deprecated typedef for backwards compatibility - typedef gtsam::PriorFactor Prior; ///< \deprecated typedef for backwards compatibility - typedef gtsam::BetweenFactor Odometry; ///< \deprecated typedef for backwards compatibility -} diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp deleted file mode 100644 index 07a89e419..000000000 --- a/gtsam/slam/pose3SLAM.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 pose3SLAM.cpp - * @brief: bearing/range measurements in 2D plane - * @author Frank Dellaert - **/ - -#include -#include - -// Use pose3SLAM namespace for specific SLAM instance -namespace pose3SLAM { - - /* ************************************************************************* */ - Values Values::Circle(size_t n, double radius) { - Values x; - double theta = 0, dtheta = 2 * M_PI / n; - // We use aerospace/navlab convention, X forward, Y right, Z down - // First pose will be at (R,0,0) - // ^y ^ X - // | | - // z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) - // Vehicle at p0 is looking towards y axis (X-axis points towards world y) - Rot3 gR0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); - for (size_t i = 0; i < n; i++, theta += dtheta) { - Point3 gti(radius*cos(theta), radius*sin(theta), 0); - Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down ! - Pose3 gTi(gR0 * _0Ri, gti); - x.insert(i, gTi); - } - return x; - } - - /* ************************************************************************* */ - Matrix Values::translations() const { - size_t j=0; - ConstFiltered poses = filter(); - Matrix result(poses.size(),3); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) - result.row(j++) = keyValue.value.translation().vector(); - return result; - } - - /* ************************************************************************* */ - void Graph::addPoseConstraint(Key i, const Pose3& p) { - sharedFactor factor(new NonlinearEquality(i, p)); - push_back(factor); - } - - /* ************************************************************************* */ - void Graph::addPosePrior(Key i, const Pose3& p, const SharedNoiseModel& model) { - sharedFactor factor(new PriorFactor(i, p, model)); - push_back(factor); - } - - /* ************************************************************************* */ - void Graph::addRelativePose(Key i1, Key i2, const Pose3& z, const SharedNoiseModel& model) { - push_back(boost::make_shared >(i1, i2, z, model)); - } - - /* ************************************************************************* */ - -} // pose3SLAM diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h deleted file mode 100644 index 20ea8b0d5..000000000 --- a/gtsam/slam/pose3SLAM.h +++ /dev/null @@ -1,120 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 pose3SLAM.h - * @brief: 3D Pose SLAM - * @author Frank Dellaert - **/ - -#pragma once - -#include -#include -#include -#include -#include - -/// Use pose3SLAM namespace for specific SLAM instance -namespace pose3SLAM { - - using namespace gtsam; - - /* - * Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Values: public gtsam::Values { - - typedef boost::shared_ptr shared_ptr; - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const gtsam::Values& values) : - gtsam::Values(values) { - } - - /** - * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) - * @param n number of poses - * @param R radius of circle - * @return circle of n 3D poses - */ - static Values Circle(size_t n, double R); - - /// insert a pose - void insertPose(Key i, const Pose3& pose) { insert(i, pose); } - - /// update a pose - void updatePose(Key i, const Pose3& pose) { update(i, pose); } - - /// get a pose - Pose3 pose(Key i) const { return at(i); } - - Matrix translations() const; ///< get all pose translations coordinates in a matrix - }; - - /** - * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Graph: public EasyFactorGraph { - - /// Default constructor - Graph(){} - - /// Copy constructor given any other NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph): - EasyFactorGraph(graph) {} - - /** - * Add a prior on a pose - * @param key variable key of the camera pose - * @param p around which soft prior is defined - * @param model uncertainty model of this prior - */ - void addPosePrior(Key poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)); - - /** - * Add a constraint on a pose (for now, *must* be satisfied in any Values) - * @param key variable key of the camera pose - * @param p to which pose to constrain it to - */ - void addPoseConstraint(Key poseKey, const Pose3& p = Pose3()); - - /** - * Add a relative pose measurement between two poses - * @param x1 variable key of the first camera pose - * @param x2 variable key of the second camera pose - * @param relative pose measurement from x1 to x2 (x1.between(x2)) - * @param model uncertainty model of this measurement - */ - void addRelativePose(Key x1, Key x2, const Pose3& z, const SharedNoiseModel& model); - }; - -} // pose3SLAM - -/** - * Backwards compatibility and wrap use only, avoid using - */ -namespace pose3SLAM { - typedef gtsam::PriorFactor Prior; ///< \deprecated typedef for backwards compatibility - typedef gtsam::BetweenFactor Constraint; ///< \deprecated typedef for backwards compatibility - typedef gtsam::NonlinearEquality HardConstraint; ///< \deprecated typedef for backwards compatibility -} -namespace gtsam { - typedef pose3SLAM::Prior Pose3Prior; ///< \deprecated typedef for backwards compatibility - typedef pose3SLAM::Constraint Pose3Factor; ///< \deprecated typedef for backwards compatibility - typedef pose3SLAM::Graph Pose3Graph; ///< \deprecated typedef for backwards compatibility -} - diff --git a/gtsam/slam/sparseBA.cpp b/gtsam/slam/sparseBA.cpp deleted file mode 100644 index 0ff4e1c64..000000000 --- a/gtsam/slam/sparseBA.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/** - * @file sparseBA.cpp - * @brief - * @date Jul 6, 2012 - * @author Yong-Dian Jian - */ - -#include - -namespace sparseBA { - -/* ************************************************************************* */ -void Graph::addSimpleCameraConstraint(Key cameraKey, const SimpleCamera &camera) { - addCameraConstraint(cameraKey, camera); -} - -/* ************************************************************************* */ -void Graph::addSimpleCameraPrior(Key cameraKey, const SimpleCamera &camera, SharedNoiseModel &model) { - addCameraPrior(cameraKey, camera, model); -} - -/* ************************************************************************* */ -void Graph::addSimpleCameraMeasurement(const Point2 &z, SharedNoiseModel& model, Index cameraKey, Index pointKey) { - addMeasurement(z, model, cameraKey, pointKey); -} - -/* ************************************************************************* */ -Matrix Graph::reprojectionErrors(const Values& values) const { - - // TODO: support the other calibration objects. Now it only works for Cal3_S2. - - typedef GeneralSFMFactor SFMFactor; - typedef GeneralSFMFactor2 SFMFactor2; - - // first count - size_t K = 0, k=0; - BOOST_FOREACH(const sharedFactor& f, *this) - if (boost::dynamic_pointer_cast(f)) ++K; - else if (boost::dynamic_pointer_cast(f)) ++K; - - // now fill - Matrix errors(2,K); - BOOST_FOREACH(const sharedFactor& f, *this) { - boost::shared_ptr p = boost::dynamic_pointer_cast(f); - if (p) { - errors.col(k++) = p->unwhitenedError(values); - continue; - } - - boost::shared_ptr p2 = boost::dynamic_pointer_cast(f); - if (p2) { - errors.col(k++) = p2->unwhitenedError(values); - } - } - return errors; -} -/* ************************************************************************* */ -} - - diff --git a/gtsam/slam/sparseBA.h b/gtsam/slam/sparseBA.h deleted file mode 100644 index 07aaa5817..000000000 --- a/gtsam/slam/sparseBA.h +++ /dev/null @@ -1,143 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 sba.h - * @brief a suite for sparse bundle adjustment - * @date Jul 5, 2012 - * @author ydjian - */ - -#pragma once - -#include -#include -#include - -namespace sparseBA { - - using namespace gtsam; - - /// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper - struct Values: public visualSLAM::Values { - - typedef boost::shared_ptr shared_ptr; - typedef gtsam::Values::ConstFiltered SimpleCameraFiltered; - typedef gtsam::Values::ConstFiltered Cal3_S2Filtered; - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const gtsam::Values& values) : visualSLAM::Values(values) {} - - /// Constructor from filtered values view of poses - Values(const SimpleCameraFiltered& view) : visualSLAM::Values(view) {} - - /// Constructor from filtered values view of points - Values(const PointFiltered& view) : visualSLAM::Values(view) {} - - SimpleCameraFiltered allSimpleCameras() const { return this->filter(); } ///< camera view - size_t nrSimpleCameras() const { return allSimpleCameras().size(); } ///< get number of poses - KeyList simpleCameraKeys() const { return allSimpleCameras().keys(); } ///< get keys to poses only - - /// insert a camera - void insertSimpleCamera(Key j, const SimpleCamera& camera) { insert(j, camera); } - - /// update a camera - void updateSimpleCamera(Key j, const SimpleCamera& camera) { update(j, camera); } - - /// get a camera - SimpleCamera simpleCamera(Key j) const { return at(j); } - }; - - /** - * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Graph: public visualSLAM::Graph { - - /// Default constructor - Graph(){} - - /// Copy constructor given any other NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph): visualSLAM::Graph(graph) {} - - /// check if two graphs are equal - bool equals(const Graph& p, double tol = 1e-9) const { - return NonlinearFactorGraph::equals(p, tol); - } - - /** - * Add a prior on a pose - * @param key variable key of the camera - * @param p around which soft prior is defined - * @param model uncertainty model of this prior - */ - template - void addCameraPrior(Key cameraKey, const Camera &camera, SharedNoiseModel &model = noiseModel::Unit::Create(Camera::Dim())) { - sharedFactor factor(new PriorFactor(cameraKey, camera, model)); - push_back(factor); - } - - /** - * Add a constraint on a camera - * @param key variable key of the camera - * @param p to which camera to constrain it to - */ - template - void addCameraConstraint(Key cameraKey, const Camera &camera) { - sharedFactor factor(new NonlinearEquality(cameraKey, camera)); - push_back(factor); - } - - /** - * Add a 2d projection measurement - * @param z the measurement - * @param model the noise model for the measurement - * @param cameraKey variable key for the pose+calibration - * @param pointKey variable key for the point - */ - template - void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index cameraKey, const Index pointKey) { - typedef GeneralSFMFactor SFMFactor; - boost::shared_ptr factor(new SFMFactor(z, model, cameraKey, pointKey)); - push_back(factor); - } - - /** - * Add a 2d projection measurement, but supports separated (or shared) pose and calibration object - * @param z the measurement - * @param model the noise model for the measurement - * @param poseKey variable key for the pose - * @param pointKey variable key for the point - * @param calibKey variable key for the calibration - */ - template - void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index posekey, const Index pointkey, const Index calibkey) { - typedef GeneralSFMFactor2 SFMFactor; - boost::shared_ptr factor(new SFMFactor(z, model, posekey, pointkey, calibkey)); - push_back(factor); - } - - /// Return a 2*K Matrix of reprojection errors - Matrix reprojectionErrors(const Values& values) const; - - /** - * Matlab-specific wrappers - */ - - void addSimpleCameraPrior(Key cameraKey, const SimpleCamera &camera, SharedNoiseModel &model); - void addSimpleCameraConstraint(Key cameraKey, const SimpleCamera &camera) ; - void addSimpleCameraMeasurement(const Point2 &z, SharedNoiseModel& model, Index cameraKey, Index pointKey); - }; - -} diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp deleted file mode 100644 index 5652b71b4..000000000 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ /dev/null @@ -1,218 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testPlanarSLAM.cpp - * @author Frank Dellaert - **/ - -#include -#include -#include -#include - -#include -#include - -using namespace std; -using namespace gtsam; - -// some shared test values -static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); -static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); -static Symbol i2('x',2), i3('x',3), j3('l',3); - -SharedNoiseModel - sigma(noiseModel::Isotropic::Sigma(1,0.1)), - sigma2(noiseModel::Isotropic::Sigma(2,0.1)), - I3(noiseModel::Unit::Create(3)); - -/* ************************************************************************* */ -TEST( planarSLAM, PriorFactor_equals ) -{ - PriorFactor factor1(i2, x1, I3), factor2(i2, x2, I3); - EXPECT(assert_equal(factor1, factor1, 1e-5)); - EXPECT(assert_equal(factor2, factor2, 1e-5)); - EXPECT(assert_inequal(factor1, factor2, 1e-5)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, BearingFactor ) -{ - // Create factor - Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 - BearingFactor factor(i2, j3, z, sigma); - - // create config - planarSLAM::Values c; - c.insert(i2, x2); - c.insert(j3, l3); - - // Check error - Vector actual = factor.unwhitenedError(c); - EXPECT(assert_equal(Vector_(1,-0.1),actual)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, BearingFactor_equals ) -{ - BearingFactor - factor1(i2, j3, Rot2::fromAngle(0.1), sigma), - factor2(i2, j3, Rot2::fromAngle(2.3), sigma); - EXPECT(assert_equal(factor1, factor1, 1e-5)); - EXPECT(assert_equal(factor2, factor2, 1e-5)); - EXPECT(assert_inequal(factor1, factor2, 1e-5)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, RangeFactor ) -{ - // Create factor - double z(sqrt(2.0) - 0.22); // h(x) - z = 0.22 - RangeFactor factor(i2, j3, z, sigma); - - // create config - planarSLAM::Values c; - c.insert(i2, x2); - c.insert(j3, l3); - - // Check error - Vector actual = factor.unwhitenedError(c); - EXPECT(assert_equal(Vector_(1,0.22),actual)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, RangeFactor_equals ) -{ - RangeFactor factor1(i2, j3, 1.2, sigma), factor2(i2, j3, 7.2, sigma); - EXPECT(assert_equal(factor1, factor1, 1e-5)); - EXPECT(assert_equal(factor2, factor2, 1e-5)); - EXPECT(assert_inequal(factor1, factor2, 1e-5)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, BearingRangeFactor ) -{ - // Create factor - Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 - double b(sqrt(2.0) - 0.22); // h(x) - z = 0.22 - BearingRangeFactor factor(i2, j3, r, b, sigma2); - - // create config - planarSLAM::Values c; - c.insert(i2, x2); - c.insert(j3, l3); - - // Check error - Vector actual = factor.unwhitenedError(c); - EXPECT(assert_equal(Vector_(2,-0.1, 0.22),actual)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, BearingRangeFactor_equals ) -{ - BearingRangeFactor - factor1(i2, j3, Rot2::fromAngle(0.1), 7.3, sigma2), - factor2(i2, j3, Rot2::fromAngle(3), 2.0, sigma2); - EXPECT(assert_equal(factor1, factor1, 1e-5)); - EXPECT(assert_equal(factor2, factor2, 1e-5)); - EXPECT(assert_inequal(factor1, factor2, 1e-5)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, BearingRangeFactor_poses ) -{ - typedef BearingRangeFactor PoseBearingRange; - PoseBearingRange actual(2, 3, Rot2::fromDegrees(60.0), 12.3, sigma2); -} - -/* ************************************************************************* */ -TEST( planarSLAM, PoseConstraint_equals ) -{ - NonlinearEquality factor1(i2, x2), factor2(i2, x3); - EXPECT(assert_equal(factor1, factor1, 1e-5)); - EXPECT(assert_equal(factor2, factor2, 1e-5)); - EXPECT(assert_inequal(factor1, factor2, 1e-5)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, constructor ) -{ - // create config - planarSLAM::Values c; - c.insert(i2, x2); - c.insert(i3, x3); - c.insert(j3, l3); - - // create graph - planarSLAM::Graph G; - - // Add pose constraint - G.addPoseConstraint(i2, x2); // make it feasible :-) - - // Add odometry - G.addRelativePose(i2, i3, Pose2(0, 0, M_PI_4), I3); - - // Create bearing factor - Rot2 z1 = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 - G.addBearing(i2, j3, z1, sigma); - - // Create range factor - double z2(sqrt(2.0) - 0.22); // h(x) - z = 0.22 - G.addRange(i2, j3, z2, sigma); - - Vector expected0 = Vector_(3, 0.0, 0.0, 0.0); - Vector expected1 = Vector_(3, 0.0, 0.0, 0.0); - Vector expected2 = Vector_(1, -0.1); - Vector expected3 = Vector_(1, 0.22); - // Get NoiseModelFactors - EXPECT(assert_equal(expected0, boost::dynamic_pointer_cast(G[0])->unwhitenedError(c))); - EXPECT(assert_equal(expected1, boost::dynamic_pointer_cast(G[1])->unwhitenedError(c))); - EXPECT(assert_equal(expected2, boost::dynamic_pointer_cast(G[2])->unwhitenedError(c))); - EXPECT(assert_equal(expected3, boost::dynamic_pointer_cast(G[3])->unwhitenedError(c))); -} - -/* ************************************************************************* */ -TEST( planarSLAM, keys_and_view ) -{ - // create config - planarSLAM::Values c; - c.insert(i2, x2); - c.insert(i3, x3); - c.insert(j3, l3); - LONGS_EQUAL(2,c.nrPoses()); - LONGS_EQUAL(1,c.nrPoints()); - { - FastList expected, actual; - expected += j3, i2, i3; - actual = c.keys(); - CHECK(expected == actual); - } - { - FastList expected, actual; - expected += i2, i3; - actual = c.poseKeys(); - CHECK(expected == actual); - } - { - FastList expected, actual; - expected += j3; - actual = c.pointKeys(); - CHECK(expected == actual); - } -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp deleted file mode 100644 index 766d06ded..000000000 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ /dev/null @@ -1,608 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testPose2SLAM.cpp - * @author Frank Dellaert, Viorela Ila - **/ - -#include -#include -#include -#include -#include -#include -using namespace gtsam; - -#include - -#include -#include -using namespace boost; -using namespace boost::assign; - -#include -using namespace std; - -typedef BetweenFactor Pose2Factor; - -// common measurement covariance -static double sx=0.5, sy=0.5,st=0.1; -static Matrix cov(Matrix_(3, 3, - sx*sx, 0.0, 0.0, - 0.0, sy*sy, 0.0, - 0.0, 0.0, st*st - )); -static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Gaussian::Covariance(cov)); - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2SLAM, XYT ) -{ - pose2SLAM::Values values; - values.insertPose(1,Pose2(1,2,3)); - values.insertPose(2,Pose2(4,5,6)); - Matrix expected = Matrix_(2,3, 1.0,2.0,3.0, 4.0,5.0,6.0-2*M_PI); - EXPECT(assert_equal(expected,values.poses())); -} - -/* ************************************************************************* */ -// Test constraint, small displacement -Vector f1(const Pose2& pose1, const Pose2& pose2) { - Pose2 z(2.1130913087, 0.468461064817, 0.436332312999); - Pose2Factor constraint(1, 2, z, covariance); - return constraint.evaluateError(pose1, pose2); -} - -TEST_UNSAFE( Pose2SLAM, constraint1 ) -{ - // create a factor between unknown poses p1 and p2 - Pose2 pose1, pose2(2.1130913087, 0.468461064817, 0.436332312999); - Pose2Factor constraint(1, 2, pose2, covariance); - Matrix H1, H2; - Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); - - Matrix numericalH1 = numericalDerivative21(&f1 , pose1, pose2); - EXPECT(assert_equal(numericalH1,H1,5e-3)); - - Matrix numericalH2 = numericalDerivative22(&f1 , pose1, pose2); - EXPECT(assert_equal(numericalH2,H2)); -} - -/* ************************************************************************* */ -// Test constraint, large displacement -Vector f2(const Pose2& pose1, const Pose2& pose2) { - Pose2 z(2,2,M_PI_2); - Pose2Factor constraint(1, 2, z, covariance); - return constraint.evaluateError(pose1, pose2); -} - -TEST_UNSAFE( Pose2SLAM, constraint2 ) -{ - // create a factor between unknown poses p1 and p2 - Pose2 pose1, pose2(2,2,M_PI_2); - Pose2Factor constraint(1, 2, pose2, covariance); - Matrix H1, H2; - Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); - - Matrix numericalH1 = numericalDerivative21(&f2 , pose1, pose2); - EXPECT(assert_equal(numericalH1,H1,5e-3)); - - Matrix numericalH2 = numericalDerivative22(&f2 , pose1, pose2); - EXPECT(assert_equal(numericalH2,H2)); -} - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2SLAM, constructor ) -{ - // create a factor between unknown poses p1 and p2 - Pose2 measured(2,2,M_PI_2); - pose2SLAM::Graph graph; - graph.addRelativePose(1,2,measured, covariance); - // get the size of the graph - size_t actual = graph.size(); - // verify - size_t expected = 1; - CHECK(actual == expected); -} - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2SLAM, linearization ) -{ - // create a factor between unknown poses p1 and p2 - Pose2 measured(2,2,M_PI_2); - Pose2Factor constraint(1, 2, measured, covariance); - pose2SLAM::Graph graph; - graph.addRelativePose(1,2,measured, covariance); - - // Choose a linearization point - Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) - Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2) - pose2SLAM::Values config; - config.insert(1,p1); - config.insert(2,p2); - // Linearize - Ordering ordering(*config.orderingArbitrary()); - boost::shared_ptr > lfg_linearized = graph.linearize(config, ordering); - //lfg_linearized->print("lfg_actual"); - - // the expected linear factor - FactorGraph lfg_expected; - Matrix A1 = Matrix_(3,3, - 0.0,-2.0, -4.2, - 2.0, 0.0, -4.2, - 0.0, 0.0,-10.0); - - Matrix A2 = Matrix_(3,3, - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 10.0); - - Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0); - SharedDiagonal probModel1 = noiseModel::Unit::Create(3); - lfg_expected.push_back(JacobianFactor::shared_ptr(new JacobianFactor(ordering[1], A1, ordering[2], A2, b, probModel1))); - - CHECK(assert_equal(lfg_expected, *lfg_linearized)); -} - -/* ************************************************************************* */ -TEST_UNSAFE(Pose2SLAM, optimize) { - - // create a Pose graph with one equality constraint and one measurement - pose2SLAM::Graph fg; - fg.addPoseConstraint(0, Pose2(0,0,0)); - fg.addRelativePose(0, 1, Pose2(1,2,M_PI_2), covariance); - - // Create initial config - Values initial; - initial.insert(0, Pose2(0,0,0)); - initial.insert(1, Pose2(0,0,0)); - - // Choose an ordering and optimize - Ordering ordering; - ordering += 0, 1; - - LevenbergMarquardtParams params; - params.relativeErrorTol = 1e-15; - params.ordering = ordering; - Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize(); - - // Check with expected config - Values expected; - expected.insert(0, Pose2(0,0,0)); - expected.insert(1, Pose2(1,2,M_PI_2)); - CHECK(assert_equal(expected, actual)); - - // Check marginals - Marginals marginals = fg.marginals(actual); - // Matrix expectedP0 = Infinity, as we have a pose constraint !? - // Matrix actualP0 = marginals.marginalCovariance(0); - // EQUALITY(expectedP0, actualP0); - Matrix expectedP1 = cov; // the second pose really should have just the noise covariance - Matrix actualP1 = marginals.marginalCovariance(1); - EQUALITY(expectedP1, actualP1); -} - -/* ************************************************************************* */ -TEST_UNSAFE(Pose2SLAM, optimizeSPCG) { - - // create a Pose graph with one equality constraint and one measurement - pose2SLAM::Graph fg; - fg.addPosePrior(0, Pose2(0,0,0), noiseModel::Diagonal::Sigmas(Vector_(3,3.0,3.0,1.0))); - fg.addRelativePose(0, 1, Pose2(1,2,M_PI_2), covariance); - - // [Duy] For some unknown reason, SPCG needs this constraint to work. GaussNewton doesn't need this. - fg.addRelativePose(0, 1, Pose2(1,2,M_PI_2), noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1))); - - // Create initial config - Values initial; - initial.insert(0, Pose2(0,0,0)); - initial.insert(1, Pose2(0,0,0)); - - // Optimize using SPCG - Values actual = fg.optimizeSPCG(initial,0); - - // Try GaussNewton without the above constraint to see if the problem is underconstrained. Still works! - Values actual2 = GaussNewtonOptimizer(fg, initial).optimize(); - - // Check with expected config - Values expected; - expected.insert(0, Pose2(0,0,0)); - expected.insert(1, Pose2(1,2,M_PI_2)); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(expected, actual2)); -} - -/* ************************************************************************* */ -// test optimization with 3 poses, note we use plain Keys here, not symbols -TEST_UNSAFE(Pose2SLAM, optimizeThreePoses) { - - // Create a hexagon of poses - pose2SLAM::Values hexagon = pose2SLAM::Values::Circle(3,1.0); - Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1); - - // create a Pose graph with one equality constraint and one measurement - pose2SLAM::Graph fg; - fg.addPoseConstraint(0, p0); - Pose2 delta = p0.between(p1); - fg.addRelativePose(0, 1, delta, covariance); - fg.addRelativePose(1, 2, delta, covariance); - fg.addRelativePose(2, 0, delta, covariance); - - // Create initial config - pose2SLAM::Values initial; - initial.insertPose(0, p0); - initial.insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1))); - initial.insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1))); - - // Choose an ordering - Ordering ordering; - ordering += 0,1,2; - - // optimize - LevenbergMarquardtParams params; - params.relativeErrorTol = 1e-15; - params.ordering = ordering; - Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize(); - - // Check with ground truth - CHECK(assert_equal((const Values&)hexagon, actual)); -} - -/* ************************************************************************* */ -// test optimization with 6 poses arranged in a hexagon and a loop closure -TEST_UNSAFE(Pose2SLAM, optimizeCircle) { - - // Create a hexagon of poses - pose2SLAM::Values hexagon = pose2SLAM::Values::Circle(6,1.0); - Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1); - - // create a Pose graph with one equality constraint and one measurement - pose2SLAM::Graph fg; - fg.addPoseConstraint(0, p0); - Pose2 delta = p0.between(p1); - fg.addRelativePose(0,1, delta, covariance); - fg.addRelativePose(1,2, delta, covariance); - fg.addRelativePose(2,3, delta, covariance); - fg.addRelativePose(3,4, delta, covariance); - fg.addRelativePose(4,5, delta, covariance); - fg.addRelativePose(5,0, delta, covariance); - - // Create initial config - pose2SLAM::Values initial; - initial.insertPose(0, p0); - initial.insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1))); - initial.insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1))); - initial.insertPose(3, hexagon.pose(3).retract(Vector_(3,-0.1, 0.1,-0.1))); - initial.insertPose(4, hexagon.pose(4).retract(Vector_(3, 0.1,-0.1, 0.1))); - initial.insertPose(5, hexagon.pose(5).retract(Vector_(3,-0.1, 0.1,-0.1))); - - // Choose an ordering - Ordering ordering; - ordering += 0,1,2,3,4,5; - - // optimize - LevenbergMarquardtParams params; - params.relativeErrorTol = 1e-15; - params.ordering = ordering; - Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize(); - - // Check with ground truth - CHECK(assert_equal((const Values&)hexagon, actual)); - - // Check loop closure - CHECK(assert_equal(delta, actual.at(5).between(actual.at(0)))); - -// Pose2SLAMOptimizer myOptimizer("3"); - -// Matrix A1 = myOptimizer.a1(); -// LONGS_EQUAL(3, A1.rows()); -// LONGS_EQUAL(17, A1.cols()); // 7 + 7 + 3 -// -// Matrix A2 = myOptimizer.a2(); -// LONGS_EQUAL(3, A1.rows()); -// LONGS_EQUAL(7, A2.cols()); // 7 -// -// Vector b1 = myOptimizer.b1(); -// LONGS_EQUAL(9, b1.size()); // 3 + 3 + 3 -// -// Vector b2 = myOptimizer.b2(); -// LONGS_EQUAL(3, b2.size()); // 3 -// -// // Here, call matlab to -// // A=[A1;A2], b=[b1;b2] -// // R=qr(A1) -// // call pcg on A,b, with preconditioner R -> get x -// -// Vector x = myOptimizer.optimize(); -// LONGS_EQUAL(9, x.size()); // 3 + 3 + 3 -// -// myOptimizer.update(x); -// -// Values expected; -// expected.insert(0, Pose2(0.,0.,0.)); -// expected.insert(1, Pose2(1.,0.,0.)); -// expected.insert(2, Pose2(2.,0.,0.)); -// -// // Check with ground truth -// CHECK(assert_equal(expected, *myOptimizer.theta())); -} - -/* ************************************************************************* */ -TEST_UNSAFE(Pose2SLAM, optimize2) { -// Pose2SLAMOptimizer myOptimizer("100"); -// Matrix A1 = myOptimizer.a1(); -// Matrix A2 = myOptimizer.a2(); -// cout << "A1: " << A1.rows() << " " << A1.cols() << endl; -// cout << "A2: " << A2.rows() << " " << A2.cols() << endl; -// -// //cout << "error: " << myOptimizer.error() << endl; -// for(int i = 0; i<10; i++) { -// myOptimizer.linearize(); -// Vector x = myOptimizer.optimize(); -// myOptimizer.update(x); -// } -// //cout << "error: " << myOptimizer.error() << endl; -// CHECK(myOptimizer.error() < 1.); -} - -///* ************************************************************************* */ -// SL-NEEDED? TEST_UNSAFE(Pose2SLAM, findMinimumSpanningTree) { -// pose2SLAM::Graph G, T, C; -// G.addPoseConstraint(1, 2, Pose2(0.,0.,0.), I3); -// G.addPoseConstraint(1, 3, Pose2(0.,0.,0.), I3); -// G.addPoseConstraint(2, 3, Pose2(0.,0.,0.), I3); -// -// PredecessorMap tree = -// G.findMinimumSpanningTree(); -// CHECK(tree[1] == 1); -// CHECK(tree[2] == 1); -// CHECK(tree[3] == 1); -//} -// -///* ************************************************************************* */ -// SL-NEEDED? TEST_UNSAFE(Pose2SLAM, split) { -// pose2SLAM::Graph G, T, C; -// G.addPoseConstraint(1, 2, Pose2(0.,0.,0.), I3); -// G.addPoseConstraint(1, 3, Pose2(0.,0.,0.), I3); -// G.addPoseConstraint(2, 3, Pose2(0.,0.,0.), I3); -// -// PredecessorMap tree; -// tree.insert(1,2); -// tree.insert(2,2); -// tree.insert(3,2); -// -// G.split(tree, T, C); -// LONGS_EQUAL(2, T.size()); -// LONGS_EQUAL(1, C.size()); -//} - -using namespace pose2SLAM; - -/* ************************************************************************* */ -TEST_UNSAFE(Pose2Values, pose2Circle ) -{ - // expected is 4 poses tangent to circle with radius 1m - pose2SLAM::Values expected; - expected.insert(0, Pose2( 1, 0, M_PI_2)); - expected.insert(1, Pose2( 0, 1, - M_PI )); - expected.insert(2, Pose2(-1, 0, - M_PI_2)); - expected.insert(3, Pose2( 0, -1, 0 )); - - pose2SLAM::Values actual = pose2SLAM::Values::Circle(4,1.0); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST_UNSAFE(Pose2SLAM, expmap ) -{ - // expected is circle shifted to right - pose2SLAM::Values expected; - expected.insert(0, Pose2( 1.1, 0, M_PI_2)); - expected.insert(1, Pose2( 0.1, 1, - M_PI )); - expected.insert(2, Pose2(-0.9, 0, - M_PI_2)); - expected.insert(3, Pose2( 0.1, -1, 0 )); - - // Note expmap coordinates are in local coordinates, so shifting to right requires thought !!! - pose2SLAM::Values circle = pose2SLAM::Values::Circle(4,1.0); - Ordering ordering(*circle.orderingArbitrary()); - VectorValues delta(circle.dims(ordering)); - delta[ordering[0]] = Vector_(3, 0.0,-0.1,0.0); - delta[ordering[1]] = Vector_(3, -0.1,0.0,0.0); - delta[ordering[2]] = Vector_(3, 0.0,0.1,0.0); - delta[ordering[3]] = Vector_(3, 0.1,0.0,0.0); - pose2SLAM::Values actual = circle.retract(delta, ordering); - CHECK(assert_equal(expected,actual)); -} - -// Common measurement covariance -static SharedNoiseModel sigmas = noiseModel::Diagonal::Sigmas(Vector_(3,sx,sy,st)); - -/* ************************************************************************* */ -// Very simple test establishing Ax-b \approx z-h(x) -TEST_UNSAFE( Pose2Prior, error ) -{ - // Choose a linearization point - Pose2 p1(1, 0, 0); // robot at (1,0) - pose2SLAM::Values x0; - x0.insert(1, p1); - - // Create factor - PriorFactor factor(1, p1, sigmas); - - // Actual linearization - Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr linear = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - - // Check error at x0, i.e. delta = zero ! - VectorValues delta(VectorValues::Zero(x0.dims(ordering))); - delta[ordering[1]] = zero(3); - Vector error_at_zero = Vector_(3,0.0,0.0,0.0); - CHECK(assert_equal(error_at_zero,factor.whitenedError(x0))); - CHECK(assert_equal(-error_at_zero,linear->error_vector(delta))); - - // Check error after increasing p2 - VectorValues addition(VectorValues::Zero(x0.dims(ordering))); - addition[ordering[1]] = Vector_(3, 0.1, 0.0, 0.0); - VectorValues plus = delta + addition; - pose2SLAM::Values x1 = x0.retract(plus, ordering); - Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! - CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); - CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); -} - -/* ************************************************************************* */ -// common Pose2Prior for tests below -static gtsam::Pose2 priorVal(2,2,M_PI_2); -static PriorFactor priorFactor(1, priorVal, sigmas); - -/* ************************************************************************* */ -// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector -// Hence i.e., b = approximates z-h(x0) = error_vector(x0) -LieVector hprior(const Pose2& p1) { - return LieVector(sigmas->whiten(priorFactor.evaluateError(p1))); -} - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2Prior, linearize ) -{ - // Choose a linearization point at ground truth - pose2SLAM::Values x0; - x0.insertPose(1, priorVal); - - // Actual linearization - Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr actual = - boost::dynamic_pointer_cast(priorFactor.linearize(x0, ordering)); - - // Test with numerical derivative - Matrix numericalH = numericalDerivative11(hprior, priorVal); - CHECK(assert_equal(numericalH,actual->getA(actual->find(ordering[1])))); -} - -/* ************************************************************************* */ -// Very simple test establishing Ax-b \approx z-h(x) -TEST_UNSAFE( Pose2Factor, error ) -{ - // Choose a linearization point - Pose2 p1; // robot at origin - Pose2 p2(1, 0, 0); // robot at (1,0) - pose2SLAM::Values x0; - x0.insertPose(1, p1); - x0.insertPose(2, p2); - - // Create factor - Pose2 z = p1.between(p2); - Pose2Factor factor(1, 2, z, covariance); - - // Actual linearization - Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr linear = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - - // Check error at x0, i.e. delta = zero ! - VectorValues delta(x0.dims(ordering)); - delta[ordering[1]] = zero(3); - delta[ordering[2]] = zero(3); - Vector error_at_zero = Vector_(3,0.0,0.0,0.0); - CHECK(assert_equal(error_at_zero,factor.unwhitenedError(x0))); - CHECK(assert_equal(-error_at_zero, linear->error_vector(delta))); - - // Check error after increasing p2 - VectorValues plus = delta; - plus[ordering[2]] = Vector_(3, 0.1, 0.0, 0.0); - pose2SLAM::Values x1 = x0.retract(plus, ordering); - Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! - CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); - CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); -} - -/* ************************************************************************* */ -// common Pose2Factor for tests below -static Pose2 measured(2,2,M_PI_2); -static Pose2Factor factor(1,2,measured, covariance); - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2Factor, rhs ) -{ - // Choose a linearization point - Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) - Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4) - pose2SLAM::Values x0; - x0.insert(1,p1); - x0.insert(2,p2); - - // Actual linearization - Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr linear = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - - // Check RHS - Pose2 hx0 = p1.between(p2); - CHECK(assert_equal(Pose2(2.1, 2.1, M_PI_2),hx0)); - Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0); - CHECK(assert_equal(expected_b,-factor.whitenedError(x0))); - CHECK(assert_equal(expected_b,linear->getb())); -} - -/* ************************************************************************* */ -// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector -// Hence i.e., b = approximates z-h(x0) = error_vector(x0) -LieVector h(const Pose2& p1,const Pose2& p2) { - return LieVector(covariance->whiten(factor.evaluateError(p1,p2))); -} - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2Factor, linearize ) -{ - // Choose a linearization point at ground truth - Pose2 p1(1,2,M_PI_2); - Pose2 p2(-1,4,M_PI); - pose2SLAM::Values x0; - x0.insert(1,p1); - x0.insert(2,p2); - - // expected linearization - Matrix expectedH1 = covariance->Whiten(Matrix_(3,3, - 0.0,-1.0,-2.0, - 1.0, 0.0,-2.0, - 0.0, 0.0,-1.0 - )); - Matrix expectedH2 = covariance->Whiten(Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0 - )); - Vector expected_b = Vector_(3, 0.0, 0.0, 0.0); - - // expected linear factor - Ordering ordering(*x0.orderingArbitrary()); - SharedDiagonal probModel1 = noiseModel::Unit::Create(3); - JacobianFactor expected(ordering[1], expectedH1, ordering[2], expectedH2, expected_b, probModel1); - - // Actual linearization - boost::shared_ptr actual = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - CHECK(assert_equal(expected,*actual)); - - // Numerical do not work out because BetweenFactor is approximate ? - Matrix numericalH1 = numericalDerivative21(h, p1, p2); - CHECK(assert_equal(expectedH1,numericalH1)); - Matrix numericalH2 = numericalDerivative22(h, p1, p2); - CHECK(assert_equal(expectedH2,numericalH2)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp deleted file mode 100644 index 23367bd8a..000000000 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ /dev/null @@ -1,211 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testpose3SLAM.cpp - * @author Frank Dellaert - * @author Viorela Ila - **/ - -#include -#include -#include - -#include - -#include -#include -#include -using namespace boost; -using namespace boost::assign; - -#include - -using namespace std; -using namespace gtsam; - -// common measurement covariance -static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Unit::Create(6)); - -const double tol=1e-5; - -/* ************************************************************************* */ -// test optimization with 6 poses arranged in a hexagon and a loop closure -TEST(Pose3Graph, optimizeCircle) { - - // Create a hexagon of poses - double radius = 10; - Values hexagon = pose3SLAM::Values::Circle(6,radius); - Pose3 gT0 = hexagon.at(0), gT1 = hexagon.at(1); - - // create a Pose graph with one equality constraint and one measurement - pose3SLAM::Graph fg; - fg.addPoseConstraint(0, gT0); - Pose3 _0T1 = gT0.between(gT1); // inv(gT0)*gT1 - double theta = M_PI/3.0; - CHECK(assert_equal(Pose3(Rot3::yaw(-theta),Point3(radius*sin(theta),-radius*cos(theta),0)),_0T1)); - fg.addRelativePose(0,1, _0T1, covariance); - fg.addRelativePose(1,2, _0T1, covariance); - fg.addRelativePose(2,3, _0T1, covariance); - fg.addRelativePose(3,4, _0T1, covariance); - fg.addRelativePose(4,5, _0T1, covariance); - fg.addRelativePose(5,0, _0T1, covariance); - - // Create initial config - Values initial; - initial.insert(0, gT0); - initial.insert(1, hexagon.at(1).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial.insert(2, hexagon.at(2).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial.insert(3, hexagon.at(3).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial.insert(4, hexagon.at(4).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial.insert(5, hexagon.at(5).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - - // Choose an ordering and optimize - Ordering ordering; - ordering += 0,1,2,3,4,5; - - Values actual = LevenbergMarquardtOptimizer(fg, initial, ordering).optimize(); - - // Check with ground truth - CHECK(assert_equal(hexagon, actual,1e-4)); - - // Check loop closure - CHECK(assert_equal(_0T1, actual.at(5).between(actual.at(0)),1e-5)); -} - -/* ************************************************************************* */ -TEST(Pose3Graph, partial_prior_height) { - typedef PartialPriorFactor Partial; - - // reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3) - - // height prior - single element interface - double exp_height = 5.0; - SharedDiagonal model = noiseModel::Unit::Create(1); - Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height)); - Partial height(1, 5, exp_height, model); - Matrix actA; - EXPECT(assert_equal(Vector_(1,-2.0), height.evaluateError(init, actA), tol)); - Matrix expA = Matrix_(1, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); - EXPECT(assert_equal(expA, actA)); - - pose3SLAM::Graph graph; - graph.add(height); - - Values values; - values.insert(1, init); - - // linearization - EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol); - - Values actual = LevenbergMarquardtOptimizer(graph, values).optimize(); - EXPECT(assert_equal(expected, actual.at(1), tol)); - EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); -} - -/* ************************************************************************* */ -TEST( Pose3Factor, error ) -{ - // Create example - Pose3 t1; // origin - Pose3 t2(Rot3::rodriguez(0.1,0.2,0.3),Point3(0,1,0)); - Pose3 z(Rot3::rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));; - - // Create factor - SharedNoiseModel I6(noiseModel::Unit::Create(6)); - BetweenFactor factor(1, 2, z, I6); - - // Create config - Values x; - x.insert(1,t1); - x.insert(2,t2); - - // Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2)) - Vector actual = factor.unwhitenedError(x); - Vector expected = z.localCoordinates(t1.between(t2)); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST(Pose3Graph, partial_prior_xy) { - typedef PartialPriorFactor Partial; - - // XY prior - full mask interface - Vector exp_xy = Vector_(2, 3.0, 4.0); - SharedDiagonal model = noiseModel::Unit::Create(2); - Pose3 init(Rot3(), Point3(1.0,-2.0, 3.0)), expected(Rot3(), Point3(3.0, 4.0, 3.0)); - vector mask; mask += 3, 4; - Partial priorXY(1, mask, exp_xy, model); - Matrix actA; - EXPECT(assert_equal(Vector_(2,-2.0,-6.0), priorXY.evaluateError(init, actA), tol)); - Matrix expA = Matrix_(2, 6, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0, 0.0); - EXPECT(assert_equal(expA, actA)); - - pose3SLAM::Graph graph; - graph.add(priorXY); - - Values values; - values.insert(1, init); - - Values actual = LevenbergMarquardtOptimizer(graph, values).optimize(); - EXPECT(assert_equal(expected, actual.at(1), tol)); - EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); -} - -// The world coordinate system has z pointing up, y north, x east -// The vehicle has X forward, Y right, Z down -Rot3 R1(Point3( 0, 1, 0), Point3( 1, 0, 0), Point3(0, 0, -1)); -Rot3 R2(Point3(-1, 0, 0), Point3( 0, 1, 0), Point3(0, 0, -1)); -Rot3 R3(Point3( 0,-1, 0), Point3(-1, 0, 0), Point3(0, 0, -1)); -Rot3 R4(Point3( 1, 0, 0), Point3( 0,-1, 0), Point3(0, 0, -1)); - -/* ************************************************************************* */ -TEST( Values, pose3Circle ) -{ - // expected is 4 poses tangent to circle with radius 1m - Values expected; - expected.insert(0, Pose3(R1, Point3( 1, 0, 0))); - expected.insert(1, Pose3(R2, Point3( 0, 1, 0))); - expected.insert(2, Pose3(R3, Point3(-1, 0, 0))); - expected.insert(3, Pose3(R4, Point3( 0,-1, 0))); - - Values actual = pose3SLAM::Values::Circle(4,1.0); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( Values, expmap ) -{ - Values expected; - expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0))); - expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0))); - expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0))); - expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0))); - - Ordering ordering(*expected.orderingArbitrary()); - VectorValues delta(expected.dims(ordering)); - delta.vector() = Vector_(24, - 0.0,0.0,0.0, 0.1, 0.0, 0.0, - 0.0,0.0,0.0, 0.1, 0.0, 0.0, - 0.0,0.0,0.0, 0.1, 0.0, 0.0, - 0.0,0.0,0.0, 0.1, 0.0, 0.0); - Values actual = pose3SLAM::Values::Circle(4,1.0).retract(delta, ordering); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testSparseBA.cpp b/gtsam/slam/tests/testSparseBA.cpp deleted file mode 100644 index cc6e11400..000000000 --- a/gtsam/slam/tests/testSparseBA.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testsparseBA.cpp - * @brief - * @date Jul 5, 2012 - * @author Yong-Dian Jian - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace boost; -using namespace gtsam; - -/* ************************************************************************* */ - -static SharedNoiseModel sigma(noiseModel::Unit::Create(2)); - -// Convenience for named keys -using symbol_shorthand::X; /* pose3 */ -using symbol_shorthand::K; /* calibration */ -using symbol_shorthand::C; /* camera = [pose calibration] */ -using symbol_shorthand::L; /* point3 */ - -static Point3 landmark1(-1.0,-1.0, 0.0); -static Point3 landmark2(-1.0, 1.0, 0.0); -static Point3 landmark3( 1.0, 1.0, 0.0); -static Point3 landmark4( 1.0,-1.0, 0.0); - -static Pose3 pose1(Matrix_(3,3, - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1.), - Point3(0,0,6.25)); - -static Pose3 pose2(Matrix_(3,3, - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1.), - Point3(0,0,5.00)); - -static Cal3_S2 calib1 (625, 625, 0, 0, 0); -static Cal3_S2 calib2 (625, 625, 0, 0, 0); - - -typedef PinholeCamera Camera; - -static Camera camera1(pose1, calib1); -static Camera camera2(pose2, calib2); - -/* ************************************************************************* */ -sparseBA::Graph testGraph1() { - Point2 z11(-100, 100); - Point2 z12(-100,-100); - Point2 z13( 100,-100); - Point2 z14( 100, 100); - Point2 z21(-125, 125); - Point2 z22(-125,-125); - Point2 z23( 125,-125); - Point2 z24( 125, 125); - - - sparseBA::Graph g; - g.addMeasurement(z11, sigma, C(1), L(1)); - g.addMeasurement(z12, sigma, C(1), L(2)); - g.addMeasurement(z13, sigma, C(1), L(3)); - g.addMeasurement(z14, sigma, C(1), L(4)); - g.addMeasurement(z21, sigma, C(2), L(1)); - g.addMeasurement(z22, sigma, C(2), L(2)); - g.addMeasurement(z23, sigma, C(2), L(3)); - g.addMeasurement(z24, sigma, C(2), L(4)); - return g; -} - -sparseBA::Graph testGraph2() { - Point2 z11(-100, 100); - Point2 z12(-100,-100); - Point2 z13( 100,-100); - Point2 z14( 100, 100); - Point2 z21(-125, 125); - Point2 z22(-125,-125); - Point2 z23( 125,-125); - Point2 z24( 125, 125); - - sparseBA::Graph g; - g.addMeasurement(z11, sigma, X(1), L(1), K(1)); - g.addMeasurement(z12, sigma, X(1), L(2), K(1)); - g.addMeasurement(z13, sigma, X(1), L(3), K(1)); - g.addMeasurement(z14, sigma, X(1), L(4), K(1)); - g.addMeasurement(z21, sigma, X(2), L(1), K(1)); - g.addMeasurement(z22, sigma, X(2), L(2), K(1)); - g.addMeasurement(z23, sigma, X(2), L(3), K(1)); - g.addMeasurement(z24, sigma, X(2), L(4), K(1)); - return g; -} - -/* ************************************************************************* */ -TEST( optimizeLM1, sparseBA ) -{ - // build a graph - sparseBA::Graph graph(testGraph1()); - - // add 3 landmark constraints - graph.addPointConstraint(L(1), landmark1); - graph.addPointConstraint(L(2), landmark2); - graph.addPointConstraint(L(3), landmark3); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(C(1), camera1); - initialEstimate.insert(C(2), camera2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - - // Create an ordering of the variables - Ordering ordering; - ordering += L(1),L(2),L(3),L(4),C(1),C(2); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); -} - -/* ************************************************************************* */ -TEST( optimizeLM2, sparseBA ) -{ - // build a graph - sparseBA::Graph graph(testGraph2()); - - // add 3 landmark constraints - graph.addPointConstraint(L(1), landmark1); - graph.addPointConstraint(L(2), landmark2); - graph.addPointConstraint(L(3), landmark3); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(X(1), pose1); - initialEstimate.insert(X(2), pose2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - initialEstimate.insert(K(1), calib2); - - // Create an ordering of the variables - Ordering ordering; - ordering += L(1),L(2),L(3),L(4),X(1),X(2),K(1); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testVisualSLAM.cpp b/gtsam/slam/tests/testVisualSLAM.cpp deleted file mode 100644 index 65c31adf9..000000000 --- a/gtsam/slam/tests/testVisualSLAM.cpp +++ /dev/null @@ -1,344 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testVisualSLAM.cpp - * @brief Unit test for two cameras and four landmarks, single camera - * @author Chris Beall - * @author Frank Dellaert - * @author Viorela Ila - */ - -#include -#include -#include -#include - -#include -#include -using namespace boost; - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ - -static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::L; - -static Point3 landmark1(-1.0,-1.0, 0.0); -static Point3 landmark2(-1.0, 1.0, 0.0); -static Point3 landmark3( 1.0, 1.0, 0.0); -static Point3 landmark4( 1.0,-1.0, 0.0); - -static Pose3 pose1(Matrix_(3,3, - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ), - Point3(0,0,6.25)); - -static Pose3 pose2(Matrix_(3,3, - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ), - Point3(0,0,5.00)); - -/* ************************************************************************* */ -visualSLAM::Graph testGraph() { - Point2 z11(-100, 100); - Point2 z12(-100,-100); - Point2 z13( 100,-100); - Point2 z14( 100, 100); - Point2 z21(-125, 125); - Point2 z22(-125,-125); - Point2 z23( 125,-125); - Point2 z24( 125, 125); - - shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0)); - visualSLAM::Graph g; - g.addMeasurement(z11, sigma, X(1), L(1), sK); - g.addMeasurement(z12, sigma, X(1), L(2), sK); - g.addMeasurement(z13, sigma, X(1), L(3), sK); - g.addMeasurement(z14, sigma, X(1), L(4), sK); - g.addMeasurement(z21, sigma, X(2), L(1), sK); - g.addMeasurement(z22, sigma, X(2), L(2), sK); - g.addMeasurement(z23, sigma, X(2), L(3), sK); - g.addMeasurement(z24, sigma, X(2), L(4), sK); - return g; -} - -/* ************************************************************************* */ -TEST( VisualSLAM, optimizeLM) -{ - // build a graph - visualSLAM::Graph graph(testGraph()); - // add 3 landmark constraints - graph.addPointConstraint(L(1), landmark1); - graph.addPointConstraint(L(2), landmark2); - graph.addPointConstraint(L(3), landmark3); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(X(1), pose1); - initialEstimate.insert(X(2), pose2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - - // Create an ordering of the variables - Ordering ordering; - ordering += L(1),L(2),L(3),L(4),X(1),X(2); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); -} - - -/* ************************************************************************* */ -TEST( VisualSLAM, optimizeLM2) -{ - // build a graph - visualSLAM::Graph graph(testGraph()); - // add 2 camera constraints - graph.addPoseConstraint(X(1), pose1); - graph.addPoseConstraint(X(2), pose2); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(X(1), pose1); - initialEstimate.insert(X(2), pose2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - - // Create an ordering of the variables - Ordering ordering; - ordering += L(1),L(2),L(3),L(4),X(1),X(2); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, LMoptimizer) -{ - // build a graph - visualSLAM::Graph graph(testGraph()); - // add 2 camera constraints - graph.addPoseConstraint(X(1), pose1); - graph.addPoseConstraint(X(2), pose2); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(X(1), pose1); - initialEstimate.insert(X(2), pose2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer = graph.optimizer(initialEstimate); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); - - // check errors - Matrix errors = graph.reprojectionErrors(optimizer.values()); - CHECK(assert_equal(zeros(2,8), errors)); -} - - -/* ************************************************************************* */ -TEST( VisualSLAM, CHECK_ORDERING) -{ - // build a graph - visualSLAM::Graph graph = testGraph(); - // add 2 camera constraints - graph.addPoseConstraint(X(1), pose1); - graph.addPoseConstraint(X(2), pose2); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(X(1), pose1); - initialEstimate.insert(X(2), pose2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, update_with_large_delta) { - // this test ensures that if the update for delta is larger than - // the size of the config, it only updates existing variables - Values init; - init.insert(X(1), Pose3()); - init.insert(L(1), Point3(1.0, 2.0, 3.0)); - - Values expected; - expected.insert(X(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); - expected.insert(L(1), Point3(1.1, 2.1, 3.1)); - - Ordering largeOrdering; - Values largeValues = init; - largeValues.insert(X(2), Pose3()); - largeOrdering += X(1),L(1),X(2); - VectorValues delta(largeValues.dims(largeOrdering)); - delta[largeOrdering[X(1)]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1); - delta[largeOrdering[L(1)]] = Vector_(3, 0.1, 0.1, 0.1); - delta[largeOrdering[X(2)]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1); - Values actual = init.retract(delta, largeOrdering); - - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, dataAssociation) { - visualSLAM::ISAM isam; - // add two landmarks - // add two cameras and constraint and odometry -// std::pair actualJoint = isam.jointPrediction(); // 4*4 - // std::pair actualMarginals = isam.individualPredictions(); // 2 times 2*2 - // std::pair actualChowLiu = isam.chowLiuPredictions(); // 2 times 2*2 -} - -/* ************************************************************************* */ -TEST( VisualSLAM, filteredValues) { - visualSLAM::Values full; - full.insert(X(1), Pose3(Pose2(1.0, 2.0, 0.3))); - full.insert(L(1), Point3(1.0, 2.0, 3.0)); - - visualSLAM::Values actPoses(full.allPoses()); - visualSLAM::Values expPoses; expPoses.insert(X(1), Pose3(Pose2(1.0, 2.0, 0.3))); - EXPECT(assert_equal(expPoses, actPoses)); - - visualSLAM::Values actPoints(full.allPoints()); - visualSLAM::Values expPoints; expPoints.insert(L(1), Point3(1.0, 2.0, 3.0)); - EXPECT(assert_equal(expPoints, actPoints)); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, keys_and_view ) -{ - // create config - visualSLAM::Values c; - c.insert(X(1), pose1); - c.insert(X(2), pose2); - c.insert(L(2), landmark2); - LONGS_EQUAL(2,c.nrPoses()); - LONGS_EQUAL(1,c.nrPoints()); - { - FastList expected, actual; - expected += L(2), X(1), X(2); - actual = c.keys(); - CHECK(expected == actual); - } - { - FastList expected, actual; - expected += X(1), X(2); - actual = c.poseKeys(); - CHECK(expected == actual); - } - { - FastList expected, actual; - expected += L(2); - actual = c.pointKeys(); - CHECK(expected == actual); - } -} - -/* ************************************************************************* */ -TEST( VisualSLAM, addMeasurements ) -{ - // create config - visualSLAM::Graph g; - Vector J = Vector_(3,1.0,2.0,3.0); - Matrix Z = Matrix_(2,3, -1.0,0.0,1.0, -1.0,0.0,1.0); - shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0)); - g.addMeasurements(0,J,Z,sigma,sK); - EXPECT_LONGS_EQUAL(3,g.size()); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, insertBackProjections ) -{ - // create config - visualSLAM::Values c; - SimpleCamera camera(pose1); - Vector J = Vector_(3,1.0,2.0,3.0); - Matrix Z = Matrix_(2,3, -1.0,0.0,1.0, -1.0,0.0,1.0); - c.insertBackprojections(camera,J,Z,1.0); - EXPECT_LONGS_EQUAL(3,c.nrPoints()); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, perturbPoints ) -{ - visualSLAM::Values c1,c2; - c1.insert(L(2), landmark2); - c1.perturbPoints(0.01,42u); - CHECK(assert_equal(Point3(-0.986984, 0.999534, -0.0147962),c1.point(L(2)),1e-6)); - c2.insert(L(2), landmark2); - c2.perturbPoints(0.01,42u); - CHECK(assert_equal(Point3(-0.986984, 0.999534, -0.0147962),c2.point(L(2)),1e-6)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp deleted file mode 100644 index 2133c0ee8..000000000 --- a/gtsam/slam/visualSLAM.cpp +++ /dev/null @@ -1,116 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 visualSLAM.cpp - * @date Jan 14, 2010 - * @author richard - */ - -#include -#include -#include -#include -#include - -namespace visualSLAM { - - using boost::make_shared; - - /* ************************************************************************* */ - void Values::insertBackprojections(const SimpleCamera& camera, - const Vector& J, const Matrix& Z, double depth) { - if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); - if (Z.cols() != J.size()) throw std::invalid_argument( - "insertBackProjections: J and Z must have same number of entries"); - for(int k=0;k points = allPoints(); - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); - Sampler sampler(model, seed); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, points) { - update(keyValue.key, keyValue.value.retract(sampler.sample())); - } - } - - /* ************************************************************************* */ - Matrix Values::points() const { - size_t j=0; - ConstFiltered points = allPoints(); - Matrix result(points.size(),3); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, points) - result.row(j++) = keyValue.value.vector(); - return result; - } - - /* ************************************************************************* */ - void Graph::addPointConstraint(Key pointKey, const Point3& p) { - push_back(make_shared >(pointKey, p)); - } - - /* ************************************************************************* */ - void Graph::addPointPrior(Key pointKey, const Point3& p, const SharedNoiseModel& model) { - push_back(make_shared >(pointKey, p, model)); - } - - /* ************************************************************************* */ - void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model, - Key poseKey, Key pointKey, const shared_ptrK K) { - push_back( - make_shared > - (measured, model, poseKey, pointKey, K)); - } - - /* ************************************************************************* */ - void Graph::addMeasurements(Key i, const Vector& J, const Matrix& Z, - const SharedNoiseModel& model, const shared_ptrK K) { - if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K"); - if (Z.cols() != J.size()) throw std::invalid_argument( - "addMeasurements: J and Z must have same number of entries"); - for (int k = 0; k < Z.cols(); k++) - addMeasurement(Point2(Z(0, k), Z(1, k)), model, i, J(k), K); - } - - /* ************************************************************************* */ - void Graph::addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model, - Key poseKey, Key pointKey, const shared_ptrKStereo K) { - push_back(make_shared >(measured, model, poseKey, pointKey, K)); - } - - /* ************************************************************************* */ - void Graph::addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model) { - push_back(make_shared >(poseKey, pointKey, range, model)); - } - - /* ************************************************************************* */ - Matrix Graph::reprojectionErrors(const Values& values) const { - // first count - size_t K = 0, k=0; - BOOST_FOREACH(const sharedFactor& f, *this) - if (boost::dynamic_pointer_cast(f)) ++K; - // now fill - Matrix errors(2,K); - BOOST_FOREACH(const sharedFactor& f, *this) { - boost::shared_ptr p = - boost::dynamic_pointer_cast(f); - if (p) errors.col(k++) = p->unwhitenedError(values); - } - return errors; - } - /* ************************************************************************* */ -} diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h deleted file mode 100644 index ae2956c5f..000000000 --- a/gtsam/slam/visualSLAM.h +++ /dev/null @@ -1,186 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 visualSLAM.h - * @brief Basic typedefs for general VisualSLAM problems. Useful for monocular and stereo systems. - * @date Jan 14, 2010 - * @author Richard Roberts - * @author Chris Beall - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace visualSLAM { - - using namespace gtsam; - - /// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper - struct Values: public pose3SLAM::Values { - - typedef boost::shared_ptr shared_ptr; - typedef gtsam::Values::ConstFiltered PoseFiltered; - typedef gtsam::Values::ConstFiltered PointFiltered; - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const gtsam::Values& values) : - pose3SLAM::Values(values) { - } - - /// Constructor from filtered values view of poses - Values(const PoseFiltered& view) : pose3SLAM::Values(view) {} - - /// Constructor from filtered values view of points - Values(const PointFiltered& view) : pose3SLAM::Values(view) {} - - PoseFiltered allPoses() const { return this->filter(); } ///< pose view - size_t nrPoses() const { return allPoses().size(); } ///< get number of poses - KeyList poseKeys() const { return allPoses().keys(); } ///< get keys to poses only - - PointFiltered allPoints() const { return this->filter(); } ///< point view - size_t nrPoints() const { return allPoints().size(); } ///< get number of points - KeyList pointKeys() const { return allPoints().keys(); } ///< get keys to points only - - /// insert a point - void insertPoint(Key j, const Point3& point) { insert(j, point); } - - /// update a point - void updatePoint(Key j, const Point3& point) { update(j, point); } - - /// get a point - Point3 point(Key j) const { return at(j); } - - /// insert a number of initial point values by backprojecting - void insertBackprojections(const SimpleCamera& c, const Vector& J, - const Matrix& Z, double depth); - - /// perturb all points using normally distributed noise - void perturbPoints(double sigma, int32_t seed = 42u); - - /// get all point coordinates in a matrix - Matrix points() const; - - }; - - /** - * Non-linear factor graph for vanilla visual SLAM - */ - struct Graph: public pose3SLAM::Graph { - - /// shared pointer to this type of graph - typedef boost::shared_ptr shared_graph; - - /// Default constructor - Graph(){} - - /// Copy constructor given any other NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph): - pose3SLAM::Graph(graph) {} - - /// check if two graphs are equal - bool equals(const Graph& p, double tol = 1e-9) const { - return NonlinearFactorGraph::equals(p, tol); - } - - /** - * Add a constraint on a point (for now, *must* be satisfied in any Values) - * @param key variable key of the point - * @param p point around which soft prior is defined - */ - void addPointConstraint(Key pointKey, const Point3& p = Point3()); - - /** - * Add a prior on a point - * @param key variable key of the point - * @param p to which point to constrain it to - * @param model uncertainty model of this prior - */ - void addPointPrior(Key pointKey, const Point3& p = Point3(), - const SharedNoiseModel& model = noiseModel::Unit::Create(3)); - - /** - * Add a range prior to a point - * @param poseKey variable key of the camera pose - * @param pointKey variable key of the point - * @param range approximate range to point - * @param model uncertainty model of this prior - */ - void addRangeFactor(Key poseKey, Key pointKey, double range, - const SharedNoiseModel& model = noiseModel::Unit::Create(1)); - - /** - * Add a projection factor measurement (monocular) - * @param measured the measurement - * @param model the noise model for the measurement - * @param poseKey variable key for the camera pose - * @param pointKey variable key for the point - * @param K shared pointer to calibration object - */ - void addMeasurement(const Point2& measured, const SharedNoiseModel& model, - Key poseKey, Key pointKey, const shared_ptrK K); - - /** - * Add a number of measurements at the same time - * @param i variable key for the camera pose - * @param J variable keys for the point, KeyVector of size K - * @param Z the 2*K measurements as a matrix - * @param model the noise model for the measurement - * @param K shared pointer to calibration object - */ - void addMeasurements(Key i, const Vector& J, const Matrix& Z, - const SharedNoiseModel& model, const shared_ptrK K); - - /** - * Add a stereo factor measurement - * @param measured the measurement - * @param model the noise model for the measurement - * @param poseKey variable key for the camera pose - * @param pointKey variable key for the point - * @param K shared pointer to stereo calibration object - */ - void addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model, - Key poseKey, Key pointKey, const shared_ptrKStereo K); - - /// Return a 2*K Matrix of reprojection errors - Matrix reprojectionErrors(const Values& values) const; - - }; // Graph - - /** - * Non-linear ISAM for vanilla incremental visual SLAM inference - */ - typedef gtsam::NonlinearISAM ISAM; - -} // visualSLAM - -/** - * Backwards compatibility and wrap use only, avoid using - */ -namespace visualSLAM { - typedef gtsam::NonlinearEquality PoseConstraint; ///< \deprecated typedef for backwards compatibility - typedef gtsam::NonlinearEquality PointConstraint; ///< \deprecated typedef for backwards compatibility - typedef gtsam::PriorFactor PosePrior; ///< \deprecated typedef for backwards compatibility - typedef gtsam::PriorFactor PointPrior; ///< \deprecated typedef for backwards compatibility - typedef gtsam::RangeFactor RangeFactor; ///< \deprecated typedef for backwards compatibility - typedef gtsam::GenericProjectionFactor ProjectionFactor; ///< \deprecated typedef for backwards compatibility - typedef gtsam::GenericStereoFactor StereoFactor; ///< \deprecated typedef for backwards compatibility -} - From 47f92ccdf252c68a4d865a6a5c4f54ada8501514 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 30 Jul 2012 15:40:58 +0000 Subject: [PATCH 63/68] Added matlab.h, a temporary file holding special namespace functions. These need to be reviewed and a permanent home found. --- gtsam.h | 19 ++++++++ matlab.h | 129 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 148 insertions(+) create mode 100644 matlab.h diff --git a/gtsam.h b/gtsam.h index 6f87670ca..891520547 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1474,4 +1474,23 @@ typedef gtsam::GenericStereoFactor GenericStereoFac pair load2D(string filename, gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); + +//************************************************************************* +// Utilities +//************************************************************************* + +namespace utilities { + + #include + Matrix extractPoint2(const gtsam::Values& values); + Matrix extractPoint3(const gtsam::Values& values); + Matrix extractPose2(const gtsam::Values& values); + Matrix extractPose3(const gtsam::Values& values); + void perturbPoint2(gtsam::Values& values, double sigma, int seed); + void perturbPoint3(gtsam::Values& values, double sigma, int seed); + void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); + Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); + +} //\namespace utilities + } diff --git a/matlab.h b/matlab.h new file mode 100644 index 000000000..1a9af8396 --- /dev/null +++ b/matlab.h @@ -0,0 +1,129 @@ +/* ---------------------------------------------------------------------------- + + * 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 matlab.h + * @brief Contains *generic* global functions designed particularly for the matlab interface + * @author Stephen Williams + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace gtsam { + + namespace utilities { + + /// Extract all Point2 values into a single matrix [x y] + Matrix extractPoint2(const Values& values) { + size_t j=0; + Values::ConstFiltered points = values.filter(); + Matrix result(points.size(),2); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + result.row(j++) = key_value.value.vector(); + return result; + } + + /// Extract all Point3 values into a single matrix [x y z] + Matrix extractPoint3(const Values& values) { + size_t j=0; + Values::ConstFiltered points = values.filter(); + Matrix result(points.size(),3); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + result.row(j++) = key_value.value.vector(); + return result; + } + + /// Extract all Pose2 values into a single matrix [x y theta] + Matrix extractPose2(const Values& values) { + size_t j=0; + Values::ConstFiltered poses = values.filter(); + Matrix result(poses.size(),3); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) + result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); + return result; + } + + /// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] + Matrix extractPose3(const Values& values) { + size_t j=0; + Values::ConstFiltered poses = values.filter(); + Matrix result(poses.size(),12); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) { + result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); + result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); + result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); + result.row(j).tail(3) = key_value.value.translation().vector(); + j++; + } + return result; + } + + + /// perturb all Point2 using normally distributed noise + void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } + } + + /// perturb all Point3 using normally distributed noise + void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } + } + + /// insert a number of initial point values by backprojecting + void insertBackprojections(Values& values, const SimpleCamera& camera, const Vector& J, const Matrix& Z, double depth) { + if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); + if (Z.cols() != J.size()) throw std::invalid_argument("insertBackProjections: J and Z must have same number of entries"); + for(int k=0;k >(f)) ++K; + // now fill + Matrix errors(2,K); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { + boost::shared_ptr > p = boost::dynamic_pointer_cast >(f); + if (p) errors.col(k++) = p->unwhitenedError(values); + } + return errors; + } + + } + +} + From a13ef979870a10f90cd769006e52f7383b243e55 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 30 Jul 2012 18:34:47 +0000 Subject: [PATCH 64/68] Wrapped default-argument versions of load2D --- gtsam.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam.h b/gtsam.h index 891520547..bdda534e0 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1473,6 +1473,12 @@ typedef gtsam::GenericStereoFactor GenericStereoFac #include pair load2D(string filename, gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxID); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model); //************************************************************************* From da1a732eff14e4c58665fea13576b3da793be56d Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 30 Jul 2012 18:34:50 +0000 Subject: [PATCH 65/68] Merged duplicate Pose2SLAMExample_graph.m examples in different directories --- examples/Pose2SLAMExample_graph.m | 47 ------------------- .../gtsam_examples/Pose2SLAMExample_graph.m | 5 +- 2 files changed, 4 insertions(+), 48 deletions(-) delete mode 100644 examples/Pose2SLAMExample_graph.m diff --git a/examples/Pose2SLAMExample_graph.m b/examples/Pose2SLAMExample_graph.m deleted file mode 100644 index 65271c053..000000000 --- a/examples/Pose2SLAMExample_graph.m +++ /dev/null @@ -1,47 +0,0 @@ -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% 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 Read graph from file and perform GraphSLAM -% @author Frank Dellaert -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%% Initialize graph, initial estimate, and odometry noise -import gtsam.* -model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); -maxID = 0; -addNoise = false; -smart = true; -[graph,initial]=load2D('Data/w100-odom.graph',model,maxID,addNoise,smart); -initial.print(sprintf('Initial estimate:\n')); - -%% Add a Gaussian prior on pose x_1 -import gtsam.* -priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin -priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]); -graph.addPosePrior(0, priorMean, priorNoise); % add directly to graph - -%% Plot Initial Estimate -figure(1);clf -P=initial.poses; -plot(P(:,1),P(:,2),'g-*'); axis equal - -%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initial,1); -P=result.poses; -hold on; plot(P(:,1),P(:,2),'b-*') -result.print(sprintf('\nFinal result:\n')); - -%% Plot Covariance Ellipses -marginals = graph.marginals(result); -P={}; -for i=1:result.size()-1 - pose_i = result.pose(i); - P{i}=marginals.marginalCovariance(i); - plotPose2(pose_i,'b',P{i}) -end -fprintf(1,'%.5f %.5f %.5f\n',P{99}) \ No newline at end of file diff --git a/matlab/gtsam_examples/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m index 980a11cfa..cc8b38cdb 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_graph.m @@ -16,7 +16,10 @@ datafile = gtsam_utils.findExampleDataFile('w100-odom.graph'); %% Initialize graph, initial estimate, and odometry noise import gtsam.* model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); -[graph,initial] = gtsam_utils.load2D(datafile, model); +maxID = 0; +addNoise = false; +smart = true; +[graph,initial] = gtsam.load2D(datafile, model, maxID, addNoise, smart); initial.print(sprintf('Initial estimate:\n')); %% Add a Gaussian prior on pose x_1 From d93aac12d01cca5e9ff88b9eb0abec2e36238ddf Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 3 Aug 2012 21:02:37 +0000 Subject: [PATCH 66/68] Fixed ambiguous sqrt/atan errors --- .../geometry/tests/testInvDepthCamera3.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index 1e1dae189..a69410e34 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -45,8 +45,8 @@ TEST( InvDepthFactor, Project2) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 1., M_PI/4., atan(1/sqrt(2))); - LieScalar inv_depth(1/sqrt(3)); + LieVector diag_landmark(5, 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))); + LieScalar inv_depth(1/sqrt(3.0)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); } @@ -60,8 +60,8 @@ TEST( InvDepthFactor, Project3) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 0., M_PI/4., atan(2./sqrt(2))); - LieScalar inv_depth( 1./sqrt(1+1+4)); + LieVector diag_landmark(5, 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))); + LieScalar inv_depth( 1./sqrt(1.0+1+4)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); } @@ -75,8 +75,8 @@ TEST( InvDepthFactor, Project4) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 0., atan(4/1), atan(2./sqrt(1+16))); - LieScalar inv_depth(1./sqrt(1+16+4)); + LieVector diag_landmark(5, 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))); + LieScalar inv_depth(1./sqrt(1.+16.+4.)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); } From 84924ad663c24333cad1f47b36c21661d580aa9c Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 3 Aug 2012 21:02:39 +0000 Subject: [PATCH 67/68] Removed remaining reference to slam namespace headers --- gtsam_unstable/slam/tests/testPosePriors.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/slam/tests/testPosePriors.cpp b/gtsam_unstable/slam/tests/testPosePriors.cpp index 4aa835266..1a40dd335 100644 --- a/gtsam_unstable/slam/tests/testPosePriors.cpp +++ b/gtsam_unstable/slam/tests/testPosePriors.cpp @@ -9,12 +9,12 @@ #include +#include +#include + #include #include -#include -#include - #include using namespace gtsam; From 061b6ddc08ba41541860a28a5cff515e67dd16c4 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 3 Aug 2012 21:02:45 +0000 Subject: [PATCH 68/68] Moved all in gtsam_utils to gtsam namespace --- matlab/{+gtsam_utils => +gtsam}/CHECK.m | 0 matlab/{+gtsam_utils => +gtsam}/EQUALITY.m | 0 .../VisualISAMGenerateData.m | 0 .../VisualISAMInitialize.m | 0 .../{+gtsam_utils => +gtsam}/VisualISAMPlot.m | 8 ++--- .../{+gtsam_utils => +gtsam}/VisualISAMStep.m | 0 matlab/{+gtsam_utils => +gtsam}/circlePose2.m | 0 matlab/{+gtsam_utils => +gtsam}/circlePose3.m | 0 .../covarianceEllipse.m | 0 .../covarianceEllipse3D.m | 0 .../findExampleDataFile.m | 0 matlab/{+gtsam_utils => +gtsam}/load3D.m | 0 .../{+gtsam_utils => +gtsam}/plot2DPoints.m | 4 +-- .../plot2DTrajectory.m | 4 +-- .../{+gtsam_utils => +gtsam}/plot3DPoints.m | 4 +-- .../plot3DTrajectory.m | 4 +-- matlab/{+gtsam_utils => +gtsam}/plotCamera.m | 0 matlab/{+gtsam_utils => +gtsam}/plotPoint2.m | 2 +- matlab/{+gtsam_utils => +gtsam}/plotPoint3.m | 2 +- matlab/{+gtsam_utils => +gtsam}/plotPose2.m | 2 +- matlab/{+gtsam_utils => +gtsam}/plotPose3.m | 2 +- matlab/+gtsam_utils/load2D.m | 31 ------------------- matlab/CMakeLists.txt | 2 +- matlab/gtsam_examples/LocalizationExample.m | 2 +- matlab/gtsam_examples/OdometryExample.m | 2 +- matlab/gtsam_examples/PlanarSLAMExample.m | 4 +-- .../PlanarSLAMExample_sampling.m | 6 ++-- matlab/gtsam_examples/Pose2SLAMExample.m | 2 +- .../gtsam_examples/Pose2SLAMExample_circle.m | 6 ++-- .../gtsam_examples/Pose2SLAMExample_graph.m | 8 ++--- matlab/gtsam_examples/Pose3SLAMExample.m | 6 ++-- .../gtsam_examples/Pose3SLAMExample_graph.m | 10 +++--- matlab/gtsam_examples/SBAExample.m | 2 +- matlab/gtsam_examples/SFMExample.m | 6 ++-- matlab/gtsam_examples/StereoVOExample.m | 6 ++-- matlab/gtsam_examples/StereoVOExample_large.m | 12 +++---- matlab/gtsam_examples/VisualISAMExample.m | 10 +++--- matlab/gtsam_examples/VisualISAM_gui.m | 14 ++++----- matlab/gtsam_tests/testJacobianFactor.m | 1 - matlab/gtsam_tests/testKalmanFilter.m | 8 ++--- matlab/gtsam_tests/testLocalizationExample.m | 1 - matlab/gtsam_tests/testOdometryExample.m | 2 +- matlab/gtsam_tests/testPlanarSLAMExample.m | 1 - matlab/gtsam_tests/testPose2SLAMExample.m | 1 - matlab/gtsam_tests/testPose3SLAMExample.m | 1 - matlab/gtsam_tests/testSFMExample.m | 2 -- matlab/gtsam_tests/testStereoVOExample.m | 1 - matlab/gtsam_tests/testVisualISAMExample.m | 6 ++-- 48 files changed, 73 insertions(+), 112 deletions(-) rename matlab/{+gtsam_utils => +gtsam}/CHECK.m (100%) rename matlab/{+gtsam_utils => +gtsam}/EQUALITY.m (100%) rename matlab/{+gtsam_utils => +gtsam}/VisualISAMGenerateData.m (100%) rename matlab/{+gtsam_utils => +gtsam}/VisualISAMInitialize.m (100%) rename matlab/{+gtsam_utils => +gtsam}/VisualISAMPlot.m (86%) rename matlab/{+gtsam_utils => +gtsam}/VisualISAMStep.m (100%) rename matlab/{+gtsam_utils => +gtsam}/circlePose2.m (100%) rename matlab/{+gtsam_utils => +gtsam}/circlePose3.m (100%) rename matlab/{+gtsam_utils => +gtsam}/covarianceEllipse.m (100%) rename matlab/{+gtsam_utils => +gtsam}/covarianceEllipse3D.m (100%) rename matlab/{+gtsam_utils => +gtsam}/findExampleDataFile.m (100%) rename matlab/{+gtsam_utils => +gtsam}/load3D.m (100%) rename matlab/{+gtsam_utils => +gtsam}/plot2DPoints.m (88%) rename matlab/{+gtsam_utils => +gtsam}/plot2DTrajectory.m (93%) rename matlab/{+gtsam_utils => +gtsam}/plot3DPoints.m (88%) rename matlab/{+gtsam_utils => +gtsam}/plot3DTrajectory.m (91%) rename matlab/{+gtsam_utils => +gtsam}/plotCamera.m (100%) rename matlab/{+gtsam_utils => +gtsam}/plotPoint2.m (76%) rename matlab/{+gtsam_utils => +gtsam}/plotPoint3.m (80%) rename matlab/{+gtsam_utils => +gtsam}/plotPose2.m (84%) rename matlab/{+gtsam_utils => +gtsam}/plotPose3.m (94%) delete mode 100644 matlab/+gtsam_utils/load2D.m diff --git a/matlab/+gtsam_utils/CHECK.m b/matlab/+gtsam/CHECK.m similarity index 100% rename from matlab/+gtsam_utils/CHECK.m rename to matlab/+gtsam/CHECK.m diff --git a/matlab/+gtsam_utils/EQUALITY.m b/matlab/+gtsam/EQUALITY.m similarity index 100% rename from matlab/+gtsam_utils/EQUALITY.m rename to matlab/+gtsam/EQUALITY.m diff --git a/matlab/+gtsam_utils/VisualISAMGenerateData.m b/matlab/+gtsam/VisualISAMGenerateData.m similarity index 100% rename from matlab/+gtsam_utils/VisualISAMGenerateData.m rename to matlab/+gtsam/VisualISAMGenerateData.m diff --git a/matlab/+gtsam_utils/VisualISAMInitialize.m b/matlab/+gtsam/VisualISAMInitialize.m similarity index 100% rename from matlab/+gtsam_utils/VisualISAMInitialize.m rename to matlab/+gtsam/VisualISAMInitialize.m diff --git a/matlab/+gtsam_utils/VisualISAMPlot.m b/matlab/+gtsam/VisualISAMPlot.m similarity index 86% rename from matlab/+gtsam_utils/VisualISAMPlot.m rename to matlab/+gtsam/VisualISAMPlot.m index 41aa49ac6..5ef64cf62 100644 --- a/matlab/+gtsam_utils/VisualISAMPlot.m +++ b/matlab/+gtsam/VisualISAMPlot.m @@ -9,7 +9,7 @@ hold on; %% Plot points % Can't use data because current frame might not see all points marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()); % TODO - this is slow -gtsam_utils.plot3DPoints(result, [], marginals); +gtsam.plot3DPoints(result, [], marginals); %% Plot cameras import gtsam.* @@ -18,13 +18,13 @@ while result.exists(symbol('x',M)) ii = symbol('x',M); pose_i = result.at(ii); if options.hardConstraint && (M==1) - gtsam_utils.plotPose3(pose_i,[],10); + gtsam.plotPose3(pose_i,[],10); else P = marginals.marginalCovariance(ii); - gtsam_utils.plotPose3(pose_i,P,10); + gtsam.plotPose3(pose_i,P,10); end if options.drawTruePoses % show ground truth - gtsam_utils.plotPose3(truth.cameras{M}.pose,[],10); + gtsam.plotPose3(truth.cameras{M}.pose,[],10); end M = M + options.cameraInterval; diff --git a/matlab/+gtsam_utils/VisualISAMStep.m b/matlab/+gtsam/VisualISAMStep.m similarity index 100% rename from matlab/+gtsam_utils/VisualISAMStep.m rename to matlab/+gtsam/VisualISAMStep.m diff --git a/matlab/+gtsam_utils/circlePose2.m b/matlab/+gtsam/circlePose2.m similarity index 100% rename from matlab/+gtsam_utils/circlePose2.m rename to matlab/+gtsam/circlePose2.m diff --git a/matlab/+gtsam_utils/circlePose3.m b/matlab/+gtsam/circlePose3.m similarity index 100% rename from matlab/+gtsam_utils/circlePose3.m rename to matlab/+gtsam/circlePose3.m diff --git a/matlab/+gtsam_utils/covarianceEllipse.m b/matlab/+gtsam/covarianceEllipse.m similarity index 100% rename from matlab/+gtsam_utils/covarianceEllipse.m rename to matlab/+gtsam/covarianceEllipse.m diff --git a/matlab/+gtsam_utils/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m similarity index 100% rename from matlab/+gtsam_utils/covarianceEllipse3D.m rename to matlab/+gtsam/covarianceEllipse3D.m diff --git a/matlab/+gtsam_utils/findExampleDataFile.m b/matlab/+gtsam/findExampleDataFile.m similarity index 100% rename from matlab/+gtsam_utils/findExampleDataFile.m rename to matlab/+gtsam/findExampleDataFile.m diff --git a/matlab/+gtsam_utils/load3D.m b/matlab/+gtsam/load3D.m similarity index 100% rename from matlab/+gtsam_utils/load3D.m rename to matlab/+gtsam/load3D.m diff --git a/matlab/+gtsam_utils/plot2DPoints.m b/matlab/+gtsam/plot2DPoints.m similarity index 88% rename from matlab/+gtsam_utils/plot2DPoints.m rename to matlab/+gtsam/plot2DPoints.m index b98434627..d4703a5d7 100644 --- a/matlab/+gtsam_utils/plot2DPoints.m +++ b/matlab/+gtsam/plot2DPoints.m @@ -22,9 +22,9 @@ for i = 0:keys.size-1 if isa(p, 'gtsam.Point2') if haveMarginals P = marginals.marginalCovariance(key); - gtsam_utils.plotPoint2(p, linespec, P); + gtsam.plotPoint2(p, linespec, P); else - gtsam_utils.plotPoint2(p, linespec); + gtsam.plotPoint2(p, linespec); end end end diff --git a/matlab/+gtsam_utils/plot2DTrajectory.m b/matlab/+gtsam/plot2DTrajectory.m similarity index 93% rename from matlab/+gtsam_utils/plot2DTrajectory.m rename to matlab/+gtsam/plot2DTrajectory.m index c962c993b..329026398 100644 --- a/matlab/+gtsam_utils/plot2DTrajectory.m +++ b/matlab/+gtsam/plot2DTrajectory.m @@ -31,7 +31,7 @@ for i = 0:keys.size-1 plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], linespec); if haveMarginals P = marginals.marginalCovariance(lastKey); - gtsam_utils.plotPose2(lastPose, 'g', P); + gtsam.plotPose2(lastPose, 'g', P); end end lastIndex = i; @@ -43,7 +43,7 @@ if ~isempty(lastIndex) && haveMarginals lastKey = keys.at(lastIndex); lastPose = values.at(lastKey); P = marginals.marginalCovariance(lastKey); - gtsam_utils.plotPose2(lastPose, 'g', P); + gtsam.plotPose2(lastPose, 'g', P); end if ~holdstate diff --git a/matlab/+gtsam_utils/plot3DPoints.m b/matlab/+gtsam/plot3DPoints.m similarity index 88% rename from matlab/+gtsam_utils/plot3DPoints.m rename to matlab/+gtsam/plot3DPoints.m index e7a5ab0a0..8feab1744 100644 --- a/matlab/+gtsam_utils/plot3DPoints.m +++ b/matlab/+gtsam/plot3DPoints.m @@ -22,9 +22,9 @@ for i = 0:keys.size-1 if isa(p, 'gtsam.Point3') if haveMarginals P = marginals.marginalCovariance(key); - gtsam_utils.plotPoint3(p, linespec, P); + gtsam.plotPoint3(p, linespec, P); else - gtsam_utils.plotPoint3(p, linespec); + gtsam.plotPoint3(p, linespec); end end end diff --git a/matlab/+gtsam_utils/plot3DTrajectory.m b/matlab/+gtsam/plot3DTrajectory.m similarity index 91% rename from matlab/+gtsam_utils/plot3DTrajectory.m rename to matlab/+gtsam/plot3DTrajectory.m index 08b2f0daa..1cd35c22f 100644 --- a/matlab/+gtsam_utils/plot3DTrajectory.m +++ b/matlab/+gtsam/plot3DTrajectory.m @@ -28,7 +28,7 @@ for i = 0:keys.size-1 else P = []; end - gtsam_utils.plotPose3(lastPose, P, scale); + gtsam.plotPose3(lastPose, P, scale); end lastIndex = i; end @@ -43,7 +43,7 @@ if ~isempty(lastIndex) else P = []; end - gtsam_utils.plotPose3(lastPose, P, scale); + gtsam.plotPose3(lastPose, P, scale); end if ~holdstate diff --git a/matlab/+gtsam_utils/plotCamera.m b/matlab/+gtsam/plotCamera.m similarity index 100% rename from matlab/+gtsam_utils/plotCamera.m rename to matlab/+gtsam/plotCamera.m diff --git a/matlab/+gtsam_utils/plotPoint2.m b/matlab/+gtsam/plotPoint2.m similarity index 76% rename from matlab/+gtsam_utils/plotPoint2.m rename to matlab/+gtsam/plotPoint2.m index a1bc85a88..e62804ce3 100644 --- a/matlab/+gtsam_utils/plotPoint2.m +++ b/matlab/+gtsam/plotPoint2.m @@ -6,5 +6,5 @@ else plot(p.x,p.y,color); end if exist('P', 'var') - gtsam_utils.covarianceEllipse([p.x;p.y],P,color(1)); + gtsam.covarianceEllipse([p.x;p.y],P,color(1)); end \ No newline at end of file diff --git a/matlab/+gtsam_utils/plotPoint3.m b/matlab/+gtsam/plotPoint3.m similarity index 80% rename from matlab/+gtsam_utils/plotPoint3.m rename to matlab/+gtsam/plotPoint3.m index 6aafd5f21..390b44939 100644 --- a/matlab/+gtsam_utils/plotPoint3.m +++ b/matlab/+gtsam/plotPoint3.m @@ -6,7 +6,7 @@ else plot3(p.x,p.y,p.z,color); end if exist('P', 'var') - gtsam_utils.covarianceEllipse3D([p.x;p.y;p.z],P); + gtsam.covarianceEllipse3D([p.x;p.y;p.z],P); end end diff --git a/matlab/+gtsam_utils/plotPose2.m b/matlab/+gtsam/plotPose2.m similarity index 84% rename from matlab/+gtsam_utils/plotPose2.m rename to matlab/+gtsam/plotPose2.m index a9997dc2c..c7fba28ed 100644 --- a/matlab/+gtsam_utils/plotPose2.m +++ b/matlab/+gtsam/plotPose2.m @@ -9,5 +9,5 @@ quiver(pose.x,pose.y,c,s,axisLength,color); if nargin>2 pPp = P(1:2,1:2); % covariance matrix in pose coordinate frame gRp = [c -s;s c]; % rotation from pose to global - gtsam_utils.covarianceEllipse([pose.x;pose.y],gRp*pPp*gRp',color); + gtsam.covarianceEllipse([pose.x;pose.y],gRp*pPp*gRp',color); end \ No newline at end of file diff --git a/matlab/+gtsam_utils/plotPose3.m b/matlab/+gtsam/plotPose3.m similarity index 94% rename from matlab/+gtsam_utils/plotPose3.m rename to matlab/+gtsam/plotPose3.m index d24a998c8..e2c254e39 100644 --- a/matlab/+gtsam_utils/plotPose3.m +++ b/matlab/+gtsam/plotPose3.m @@ -25,7 +25,7 @@ end if (nargin>2) && (~isempty(P)) pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame - gtsam_utils.covarianceEllipse3D(C,gPp); + gtsam.covarianceEllipse3D(C,gPp); end end diff --git a/matlab/+gtsam_utils/load2D.m b/matlab/+gtsam_utils/load2D.m deleted file mode 100644 index c4b2e105d..000000000 --- a/matlab/+gtsam_utils/load2D.m +++ /dev/null @@ -1,31 +0,0 @@ -function [graph,initial] = load2D(filename,model) -% load2D: read TORO pose graph -% cannot read noise model from file yet, uses specified model - -import gtsam.* - -fid = fopen(filename); -if fid < 0 - error(['load2D: Cannot open file ' filename]); -end - -% scan all lines into a cell array -columns=textscan(fid,'%s','delimiter','\n'); -fclose(fid); -lines=columns{1}; - -% loop over lines and add vertices -graph = NonlinearFactorGraph; -initial = Values; -n=size(lines,1); -for i=1:n - line_i=lines{i}; - if strcmp('VERTEX2',line_i(1:7)) - v = textscan(line_i,'%s %d %f %f %f',1); - initial.insert(v{2}, Pose2(v{3}, v{4}, v{5})); - elseif strcmp('EDGE2',line_i(1:5)) - e = textscan(line_i,'%s %d %d %f %f %f',1); - graph.add(BetweenFactorPose2(e{2}, e{3}, Pose2(e{4}, e{5}, e{6}), model)); - end -end - diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 66fa99def..b4e43d3d6 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -20,7 +20,7 @@ install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION " # Utilities message(STATUS "Installing Matlab Toolbox Utilities") -install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam_utils" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") message(STATUS "Installing Matlab Toolbox Examples (Data)") # Data files: *.graph and *.txt diff --git a/matlab/gtsam_examples/LocalizationExample.m b/matlab/gtsam_examples/LocalizationExample.m index aad3af98c..c877c70c7 100644 --- a/matlab/gtsam_examples/LocalizationExample.m +++ b/matlab/gtsam_examples/LocalizationExample.m @@ -55,7 +55,7 @@ import gtsam.* cla; hold on; -gtsam_utils.plot2DTrajectory(result, [], Marginals(graph, result)); +gtsam.plot2DTrajectory(result, [], Marginals(graph, result)); axis([-0.6 4.8 -1 1]) axis equal diff --git a/matlab/gtsam_examples/OdometryExample.m b/matlab/gtsam_examples/OdometryExample.m index e47becda5..b913b187a 100644 --- a/matlab/gtsam_examples/OdometryExample.m +++ b/matlab/gtsam_examples/OdometryExample.m @@ -52,7 +52,7 @@ import gtsam.* cla; hold on; -gtsam_utils.plot2DTrajectory(result, [], Marginals(graph, result)); +gtsam.plot2DTrajectory(result, [], Marginals(graph, result)); axis([-0.6 4.8 -1 1]) axis equal diff --git a/matlab/gtsam_examples/PlanarSLAMExample.m b/matlab/gtsam_examples/PlanarSLAMExample.m index 056db501e..e37326275 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample.m +++ b/matlab/gtsam_examples/PlanarSLAMExample.m @@ -72,8 +72,8 @@ import gtsam.* cla;hold on marginals = Marginals(graph, result); -gtsam_utils.plot2DTrajectory(result, [], marginals); -gtsam_utils.plot2DPoints(result, [], marginals); +gtsam.plot2DTrajectory(result, [], marginals); +gtsam.plot2DPoints(result, [], marginals); plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-'); plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-'); diff --git a/matlab/gtsam_examples/PlanarSLAMExample_sampling.m b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m index 5604d0ddc..59c99df53 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample_sampling.m +++ b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m @@ -52,8 +52,8 @@ import gtsam.* cla;hold on marginals = Marginals(graph, sample); -gtsam_utils.plot2DTrajectory(sample, [], marginals); -gtsam_utils.plot2DPoints(sample, [], marginals); +gtsam.plot2DTrajectory(sample, [], marginals); +gtsam.plot2DPoints(sample, [], marginals); for j=1:2 key = symbol('l',j); @@ -73,5 +73,5 @@ N=1000; for s=1:N delta = S{2}*randn(2,1); proposedPoint = Point2(point{2}.x+delta(1),point{2}.y+delta(2)); - gtsam_utils.plotPoint2(proposedPoint,'k.') + gtsam.plotPoint2(proposedPoint,'k.') end \ No newline at end of file diff --git a/matlab/gtsam_examples/Pose2SLAMExample.m b/matlab/gtsam_examples/Pose2SLAMExample.m index a8ad32254..fccacc894 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample.m +++ b/matlab/gtsam_examples/Pose2SLAMExample.m @@ -68,7 +68,7 @@ hold on plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-','LineWidth',2); marginals = Marginals(graph, result); -gtsam_utils.plot2DTrajectory(result, [], marginals); +gtsam.plot2DTrajectory(result, [], marginals); axis([-0.6 4.8 -1 1]) axis equal diff --git a/matlab/gtsam_examples/Pose2SLAMExample_circle.m b/matlab/gtsam_examples/Pose2SLAMExample_circle.m index b40c67322..957a30df4 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_circle.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_circle.m @@ -12,7 +12,7 @@ %% Create a hexagon of poses import gtsam.* -hexagon = gtsam_utils.circlePose2(6,1.0); +hexagon = gtsam.circlePose2(6,1.0); p0 = hexagon.at(0); p1 = hexagon.at(1); @@ -42,7 +42,7 @@ initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]')); %% Plot Initial Estimate import gtsam.* cla -gtsam_utils.plot2DTrajectory(initial, 'g*-'); axis equal +gtsam.plot2DTrajectory(initial, 'g*-'); axis equal %% optimize import gtsam.* @@ -51,7 +51,7 @@ result = optimizer.optimizeSafely; %% Show Result import gtsam.* -hold on; gtsam_utils.plot2DTrajectory(result, 'b*-'); +hold on; gtsam.plot2DTrajectory(result, 'b*-'); view(2); axis([-1.5 1.5 -1.5 1.5]); result.print(sprintf('\nFinal result:\n')); diff --git a/matlab/gtsam_examples/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m index cc8b38cdb..3eed090a2 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_graph.m @@ -11,7 +11,7 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Find data file -datafile = gtsam_utils.findExampleDataFile('w100-odom.graph'); +datafile = gtsam.findExampleDataFile('w100-odom.graph'); %% Initialize graph, initial estimate, and odometry noise import gtsam.* @@ -31,13 +31,13 @@ graph.add(PriorFactorPose2(0, priorMean, priorNoise)); % add directly to graph %% Plot Initial Estimate import gtsam.* cla -gtsam_utils.plot2DTrajectory(initial, 'g-*'); axis equal +gtsam.plot2DTrajectory(initial, 'g-*'); axis equal %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd import gtsam.* optimizer = LevenbergMarquardtOptimizer(graph, initial); result = optimizer.optimizeSafely; -hold on; gtsam_utils.plot2DTrajectory(result, 'b-*'); +hold on; gtsam.plot2DTrajectory(result, 'b-*'); result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses @@ -47,7 +47,7 @@ P={}; for i=1:result.size()-1 pose_i = result.at(i); P{i}=marginals.marginalCovariance(i); - gtsam_utils.plotPose2(pose_i,'b',P{i}) + gtsam.plotPose2(pose_i,'b',P{i}) end view(2) axis([-15 10 -15 10]); axis equal; diff --git a/matlab/gtsam_examples/Pose3SLAMExample.m b/matlab/gtsam_examples/Pose3SLAMExample.m index bdc101b9d..b1521ff5b 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample.m +++ b/matlab/gtsam_examples/Pose3SLAMExample.m @@ -12,7 +12,7 @@ %% Create a hexagon of poses import gtsam.* -hexagon = gtsam_utils.circlePose3(6,1.0); +hexagon = gtsam.circlePose3(6,1.0); p0 = hexagon.at(0); p1 = hexagon.at(1); @@ -43,7 +43,7 @@ initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); %% Plot Initial Estimate import gtsam.* cla -gtsam_utils.plot3DTrajectory(initial, 'g-*'); +gtsam.plot3DTrajectory(initial, 'g-*'); %% optimize import gtsam.* @@ -52,7 +52,7 @@ result = optimizer.optimizeSafely(); %% Show Result import gtsam.* -hold on; gtsam_utils.plot3DTrajectory(result, 'b-*', true, 0.3); +hold on; gtsam.plot3DTrajectory(result, 'b-*', true, 0.3); axis([-2 2 -2 2 -1 1]); axis equal view(-37,40) diff --git a/matlab/gtsam_examples/Pose3SLAMExample_graph.m b/matlab/gtsam_examples/Pose3SLAMExample_graph.m index 574894878..19cc31439 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose3SLAMExample_graph.m @@ -16,27 +16,27 @@ N = 2500; % dataset = 'sphere2500_groundtruth.txt'; dataset = 'sphere2500.txt'; -datafile = gtsam_utils.findExampleDataFile(dataset); +datafile = gtsam.findExampleDataFile(dataset); %% Initialize graph, initial estimate, and odometry noise import gtsam.* model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); -[graph,initial]=gtsam_utils.load3D(datafile,model,true,N); +[graph,initial]=gtsam.load3D(datafile,model,true,N); %% Plot Initial Estimate import gtsam.* cla first = initial.at(0); plot3(first.x(),first.y(),first.z(),'r*'); hold on -gtsam_utils.plot3DTrajectory(initial,'g-',false); +gtsam.plot3DTrajectory(initial,'g-',false); drawnow; %% Read again, now with all constraints, and optimize import gtsam.* -graph = gtsam_utils.load3D(datafile, model, false, N); +graph = gtsam.load3D(datafile, model, false, N); graph.add(NonlinearEqualityPose3(0, first)); optimizer = LevenbergMarquardtOptimizer(graph, initial); result = optimizer.optimizeSafely(); -gtsam_utils.plot3DTrajectory(result, 'r-', false); axis equal; +gtsam.plot3DTrajectory(result, 'r-', false); axis equal; view(3); axis equal; \ No newline at end of file diff --git a/matlab/gtsam_examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m index 852c83fdc..7e4ac4031 100644 --- a/matlab/gtsam_examples/SBAExample.m +++ b/matlab/gtsam_examples/SBAExample.m @@ -22,7 +22,7 @@ options.nrCameras = 10; options.showImages = false; %% Generate data -[data,truth] = gtsam_utils.VisualISAMGenerateData(options); +[data,truth] = gtsam.VisualISAMGenerateData(options); measurementNoiseSigma = 1.0; pointNoiseSigma = 0.1; diff --git a/matlab/gtsam_examples/SFMExample.m b/matlab/gtsam_examples/SFMExample.m index 7f583b386..92eeb482b 100644 --- a/matlab/gtsam_examples/SFMExample.m +++ b/matlab/gtsam_examples/SFMExample.m @@ -22,7 +22,7 @@ options.nrCameras = 10; options.showImages = false; %% Generate data -[data,truth] = gtsam_utils.VisualISAMGenerateData(options); +[data,truth] = gtsam.VisualISAMGenerateData(options); measurementNoiseSigma = 1.0; pointNoiseSigma = 0.1; @@ -86,8 +86,8 @@ marginals = Marginals(graph, result); cla hold on; -gtsam_utils.plot3DPoints(result, [], marginals); -gtsam_utils.plot3DTrajectory(result, '*', 1, 8, marginals); +gtsam.plot3DPoints(result, [], marginals); +gtsam.plot3DTrajectory(result, '*', 1, 8, marginals); axis([-40 40 -40 40 -10 20]);axis equal view(3) diff --git a/matlab/gtsam_examples/StereoVOExample.m b/matlab/gtsam_examples/StereoVOExample.m index e62c4395c..94c4b3f37 100644 --- a/matlab/gtsam_examples/StereoVOExample.m +++ b/matlab/gtsam_examples/StereoVOExample.m @@ -75,6 +75,6 @@ axis equal view(-38,12) camup([0;1;0]); -gtsam_utils.plot3DTrajectory(initialEstimate, 'r', 1, 0.3); -gtsam_utils.plot3DTrajectory(result, 'g', 1, 0.3); -gtsam_utils.plot3DPoints(result); +gtsam.plot3DTrajectory(initialEstimate, 'r', 1, 0.3); +gtsam.plot3DTrajectory(result, 'g', 1, 0.3); +gtsam.plot3DPoints(result); diff --git a/matlab/gtsam_examples/StereoVOExample_large.m b/matlab/gtsam_examples/StereoVOExample_large.m index 5ae3a8fd7..850af4f80 100644 --- a/matlab/gtsam_examples/StereoVOExample_large.m +++ b/matlab/gtsam_examples/StereoVOExample_large.m @@ -13,7 +13,7 @@ %% Load calibration import gtsam.* % format: fx fy skew cx cy baseline -calib = dlmread(gtsam_utils.findExampleDataFile('VO_calibration.txt')); +calib = dlmread(gtsam.findExampleDataFile('VO_calibration.txt')); K = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6)); stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]); @@ -26,7 +26,7 @@ initial = Values; % row format: camera_id 4x4 pose (row, major) import gtsam.* fprintf(1,'Reading data\n'); -cameras = dlmread(gtsam_utils.findExampleDataFile('VO_camera_poses_large.txt')); +cameras = dlmread(gtsam.findExampleDataFile('VO_camera_poses_large.txt')); for i=1:size(cameras,1) pose = Pose3(reshape(cameras(i,2:17),4,4)'); initial.insert(symbol('x',cameras(i,1)),pose); @@ -35,7 +35,7 @@ end %% load stereo measurements and initialize landmarks % camera_id landmark_id uL uR v X Y Z import gtsam.* -measurements = dlmread(gtsam_utils.findExampleDataFile('VO_stereo_factors_large.txt')); +measurements = dlmread(gtsam.findExampleDataFile('VO_stereo_factors_large.txt')); fprintf(1,'Creating Graph\n'); tic for i=1:size(measurements,1) @@ -69,9 +69,9 @@ toc %% visualize initial trajectory, final trajectory, and final points cla; hold on; -gtsam_utils.plot3DTrajectory(initial, 'r', 1, 0.5); -gtsam_utils.plot3DTrajectory(result, 'g', 1, 0.5); -gtsam_utils.plot3DPoints(result); +gtsam.plot3DTrajectory(initial, 'r', 1, 0.5); +gtsam.plot3DTrajectory(result, 'g', 1, 0.5); +gtsam.plot3DPoints(result); axis([-5 20 -20 20 0 100]); axis equal diff --git a/matlab/gtsam_examples/VisualISAMExample.m b/matlab/gtsam_examples/VisualISAMExample.m index 64634d336..31c240029 100644 --- a/matlab/gtsam_examples/VisualISAMExample.m +++ b/matlab/gtsam_examples/VisualISAMExample.m @@ -32,17 +32,17 @@ options.saveFigures = false; options.saveDotFiles = false; %% Generate data -[data,truth] = gtsam_utils.VisualISAMGenerateData(options); +[data,truth] = gtsam.VisualISAMGenerateData(options); %% Initialize iSAM with the first pose and points -[noiseModels,isam,result,nextPose] = gtsam_utils.VisualISAMInitialize(data,truth,options); +[noiseModels,isam,result,nextPose] = gtsam.VisualISAMInitialize(data,truth,options); cla; -gtsam_utils.VisualISAMPlot(truth, data, isam, result, options) +gtsam.VisualISAMPlot(truth, data, isam, result, options) %% Main loop for iSAM: stepping through all poses for frame_i=3:options.nrCameras - [isam,result,nextPose] = gtsam_utils.VisualISAMStep(data,noiseModels,isam,result,truth,nextPose); + [isam,result,nextPose] = gtsam.VisualISAMStep(data,noiseModels,isam,result,truth,nextPose); if mod(frame_i,options.drawInterval)==0 - gtsam_utils.VisualISAMPlot(truth, data, isam, result, options) + gtsam.VisualISAMPlot(truth, data, isam, result, options) end end diff --git a/matlab/gtsam_examples/VisualISAM_gui.m b/matlab/gtsam_examples/VisualISAM_gui.m index f7990268f..bc08e7abf 100644 --- a/matlab/gtsam_examples/VisualISAM_gui.m +++ b/matlab/gtsam_examples/VisualISAM_gui.m @@ -230,12 +230,12 @@ global frame_i truth data noiseModels isam result nextPoseIndex options initOptions(handles) % Generate Data -[data,truth] = gtsam_utils.VisualISAMGenerateData(options); +[data,truth] = gtsam.VisualISAMGenerateData(options); % Initialize and plot -[noiseModels,isam,result,nextPoseIndex] = gtsam_utils.VisualISAMInitialize(data,truth,options); +[noiseModels,isam,result,nextPoseIndex] = gtsam.VisualISAMInitialize(data,truth,options); cla -gtsam_utils.VisualISAMPlot(truth, data, isam, result, options) +gtsam.VisualISAMPlot(truth, data, isam, result, options) frame_i = 2; showFramei(hObject, handles) @@ -246,10 +246,10 @@ global frame_i truth data noiseModels isam result nextPoseIndex options while (frame_i