Method getMarginalizationInfo

src/FactorGraphStructure.cpp:30–107

  {}

  void FactorGraphStructure::getMarginalizationInfo(const std::vector<double*> &BaseStates,
                                              std::vector<double*> &ConnectedStates,
                                              std::vector<ceres::ResidualBlockId> &ConnectedFactors,
                                              std::vector<int> &StateDims,
                                              std::vector<int> &StateDimsLocal,
                                              std::vector<StateID> &StateIDs,
                                              std::vector<DataType> &StateTypes) const
  {
    /** sets to store unique values */
    std::set<double*> BaseStatePointers;
    std::set<ceres::ResidualBlockId> ConnectedFactorsIDs;

    /** get all connected factors */
    for (double* const State : BaseStates)
    {
      /** safety check */
      if(_Graph->HasParameterBlock(State) == false)
      {
        PRINT_ERROR("State is not part of graph!");
        return;
      }

      /** store pointer for check */
      if(BaseStatePointers.count(State) != 0)
      {
        PRINT_ERROR("Duplicated pointer for marginalized states!");
        return;
      }
      BaseStatePointers.emplace(State);

      /** query residual IDs */
      std::vector<ceres::ResidualBlockId> Factors;
      _Graph->GetResidualBlocksForParameterBlock(State, &Factors);
      for(ceres::ResidualBlockId Factor : Factors)
      {
        /** add only new factors */
        if(ConnectedFactorsIDs.count(Factor) == 0)
        {
          ConnectedFactorsIDs.emplace(Factor);
        }
      }
    }

    /** get all connected states */
    std::set<double*> ConnectedStatePointers;
    for (const ceres::ResidualBlockId Factor : ConnectedFactorsIDs)
    {
      std::vector<double*> CurrentStatePointers;

      /** query states that are connected with a certain factor */
      _Graph->GetParameterBlocksForResidualBlock(Factor, &CurrentStatePointers);

      /** add only unique states */
      for (double* StatePointer : CurrentStatePointers)
      {
        if(ConnectedStatePointers.count(StatePointer) == 0 && BaseStatePointers.count(StatePointer) == 0)
        {
          ConnectedStatePointers.emplace(StatePointer);