INSTINCT Code Coverage Report

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