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

Adapt to 4.2 function

release/4.3a0
Fan Jiang 2024-08-16 13:16:26 -04:00
parent 110086749f
commit 6e8952b5e8
1 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
tests/testGaussianISAM2.cpp
View File

@ -11,6 +11,7 @@
#include <gtsam/sam/BearingRangeFactor.h> #include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Marginals.h> #include <gtsam/nonlinear/Marginals.h>
@ -880,16 +881,16 @@ TEST(ISAM2, marginalizeLeaves6)
// Create a grid of pose variables // Create a grid of pose variables
for (int i = 0; i < gridDim; ++i) { for (int i = 0; i < gridDim; ++i) {
for (int j = 0; j < gridDim; ++j) { for (int j = 0; j < gridDim; ++j) {
Pose3 pose = Pose3(Rot3::identity(), Point3(i, j, 0)); Pose3 pose = Pose3(Rot3::Identity(), Point3(i, j, 0));
Key key = idxToKey(i, j); Key key = idxToKey(i, j);
// Place a prior on the first pose // Place a prior on the first pose
factors.addPrior(key, Pose3(Rot3::identity(), Point3(i, j, 0)), nm); factors.addPrior(key, Pose3(Rot3::Identity(), Point3(i, j, 0)), nm);
values.insert(key, pose); values.insert(key, pose);
if (i > 0) { if (i > 0) {
factors.emplace_shared<BetweenFactor<Pose3>>(idxToKey(i - 1, j), key, Pose3(Rot3::identity(), Point3(1, 0, 0)),nm); factors.emplace_shared<BetweenFactor<Pose3>>(idxToKey(i - 1, j), key, Pose3(Rot3::Identity(), Point3(1, 0, 0)),nm);
} }
if (j > 0) { if (j > 0) {
factors.emplace_shared<BetweenFactor<Pose3>>(idxToKey(i, j - 1), key, Pose3(Rot3::identity(), Point3(0, 1, 0)),nm); factors.emplace_shared<BetweenFactor<Pose3>>(idxToKey(i, j - 1), key, Pose3(Rot3::Identity(), Point3(0, 1, 0)),nm);
} }
} }
} }
@ -923,10 +924,10 @@ TEST(ISAM2, MarginalizeRoot)
ISAM2 isam; ISAM2 isam;
// Create a factor graph with one variable and a prior // Create a factor graph with one variable and a prior
Pose3 root = Pose3::identity(); Pose3 root = Pose3::Identity();
Key rootKey(0); Key rootKey(0);
values.insert(rootKey, root); values.insert(rootKey, root);
factors.addPrior(rootKey, Pose3::identity(), nm); factors.addPrior(rootKey, Pose3::Identity(), nm);
// Optimize the graph // Optimize the graph
EXPECT(updateAndMarginalize(factors, values, {}, isam)); EXPECT(updateAndMarginalize(factors, values, {}, isam));
@ -956,13 +957,13 @@ TEST(ISAM2, marginalizationSize)
// Create a pose variable // Create a pose variable
Key aKey(0); Key aKey(0);
values.insert(aKey, Pose3::identity()); values.insert(aKey, Pose3::Identity());
factors.addPrior(aKey, Pose3::identity(), nm); factors.addPrior(aKey, Pose3::Identity(), nm);
// Create another pose variable linked to the first // Create another pose variable linked to the first
Pose3 b = Pose3::identity(); Pose3 b = Pose3::Identity();
Key bKey(1); Key bKey(1);
values.insert(bKey, Pose3::identity()); values.insert(bKey, Pose3::Identity());
factors.emplace_shared<BetweenFactor<Pose3>>(aKey, bKey, Pose3::identity(), nm); factors.emplace_shared<BetweenFactor<Pose3>>(aKey, bKey, Pose3::Identity(), nm);
// Optimize the graph // Optimize the graph
EXPECT(updateAndMarginalize(factors, values, {}, isam)); EXPECT(updateAndMarginalize(factors, values, {}, isam));