Method computeUnweightedError

src/FactorGraph.cpp:777–843

  }

  void FactorGraph::computeUnweightedError(const FactorType CurrentFactorType, std::vector<double> &ErrorData)
  {
    /** get residual IDs */
    std::vector<ceres::ResidualBlockId> IDs;
    _Structure.getResidualIDs(CurrentFactorType, IDs);

    /** terminate here, if factors are missing */
    if (IDs.empty() == true)
    {
      PRINT_WARNING("Factors of type ", CurrentFactorType, " are missing!");
      return;
    }

    /** check IDs */
    std::vector<ceres::ResidualBlockId> IDsCeres;
    std::vector<bool> Existence;
    _Graph.GetResidualBlocks(&IDsCeres);
    for (auto const &ID1 : IDs)
    {
      bool Exists = false;
      for (auto const &ID2 : IDsCeres)
      {
        if (ID1 == ID2)
        {
          Exists = true;
        }
      }
      Existence.push_back(Exists);
      if (Exists == false)
      {
        PRINT_ERROR("Found missing ID");
      }
    }

    /** configure evaluation */
    ceres::Problem::EvaluateOptions Options;
    Options.num_threads = std::thread::hardware_concurrency();
    Options.residual_blocks = IDs;
    Options.apply_loss_function = false;

    /** disable error model during evaluation */
    disableErrorModel(CurrentFactorType);

    /** compute the errors */
    _Graph.Evaluate(Options, nullptr, &ErrorData, nullptr, nullptr);

    /** re-enable error model */
    enableErrorModel(CurrentFactorType);

    /** create factor ID */
    double TimeFirst;
    _Structure.getTimeFirst(CurrentFactorType, TimeFirst);
    FactorID FirstID(CurrentFactorType, TimeFirst, 0);

    /** remove unused dimensions */
    int InputSize, OutputSize;
    _Structure.getErrorInputSize(FirstID, InputSize);
    _Structure.getErrorOutputSize(FirstID, OutputSize);