Function EvaluateCostSurfacePoints

src/FactorGraphSampling.cpp:27–60

namespace libRSF
{
  void EvaluateCostSurfacePoints(ceres::Problem &Graph,
                                 double* const StatePointer,
                                 const int Dim,
                                 const VectorVectorSTL<Dynamic> Points,
                                 std::vector<double> &Costs)
  {

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

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

    /** loop over grid */
    Costs.clear();
    for (int n = 0; n < static_cast<int>(Points.size()); ++n)
    {
      State = Points.at(n);

      /** evaluate */
      double Cost;
      Graph.Evaluate(Options, &Cost, nullptr, nullptr, nullptr);

      /** save result */
      Costs.push_back(Cost);
    }

    /** restore original cost */
    State = OriginalState;
  }
}