12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

smart indent, and change push_back to add

release/4.3a0
Frank Dellaert 2013-05-21 21:22:55 +00:00
parent 8e26da7396
commit 75803f0229
1 changed files with 85 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

129
matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m
View File

@ -19,6 +19,7 @@
% - The robot moves forward at 2m/s % - The robot moves forward at 2m/s
% - We have measurements between each pose from multiple odometry sensors % - We have measurements between each pose from multiple odometry sensors
clear all;
clear all; clear all;
import gtsam.*; import gtsam.*;
@ -49,82 +50,82 @@ concurrentFilter.update(newFactors, newValues);
deltaT = 0.25; deltaT = 0.25;
for time = deltaT : deltaT : 10.0 for time = deltaT : deltaT : 10.0
%% Initialize factor and values for this loop iteration %% Initialize factor and values for this loop iteration
newFactors = NonlinearFactorGraph; newFactors = NonlinearFactorGraph;
newValues = Values; newValues = Values;
%% Define the keys related to this timestamp %% Define the keys related to this timestamp
previousKey = uint64(1000 * (time-deltaT)); previousKey = uint64(1000 * (time-deltaT));
currentKey = uint64(1000 * (time)); currentKey = uint64(1000 * (time));
%% Add a guess for this pose to the new values %% 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] % 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} % {This is not a particularly good way to guess, but this is just an example}
currentPose = Pose2(time * 2.0, 0.0, 0.0); currentPose = Pose2(time * 2.0, 0.0, 0.0);
newValues.insert(currentKey, currentPose); newValues.insert(currentKey, currentPose);
%% Add odometry factors from two different sources with different error stats %% Add odometry factors from two different sources with different error stats
odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
odometryNoise1 = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.05]); odometryNoise1 = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.05]);
newFactors.add(BetweenFactorPose2(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); newFactors.add(BetweenFactorPose2(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
odometryNoise2 = noiseModel.Isotropic.Sigma(3, 0.05); odometryNoise2 = noiseModel.Isotropic.Sigma(3, 0.05);
newFactors.add(BetweenFactorPose2(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); newFactors.add(BetweenFactorPose2(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
%% Unlike the fixed-lag versions, the concurrent filter implementation %% Unlike the fixed-lag versions, the concurrent filter implementation
% requires the user to supply the specify which keys to move to the smoother % requires the user to supply the specify which keys to move to the smoother
oldKeys = KeyList; oldKeys = KeyList;
if time >= lag+deltaT if time >= lag+deltaT
oldKeys.push_back(uint64(1000 * (time-lag-deltaT))); oldKeys.push_back(uint64(1000 * (time-lag-deltaT)));
end end
%% Update the various inference engines %% Update the various inference engines
concurrentFilter.update(newFactors, newValues, oldKeys); concurrentFilter.update(newFactors, newValues, oldKeys);
%% Add a loop closure to the smoother at a particular time %% Add a loop closure to the smoother at a particular time
if time == 8.0 if time == 8.0
% Now lets create a "loop closure" factor between some poses % Now lets create a "loop closure" factor between some poses
loopKey1 = uint64(1000 * (0.0)); loopKey1 = uint64(1000 * (0.0));
loopKey2 = uint64(1000 * (5.0)); loopKey2 = uint64(1000 * (5.0));
loopMeasurement = Pose2(9.5, 1.00, 0.00); loopMeasurement = Pose2(9.5, 1.00, 0.00);
loopNoise = noiseModel.Diagonal.Sigmas([0.5; 0.5; 0.25]); loopNoise = noiseModel.Diagonal.Sigmas([0.5; 0.5; 0.25]);
loopFactor = BetweenFactorPose2(loopKey1, loopKey2, loopMeasurement, loopNoise); 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. % The state at 5.0s should have been transferred to the concurrent smoother at this point. Add the loop closure.
smootherFactors = NonlinearFactorGraph; smootherFactors = NonlinearFactorGraph;
smootherFactors.push_back(loopFactor); smootherFactors.add(loopFactor);
concurrentSmoother.update(smootherFactors, Values()); concurrentSmoother.update(smootherFactors, Values());
end end
%% Manually synchronize the Concurrent Filter and Smoother every 1.0 s %% Manually synchronize the Concurrent Filter and Smoother every 1.0 s
if mod(time, 1.0) < 0.01 if mod(time, 1.0) < 0.01
% Synchronize the Filter and Smoother % Synchronize the Filter and Smoother
concurrentSmoother.update(); concurrentSmoother.update();
synchronize(concurrentFilter, concurrentSmoother); synchronize(concurrentFilter, concurrentSmoother);
end end
%% Print the filter optimized poses %% Print the filter optimized poses
fprintf(1, 'Timestamp = %5.3f\n', time); fprintf(1, 'Timestamp = %5.3f\n', time);
filterResult = concurrentFilter.calculateEstimate; filterResult = concurrentFilter.calculateEstimate;
filterResult.at(currentKey).print('Concurrent Estimate: '); filterResult.at(currentKey).print('Concurrent Estimate: ');
%% Plot Covariance Ellipses %% Plot Covariance Ellipses
cla; cla;
hold on hold on
filterMarginals = Marginals(concurrentFilter.getFactors, filterResult); filterMarginals = Marginals(concurrentFilter.getFactors, filterResult);
plot2DTrajectory(filterResult, 'k*-', filterMarginals); plot2DTrajectory(filterResult, 'k*-', filterMarginals);
smootherGraph = concurrentSmoother.getFactors; smootherGraph = concurrentSmoother.getFactors;
if smootherGraph.size > 0 if smootherGraph.size > 0
smootherResult = concurrentSmoother.calculateEstimate; smootherResult = concurrentSmoother.calculateEstimate;
smootherMarginals = Marginals(smootherGraph, smootherResult); smootherMarginals = Marginals(smootherGraph, smootherResult);
plot2DTrajectory(smootherResult, 'r*-', smootherMarginals); plot2DTrajectory(smootherResult, 'r*-', smootherMarginals);
end end
axis equal axis equal
axis tight axis tight
view(2) view(2)
pause pause(0.01)
end end