testKruskal.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/kruskal.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>

#include <list>
#include <memory>
#include <vector>

gtsam::GaussianFactorGraph makeTestGaussianFactorGraph() {
  using namespace gtsam;
  using namespace symbol_shorthand;

  GaussianFactorGraph gfg;
  Matrix I = I_2x2;
  Vector2 b(0, 0);
  const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5));
  gfg.emplace_shared<JacobianFactor>(X(1), I, X(2), I, b, model);
  gfg.emplace_shared<JacobianFactor>(X(1), I, X(3), I, b, model);
  gfg.emplace_shared<JacobianFactor>(X(1), I, X(4), I, b, model);
  gfg.emplace_shared<JacobianFactor>(X(2), I, X(3), I, b, model);
  gfg.emplace_shared<JacobianFactor>(X(2), I, X(4), I, b, model);
  gfg.emplace_shared<JacobianFactor>(X(3), I, X(4), I, b, model);

  return gfg;
}

gtsam::NonlinearFactorGraph makeTestNonlinearFactorGraph() {
  using namespace gtsam;
  using namespace symbol_shorthand;

  NonlinearFactorGraph nfg;
  const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5));
  nfg.emplace_shared<BetweenFactor<Rot3>>(X(1), X(2), Rot3(), model);
  nfg.emplace_shared<BetweenFactor<Rot3>>(X(1), X(3), Rot3(), model);
  nfg.emplace_shared<BetweenFactor<Rot3>>(X(1), X(4), Rot3(), model);
  nfg.emplace_shared<BetweenFactor<Rot3>>(X(2), X(3), Rot3(), model);
  nfg.emplace_shared<BetweenFactor<Rot3>>(X(2), X(4), Rot3(), model);
  nfg.emplace_shared<BetweenFactor<Rot3>>(X(3), X(4), Rot3(), model);

  return nfg;
}

/* ************************************************************************* */
TEST(kruskal, GaussianFactorGraph) {
  using namespace gtsam;

  // Create factor graph.
  const auto g = makeTestGaussianFactorGraph();

  // Assign weights to all the edges in the graph.
  const auto weights = std::vector<double>(g.size(), 1.0);

  const auto mstEdgeIndices = utils::kruskal(g, weights);

  EXPECT(mstEdgeIndices[0] == 0);
  EXPECT(mstEdgeIndices[1] == 1);
  EXPECT(mstEdgeIndices[2] == 2);
}

/* ************************************************************************* */
TEST(kruskal, NonlinearFactorGraph) {
  using namespace gtsam;

  // Create factor graph.
  const auto g = makeTestNonlinearFactorGraph();

  // Assign weights to all the edges in the graph.
  const auto weights = std::vector<double>(g.size(), 1.0);

  const auto mstEdgeIndices = utils::kruskal(g, weights);

  EXPECT(mstEdgeIndices[0] == 0);
  EXPECT(mstEdgeIndices[1] == 1);
  EXPECT(mstEdgeIndices[2] == 2);
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */