testRobust.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/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
 
using namespace gtsam;
 
TEST(RobustNoise, loss) {
  // Keys.
  gtsam::Key x1_key = 1;
  gtsam::Key x2_key = 2;
 
  auto gm = noiseModel::mEstimator::GemanMcClure::Create(1.0);
  auto noise = noiseModel::Robust::Create(gm, noiseModel::Unit::Create(1));
 
  auto factor = PriorFactor<double>(x1_key, 0.0, noise);
  auto between_factor = BetweenFactor<double>(x1_key, x2_key, 0.0, noise);
 
  Values values;
  values.insert(x1_key, 10.0);
  values.insert(x2_key, 0.0);
 
  EXPECT_DOUBLES_EQUAL(0.49505, factor.error(values), 1e-5);
  EXPECT_DOUBLES_EQUAL(0.49505, between_factor.error(values), 1e-5);
  EXPECT_DOUBLES_EQUAL(0.49505, gm->loss(10.0), 1e-5);
}
 
int main() {
  TestResult tr;
 
  return TestRegistry::runAllTests(tr);
}