testBinaryMeasurement.cpp

Go to the documentation of this file.

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

 * GTSAM Copyright 2010-2020, 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 <gtsam/sfm/BinaryMeasurement.h>

#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Pose3.h>

#include <CppUnitLite/TestHarness.h>

using namespace std;
using namespace gtsam;

static const Key kKey1(2), kKey2(1);

// Create noise models for unit3 and rot3
static SharedNoiseModel unit3_model(noiseModel::Isotropic::Sigma(2, 0.05));
static SharedNoiseModel rot3_model(noiseModel::Isotropic::Sigma(3, 0.05));

const Unit3 unit3Measured(Vector3(1, 1, 1));
const Rot3 rot3Measured;

/* ************************************************************************* */
TEST(BinaryMeasurement, Unit3) {
  BinaryMeasurement<Unit3> unit3Measurement(kKey1, kKey2, unit3Measured,
                                            unit3_model);
  EXPECT_LONGS_EQUAL(unit3Measurement.key1(), kKey1);
  EXPECT_LONGS_EQUAL(unit3Measurement.key2(), kKey2);
  EXPECT(unit3Measurement.measured().equals(unit3Measured));

  BinaryMeasurement<Unit3> unit3MeasurementCopy(kKey1, kKey2, unit3Measured,
                                                unit3_model);
  EXPECT(unit3Measurement.equals(unit3MeasurementCopy));
}

/* ************************************************************************* */
TEST(BinaryMeasurement, Rot3) {
  // testing the accessors
  BinaryMeasurement<Rot3> rot3Measurement(kKey1, kKey2, rot3Measured,
                                          rot3_model);
  EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1);
  EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2);
  EXPECT(rot3Measurement.measured().equals(rot3Measured));

  // testing the equals function
  BinaryMeasurement<Rot3> rot3MeasurementCopy(kKey1, kKey2, rot3Measured,
                                              rot3_model);
  EXPECT(rot3Measurement.equals(rot3MeasurementCopy));
}

/* ************************************************************************* */
TEST(BinaryMeasurement, Rot3MakeRobust) {
  auto huber_model = noiseModel::Robust::Create(
      noiseModel::mEstimator::Huber::Create(1.345), rot3_model);
  BinaryMeasurement<Rot3> rot3Measurement(kKey1, kKey2, rot3Measured,
                                          huber_model);

  EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1);
  EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2);
  EXPECT(rot3Measurement.measured().equals(rot3Measured));
  const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(
      rot3Measurement.noiseModel());
  EXPECT(robust);
}

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