Function TEST

test/Test_Example_FG_Range.cpp:36–112

#include "gtest/gtest.h"

TEST(Example, FG_Range)
{
  libRSF::Vector1 StdDev;
  StdDev << STDDEV_RANGE;

  ceres::Solver::Options SolverOptions;
  SolverOptions.trust_region_strategy_type = ceres::TrustRegionStrategyType::DOGLEG;
  SolverOptions.dogleg_type = ceres::DoglegType::SUBSPACE_DOGLEG;
  SolverOptions.num_threads = std::thread::hardware_concurrency();
  SolverOptions.minimizer_progress_to_stdout = true;

  libRSF::FactorGraph SimpleGraph;
  libRSF::SensorDataSet RangeMeasurements;
  libRSF::StateList PositionList;
  libRSF::GaussianDiagonal<1> NoiseModel;
  NoiseModel.setStdDevDiagonal(StdDev);

  /** construct a set of range Measurements */
  CreateData(RangeMeasurements);

  /** add position variables to graph */
  SimpleGraph.addState(POSITION_STATE, libRSF::DataType::Point2, 0);
  SimpleGraph.addState(POSITION_STATE, libRSF::DataType::Point2, 1);
  SimpleGraph.addState(POSITION_STATE, libRSF::DataType::Point2, 2);

  /** loop over timestamps */
  double Time = 0.0;
  RangeMeasurements.getTimeFirst(RANGE_MEASUREMENT, Time);
  do
  {
    PositionList.add(POSITION_STATE, Time);
    /** loop over measurements */
    for (int nMeasurement = RangeMeasurements.countElement(RANGE_MEASUREMENT, Time); nMeasurement > 0; nMeasurement--)
    {
      SimpleGraph.addFactor<libRSF::FactorType::Range2>(PositionList, RangeMeasurements.getElement(RANGE_MEASUREMENT, Time, nMeasurement-1), NoiseModel);
    }
    PositionList.clear();
  }
  while(RangeMeasurements.getTimeNext(RANGE_MEASUREMENT, Time, Time));

  /** output initialization */
  std::cout << SimpleGraph.getStateData().getElement(POSITION_STATE, 0.0).getNameValueString() << std::endl;
  std::cout << SimpleGraph.getStateData().getElement(POSITION_STATE, 1.0).getNameValueString() << std::endl;
  std::cout << SimpleGraph.getStateData().getElement(POSITION_STATE, 2.0).getNameValueString() << std::endl;

  /** solve graph */
  SimpleGraph.solve(SolverOptions);
  SimpleGraph.printReport();

  /** output result */
  std::cout << SimpleGraph.getStateData().getElement(POSITION_STATE, 0.0).getNameValueString() << std::endl;
  std::cout << SimpleGraph.getStateData().getElement(POSITION_STATE, 1.0).getNameValueString() << std::endl;
  std::cout << SimpleGraph.getStateData().getElement(POSITION_STATE, 2.0).getNameValueString() << std::endl;

  /** load expected results - point2*/
  libRSF::SensorDataSet Expected;
  Expected.addElement(libRSF::Data("point2 0.0 0.0 0.0 0.0 0.0 0.0 0.0"));
  Expected.addElement(libRSF::Data("point2 1.0 1.0 0.0 0.0 0.0 0.0 0.0"));