2016-06-11 08:48:42 +08:00
|
|
|
// add unary measurement factors, like GPS, on all three poses
|
2022-03-21 01:24:15 +08:00
|
|
|
auto unaryNoise =
|
2016-06-16 03:00:24 +08:00
|
|
|
noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
|
2022-03-21 01:24:15 +08:00
|
|
|
graph.emplace_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise);
|
|
|
|
|
graph.emplace_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise);
|
|
|
|
|
graph.emplace_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise);
|
2016-06-11 08:48:42 +08:00
|
|
|
|
