Method addFactor

include/FactorGraphStructure.h:82–120

      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);
        _Factors.emplace(CeresID, Info);

        /** collect state information */
        for(int n = 0; n < static_cast<int>(States.size()); n++)
        {
          if(_States.count(StatePointers.at(n)) == 0) /**< check if already existing */
          {
            StateInfo State;
            State.Name = States.at(n).ID;
            State.Timestamp = States.at(n).Timestamp;
            State.Number = States.at(n).Number;
            State.Type = StateTypes.at(n);

            _States.emplace(StatePointers.at(n), State);
          }
        }
      }

      void removeFactor(const FactorID &Factor);
      void removeFactor(const ceres::ResidualBlockId Factor);