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

update timeLago.cpp with newer Sampler interface

release/4.3a0
acxz 2021-06-14 15:33:17 -04:00
parent 94308076ca
commit 7bdaff3cd8
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 = //