| 107 | } |
| 108 | |
| 109 | void FactorGraphStructure::getFactorsOfState(const StateID &State, std::vector<FactorID> &Factors) const |
| 110 | { |
| 111 | if (_Data->checkElement(State.ID, State.Timestamp, State.Number) == false) |
| 112 | { |
| 113 | PRINT_ERROR("State does not exist: ", State.ID, " ", State.Timestamp, " ", State.Number); |
| 114 | return; |
| 115 | } |
| 116 | |
| 117 | /** get state pointer */ |
| 118 | double* StatePointer; |
| 119 | StatePointer = _Data->getElement(State.ID, State.Timestamp, State.Number).getMeanPointer(); |
| 120 | |
| 121 | /** get connected residual IDs */ |
| 122 | std::vector<ceres::ResidualBlockId> Residuals; |
| 123 | _Graph->GetResidualBlocksForParameterBlock(StatePointer, &Residuals); |
| 124 | |
| 125 | /** translate to factor IDs */ |
| 126 | Factors.clear(); |
| 127 | for(const ceres::ResidualBlockId Res: Residuals) |
| 128 | { |
| 129 | FactorID Factor(_Factors.at(Res).Type, _Factors.at(Res).Timestamp, _Factors.at(Res).Number); |
| 130 | Factors.push_back(Factor); |
| 131 | } |
| 132 | |
| 133 | } |
| 134 | |
| 135 | void FactorGraphStructure::removeState(const StateID &State) |
| 136 | { |
nothing calls this directly
no test coverage detected