testKruskal.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 #include <gtsam/base/kruskal.h>
21 #include <gtsam/geometry/Rot3.h>
23 #include <gtsam/inference/Symbol.h>
27 
28 #include <list>
29 #include <memory>
30 #include <vector>
31 
33  using namespace gtsam;
34  using namespace symbol_shorthand;
35 
37  Matrix I = I_2x2;
38  Vector2 b(0, 0);
40  gfg.emplace_shared<JacobianFactor>(X(1), I, X(2), I, b, model);
41  gfg.emplace_shared<JacobianFactor>(X(1), I, X(3), I, b, model);
42  gfg.emplace_shared<JacobianFactor>(X(1), I, X(4), I, b, model);
43  gfg.emplace_shared<JacobianFactor>(X(2), I, X(3), I, b, model);
44  gfg.emplace_shared<JacobianFactor>(X(2), I, X(4), I, b, model);
45  gfg.emplace_shared<JacobianFactor>(X(3), I, X(4), I, b, model);
46 
47  return gfg;
48 }
49 
51  using namespace gtsam;
52  using namespace symbol_shorthand;
53 
62 
63  return nfg;
64 }
65 
66 /* ************************************************************************* */
68  using namespace gtsam;
69 
70  // Create factor graph.
71  const auto g = makeTestGaussianFactorGraph();
72 
73  // Assign weights to all the edges in the graph.
74  const auto weights = std::vector<double>(g.size(), 1.0);
75 
76  const auto mstEdgeIndices = utils::kruskal(g, weights);
77 
78  EXPECT(mstEdgeIndices[0] == 0);
79  EXPECT(mstEdgeIndices[1] == 1);
80  EXPECT(mstEdgeIndices[2] == 2);
81 }
82 
83 /* ************************************************************************* */
85  using namespace gtsam;
86 
87  // Create factor graph.
88  const auto g = makeTestNonlinearFactorGraph();
89 
90  // Assign weights to all the edges in the graph.
91  const auto weights = std::vector<double>(g.size(), 1.0);
92 
93  const auto mstEdgeIndices = utils::kruskal(g, weights);
94 
95  EXPECT(mstEdgeIndices[0] == 0);
96  EXPECT(mstEdgeIndices[1] == 1);
97  EXPECT(mstEdgeIndices[2] == 2);
98 }
99 
100 /* ************************************************************************* */
101 int main() {
102  TestResult tr;
103  return TestRegistry::runAllTests(tr);
104 }
105 /* ************************************************************************* */
Provides additional testing facilities for common data structures.
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
static int runAllTests(TestResult &result)
Factor Graph consisting of non-linear factors.
noiseModel::Diagonal::shared_ptr model
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Key I(std::uint64_t j)
Key X(std::uint64_t j)
gtsam::GaussianFactorGraph makeTestGaussianFactorGraph()
Definition: testKruskal.cpp:32
gtsam::NonlinearFactorGraph makeTestNonlinearFactorGraph()
Definition: testKruskal.cpp:50
std::vector< size_t > kruskal(const FactorGraph< FACTOR > &fg, const std::vector< double > &weights)
Definition: kruskal-inl.h:55
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
void g(const string &key, int i)
Definition: testBTree.cpp:41
int main()
#define EXPECT(condition)
Definition: Test.h:150
Linear Factor Graph where all factors are Gaussians.
const G & b
Definition: Group.h:86
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
traits
Definition: chartTesting.h:28
Eigen::Vector2d Vector2
Definition: Vector.h:42
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:270
TEST(SmartFactorBase, Pinhole)
3D rotation represented as a rotation matrix or quaternion


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:33