Method addFactorGeneric

include/FactorGraph.h:308–357

      /** add and remove factors */
      template <typename ErrorType, typename FactorClass, typename... FactorParameters>
      void addFactorGeneric (ErrorType &NoiseModel,
                             const std::vector<StateID> &StateList,
                             const FactorType FactorTypeEnum,
                             ceres::LossFunction* RobustLoss,
                             const bool DoPrediction,
                             const double Timestamp,
                             FactorParameters... Params)
      {
        /** build list of states */
        std::vector<double*> StatePointers;
        for (const StateID &State : StateList)
        {
          StatePointers.emplace_back(_StateData.getElement(State.ID , State.Timestamp, State.Number).getMeanPointer());
        }

        /** create factor object */
        FactorClass* Factor = new FactorClass(NoiseModel, Params...);

        /** use the factor to predict */
        if (DoPrediction)
        {
          Factor->predict(StatePointers);
        }

        /** wrap it in ceres cost function */
        auto CostFunction = makeAutoDiffCostFunction<ErrorType::OutputDim, FactorClass> (Factor,
                                                                                         typename FactorClass::StateDims{},
                                                                                         typename ErrorType::StateDims{});

        /** add it to the estimation problem  */
        ceres::ResidualBlockId CurrentCeresFactorID = _Graph.AddResidualBlock(CostFunction,
                                                                              RobustLoss,
                                                                              StatePointers);

        /** store state types */
        std::vector<DataType> StateTypes;
        for(const StateID &State : StateList)
        {
          StateTypes.emplace_back(_StateData.getElement(State.ID , State.Timestamp, State.Number).getType());
        }

        /** store the graphs structure */
        _Structure.addFactor<ErrorType>(FactorTypeEnum,
                                        Timestamp,
                                        CurrentCeresFactorID,
                                        Factor->getErrorModel(),
                                        StateList,
                                        StatePointers,
                                        StateTypes);
      }

      template <FactorType CurrentFactorType, typename ErrorType>
      void addFactorBase(StateList &States, ErrorType &NoiseModel, const Data &Measurement, ceres::LossFunction* RobustLoss, const bool DoPrediction)