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

Merged in fix/283_concurrent (pull request #429) 

close issue #283
Fix/283 concurrent
release/4.3a0
Frank Dellaert 2019-05-17 04:38:30 +00:00
parent 340fe9afc8 209bc23abe
commit 7a223bf07e
2 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp
View File

@ -84,7 +84,7 @@ int main(int argc, char** argv) {
// Create a prior on the first pose, placing it at the origin // Create a prior on the first pose, placing it at the origin
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
Key priorKey = 0; Key priorKey = 0;
newFactors.push_back(PriorFactor<Pose2>(priorKey, priorMean, priorNoise)); newFactors.push_back(PriorFactor<Pose2>(priorKey, priorMean, priorNoise));
newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior
@ -110,11 +110,11 @@ int main(int argc, char** argv) {
// Add odometry factors from two different sources with different error stats // Add odometry factors from two different sources with different error stats
Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05)); auto odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); auto odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Unlike the fixed-lag versions, the concurrent filter implementation // Unlike the fixed-lag versions, the concurrent filter implementation
@ -159,7 +159,7 @@ int main(int argc, char** argv) {
Key loopKey1(1000 * (0.0)); Key loopKey1(1000 * (0.0));
Key loopKey2(1000 * (5.0)); Key loopKey2(1000 * (5.0));
Pose2 loopMeasurement = Pose2(9.5, 1.00, 0.00); Pose2 loopMeasurement = Pose2(9.5, 1.00, 0.00);
noiseModel::Diagonal::shared_ptr loopNoise = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.25)); auto loopNoise = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.25));
NonlinearFactor::shared_ptr loopFactor(new BetweenFactor<Pose2>(loopKey1, loopKey2, loopMeasurement, loopNoise)); NonlinearFactor::shared_ptr loopFactor(new BetweenFactor<Pose2>(loopKey1, loopKey2, loopMeasurement, loopNoise));
// This measurement cannot be added directly to the concurrent filter because it connects a filter state to a smoother state // This measurement cannot be added directly to the concurrent filter because it connects a filter state to a smoother state
@ -189,11 +189,11 @@ int main(int argc, char** argv) {
// Add odometry factors from two different sources with different error stats // Add odometry factors from two different sources with different error stats
Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05)); auto odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); auto odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Unlike the fixed-lag versions, the concurrent filter implementation // Unlike the fixed-lag versions, the concurrent filter implementation
@ -262,11 +262,11 @@ int main(int argc, char** argv) {
// Add odometry factors from two different sources with different error stats // Add odometry factors from two different sources with different error stats
Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05)); auto odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); auto odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Unlike the fixed-lag versions, the concurrent filter implementation // Unlike the fixed-lag versions, the concurrent filter implementation
@ -309,19 +309,19 @@ int main(int argc, char** argv) {
// And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds
cout << "After 15.0 seconds, each version contains to the following keys:" << endl; cout << "After 15.0 seconds, each version contains to the following keys:" << endl;
cout << " Concurrent Filter Keys: " << endl; cout << " Concurrent Filter Keys: " << endl;
for(const Values::ConstKeyValuePair& key_value: concurrentFilter.getLinearizationPoint()) { for(const auto& key_value: concurrentFilter.getLinearizationPoint()) {
cout << setprecision(5) << " Key: " << key_value.key << endl; cout << setprecision(5) << " Key: " << key_value.key << endl;
} }
cout << " Concurrent Smoother Keys: " << endl; cout << " Concurrent Smoother Keys: " << endl;
for(const Values::ConstKeyValuePair& key_value: concurrentSmoother.getLinearizationPoint()) { for(const auto& key_value: concurrentSmoother.getLinearizationPoint()) {
cout << setprecision(5) << " Key: " << key_value.key << endl; cout << setprecision(5) << " Key: " << key_value.key << endl;
} }
cout << " Fixed-Lag Smoother Keys: " << endl; cout << " Fixed-Lag Smoother Keys: " << endl;
for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: fixedlagSmoother.timestamps()) { for(const auto& key_timestamp: fixedlagSmoother.timestamps()) {
cout << setprecision(5) << " Key: " << key_timestamp.first << endl; cout << setprecision(5) << " Key: " << key_timestamp.first << endl;
} }
cout << " Batch Smoother Keys: " << endl; cout << " Batch Smoother Keys: " << endl;
for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: batchSmoother.timestamps()) { for(const auto& key_timestamp: batchSmoother.timestamps()) {
cout << setprecision(5) << " Key: " << key_timestamp.first << endl; cout << setprecision(5) << " Key: " << key_timestamp.first << endl;
} }

2
matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m
View File

@ -107,7 +107,7 @@ for time = deltaT : deltaT : 10.0
%% 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.atPose2(currentKey).print('Concurrent Estimate: ');
%% Plot Covariance Ellipses %% Plot Covariance Ellipses
cla; cla;