testShonanAveraging.cpp

Go to the documentation of this file.

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

 * GTSAM Copyright 2010-2019, 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/sfm/ShonanAveraging.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/FrobeniusFactor.h>

#include <algorithm>
#include <iostream>
#include <map>
#include <random>

using namespace std;
using namespace gtsam;

template <size_t d>
using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;

template <size_t d>
using Pose = typename std::conditional<d == 2, Pose2, Pose3>::type;

ShonanAveraging3 fromExampleName(
    const std::string &name,
    ShonanAveraging3::Parameters parameters = ShonanAveraging3::Parameters()) {
  string g2oFile = findExampleDataFile(name);
  return ShonanAveraging3(g2oFile, parameters);
}

static const ShonanAveraging3 kShonan = fromExampleName("toyExample.g2o");

static std::mt19937 kRandomNumberGenerator(42);

/* ************************************************************************* */
TEST(ShonanAveraging3, checkConstructor) {
  EXPECT_LONGS_EQUAL(5, kShonan.nrUnknowns());

  EXPECT_LONGS_EQUAL(15, kShonan.D().rows());
  EXPECT_LONGS_EQUAL(15, kShonan.D().cols());
  auto D = kShonan.denseD();
  EXPECT_LONGS_EQUAL(15, D.rows());
  EXPECT_LONGS_EQUAL(15, D.cols());

  EXPECT_LONGS_EQUAL(15, kShonan.Q().rows());
  EXPECT_LONGS_EQUAL(15, kShonan.Q().cols());
  auto Q = kShonan.denseQ();
  EXPECT_LONGS_EQUAL(15, Q.rows());
  EXPECT_LONGS_EQUAL(15, Q.cols());

  EXPECT_LONGS_EQUAL(15, kShonan.L().rows());
  EXPECT_LONGS_EQUAL(15, kShonan.L().cols());
  auto L = kShonan.denseL();
  EXPECT_LONGS_EQUAL(15, L.rows());
  EXPECT_LONGS_EQUAL(15, L.cols());
}

/* ************************************************************************* */
TEST(ShonanAveraging3, buildGraphAt) {
  auto graph = kShonan.buildGraphAt(5);
  EXPECT_LONGS_EQUAL(7, graph.size());
}

/* ************************************************************************* */
TEST(ShonanAveraging3, checkOptimality) {
  const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
  Values random = ShonanAveraging3::LiftTo<Rot3>(4, randomRotations);  // lift to 4!
  auto Lambda = kShonan.computeLambda(random);
  EXPECT_LONGS_EQUAL(15, Lambda.rows());
  EXPECT_LONGS_EQUAL(15, Lambda.cols());
  EXPECT_LONGS_EQUAL(45, Lambda.nonZeros());
  auto lambdaMin = kShonan.computeMinEigenValue(random);
  // EXPECT_DOUBLES_EQUAL(-5.2964625490657866, lambdaMin,
  //                      1e-4);  // Regression test
  EXPECT_DOUBLES_EQUAL(-414.87376657555996, lambdaMin,
                       1e-4); // Regression test
  EXPECT(!kShonan.checkOptimality(random));
}

/* ************************************************************************* */
TEST(ShonanAveraging3, checkSubgraph) {
  // Create parameter with solver set to SUBGRAPH
  auto params = ShonanAveragingParameters3(
      gtsam::LevenbergMarquardtParams::CeresDefaults(), "SUBGRAPH");
  ShonanAveraging3::Measurements measurements;

  // The toyExample.g2o has 5 vertices, from 0-4
  // The edges are: 1-2, 2-3, 3-4, 3-1, 1-4, 0-1,
  // which can build a connected graph
  auto subgraphShonan = fromExampleName("toyExample.g2o", params);

  // Create initial random estimation
  Values initial;
  initial = subgraphShonan.initializeRandomly(kRandomNumberGenerator);

  // Run Shonan with SUBGRAPH solver
  auto result = subgraphShonan.run(initial, 3, 3);
  EXPECT_DOUBLES_EQUAL(1e-11, subgraphShonan.cost(result.first), 1e-4);
}

/* ************************************************************************* */
TEST(ShonanAveraging3, tryOptimizingAt3) {
  const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
  Values initial = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations);  // convert to SOn
  EXPECT(!kShonan.checkOptimality(initial));
  const Values result = kShonan.tryOptimizingAt(3, initial);
  EXPECT(kShonan.checkOptimality(result));
  auto lambdaMin = kShonan.computeMinEigenValue(result);
  EXPECT_DOUBLES_EQUAL(-5.427688831332745e-07, lambdaMin,
                       1e-4); // Regression test
  EXPECT_DOUBLES_EQUAL(0, kShonan.costAt(3, result), 1e-4);
  const Values SO3Values = kShonan.roundSolution(result);
  EXPECT_DOUBLES_EQUAL(0, kShonan.cost(SO3Values), 1e-4);
}

/* ************************************************************************* */
TEST(ShonanAveraging3, tryOptimizingAt4) {
  const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
  Values random = ShonanAveraging3::LiftTo<Rot3>(4, randomRotations);  // lift to 4!
  const Values result = kShonan.tryOptimizingAt(4, random);
  EXPECT(kShonan.checkOptimality(result));
  EXPECT_DOUBLES_EQUAL(0, kShonan.costAt(4, result), 1e-3);
  auto lambdaMin = kShonan.computeMinEigenValue(result);
  EXPECT_DOUBLES_EQUAL(-5.427688831332745e-07, lambdaMin,
                       1e-4); // Regression test
  const Values SO3Values = kShonan.roundSolution(result);
  EXPECT_DOUBLES_EQUAL(0, kShonan.cost(SO3Values), 1e-4);
}

/* ************************************************************************* */
TEST(ShonanAveraging3, TangentVectorValues) {
  Vector9 v;
  v << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  Vector expected0(10), expected1(10), expected2(10);
  expected0 << 0, 3, -2, 1, 0, 0, 0, 0, 0, 0;
  expected1 << 0, 6, -5, 4, 0, 0, 0, 0, 0, 0;
  expected2 << 0, 9, -8, 7, 0, 0, 0, 0, 0, 0;
  const VectorValues xi = ShonanAveraging3::TangentVectorValues(5, v);
  EXPECT(assert_equal(expected0, xi[0]));
  EXPECT(assert_equal(expected1, xi[1]));
  EXPECT(assert_equal(expected2, xi[2]));
}

/* ************************************************************************* */
TEST(ShonanAveraging3, LiftTo) {
  auto I = genericValue(Rot3());
  Values initial{{0, I}, {1, I}, {2, I}};
  Values lifted = ShonanAveraging3::LiftTo<Rot3>(5, initial);
  EXPECT(assert_equal(SOn(5), lifted.at<SOn>(0)));
}

/* ************************************************************************* */
TEST(ShonanAveraging3, CheckWithEigen) {
  // control randomness
  static std::mt19937 rng(0);
  Vector descentDirection = Vector::Random(15); // for use below
  const Values randomRotations = kShonan.initializeRandomly(rng);
  Values random = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations);

  // Optimize
  const Values Qstar3 = kShonan.tryOptimizingAt(3, random);

  // Compute Eigenvalue with Spectra solver
  double lambda = kShonan.computeMinEigenValue(Qstar3);

  // Check Eigenvalue with slow Eigen version, converts matrix A to dense matrix!
  const Matrix S = ShonanAveraging3::StiefelElementMatrix(Qstar3);
  auto A = kShonan.computeA(S);
  bool computeEigenvectors = false;
  Eigen::EigenSolver<Matrix> eigenSolver(Matrix(A), computeEigenvectors);
  auto lambdas = eigenSolver.eigenvalues().real();
  double minEigenValue = lambdas(0);
  for (int i = 1; i < lambdas.size(); i++)
      minEigenValue = min(lambdas(i), minEigenValue);

  // Compute Eigenvalue with Accelerated Power method
  double lambdaAP = kShonan.computeMinEigenValueAP(Qstar3);

  // Actual check
  EXPECT_DOUBLES_EQUAL(0, lambda, 1e-11);
  EXPECT_DOUBLES_EQUAL(0, minEigenValue, 1e-11);
  EXPECT_DOUBLES_EQUAL(0, lambdaAP, 1e-11);


  // Construct test descent direction (as minEigenVector is not predictable
  // across platforms, being one from a basically flat 3d- subspace)

  // Check descent
  Values initialQ4 =
      ShonanAveraging3::LiftwithDescent(4, Qstar3, descentDirection);
  EXPECT_LONGS_EQUAL(5, initialQ4.size());

  // TODO(frank): uncomment this regression test: currently not repeatable
  // across platforms.
  // Matrix expected(4, 4);
  // expected << 0.0459224, -0.688689, -0.216922, 0.690321, //
  //     0.92381, 0.191931, 0.255854, 0.21042,              //
  //     -0.376669, 0.301589, 0.687953, 0.542111,           //
  //     -0.0508588, 0.630804, -0.643587, 0.43046;
  // EXPECT(assert_equal(SOn(expected), initialQ4.at<SOn>(0), 1e-5));
}

/* ************************************************************************* */
TEST(ShonanAveraging3, initializeWithDescent) {
  const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
  Values random = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations);
  const Values Qstar3 = kShonan.tryOptimizingAt(3, random);
  Vector minEigenVector;
  double lambdaMin = kShonan.computeMinEigenValue(Qstar3, &minEigenVector);
  Values initialQ4 =
      kShonan.initializeWithDescent(4, Qstar3, minEigenVector, lambdaMin);
  EXPECT_LONGS_EQUAL(5, initialQ4.size());
}

/* ************************************************************************* */
TEST(ShonanAveraging3, run) {
  auto initial = kShonan.initializeRandomly(kRandomNumberGenerator); 
  auto result = kShonan.run(initial, 5);
  EXPECT_DOUBLES_EQUAL(0, kShonan.cost(result.first), 1e-3);
  EXPECT_DOUBLES_EQUAL(-5.427688831332745e-07, result.second,
                       1e-4); // Regression test
}

/* ************************************************************************* */
namespace klaus {
// The data in the file is the Colmap solution
const Rot3 wR0(0.9992281076190063, -0.02676080288219576, -0.024497002638379624,
               -0.015064701622500615);
const Rot3 wR1(0.998239108728862, -0.049543805396343954, -0.03232420352077356,
               -0.004386230477751116);
const Rot3 wR2(0.9925378735259738, -0.07993768981394891, 0.0825062894866454,
               -0.04088089479075661);
} // namespace klaus

TEST(ShonanAveraging3, runKlaus) {
  using namespace klaus;

  // Initialize a Shonan instance without the Karcher mean
  ShonanAveraging3::Parameters parameters;
  parameters.setKarcherWeight(0);

  // Load 3 pose example taken in Klaus by Shicong
  static const ShonanAveraging3 shonan =
      fromExampleName("Klaus3.g2o", parameters);

  // Check nr poses
  EXPECT_LONGS_EQUAL(3, shonan.nrUnknowns());

  // Colmap uses the Y-down vision frame, and the first 3 rotations are close to
  // identity. We check that below. Note tolerance is quite high.
  static const Rot3 identity;
  EXPECT(assert_equal(identity, wR0, 0.2));
  EXPECT(assert_equal(identity, wR1, 0.2));
  EXPECT(assert_equal(identity, wR2, 0.2));

  // Get measurements
  const Rot3 R01 = shonan.measured(0);
  const Rot3 R12 = shonan.measured(1);
  const Rot3 R02 = shonan.measured(2);

  // Regression test to make sure data did not change.
  EXPECT(assert_equal(Rot3(0.9995433591728293, -0.022048798853273946,
                           -0.01796327847857683, 0.010210006313668573),
                      R01));

  // Check Colmap solution agrees OK with relative rotation measurements.
  EXPECT(assert_equal(R01, wR0.between(wR1), 0.1));
  EXPECT(assert_equal(R12, wR1.between(wR2), 0.1));
  EXPECT(assert_equal(R02, wR0.between(wR2), 0.1));

  // Run Shonan (with prior on first rotation)
  auto initial = shonan.initializeRandomly(kRandomNumberGenerator); 
  auto result = shonan.run(initial, 5);
  EXPECT_DOUBLES_EQUAL(0, shonan.cost(result.first), 1e-2);
  EXPECT_DOUBLES_EQUAL(-9.2259161494467889e-05, result.second,
                       1e-4); // Regression

  // Get Shonan solution in new frame R (R for result)
  const Rot3 rR0 = result.first.at<Rot3>(0);
  const Rot3 rR1 = result.first.at<Rot3>(1);
  const Rot3 rR2 = result.first.at<Rot3>(2);

  // rR0 = rRw * wR0 => rRw = rR0 * wR0.inverse()
  // rR1 = rRw * wR1
  // rR2 = rRw * wR2

  const Rot3 rRw = rR0 * wR0.inverse();
  EXPECT(assert_equal(rRw * wR1, rR1, 0.1))
  EXPECT(assert_equal(rRw * wR2, rR2, 0.1))
}

/* ************************************************************************* */
TEST(ShonanAveraging3, runKlausKarcher) {
  using namespace klaus;

  // Load 3 pose example taken in Klaus by Shicong
  static const ShonanAveraging3 shonan = fromExampleName("Klaus3.g2o");

  // Run Shonan (with Karcher mean prior)
  auto initial = shonan.initializeRandomly(kRandomNumberGenerator); 
  auto result = shonan.run(initial, 5);
  EXPECT_DOUBLES_EQUAL(0, shonan.cost(result.first), 1e-2);
  EXPECT_DOUBLES_EQUAL(-1.361402670507772e-05, result.second,
                       1e-4); // Regression test

  // Get Shonan solution in new frame R (R for result)
  const Rot3 rR0 = result.first.at<Rot3>(0);
  const Rot3 rR1 = result.first.at<Rot3>(1);
  const Rot3 rR2 = result.first.at<Rot3>(2);

  const Rot3 rRw = rR0 * wR0.inverse();
  EXPECT(assert_equal(rRw * wR1, rR1, 0.1))
  EXPECT(assert_equal(rRw * wR2, rR2, 0.1))
}

/* ************************************************************************* */
TEST(ShonanAveraging2, noisyToyGraph) {
  // Load 2D toy example
  auto lmParams = LevenbergMarquardtParams::CeresDefaults();
  // lmParams.setVerbosityLM("SUMMARY");
  string g2oFile = findExampleDataFile("noisyToyGraph.txt");
  ShonanAveraging2::Parameters parameters(lmParams);
  auto measurements = parseMeasurements<Rot2>(g2oFile);
  ShonanAveraging2 shonan(measurements, parameters);
  EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns());

  // Check graph building
  NonlinearFactorGraph graph = shonan.buildGraphAt(2);
  EXPECT_LONGS_EQUAL(6, graph.size());
  auto initial = shonan.initializeRandomly(kRandomNumberGenerator); 
  auto result = shonan.run(initial, 2);
  EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6);
  EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate!
}

/* ************************************************************************* */
TEST(ShonanAveraging2, noisyToyGraphWithHuber) {
  // Load 2D toy example
  auto lmParams = LevenbergMarquardtParams::CeresDefaults();
  string g2oFile = findExampleDataFile("noisyToyGraph.txt");
  ShonanAveraging2::Parameters parameters(lmParams);
  auto measurements = parseMeasurements<Rot2>(g2oFile);
  parameters.setUseHuber(true);
  parameters.setCertifyOptimality(false);

  string parameters_print =
      " ShonanAveragingParameters: \n alpha: 0\n beta: 1\n gamma: 0\n "
      "useHuber: 1\n";
  assert_print_equal(parameters_print, parameters);

  ShonanAveraging2 shonan(measurements, parameters);
  EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns());

  // Check graph building
  NonlinearFactorGraph graph = shonan.buildGraphAt(2);
  EXPECT_LONGS_EQUAL(6, graph.size());

  // test that each factor is actually robust
  for (size_t i=0; i<=4; i++) { // note: last is the Gauge factor and is not robust
          const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(
                          std::dynamic_pointer_cast<NoiseModelFactor>(graph[i])->noiseModel());
          EXPECT(robust); // we expect the factors to be use a robust noise model (in particular, Huber)
  }

  // test result
  auto initial = shonan.initializeRandomly(kRandomNumberGenerator);
  auto result = shonan.run(initial, 2,2);
  EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6);
  EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate!
}

/* ************************************************************************* */
// Test alpha/beta/gamma prior weighting.
TEST(ShonanAveraging3, PriorWeights) {
  auto lmParams = LevenbergMarquardtParams::CeresDefaults();
  ShonanAveraging3::Parameters params(lmParams);
  EXPECT_DOUBLES_EQUAL(0, params.alpha, 1e-9);
  EXPECT_DOUBLES_EQUAL(1, params.beta, 1e-9);
  EXPECT_DOUBLES_EQUAL(0, params.gamma, 1e-9);
  double alpha = 100.0, beta = 200.0, gamma = 300.0;
  params.setAnchorWeight(alpha);
  params.setKarcherWeight(beta);
  params.setGaugesWeight(gamma);
  EXPECT_DOUBLES_EQUAL(alpha, params.alpha, 1e-9);
  EXPECT_DOUBLES_EQUAL(beta, params.beta, 1e-9);
  EXPECT_DOUBLES_EQUAL(gamma, params.gamma, 1e-9);
  params.setKarcherWeight(0);
  static const ShonanAveraging3 shonan = fromExampleName("Klaus3.g2o", params);
  for (auto i : {0,1,2}) {
    const auto& m = shonan.measurement(i);
    auto isotropic =
        std::static_pointer_cast<noiseModel::Isotropic>(m.noiseModel());
    CHECK(isotropic != nullptr);
    EXPECT_LONGS_EQUAL(3, isotropic->dim());
    EXPECT_DOUBLES_EQUAL(0.2, isotropic->sigma(), 1e-9);
  }
  auto I = genericValue(Rot3());
  Values initial{{0, I}, {1, I}, {2, I}};
  EXPECT_DOUBLES_EQUAL(3.0756, shonan.cost(initial), 1e-4);
  auto result = shonan.run(initial, 3, 3);
  EXPECT_DOUBLES_EQUAL(0.0015, shonan.cost(result.first), 1e-4);
}
/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */