From 75803f02294e2136e8af2f2a7f11455e022731fb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 21 May 2013 21:22:55 +0000 Subject: [PATCH] smart indent, and change push_back to add --- .../ConcurrentFilteringAndSmoothingExample.m | 169 +++++++++--------- 1 file changed, 85 insertions(+), 84 deletions(-) diff --git a/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m b/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m index bb6e63dc6..d1d225545 100644 --- a/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m +++ b/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m @@ -1,14 +1,14 @@ % ---------------------------------------------------------------------------- -% -% 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 -% +% % -------------------------------------------------------------------------- - + % @file ConcurrentFilteringAndSmoothingExample.m % @brief Demonstration of the concurrent filtering and smoothing architecture using % a planar robot example and multiple odometry-like sensors @@ -19,6 +19,7 @@ % - The robot moves forward at 2m/s % - We have measurements between each pose from multiple odometry sensors +clear all; clear all; import gtsam.*; @@ -33,7 +34,7 @@ concurrentSmoother = ConcurrentBatchSmoother; % will be sent to the smoothers newFactors = NonlinearFactorGraph; newValues = Values; - + %% Create a prior on the first pose, placing it at the origin priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3 ; 0.3 ; 0.1]); @@ -48,84 +49,84 @@ concurrentFilter.update(newFactors, newValues); % and adding them to the fixed-lag smoothers deltaT = 0.25; for time = deltaT : deltaT : 10.0 - - %% Initialize factor and values for this loop iteration - newFactors = NonlinearFactorGraph; - newValues = Values; - - %% Define the keys related to this timestamp - previousKey = uint64(1000 * (time-deltaT)); - currentKey = uint64(1000 * (time)); - - %% Add a guess for this pose to the new values - % Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s] - % {This is not a particularly good way to guess, but this is just an example} - currentPose = Pose2(time * 2.0, 0.0, 0.0); - newValues.insert(currentKey, currentPose); - - %% Add odometry factors from two different sources with different error stats - odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - odometryNoise1 = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.05]); - newFactors.add(BetweenFactorPose2(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); - - odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - odometryNoise2 = noiseModel.Isotropic.Sigma(3, 0.05); - newFactors.add(BetweenFactorPose2(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); - - %% Unlike the fixed-lag versions, the concurrent filter implementation - % requires the user to supply the specify which keys to move to the smoother - oldKeys = KeyList; - if time >= lag+deltaT - oldKeys.push_back(uint64(1000 * (time-lag-deltaT))); - end - - %% Update the various inference engines - concurrentFilter.update(newFactors, newValues, oldKeys); - - %% Add a loop closure to the smoother at a particular time - if time == 8.0 - % Now lets create a "loop closure" factor between some poses - loopKey1 = uint64(1000 * (0.0)); - loopKey2 = uint64(1000 * (5.0)); - loopMeasurement = Pose2(9.5, 1.00, 0.00); - loopNoise = noiseModel.Diagonal.Sigmas([0.5; 0.5; 0.25]); - loopFactor = BetweenFactorPose2(loopKey1, loopKey2, loopMeasurement, loopNoise); - % The state at 5.0s should have been transferred to the concurrent smoother at this point. Add the loop closure. - smootherFactors = NonlinearFactorGraph; - smootherFactors.push_back(loopFactor); - concurrentSmoother.update(smootherFactors, Values()); - end - - %% Manually synchronize the Concurrent Filter and Smoother every 1.0 s - if mod(time, 1.0) < 0.01 - % Synchronize the Filter and Smoother - concurrentSmoother.update(); - synchronize(concurrentFilter, concurrentSmoother); - end - - %% Print the filter optimized poses - fprintf(1, 'Timestamp = %5.3f\n', time); - filterResult = concurrentFilter.calculateEstimate; - filterResult.at(currentKey).print('Concurrent Estimate: '); - - %% Plot Covariance Ellipses - cla; - hold on - filterMarginals = Marginals(concurrentFilter.getFactors, filterResult); - plot2DTrajectory(filterResult, 'k*-', filterMarginals); - - smootherGraph = concurrentSmoother.getFactors; - if smootherGraph.size > 0 - smootherResult = concurrentSmoother.calculateEstimate; - smootherMarginals = Marginals(smootherGraph, smootherResult); - plot2DTrajectory(smootherResult, 'r*-', smootherMarginals); - end - - axis equal - axis tight - view(2) -pause -end + %% Initialize factor and values for this loop iteration + newFactors = NonlinearFactorGraph; + newValues = Values; + + %% Define the keys related to this timestamp + previousKey = uint64(1000 * (time-deltaT)); + currentKey = uint64(1000 * (time)); + + %% Add a guess for this pose to the new values + % Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s] + % {This is not a particularly good way to guess, but this is just an example} + currentPose = Pose2(time * 2.0, 0.0, 0.0); + newValues.insert(currentKey, currentPose); + + %% Add odometry factors from two different sources with different error stats + odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); + odometryNoise1 = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.05]); + newFactors.add(BetweenFactorPose2(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); + + odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); + odometryNoise2 = noiseModel.Isotropic.Sigma(3, 0.05); + newFactors.add(BetweenFactorPose2(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); + + %% Unlike the fixed-lag versions, the concurrent filter implementation + % requires the user to supply the specify which keys to move to the smoother + oldKeys = KeyList; + if time >= lag+deltaT + oldKeys.push_back(uint64(1000 * (time-lag-deltaT))); + end + + %% Update the various inference engines + concurrentFilter.update(newFactors, newValues, oldKeys); + + %% Add a loop closure to the smoother at a particular time + if time == 8.0 + % Now lets create a "loop closure" factor between some poses + loopKey1 = uint64(1000 * (0.0)); + loopKey2 = uint64(1000 * (5.0)); + loopMeasurement = Pose2(9.5, 1.00, 0.00); + loopNoise = noiseModel.Diagonal.Sigmas([0.5; 0.5; 0.25]); + loopFactor = BetweenFactorPose2(loopKey1, loopKey2, loopMeasurement, loopNoise); + % The state at 5.0s should have been transferred to the concurrent smoother at this point. Add the loop closure. + smootherFactors = NonlinearFactorGraph; + smootherFactors.add(loopFactor); + concurrentSmoother.update(smootherFactors, Values()); + end + + %% Manually synchronize the Concurrent Filter and Smoother every 1.0 s + if mod(time, 1.0) < 0.01 + % Synchronize the Filter and Smoother + concurrentSmoother.update(); + synchronize(concurrentFilter, concurrentSmoother); + end + + %% Print the filter optimized poses + fprintf(1, 'Timestamp = %5.3f\n', time); + filterResult = concurrentFilter.calculateEstimate; + filterResult.at(currentKey).print('Concurrent Estimate: '); + + %% Plot Covariance Ellipses + cla; + hold on + filterMarginals = Marginals(concurrentFilter.getFactors, filterResult); + plot2DTrajectory(filterResult, 'k*-', filterMarginals); + + smootherGraph = concurrentSmoother.getFactors; + if smootherGraph.size > 0 + smootherResult = concurrentSmoother.calculateEstimate; + smootherMarginals = Marginals(smootherGraph, smootherResult); + plot2DTrajectory(smootherResult, 'r*-', smootherMarginals); + end + + axis equal + axis tight + view(2) + pause(0.01) +end +