Method marginalizeAllStatesOutsideWindow

src/FactorGraph.cpp:360–407

  }

  bool FactorGraph::marginalizeAllStatesOutsideWindow(const double TimeWindow, const double CurrentTime, const double Inflation)
  {
    /** calculate time boarder */
    const double CutTime = roundToTick(CurrentTime - TimeWindow);

    /** check if marginalization is required before doing anything  */
    double TimeFirst = CutTime;
    _StateData.getTimeFirstOverall(TimeFirst);
    if (TimeFirst > CutTime)
    {
      /** no marginalization required, exit here... */
      return true;
    }

    /** collect relevant states */
    std::vector<StateID> States;

    /** iterate over state names */
    const std::vector<string> StateNames = _StateData.getKeysAll();
    for (const string &Name : StateNames)
    {
      /** check if there is an element below cut time */
      double TimeFirstState;
      if (_StateData.getTimeFirst(Name, TimeFirstState))
      {
        if (TimeFirstState <= CutTime)
        {
          /** iterate over timestamps */
          std::vector<double> Times;
          if (_StateData.getTimesBelowOrEqual(Name, CutTime, Times))
          {
            for (const double Time : Times)
            {
              /** iterate over number */
              int Numbers = _StateData.countElement(Name, Time);
              for (int n = 0; n < Numbers; n++)
              {
                States.emplace_back(StateID(Name, Time, n));
              }
            }
          }
        }
      }
    }

    /** marginalize */
    return this->marginalizeStates(States, Inflation);
  }

  bool FactorGraph::computeCovariance(const string Name, const double Timestamp)
  {