INSTINCT Code Coverage Report

Directory: src/
File: Nodes/DataProcessor/Integrator/ImuIntegrator.cpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 21 106 19.8%
Functions: 5 13 38.5%
Branches: 23 196 11.7%
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 "ImuIntegrator.hpp"
10
11 #include <memory>
12 #include <algorithm>
13 #include <cstddef>
14
15 #include <imgui.h>
16 #include <imgui_internal.h>
17
18 #include "Navigation/INS/InertialPreIntegrator.hpp"
19 #include "internal/gui/widgets/HelpMarker.hpp"
20
21 #include "NodeRegistry.hpp"
22 #include <fmt/format.h>
23 #include "internal/FlowManager.hpp"
24
25 #include "Navigation/Constants.hpp"
26 #include "Navigation/Math/Math.hpp"
27
28 #include "util/Logger.hpp"
29 #include "util/Eigen.hpp"
30
31 115 NAV::ImuIntegrator::ImuIntegrator()
32
4/8
✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 115 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 115 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 115 times.
✗ Branch 12 not taken.
 115 : Node(typeStatic())
33 {
34 LOG_TRACE("{}: called", name);
35
36 115 _hasConfig = true;
37 115 _guiConfigDefaultWindowSize = { 422, 146 };
38
39
4/8
✓ Branch 2 taken 115 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 115 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 230 times.
✓ Branch 10 taken 115 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 575 CreateInputPin("ImuObs", Pin::Type::Flow, { NAV::ImuObs::type(), NAV::ImuObsWDelta::type() }, &ImuIntegrator::recvObservation,
40 ✗ [](const Node* node, const InputPin& inputPin) {
41 ✗ const auto* imuIntegrator = static_cast<const ImuIntegrator*>(node); // NOLINT(cppcoreguidelines-pro-type-static-cast-downcast)
42 ✗ return !inputPin.queue.empty() && imuIntegrator->_inertialIntegrator.hasInitialPosition();
43 });
44
4/8
✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 115 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 115 times.
✓ Branch 9 taken 115 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 345 CreateInputPin("PosVelAttInit", Pin::Type::Flow, { NAV::PosVelAtt::type() }, &ImuIntegrator::recvPosVelAttInit, nullptr, 1);
45
46
4/8
✓ Branch 2 taken 115 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 115 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 115 times.
✓ Branch 10 taken 115 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 460 CreateOutputPin("PosVelAtt", Pin::Type::Flow, { NAV::PosVelAtt::type() });
47 460 }
48
49 232 NAV::ImuIntegrator::~ImuIntegrator()
50 {
51 LOG_TRACE("{}: called", nameId());
52 232 }
53
54 229 std::string NAV::ImuIntegrator::typeStatic()
55 {
56
1/2
✓ Branch 1 taken 229 times.
✗ Branch 2 not taken.
 458 return "ImuIntegrator";
57 }
58
59 ✗ std::string NAV::ImuIntegrator::type() const
60 {
61 ✗ return typeStatic();
62 }
63
64 114 std::string NAV::ImuIntegrator::category()
65 {
66
1/2
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
 228 return "Data Processor";
67 }
68
69 ✗ void NAV::ImuIntegrator::guiConfig()
70 {
71 ✗ if (ImGui::Checkbox(fmt::format("IMU Preintegration##{}", size_t(id)).c_str(), &_imuPreintegration))
72 {
73 ✗ flow::ApplyChanges();
74 }
75
76 ✗ if (_imuPreintegration)
77 {
78 ✗ if (ImGui::Checkbox(fmt::format("Reset every epoch##{}", size_t(id)).c_str(), &_resetPreintegratorEveryEpoch))
79 {
80 ✗ flow::ApplyChanges();
81 }
82 ✗ ImGui::SameLine();
83 ✗ gui::widgets::HelpMarker("Resetting the preintegrator every epoch makes it behave like a normal integrator");
84
85 ✗ if (InertialPreIntegratorGui(std::to_string(size_t(id)).c_str(), _inertialPreintegrator))
86 {
87 ✗ flow::ApplyChanges();
88 }
89 }
90 else
91 {
92 ✗ if (InertialIntegratorGui(std::to_string(size_t(id)).c_str(), _inertialIntegrator, _preferAccelerationOverDeltaMeasurements))
93 {
94 ✗ flow::ApplyChanges();
95 }
96 }
97 ✗ }
98
99 ✗ [[nodiscard]] json NAV::ImuIntegrator::save() const
100 {
101 LOG_TRACE("{}: called", nameId());
102
103 return {
104 ✗ { "imuPreintegration", _imuPreintegration },
105 ✗ { "resetPreintegratorEveryEpoch", _resetPreintegratorEveryEpoch },
106 ✗ { "inertialIntegrator", _inertialIntegrator },
107 ✗ { "inertialPreintegrator", _inertialPreintegrator },
108 ✗ { "preferAccelerationOverDeltaMeasurements", _preferAccelerationOverDeltaMeasurements },
109 ✗ };
110 ✗ }
111
112 1 void NAV::ImuIntegrator::restore(json const& j)
113 {
114 LOG_TRACE("{}: called", nameId());
115
116
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (j.contains("imuPreintegration")) { j.at("imuPreintegration").get_to(_imuPreintegration); }
117
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (j.contains("resetPreintegratorEveryEpoch")) { j.at("resetPreintegratorEveryEpoch").get_to(_resetPreintegratorEveryEpoch); }
118
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("inertialIntegrator")) { j.at("inertialIntegrator").get_to(_inertialIntegrator); }
119
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (j.contains("inertialPreintegrator")) { j.at("inertialPreintegrator").get_to(_inertialPreintegrator); }
120
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("preferAccelerationOverDeltaMeasurements")) { j.at("preferAccelerationOverDeltaMeasurements").get_to(_preferAccelerationOverDeltaMeasurements); }
121 1 }
122
123 ✗ bool NAV::ImuIntegrator::initialize()
124 {
125 LOG_TRACE("{}: called", nameId());
126
127 ✗ _inertialIntegrator.reset();
128 ✗ _inertialPreintegrator.reset();
129 ✗ _lastImuObs.reset();
130 ✗ _lastPosVelAtt.reset();
131
132 ✗ LOG_DEBUG("ImuIntegrator initialized");
133
134 ✗ return true;
135 }
136
137 ✗ void NAV::ImuIntegrator::deinitialize()
138 {
139 LOG_TRACE("{}: called", nameId());
140 ✗ }
141
142 ✗ void NAV::ImuIntegrator::recvObservation(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
143 {
144 ✗ auto nodeData = queue.extract_front();
145 ✗ if (nodeData->insTime.empty())
146 {
147 ✗ LOG_ERROR("{}: Can't set new imuObs__t0 because the observation has no time tag (insTime)", nameId());
148 ✗ return;
149 }
150
151 ✗ std::shared_ptr<NAV::PosVelAtt> integratedPosVelAtt = nullptr;
152
153 ✗ if (_imuPreintegration)
154 {
155 ✗ auto obs = std::static_pointer_cast<const ImuObs>(nodeData);
156 LOG_DATA("{}: recvImuObs at time [{}] (preintegration)", nameId(), obs->insTime.toYMDHMS(GPST));
157 // LOG_DATA("{}: p_accel = {}", nameId(), obs->p_acceleration.transpose());
158 // LOG_DATA("{}: p_gyro = {}", nameId(), obs->p_angularRate.transpose());
159
160 ✗ if (!_lastImuObs)
161 {
162 ✗ _lastImuObs = obs;
163 LOG_DATA("{}: Skipping as first epoch", nameId());
164 ✗ return;
165 }
166
167 ✗ auto dt = static_cast<double>((obs->insTime - _lastImuObs->insTime).count());
168 // LOG_DATA("{}: dt(imu) = {}", nameId(), dt);
169 ✗ if (dt <= 1e-9)
170 {
171 LOG_DATA("{}: Skipping as dt is negative or zero", nameId());
172 ✗ return;
173 }
174 InertialPreIntegrator::Measurement meas{
175 .meas = InertialPreIntegrator::ImuMeasurement{ .dt = dt,
176 ✗ .p_acceleration = _lastImuObs->p_acceleration,
177 ✗ .p_angularRate = _lastImuObs->p_angularRate },
178 .imu = InertialPreIntegrator::ImuState<double>{}
179 ✗ };
180 InertialPreIntegrator::PVAState<double> pvaOld{
181 ✗ .position = _lastPosVelAtt->e_position(),
182 ✗ .velocity = _lastPosVelAtt->e_velocity(),
183 ✗ .attitude = _lastPosVelAtt->e_Quat_b()
184 ✗ };
185
186 ✗ _inertialPreintegrator.addInertialMeasurement(meas, obs->imuPos, nameId());
187
188 ✗ auto pvaNew = _inertialPreintegrator.calcIntegratedState(pvaOld, InertialPreIntegrator::ImuState<double>{}, obs->imuPos, nameId());
189
190 ✗ integratedPosVelAtt = std::make_shared<PosVelAtt>();
191 ✗ integratedPosVelAtt->insTime = obs->insTime;
192 ✗ integratedPosVelAtt->setPosVelAtt_e(pvaNew.position, pvaNew.velocity, pvaNew.attitude);
193
194 ✗ _lastImuObs = obs;
195
196 ✗ if (_resetPreintegratorEveryEpoch)
197 {
198 ✗ _lastPosVelAtt = integratedPosVelAtt;
199 ✗ _inertialPreintegrator.reset();
200 }
201 ✗ }
202 else
203 {
204 ✗ if (!_preferAccelerationOverDeltaMeasurements
205 ✗ && NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(inputPins.at(INPUT_PORT_INDEX_IMU_OBS).link.getConnectedPin()->dataIdentifier, { ImuObsWDelta::type() }))
206 {
207 ✗ auto obs = std::static_pointer_cast<const ImuObsWDelta>(nodeData);
208 LOG_DATA("{}: recvImuObsWDelta at time [{}]", nameId(), obs->insTime.toYMDHMS(GPST));
209
210 ✗ integratedPosVelAtt = _inertialIntegrator.calcInertialSolutionDelta(obs->insTime, obs->dtime, obs->dvel, obs->dtheta, obs->imuPos, nameId().c_str());
211 ✗ }
212 else
213 {
214 ✗ auto obs = std::static_pointer_cast<const ImuObs>(nodeData);
215 LOG_DATA("{}: recvImuObs at time [{}]", nameId(), obs->insTime.toYMDHMS(GPST));
216
217 ✗ integratedPosVelAtt = _inertialIntegrator.calcInertialSolution(obs->insTime, obs->p_acceleration, obs->p_angularRate, obs->imuPos, nameId().c_str());
218 ✗ }
219 }
220
221 ✗ if (integratedPosVelAtt)
222 {
223 LOG_DATA("{}: e_position = {}", nameId(), integratedPosVelAtt->e_position().transpose());
224 LOG_DATA("{}: e_velocity = {}", nameId(), integratedPosVelAtt->e_velocity().transpose());
225 LOG_DATA("{}: rollPitchYaw = {}", nameId(), rad2deg(integratedPosVelAtt->rollPitchYaw()).transpose());
226 ✗ invokeCallbacks(OUTPUT_PORT_INDEX_INERTIAL_NAV_SOL, integratedPosVelAtt);
227 }
228 ✗ }
229
230 ✗ void NAV::ImuIntegrator::recvPosVelAttInit(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
231 {
232 ✗ auto posVelAtt = std::static_pointer_cast<const PosVelAtt>(queue.extract_front());
233 ✗ inputPins[INPUT_PORT_INDEX_POS_VEL_ATT_INIT].queueBlocked = true;
234 ✗ inputPins[INPUT_PORT_INDEX_POS_VEL_ATT_INIT].queue.clear();
235
236 LOG_DATA("{}: recvPosVelAttInit at time [{}]", nameId(), posVelAtt->insTime.toYMDHMS(GPST));
237
238 ✗ _lastPosVelAtt = posVelAtt;
239 ✗ _inertialIntegrator.setInitialState(*posVelAtt, nameId().c_str());
240 LOG_DATA("{}: e_position = {}", nameId(), posVelAtt->e_position().transpose());
241 LOG_DATA("{}: e_velocity = {}", nameId(), posVelAtt->e_velocity().transpose());
242 LOG_DATA("{}: rollPitchYaw = {}", nameId(), rad2deg(posVelAtt->rollPitchYaw()).transpose());
243
244 ✗ invokeCallbacks(OUTPUT_PORT_INDEX_INERTIAL_NAV_SOL, posVelAtt);
245 ✗ }
246