Class FactorGraphStructure

include/FactorGraphStructure.h:46–169

  class FactorGraphStructure
  {
    public:
      FactorGraphStructure(ceres::Problem *GraphPointer, StateDataSet *Data);
      virtual ~FactorGraphStructure() = default;

      /** define a structure that stores all relevant information */
      struct StateInfo
      {
        std::string Name;
        double Timestamp;
        int Number;
        DataType Type;
      };

      struct FactorInfo
      {
        FactorType Type;
        double Timestamp;
        int Number;
        int ErrorInputSize;
        int ErrorOutputSize;
        ErrorModelBase* ErrorModel = nullptr;
      };

      /** collect all informations that are required to remove the BaseState from the graph */
      void 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;


      template <typename ErrorType>
      void addFactor(const FactorType Type,
                     const double Timestamp,
                     const ceres::ResidualBlockId CeresID,
                     ErrorType* const ErrorModel,
                     const std::vector<StateID> &States,
                     const std::vector<double*> &StatePointers,
                     const std::vector<DataType> &StateTypes)
      {
        /** add to time dependent representation */
        _FactorList.addElement(Type, Timestamp, CeresID);

        /** create ID and add to unordered maps */
        const FactorID Factor(Type, Timestamp, _FactorList.countElement(Type, Timestamp)-1);

        /** collect factor information */
        FactorInfo Info;
        Info.Type = Type;
        Info.Timestamp = Timestamp;
        Info.Number = Factor.Number;
        Info.ErrorInputSize = ErrorModel->InputDim;
        Info.ErrorOutputSize = ErrorModel->OutputDim;
        Info.ErrorModel = static_cast<ErrorModelBase*>(ErrorModel);