Function EvaluateCostSurface

include/FactorGraphSampling.h:50–158

  template<int Dim>
  void EvaluateCostSurface(ceres::Problem &Graph,
                           double * const StatePointer,
                           const int Points,
                           const double Range,
                           StateDataSet &Result)
  {
    typedef VectorStatic<Dim> SizedVector;

    /** configure evaluation */
    ceres::Problem::EvaluateOptions Options;
    Options.apply_loss_function = true;
    Options.num_threads = std::thread::hardware_concurrency();
    Options.parameter_blocks = {StatePointer};

    /** save original value */
    VectorRef<double, Dim> State(StatePointer);
    const SizedVector OriginalState = State;

    /** create 1D point set */
    Vector Lin = Vector::LinSpaced(Points, -Range/2, Range/2);

    /** create nD point set */
    std::vector<Vector> Grid;
    for (int nDim = 0; nDim < Dim; ++nDim)
    {
      Grid.push_back((Lin.array() + State(nDim)).matrix());
    }

    /** create multi dimensional index */
    std::vector<int> Index;
    for (int nDim = 0; nDim < Dim; ++nDim)
    {
      Index.push_back(0);
    }

    /** loop over grid */
    for (uint64_t n = 0; n < std::pow(Points,Dim); ++n)
    {
      /** write point */
      for (int nDim = 0; nDim < Dim; ++nDim)
      {
        State.segment(nDim,1) = Grid.at(nDim).segment(Index.at(nDim),1);
      }

      /** evaluate */
      double Cost;
      std::vector<double> Gradient;
      ceres::CRSMatrix JacobianCRS;
      Graph.Evaluate(Options, &Cost, nullptr, &Gradient, &JacobianCRS);

      /** calculate the Hessian */
      Matrix Jacobian;
      CRSToMatrix(JacobianCRS, Jacobian);
      MatrixStatic<Dim,Dim> Hessian = Jacobian.transpose()*Jacobian;

      /** create state data to store the result */
      Data CostState;
      if (Dim == 1)