From f924d01bc345ac917728a2f5cf335fb607354304 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 3 Jun 2012 05:25:05 +0000 Subject: [PATCH] Synchronized mixed case naming --- examples/LocalizationExample.cpp | 12 ++++---- examples/PlanarSLAMSelfContained_advanced.cpp | 14 ++++----- examples/Pose2SLAMExample_advanced.cpp | 30 ++++++++----------- examples/matlab/LocalizationExample.m | 6 ++-- examples/matlab/Pose2SLAMExample_advanced.m | 28 +++++++---------- examples/matlab/Pose2SLAMExample_easy.m | 23 +++++++------- tests/testMarginals.cpp | 14 ++++----- 7 files changed, 57 insertions(+), 70 deletions(-) diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index a8ff59d68..578f33558 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -35,7 +35,7 @@ using namespace gtsam; */ int main(int argc, char** argv) { - // create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) + // create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) pose2SLAM::Graph graph; // add a Gaussian prior on pose x_1 @@ -55,7 +55,7 @@ int main(int argc, char** argv) { // 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(2, Pose2(2.3, 0.1, -0.2)); initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); initialEstimate.print("\nInitial estimate:\n "); @@ -64,11 +64,11 @@ int main(int argc, char** argv) { result.print("\nFinal result:\n "); // Query the marginals - Marginals marginals = graph.marginals(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; + 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; } diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp index bff4ee7ea..f7c321f2d 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -60,18 +60,18 @@ int main(int argc, char** argv) { /* add prior */ // gaussian for prior - SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - PriorFactor posePrior(i1, prior_measurement, prior_model); // create the factor + 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 odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) + 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, odom_measurement, odom_model); - BetweenFactor odom23(i2, i3, odom_measurement, odom_model); + BetweenFactor odom12(i1, i2, odometry, odometryNoise); + BetweenFactor odom23(i2, i3, odometry, odometryNoise); graph.add(odom12); // add both to graph graph.add(odom23); diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index 3884e03e4..083585c73 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -35,26 +35,22 @@ int main(int argc, char** argv) { pose2SLAM::Graph graph; /* 2.a add prior */ - // gaussian for prior - SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - graph.addPrior(1, prior_measurement, prior_model); // add directly to graph + Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin + SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta + graph.addPrior(1, priorMean, priorNoise); // add directly to graph /* 2.b add odometry */ - // general noisemodel for odometry - SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - - /* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/ - Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - graph.addOdometry(1, 2, odom_measurement, odom_model); - graph.addOdometry(2, 3, odom_measurement, odom_model); + SharedDiagonal odometryNoise(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.addOdometry(1, 2, odometry, odometryNoise); + graph.addOdometry(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 */ + /* 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(2, Pose2(2.3, 0.1, -0.2)); initial.insertPose(3, Pose2(4.1, 0.1, 0.1)); initial.print("initial estimate"); @@ -67,15 +63,15 @@ int main(int argc, char** argv) { params.absoluteErrorTol = 1e-15; params.relativeErrorTol = 1e-15; params.ordering = ordering; - LevenbergMarquardtOptimizer optimizer(graph, initial, params); + 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); + Matrix covariance1 = marginals.marginalCovariance(1); + Matrix covariance2 = marginals.marginalCovariance(2); print(covariance1, "Covariance1"); print(covariance2, "Covariance2"); diff --git a/examples/matlab/LocalizationExample.m b/examples/matlab/LocalizationExample.m index 3a8282e3a..ae66a131c 100644 --- a/examples/matlab/LocalizationExample.m +++ b/examples/matlab/LocalizationExample.m @@ -45,9 +45,9 @@ result.print(sprintf('\nFinal result:\n ')); %% Query the marginals marginals = graph.marginals(result); -P{1}=marginals.marginalCovariance(1) -P{2}=marginals.marginalCovariance(2) -P{3}=marginals.marginalCovariance(3) +P{1}=marginals.marginalCovariance(1); +P{2}=marginals.marginalCovariance(2); +P{3}=marginals.marginalCovariance(3); %% Plot Trajectory figure(1) diff --git a/examples/matlab/Pose2SLAMExample_advanced.m b/examples/matlab/Pose2SLAMExample_advanced.m index 2199def5b..f2739803e 100644 --- a/examples/matlab/Pose2SLAMExample_advanced.m +++ b/examples/matlab/Pose2SLAMExample_advanced.m @@ -20,24 +20,21 @@ % - We have full odometry for measurements % - The robot is on a grid, moving 2 meters each step -%% Create keys for variables -x1 = 1; x2 = 2; x3 = 3; - %% Create graph container and add factors to it graph = pose2SLAMGraph; %% Add prior % gaussian for prior -prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); -prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin -graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph +priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); +priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin +graph.addPrior(1, priorMean, priorNoise); % add directly to graph %% Add odometry % general noisemodel for odometry -odom_model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); -odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) -graph.addOdometry(x1, x2, odom_measurement, odom_model); -graph.addOdometry(x2, x3, odom_measurement, odom_model); +odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); +odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) +graph.addOdometry(1, 2, odometry, odometryNoise); +graph.addOdometry(2, 3, odometry, odometryNoise); %% Add measurements % general noisemodel for measurements @@ -48,9 +45,9 @@ graph.print('full graph'); %% Initialize to noisy points initialEstimate = pose2SLAMValues; -initialEstimate.insertPose(x1, gtsamPose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(x2, gtsamPose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(x3, gtsamPose2(4.1, 0.1, 0.1)); +initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2)); +initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2)); +initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1)); initialEstimate.print('initial estimate'); @@ -62,10 +59,7 @@ initialEstimate.print('initial estimate'); %result = pose2SLAMOptimizer(graph,initialEstimate,ord,params); %result.print('final result'); -% % -% % disp('\\\'); -% % -% % %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd +%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd result = graph.optimize(initialEstimate); result.print('final result'); diff --git a/examples/matlab/Pose2SLAMExample_easy.m b/examples/matlab/Pose2SLAMExample_easy.m index 6c359f314..b0200946b 100644 --- a/examples/matlab/Pose2SLAMExample_easy.m +++ b/examples/matlab/Pose2SLAMExample_easy.m @@ -18,24 +18,21 @@ % - We have full odometry for measurements % - The robot is on a grid, moving 2 meters each step -%% Create keys for variables -x1 = 1; x2 = 2; x3 = 3; - %% Create graph container and add factors to it graph = pose2SLAMGraph; %% Add prior % gaussian for prior -prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); -prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin -graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph +priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); +priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin +graph.addPrior(1, priorMean, priorNoise); % add directly to graph %% Add odometry % general noisemodel for odometry -odom_model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); -odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) -graph.addOdometry(x1, x2, odom_measurement, odom_model); -graph.addOdometry(x2, x3, odom_measurement, odom_model); +odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); +odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) +graph.addOdometry(1, 2, odometry, odometryNoise); +graph.addOdometry(2, 3, odometry, odometryNoise); %% Add measurements % general noisemodel for measurements @@ -46,9 +43,9 @@ graph.print('full graph'); %% Initialize to noisy points initialEstimate = pose2SLAMValues; -initialEstimate.insertPose(x1, gtsamPose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(x2, gtsamPose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(x3, gtsamPose2(4.1, 0.1, 0.1)); +initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2)); +initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2)); +initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1)); initialEstimate.print('initial estimate'); diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index a91e6b576..76b1f7c82 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -51,17 +51,17 @@ TEST(Marginals, planarSLAMmarginals) { /* add prior */ // gaussian for prior - SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - graph.add(PriorFactor(x1, prior_measurement, prior_model)); // add the factor to the graph + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin + graph.add(PriorFactor(x1, priorMean, priorNoise)); // add the factor to the graph /* add odometry */ // general noisemodel for odometry - SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) + 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 - graph.add(BetweenFactor(x1, x2, odom_measurement, odom_model)); - graph.add(BetweenFactor(x2, x3, odom_measurement, odom_model)); + graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); + graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); /* add measurements */ // general noisemodel for measurements