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

Merge pull request #797 from acxz/timing-fix

release/4.3a0
Varun Agrawal 2021-06-16 23:32:05 -04:00 committed by GitHub
parent ddbe62eda0 7bdaff3cd8
commit 3b4eeebb8c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
timing/timeLago.cpp
View File

@ -41,11 +41,11 @@ int main(int argc, char *argv[]) {
// add noise to create initial estimate
Values initial;
Sampler sampler(42u);
Values::ConstFiltered<Pose2> poses = solution->filter<Pose2>();
SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished());
Sampler sampler(noise);
for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: poses)
initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise)));
initial.insert(it.key, it.value.retract(sampler.sample()));
// Add prior on the pose having index (key) = 0
noiseModel::Diagonal::shared_ptr priorModel = //