INSTINCT Code Coverage Report


Directory: src/
File: Nodes/DataProcessor/Filter/LowPassFilter.cpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 13 194 6.7%
Functions: 4 21 19.0%
Branches: 12 373 3.2%

Line Branch Exec Source
1 // This file is part of INSTINCT, the INS Toolkit for Integrated
2 // Navigation Concepts and Training by the Institute of Navigation of
3 // the University of Stuttgart, Germany.
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9 #include "LowPassFilter.hpp"
10
11 #include "NodeRegistry.hpp"
12 #include <algorithm>
13 #include <imgui.h>
14 #include "Navigation/INS/Units.hpp"
15 #include "internal/NodeManager.hpp"
16 namespace nm = NAV::NodeManager;
17 #include "internal/FlowManager.hpp"
18
19 #include "NodeData/IMU/ImuObsSimulated.hpp"
20 #include "NodeData/IMU/ImuObsWDelta.hpp"
21
22 #include "internal/gui/widgets/HelpMarker.hpp"
23 #include "internal/gui/widgets/imgui_ex.hpp"
24 #include "internal/gui/widgets/InputWithUnit.hpp"
25 #include "internal/gui/NodeEditorApplication.hpp"
26
27 #include "Navigation/Transformations/CoordinateFrames.hpp"
28 #include "Navigation/Transformations/Units.hpp"
29 #include "Navigation/GNSS/Functions.hpp"
30
31 #include "util/Eigen.hpp"
32 #include "util/StringUtil.hpp"
33 #include "util/Logger.hpp"
34
35 #include <imgui_internal.h>
36 #include <limits>
37 #include <set>
38 #include <type_traits>
39
40 112 NAV::LowPassFilter::LowPassFilter()
41
2/4
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
112 : Node(typeStatic())
42 {
43 LOG_TRACE("{}: called", name);
44 112 _hasConfig = true;
45 112 _guiConfigDefaultWindowSize = { 500, 300 };
46
47
4/8
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 112 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 112 times.
✓ Branch 9 taken 112 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
336 nm::CreateInputPin(this, "Original", Pin::Type::Flow, { NAV::NodeData::type() }, &LowPassFilter::receiveObs);
48
49
4/8
✓ Branch 2 taken 112 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 112 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 112 times.
✓ Branch 10 taken 112 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
448 nm::CreateOutputPin(this, "Filtered", Pin::Type::Flow, { NAV::NodeData::type() });
50 336 }
51
52 224 NAV::LowPassFilter::~LowPassFilter()
53 {
54 LOG_TRACE("{}: called", nameId());
55 224 }
56
57 224 std::string NAV::LowPassFilter::typeStatic()
58 {
59
1/2
✓ Branch 1 taken 224 times.
✗ Branch 2 not taken.
448 return "LowPassFilter";
60 }
61
62 std::string NAV::LowPassFilter::type() const
63 {
64 return typeStatic();
65 }
66
67 112 std::string NAV::LowPassFilter::category()
68 {
69
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
224 return "Data Processor";
70 }
71
72 void NAV::LowPassFilter::guiConfig()
73 {
74 if (outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier.size() != 1)
75 {
76 ImGui::TextUnformatted("Please connect the input pin to show the options");
77 return;
78 }
79
80 ImGui::SetNextItemWidth(400.0F * gui::NodeEditorApplication::defaultFontRatio());
81
82 if (_gui_availableItemsSelection > _availableItems.size()) { _gui_availableItemsSelection = _availableItems.size() - (_availableItems.empty() ? 0 : 1); }
83 bool noMoreItems = _filterItems.size() == _availableItems.size();
84 if (noMoreItems) { ImGui::BeginDisabled(); }
85 if (ImGui::BeginCombo(fmt::format("##Available data combo {}", size_t(id)).c_str(), !_availableItems.empty() ? _availableItems.at(_gui_availableItemsSelection).c_str() : ""))
86 {
87 for (size_t i = 0; i < _availableItems.size(); i++)
88 {
89 const auto& item = _availableItems.at(i);
90 if (std::ranges::find_if(_filterItems, [&](const FilterItem& filterItem) {
91 return filterItem.dataDescription == item;
92 })
93 != _filterItems.end()) { continue; }
94
95 const bool is_selected = (_gui_availableItemsSelection == i);
96 if (ImGui::Selectable(item.c_str(), is_selected))
97 {
98 _gui_availableItemsSelection = i;
99 }
100 if (is_selected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
101 {
102 ImGui::SetItemDefaultFocus();
103 }
104 }
105 ImGui::EndCombo();
106 }
107 ImGui::SameLine();
108 if (ImGui::Button(fmt::format("Add##Filter item {}", size_t(id)).c_str()))
109 {
110 _filterItems.emplace_back(_availableItems.at(_gui_availableItemsSelection), _gui_availableItemsSelection);
111 flow::ApplyChanges();
112 bool selectionChanged = false;
113 for (size_t i = _gui_availableItemsSelection + 1; i < _availableItems.size(); i++)
114 {
115 const auto& item = _availableItems.at(i);
116 if (std::ranges::find_if(_filterItems, [&](const FilterItem& filterItem) {
117 return filterItem.dataDescription == item;
118 })
119 != _filterItems.end()) { continue; }
120
121 _gui_availableItemsSelection = i;
122 selectionChanged = true;
123 break;
124 }
125 if (!selectionChanged && _gui_availableItemsSelection != 0)
126 {
127 for (int i = static_cast<int>(_gui_availableItemsSelection) - 1; i >= 0; i--)
128 {
129 const auto& item = _availableItems.at(static_cast<size_t>(i));
130 if (std::ranges::find_if(_filterItems, [&](const FilterItem& filterItem) {
131 return filterItem.dataDescription == item;
132 })
133 != _filterItems.end()) { continue; }
134
135 _gui_availableItemsSelection = static_cast<size_t>(i);
136 break;
137 }
138 }
139 }
140 if (noMoreItems) { ImGui::EndDisabled(); }
141
142 const float COMBO_WIDTH = 100.0F * gui::NodeEditorApplication::windowFontRatio();
143 const float ITEM_WIDTH = 140.0F * gui::NodeEditorApplication::windowFontRatio();
144
145 std::optional<size_t> itemToDelete;
146 for (size_t i = 0; i < _filterItems.size(); i++)
147 {
148 auto& item = _filterItems.at(i);
149 bool keep = true;
150 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
151 if (ImGui::CollapsingHeader(fmt::format("{}##{}", item.dataDescription, size_t(id)).c_str(), &keep))
152 {
153 ImGui::SetNextItemWidth(COMBO_WIDTH);
154 if (ImGui::BeginCombo(fmt::format("Filter Type##{}", size_t(id)).c_str(), to_string(item.filterType)))
155 {
156 for (size_t i = 0; i < static_cast<size_t>(FilterType::COUNT); i++)
157 {
158 const bool is_selected = (static_cast<size_t>(item.filterType) == i);
159 if (ImGui::Selectable(to_string(static_cast<FilterType>(i)), is_selected))
160 {
161 item.filterType = static_cast<FilterType>(i);
162 LOG_DEBUG("{}: filterType changed to {}", nameId(), fmt::underlying(item.filterType));
163 flow::ApplyChanges();
164 }
165 if (is_selected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
166 {
167 ImGui::SetItemDefaultFocus();
168 }
169 }
170 ImGui::EndCombo();
171 }
172
173 ImGui::SameLine();
174 float size = 7.0F * gui::NodeEditorApplication::windowFontRatio();
175 ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size / 1.2F,
176 ImGui::GetCursorScreenPos().y + size * 1.8F),
177 size,
178 item.modified
179 ? ImColor(0.0F, 255.0F, 0.0F)
180 : ImColor(255.0F, 0.0F, 0.0F));
181 ImGui::Dummy(ImVec2(2 * size, 3.0F * size));
182 if (ImGui::IsItemHovered())
183 {
184 ImGui::SetTooltip(item.modified
185 ? "Indicates wether the filter is working."
186 : "Indicates wether the filter is working.\n"
187 "Reasons why it is not working can be:\n"
188 "- Data rate of the incoming values must be greater then 2 * dt\n"
189 "- The data was never included in the observations (dynamic data)\n"
190 "- The data cannot be modified because it is not implemented yet");
191 }
192
193 ImGui::Indent();
194 if (item.filterType == FilterType::Linear)
195 {
196 ImGui::SetNextItemWidth(ITEM_WIDTH);
197 if (ImGui::InputDoubleL(fmt::format("Cutoff Frequency##{} {}", size_t(id), item.dataDescription).c_str(), &item.linear_filter_cutoff_frequency, 1e-5, 1e5, 0.0, 0.0, "%.5f Hz"))
198 {
199 LOG_DEBUG("{}: Cutoff Freq. {} changed to {}", nameId(), item.dataDescription, item.linear_filter_cutoff_frequency);
200 flow::ApplyChanges();
201 }
202 }
203 ImGui::Unindent();
204 }
205 if (!keep) { itemToDelete = i; }
206 }
207 if (itemToDelete) { _filterItems.erase(std::next(_filterItems.begin(), static_cast<int64_t>(*itemToDelete))); }
208 }
209
210 json NAV::LowPassFilter::save() const
211 {
212 LOG_TRACE("{}: called", nameId());
213
214 json j;
215
216 j["availableItems"] = _availableItems;
217 j["filterItems"] = _filterItems;
218
219 return j;
220 }
221
222 void NAV::LowPassFilter::restore(json const& j)
223 {
224 LOG_TRACE("{}: called", nameId());
225 if (j.contains("availableItems")) { j.at("availableItems").get_to(_availableItems); }
226 if (j.contains("filterItems")) { j.at("filterItems").get_to(_filterItems); }
227 }
228
229 bool NAV::LowPassFilter::resetNode()
230 {
231 LOG_TRACE("{}: called", nameId());
232
233 for (auto& item : _filterItems)
234 {
235 item.dataToFilter.clear();
236 item.modified = false;
237 }
238
239 return true;
240 }
241
242 void NAV::LowPassFilter::afterCreateLink(OutputPin& startPin, [[maybe_unused]] InputPin& endPin)
243 {
244 LOG_TRACE("{}: called for {} ==> {}", nameId(), size_t(startPin.id), size_t(endPin.id));
245
246 if (endPin.parentNode->id != id)
247 {
248 return; // Link on Output Port
249 }
250
251 // Store previous output pin identifier
252 auto previousOutputPinDataIdentifier = outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier;
253 // Overwrite output pin identifier with input pin identifier
254 outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier = startPin.dataIdentifier;
255
256 if (previousOutputPinDataIdentifier != outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier) // If the identifier changed
257 {
258 // Check if connected links on output port are still valid
259 for (auto& link : outputPins.at(OUTPUT_PORT_INDEX_FLOW).links)
260 {
261 if (auto* endPin = link.getConnectedPin())
262 {
263 if (!outputPins.at(OUTPUT_PORT_INDEX_FLOW).canCreateLink(*endPin))
264 {
265 // If the link is not valid anymore, delete it
266 outputPins.at(OUTPUT_PORT_INDEX_FLOW).deleteLink(*endPin);
267 }
268 }
269 }
270
271 // Refresh all links connected to the output pin if the type changed
272 if (outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier != previousOutputPinDataIdentifier)
273 {
274 for (auto& link : outputPins.at(OUTPUT_PORT_INDEX_FLOW).links)
275 {
276 if (auto* connectedPin = link.getConnectedPin())
277 {
278 outputPins.at(OUTPUT_PORT_INDEX_FLOW).recreateLink(*connectedPin);
279 }
280 }
281 }
282 }
283
284 if (auto* pin = inputPins.at(INPUT_PORT_INDEX_FLOW).link.getConnectedPin();
285 pin && _availableItems.empty())
286 {
287 _gui_availableItemsSelection = 0;
288 _availableItems = NAV::NodeRegistry::GetStaticDataDescriptors(pin->dataIdentifier);
289 }
290 }
291
292 void NAV::LowPassFilter::afterDeleteLink(OutputPin& startPin, InputPin& endPin)
293 {
294 LOG_TRACE("{}: called for {} ==> {}", nameId(), size_t(startPin.id), size_t(endPin.id));
295
296 _gui_availableItemsSelection = 0;
297 _availableItems.clear();
298
299 if ((endPin.parentNode->id != id // Link on Output port is removed
300 && !inputPins.at(INPUT_PORT_INDEX_FLOW).isPinLinked()) // and the Input port is not linked
301 || (startPin.parentNode->id != id // Link on Input port is removed
302 && !outputPins.at(OUTPUT_PORT_INDEX_FLOW).isPinLinked())) // and the Output port is not linked
303 {
304 outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier = { NodeData::type() };
305 }
306 }
307
308 void NAV::LowPassFilter::receiveObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
309 {
310 auto obs = queue.extract_front();
311
312 for (const auto& desc : obs->dynamicDataDescriptors())
313 {
314 if (std::ranges::none_of(_availableItems, [&](const auto& header) { return header == desc; }))
315 {
316 _availableItems.push_back(desc);
317 flow::ApplyChanges();
318 }
319 }
320
321 auto out = NAV::NodeRegistry::CopyNodeData(obs);
322
323 for (auto& item : _filterItems)
324 {
325 LOG_DATA("{}: [{}] {}", nameId(), item.dataIndex, item.dataDescription);
326 if (item.dataIndex < out->staticDescriptorCount())
327 {
328 if (auto value = out->getValueAt(item.dataIndex))
329 {
330 if (auto newValue = filterData(item, out->insTime, *value))
331 {
332 item.modified |= out->setValueAt(item.dataIndex, *newValue);
333 }
334 }
335 }
336 else if (auto value = out->getDynamicDataAt(item.dataDescription))
337 {
338 if (auto newValue = filterData(item, out->insTime, *value))
339 {
340 item.modified |= out->setDynamicDataAt(item.dataDescription, *newValue);
341 }
342 }
343 }
344
345 invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, out);
346 }
347
348 std::optional<double> NAV::LowPassFilter::filterData(FilterItem& item, const InsTime& insTime, double value)
349 {
350 if (item.filterType == FilterType::Linear)
351 {
352 // first we filter accelerations
353 item.dataToFilter[insTime] = value;
354 // for testing at the moment
355 double dt = 1.0 / item.linear_filter_cutoff_frequency;
356 // remove all entries that are outside filter time window
357 std::erase_if(item.dataToFilter, [&](const auto& pair) { return static_cast<double>((insTime - pair.first).count()) > dt; });
358
359 if (item.dataToFilter.size() > 2)
360 {
361 // average accelerations first
362 auto N11 = static_cast<double>(item.dataToFilter.size());
363 double N12 = 0.0;
364 double N22 = 0.0;
365 double n1 = 0.0;
366 double n2 = 0.0;
367 for (const auto& key_val : item.dataToFilter)
368 {
369 auto delta_t = static_cast<double>((key_val.first - insTime).count());
370 N12 += delta_t;
371 N22 += delta_t * delta_t;
372 n1 += key_val.second;
373 n2 += delta_t * key_val.second;
374 }
375 double determinant_inverse = 1.0 / (N11 * N22 - N12 * N12);
376 return determinant_inverse * (N22 * n1 - N12 * n2);
377 }
378 }
379 return {};
380 }
381
382 const char* NAV::LowPassFilter::to_string(FilterType value)
383 {
384 switch (value)
385 {
386 case FilterType::Linear:
387 return "Linear fit";
388 // case FilterType::Experimental:
389 // return "Experimental";
390 case FilterType::COUNT:
391 return "";
392 }
393 return "";
394 }
395
396 namespace NAV
397 {
398
399 void to_json(json& j, const LowPassFilter::FilterItem& data)
400 {
401 j = json{
402 { "dataDescription", data.dataDescription },
403 { "filterType", data.filterType },
404 { "linear_filter_cutoff_frequency", data.linear_filter_cutoff_frequency },
405 };
406 }
407
408 void from_json(const json& j, LowPassFilter::FilterItem& data)
409 {
410 if (j.contains("dataDescription")) { j.at("dataDescription").get_to(data.dataDescription); }
411 if (j.contains("filterType")) { j.at("filterType").get_to(data.filterType); }
412 if (j.contains("linear_filter_cutoff_frequency")) { j.at("linear_filter_cutoff_frequency").get_to(data.linear_filter_cutoff_frequency); }
413 }
414
415 } // namespace NAV
416