Method marginalizeStates

src/FactorGraph.cpp:242–350

  }

  bool FactorGraph::marginalizeStates(std::vector<StateID> States, const double Inflation)
  {
    /** time measurement */
    Timer MargTimer;

    /**check if state vector has been filled */
    if (States.empty() == true)
    {
      PRINT_ERROR("Request for marginalization without any state.");
      return false;
    }

    std::vector<double*> MarginalStates;
    int MarginalSize = 0;
    for (const StateID &State : States)
    {
      /** check if this is a set of valid states */
      if (this->_StateData.checkElement(State.ID, State.Timestamp, State.Number) == false)
      {
        PRINT_ERROR("State doesn't exist at: ", State.Timestamp, " Type: ", State.ID, " Number: ", State.Number);
        return false;
      }

      /** store pointer */
      MarginalStates.push_back(_StateData.getElement(State.ID, State.Timestamp, State.Number).getMeanPointer());

      /** compute size of the marginalized system */
      MarginalSize += _Graph.ParameterBlockLocalSize(MarginalStates.back());
    }

    /** get connected states */
    std::vector<double*> ConnectedStates;
    std::vector<ceres::ResidualBlockId> Factors;
    std::vector<int> GlobalSize;
    std::vector<int> LocalSize;
    std::vector<StateID> StateIDs;
    std::vector<DataType> StateTypes;
    _Structure.getMarginalizationInfo(MarginalStates, ConnectedStates, Factors, GlobalSize, LocalSize, StateIDs, StateTypes);

    if (ConnectedStates.empty() == false)
    {
      /** calculate mean and uncertainty*/
      ceres::Problem::EvaluateOptions Options;
      Options.apply_loss_function = true;
      Options.num_threads = std::thread::hardware_concurrency();

      /** set which states should be evaluated (base + the connected ones) */
      std::vector<double*> CombinedStates;
      CombinedStates.insert(CombinedStates.end(), MarginalStates.begin(), MarginalStates.end());
      CombinedStates.insert(CombinedStates.end(), ConnectedStates.begin(), ConnectedStates.end());
      Options.parameter_blocks = CombinedStates;

      /** set which factors should be evaluated */
      Options.residual_blocks = Factors;

      /** evaluate sub-problem */
      ceres::CRSMatrix JacobianCRS;
      std::vector<double> ResidualVec;