From 6895481f0dcd29d7d991067e73a934c8f8d5fc66 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 3 Jun 2012 20:12:12 +0000 Subject: [PATCH] Synchronized C++ and MATLAB --- examples/Pose2SLAMExample.cpp | 6 ++--- examples/matlab/Pose2SLAMExample_easy.m | 34 +++++++++++++----------- examples/matlab/Pose2SLAMExample_graph.m | 4 +-- 3 files changed, 23 insertions(+), 21 deletions(-) diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 851363b2e..265e323db 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -47,18 +47,18 @@ int main(int argc, char** argv) { // print graph.print("\nFactor graph:\n"); - // 3. Create the data structure to hold the initialEstimate estinmate to the solution + // 3. Create the data structure to hold the initialEstimate estimate 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 "); + initialEstimate.print("\nInitial estimate:\n"); // 4. Single Step Optimization using Levenberg-Marquardt pose2SLAM::Values result = graph.optimize(initialEstimate); - result.print("\nFinal result:\n "); + result.print("\nFinal result:\n"); return 0; } diff --git a/examples/matlab/Pose2SLAMExample_easy.m b/examples/matlab/Pose2SLAMExample_easy.m index b0200946b..2172f5767 100644 --- a/examples/matlab/Pose2SLAMExample_easy.m +++ b/examples/matlab/Pose2SLAMExample_easy.m @@ -1,9 +1,9 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% GTSAM Copyright 2010, Georgia Tech Research Corporation, +% GTSAM Copyright 2010, Georgia Tech Research Corporation, % Atlanta, Georgia 30332-0415 % All Rights Reserved % Authors: Frank Dellaert, et al. (see THANKS for the full author list) -% +% % See LICENSE for the license information % % @brief Simple robotics example using the pre-built planar SLAM domain @@ -23,32 +23,34 @@ graph = pose2SLAMGraph; %% Add prior % gaussian for prior -priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin +priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); graph.addPrior(1, priorMean, priorNoise); % add directly to graph %% Add odometry % general noisemodel for odometry 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); +graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise); +graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); +graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); +graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); -%% Add measurements -% general noisemodel for measurements -meas_model = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]); +%% Add pose constraint +constraintUncertainty = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); +graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), constraintUncertainty); % print -graph.print('full graph'); +graph.print(sprintf('/nFactor graph:\n')); %% Initialize to noisy points initialEstimate = pose2SLAMValues; -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'); +x1 = gtsamPose2(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1); +x2 = gtsamPose2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2); +x3 = gtsamPose2(4.1, 0.1, pi/2); initialEstimate.insertPose(3, x3); +x4 = gtsamPose2(4.0, 2.0, pi ); initialEstimate.insertPose(4, x4); +x5 = gtsamPose2(2.1, 2.1,-pi/2); initialEstimate.insertPose(5, x5); +initialEstimate.print(sprintf('\nInitial estimate:\n')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd result = graph.optimize(initialEstimate); -result.print('final result'); +result.print(sprintf('\nFinal result:\n')); diff --git a/examples/matlab/Pose2SLAMExample_graph.m b/examples/matlab/Pose2SLAMExample_graph.m index 7118efe01..142c9b36d 100644 --- a/examples/matlab/Pose2SLAMExample_graph.m +++ b/examples/matlab/Pose2SLAMExample_graph.m @@ -13,7 +13,7 @@ %% Initialize graph, initial estimate, and odometry noise model = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 5*pi/180]); [graph,initial]=load2D('../Data/w100-odom.graph',model); -initial.print('Initial estimate:\n'); +initial.print(sprintf('Initial estimate:\n')); %% Add a Gaussian prior on pose x_1 priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin @@ -27,7 +27,7 @@ plot(initial.xs(),initial.ys(),'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-*') -result.print('\nFinal result:\n'); +result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses marginals = graph.marginals(result);