| 96 | |
| 97 | template <typename Scalar, int POSE_SIZE> |
| 98 | Scalar LinearizationRelSC<Scalar, POSE_SIZE>::linearizeProblem( |
| 99 | bool* numerically_valid) { |
| 100 | // reset damping and scaling (might be set from previous iteration) |
| 101 | pose_damping_diagonal = 0; |
| 102 | pose_damping_diagonal_sqrt = 0; |
| 103 | marg_scaling = VecX(); |
| 104 | |
| 105 | std::unordered_map<TimeCamId, std::map<TimeCamId, std::set<KeypointId>>> |
| 106 | obs_to_lin; |
| 107 | |
| 108 | if (used_frames) { |
| 109 | const auto& obs = lmdb_.getObservations(); |
| 110 | |
| 111 | // select all observations where the host frame is about to be |
| 112 | // marginalized |
| 113 | |
| 114 | if (lost_landmarks) { |
| 115 | for (auto it = obs.cbegin(); it != obs.cend(); ++it) { |
| 116 | if (used_frames->count(it->first.frame_id) > 0) { |
| 117 | for (auto it2 = it->second.cbegin(); it2 != it->second.cend(); |
| 118 | ++it2) { |
| 119 | if (it2->first.frame_id <= last_state_to_marg) |
| 120 | obs_to_lin[it->first].emplace(*it2); |
| 121 | } |
| 122 | } else { |
| 123 | std::map<TimeCamId, std::set<KeypointId>> lost_obs_map; |
| 124 | for (auto it2 = it->second.cbegin(); it2 != it->second.cend(); |
| 125 | ++it2) { |
| 126 | for (const auto& lm_id : it2->second) { |
| 127 | if (lost_landmarks->count(lm_id)) { |
| 128 | if (it2->first.frame_id <= last_state_to_marg) |
| 129 | lost_obs_map[it2->first].emplace(lm_id); |
| 130 | } |
| 131 | } |
| 132 | } |
| 133 | if (!lost_obs_map.empty()) { |
| 134 | obs_to_lin[it->first] = lost_obs_map; |
| 135 | } |
| 136 | } |
| 137 | } |
| 138 | |
| 139 | } else { |
| 140 | for (auto it = obs.cbegin(); it != obs.cend(); ++it) { |
| 141 | if (used_frames->count(it->first.frame_id) > 0) { |
| 142 | for (auto it2 = it->second.cbegin(); it2 != it->second.cend(); |
| 143 | ++it2) { |
| 144 | // TODO: Check how ABS_QR works without last_state_to_marg |
| 145 | if (it2->first.frame_id <= last_state_to_marg) |
| 146 | obs_to_lin[it->first].emplace(*it2); |
| 147 | } |
| 148 | } |
| 149 | } |
| 150 | } |
| 151 | } else { |
| 152 | obs_to_lin = lmdb_.getObservations(); |
| 153 | } |
| 154 | |
| 155 | Scalar error; |
nothing calls this directly
no test coverage detected