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

explicitly initialize Point3

release/4.3a0
Varun Agrawal 2020-12-10 10:07:49 -05:00
parent 79aedfbf7e
commit 43b0225557
1 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/slam/tests/testInitializePose3.cpp
View File

@ -75,8 +75,14 @@ NonlinearFactorGraph graph2() {
g.add(BetweenFactor<Pose3>(x0, x1, pose0.between(pose1), noiseModel::Isotropic::Precision(6, 1.0)));
g.add(BetweenFactor<Pose3>(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0)));
g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0)));
g.add(BetweenFactor<Pose3>(x2, x0, Pose3(Rot3::Ypr(0.1,0,0.1), Point3()), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero information
g.add(BetweenFactor<Pose3>(x0, x3, Pose3(Rot3::Ypr(0.5,-0.2,0.2), Point3(10,20,30)), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero informatoin
// random pose, but zero information
g.add(BetweenFactor<Pose3>(x2, x0,
Pose3(Rot3::Ypr(0.1, 0, 0.1), Point3(0, 0, 0)),
noiseModel::Isotropic::Precision(6, 0.0)));
// random pose, but zero informatoin
g.add(BetweenFactor<Pose3>(
x0, x3, Pose3(Rot3::Ypr(0.5, -0.2, 0.2), Point3(10, 20, 30)),
noiseModel::Isotropic::Precision(6, 0.0)));
g.addPrior(x0, pose0, model);
return g;
}