Rework radar system
This commit is contained in:
Ayzen
@@ -0,0 +1,55 @@
|
||||
#pragma once
|
||||
|
||||
#include <span>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include "shared_types.hpp"
|
||||
|
||||
namespace radar::preprocessing {
|
||||
|
||||
struct ChannelTrace {
|
||||
std::vector<float> frequency_hz{};
|
||||
std::vector<ipc::Complex32> samples{};
|
||||
};
|
||||
|
||||
class S21CalibrationBundle {
|
||||
public:
|
||||
void load(const std::string& path);
|
||||
|
||||
void validate_combos(const std::vector<ipc::ComboKey>& combos) const;
|
||||
void apply(
|
||||
const ipc::ComboKey& combo,
|
||||
std::span<const float> frequency_hz,
|
||||
std::span<ipc::Complex32> measured
|
||||
) const;
|
||||
|
||||
private:
|
||||
std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> traces_by_combo_{};
|
||||
};
|
||||
|
||||
class S21ReferenceBundle {
|
||||
public:
|
||||
// Load and through-calibrate the active references once at startup.
|
||||
void load(const std::string& path, const S21CalibrationBundle& calibration,
|
||||
const std::vector<ipc::ComboKey>& combos);
|
||||
void apply(ipc::SweepTraceBlock& trace) const;
|
||||
|
||||
private:
|
||||
std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> traces_by_combo_{};
|
||||
};
|
||||
|
||||
// Object-derived MIMO correction, exported on each pair's runtime frequency grid.
|
||||
// An empty path disables it; enabled bundles must cover every active pair.
|
||||
class S21ObjectCalibrationBundle {
|
||||
public:
|
||||
void load(const std::string& path);
|
||||
void validate_combos(const std::vector<ipc::ComboKey>& combos) const;
|
||||
void apply(ipc::SweepTraceBlock& trace) const;
|
||||
|
||||
private:
|
||||
std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> factors_by_combo_{};
|
||||
};
|
||||
|
||||
} // namespace radar::preprocessing
|
||||
@@ -0,0 +1,255 @@
|
||||
#include "channel_bundle_support.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <complex>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <fstream>
|
||||
#include <iterator>
|
||||
#include <limits>
|
||||
#include <span>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace radar::preprocessing {
|
||||
namespace {
|
||||
|
||||
constexpr float kFrequencyRelativeTolerance = 1e-5F;
|
||||
constexpr float kFrequencyAbsoluteTolerance = 1e-3F;
|
||||
[[nodiscard]] auto combo_to_string(const ipc::ComboKey& combo) -> std::string {
|
||||
return "input=" + std::to_string(combo.input_pos) + " output=" + std::to_string(combo.output_pos);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto read_binary_file(const std::string& path, const std::string& bundle_label)
|
||||
-> std::vector<std::uint8_t> {
|
||||
std::ifstream stream(path, std::ios::binary);
|
||||
if (!stream.is_open()) {
|
||||
throw std::runtime_error("Failed to open " + bundle_label + " bundle: " + path);
|
||||
}
|
||||
|
||||
return std::vector<std::uint8_t>(std::istreambuf_iterator<char>(stream), std::istreambuf_iterator<char>());
|
||||
}
|
||||
|
||||
void validate_channel_trace_layout(
|
||||
const ChannelTrace& trace,
|
||||
const std::string& trace_label,
|
||||
const ipc::ComboKey& combo
|
||||
) {
|
||||
if (trace.frequency_hz.size() != trace.samples.size()) {
|
||||
throw std::runtime_error(
|
||||
trace_label + " frequency/channel vector size mismatch for combo " + combo_to_string(combo)
|
||||
);
|
||||
}
|
||||
for (std::size_t i = 0; i < trace.samples.size(); ++i) {
|
||||
if (!std::isfinite(trace.frequency_hz[i]) || !std::isfinite(trace.samples[i].re)
|
||||
|| !std::isfinite(trace.samples[i].im)) {
|
||||
throw std::runtime_error(trace_label + " contains non-finite values for combo " + combo_to_string(combo));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto frequency_axes_match(std::span<const float> left, std::span<const float> right) -> bool {
|
||||
if (left.size() != right.size()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (std::size_t index = 0; index < left.size(); ++index) {
|
||||
if (!std::isfinite(left[index]) || !std::isfinite(right[index])) {
|
||||
return false;
|
||||
}
|
||||
const auto delta = std::fabs(left[index] - right[index]);
|
||||
const auto scale = std::max(std::fabs(left[index]), std::fabs(right[index]));
|
||||
const auto tolerance = std::max(kFrequencyAbsoluteTolerance, scale * kFrequencyRelativeTolerance);
|
||||
if (delta > tolerance) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void ensure_frequency_axis_match(
|
||||
std::span<const float> expected,
|
||||
std::span<const float> actual,
|
||||
const std::string& label
|
||||
) {
|
||||
if (!frequency_axes_match(expected, actual)) {
|
||||
throw std::runtime_error(label + " frequency axis mismatch");
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] auto to_channel_trace(
|
||||
const ipc::SweepTraceBlock& trace,
|
||||
const std::string& bundle_label
|
||||
) -> ChannelTrace {
|
||||
if (trace.frequency_hz.empty()) {
|
||||
throw std::runtime_error(bundle_label + " trace contains no frequency samples");
|
||||
}
|
||||
|
||||
ChannelTrace channel_trace{};
|
||||
channel_trace.frequency_hz = trace.frequency_hz;
|
||||
channel_trace.samples = trace.s21;
|
||||
validate_channel_trace_layout(channel_trace, bundle_label + " trace", trace.combo);
|
||||
return channel_trace;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto load_channel_traces(
|
||||
const std::string& path,
|
||||
const std::string& bundle_label
|
||||
) -> std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> {
|
||||
const auto bytes = read_binary_file(path, bundle_label);
|
||||
if (bytes.empty()) {
|
||||
throw std::runtime_error(bundle_label + " bundle is empty: " + path);
|
||||
}
|
||||
|
||||
const auto collection = ipc::deserialize_raw_collection(bytes);
|
||||
std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash> traces_by_combo{};
|
||||
traces_by_combo.reserve(collection.traces.size());
|
||||
for (const auto& trace : collection.traces) {
|
||||
// #47: reject duplicate combos instead of silently overwriting an earlier trace.
|
||||
const auto inserted =
|
||||
traces_by_combo.insert({trace.combo, to_channel_trace(trace, bundle_label)});
|
||||
if (!inserted.second) {
|
||||
throw std::runtime_error(
|
||||
bundle_label + " bundle contains duplicate combo " + combo_to_string(trace.combo) + ": " + path
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
if (traces_by_combo.empty()) {
|
||||
throw std::runtime_error(bundle_label + " bundle does not contain traces: " + path);
|
||||
}
|
||||
return traces_by_combo;
|
||||
}
|
||||
|
||||
void validate_bundle_combos(
|
||||
const std::unordered_map<ipc::ComboKey, ChannelTrace, ipc::ComboKeyHash>& traces_by_combo,
|
||||
const std::vector<ipc::ComboKey>& combos,
|
||||
const std::string& bundle_label
|
||||
) {
|
||||
if (traces_by_combo.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (const auto& combo : combos) {
|
||||
if (!traces_by_combo.contains(combo)) {
|
||||
throw std::runtime_error(bundle_label + " is missing combo " + combo_to_string(combo));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <typename TValue>
|
||||
auto resolve_required(
|
||||
const std::unordered_map<ipc::ComboKey, TValue, ipc::ComboKeyHash>& values_by_combo,
|
||||
const ipc::ComboKey& combo,
|
||||
const char* value_label
|
||||
) -> const TValue& {
|
||||
const auto found = values_by_combo.find(combo);
|
||||
if (found == values_by_combo.end()) {
|
||||
throw std::runtime_error(std::string(value_label) + " is missing for combo " + combo_to_string(combo));
|
||||
}
|
||||
return found->second;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void S21CalibrationBundle::load(const std::string& path) {
|
||||
if (path.empty()) {
|
||||
throw std::runtime_error("S21 calibration bundle path must not be empty");
|
||||
}
|
||||
|
||||
traces_by_combo_ = load_channel_traces(path, "S21 calibration");
|
||||
}
|
||||
|
||||
void S21CalibrationBundle::validate_combos(const std::vector<ipc::ComboKey>& combos) const {
|
||||
validate_bundle_combos(traces_by_combo_, combos, "S21 calibration bundle");
|
||||
}
|
||||
|
||||
void S21CalibrationBundle::apply(
|
||||
const ipc::ComboKey& combo,
|
||||
std::span<const float> frequency_hz,
|
||||
std::span<ipc::Complex32> measured
|
||||
) const {
|
||||
const auto& calibration_trace = resolve_required(traces_by_combo_, combo, "S21 calibration trace");
|
||||
ensure_frequency_axis_match(calibration_trace.frequency_hz, frequency_hz, "S21 calibration");
|
||||
if (measured.size() != calibration_trace.samples.size()) {
|
||||
throw std::runtime_error("S21 calibration vector size mismatch");
|
||||
}
|
||||
constexpr float epsilon = 1e-12F;
|
||||
for (std::size_t index = 0; index < measured.size(); ++index) {
|
||||
const auto standard = calibration_trace.samples[index];
|
||||
const float magnitude_squared = standard.re * standard.re + standard.im * standard.im;
|
||||
if (magnitude_squared > epsilon) {
|
||||
const auto value = measured[index];
|
||||
measured[index] = {
|
||||
(value.re * standard.re + value.im * standard.im) / magnitude_squared,
|
||||
(value.im * standard.re - value.re * standard.im) / magnitude_squared,
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void S21ReferenceBundle::load(
|
||||
const std::string& path,
|
||||
const S21CalibrationBundle& calibration,
|
||||
const std::vector<ipc::ComboKey>& combos
|
||||
) {
|
||||
const auto raw = load_channel_traces(path, "S21 reference");
|
||||
decltype(traces_by_combo_) calibrated;
|
||||
for (const auto& combo : combos) {
|
||||
auto trace = resolve_required(raw, combo, "S21 reference");
|
||||
calibration.apply(combo, trace.frequency_hz, trace.samples);
|
||||
validate_channel_trace_layout(trace, "Calibrated S21 reference", combo);
|
||||
calibrated.emplace(combo, std::move(trace));
|
||||
}
|
||||
traces_by_combo_ = std::move(calibrated);
|
||||
}
|
||||
|
||||
void S21ReferenceBundle::apply(ipc::SweepTraceBlock& trace) const {
|
||||
const auto& reference = resolve_required(traces_by_combo_, trace.combo, "Calibrated S21 reference");
|
||||
ensure_frequency_axis_match(reference.frequency_hz, trace.frequency_hz, "S21 reference");
|
||||
if (trace.s21.size() != reference.samples.size()) {
|
||||
throw std::runtime_error("S21 reference point count mismatch");
|
||||
}
|
||||
for (std::size_t index = 0; index < trace.s21.size(); ++index) {
|
||||
trace.s21[index].re -= reference.samples[index].re;
|
||||
trace.s21[index].im -= reference.samples[index].im;
|
||||
}
|
||||
}
|
||||
|
||||
void S21ObjectCalibrationBundle::load(const std::string& path) {
|
||||
if (path.empty()) {
|
||||
factors_by_combo_.clear();
|
||||
return;
|
||||
}
|
||||
factors_by_combo_ = load_channel_traces(path, "S21 object calibration");
|
||||
}
|
||||
|
||||
void S21ObjectCalibrationBundle::validate_combos(const std::vector<ipc::ComboKey>& combos) const {
|
||||
validate_bundle_combos(factors_by_combo_, combos, "S21 object calibration bundle");
|
||||
}
|
||||
|
||||
void S21ObjectCalibrationBundle::apply(ipc::SweepTraceBlock& trace) const {
|
||||
if (factors_by_combo_.empty()) { return; }
|
||||
const auto& factors = resolve_required(factors_by_combo_, trace.combo, "S21 object calibration");
|
||||
ensure_frequency_axis_match(factors.frequency_hz, trace.frequency_hz, "S21 object calibration");
|
||||
if (trace.s21.size() != factors.samples.size()) {
|
||||
throw std::runtime_error("S21 object calibration vector size mismatch");
|
||||
}
|
||||
for (std::size_t index = 0; index < trace.s21.size(); ++index) {
|
||||
const auto value = trace.s21[index];
|
||||
const auto factor = factors.samples[index];
|
||||
const auto corrected = std::complex<double>(value.re, value.im) * std::complex<double>(factor.re, factor.im);
|
||||
if (!std::isfinite(corrected.real()) || !std::isfinite(corrected.imag()) ||
|
||||
std::abs(corrected.real()) > std::numeric_limits<float>::max() ||
|
||||
std::abs(corrected.imag()) > std::numeric_limits<float>::max()) {
|
||||
throw std::runtime_error("S21 object calibration result must fit finite float32");
|
||||
}
|
||||
trace.s21[index] = {.re = static_cast<float>(corrected.real()), .im = static_cast<float>(corrected.imag())};
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace radar::preprocessing
|
||||
Reference in New Issue
Block a user