Function TEST

test/Test_Example_Marginalization.cpp:36–284

#include "gtest/gtest.h"

TEST(Example, Marginalization)
{
  ceres::Solver::Options FGOptions;
  FGOptions.trust_region_strategy_type = ceres::TrustRegionStrategyType::DOGLEG;
  FGOptions.dogleg_type = ceres::DoglegType::SUBSPACE_DOGLEG;

  /** set uncertainty */
  libRSF::Vector3 NoisePosVec, NoiseRotVec;
  NoisePosVec.fill(STDDEV_POS);
  NoiseRotVec.fill(STDDEV_ROT);

  libRSF::Vector6 NoisePoseVec;
  NoisePoseVec << NoisePosVec , NoiseRotVec;

  /** create noise models */
  libRSF::GaussianDiagonal<3> GaussPos, GausRot;
  GaussPos.setStdDevSharedDiagonal(STDDEV_POS);
  GausRot.setStdDevSharedDiagonal(STDDEV_ROT);

  libRSF::GaussianDiagonal<6> GaussPose;
  GaussPose.setStdDevDiagonal(NoisePoseVec);

  /** create desired states */
  libRSF::Vector3 PosVec0, PosVec1;
  PosVec0 << 0, 0, 0;
  PosVec1 << 1, 0.5, 0.25;

  libRSF::Quaternion Quat0, Quat1;
  Quat0.setIdentity();
  Quat1 = Quat0 * (Eigen::AngleAxisd(1, libRSF::Vector3::UnitX())
                 * Eigen::AngleAxisd(0, libRSF::Vector3::UnitY())
                 * Eigen::AngleAxisd(0, libRSF::Vector3::UnitZ()));

  libRSF::Vector4 RotVec0, RotVec1;
  RotVec0 << Quat0.vec(), Quat0.w();
  RotVec1 << Quat1.vec(), Quat1.w();

  libRSF::Vector7 PoseVec0, PoseVec1;
  PoseVec0 << PosVec0, Quat0.vec(), Quat0.w();
  PoseVec1 << PosVec1, Quat1.vec(), Quat1.w();

  /** create state objects as GT */
  libRSF::Data StatePos0 (libRSF::DataType::Point3, 0.0);
  StatePos0.setMean(PosVec0);
  libRSF::Data StatePos1 (libRSF::DataType::Point3, 1.0);
  StatePos1.setMean(PosVec1);

  libRSF::Data StateRot0 (libRSF::DataType::Quaternion, 0.0);
  StateRot0.setMean(RotVec0);
  libRSF::Data StateRot1 (libRSF::DataType::Quaternion, 1.0);
  StateRot1.setMean(RotVec1);

  /** calculate relative transformations */
  libRSF::Vector3 PosVecRel = PosVec1 - PosVec0;
  libRSF::Quaternion QuatRel = Quat0.conjugate()*Quat1;

  libRSF::Vector7 PoseVecRel;
  PoseVecRel << Quat0.conjugate()*PosVecRel, QuatRel.vec(), QuatRel.w();