From 739a61baca42ea94886a736ef36b01d8892cb28c Mon Sep 17 00:00:00 2001 From: Josh Poole Date: Mon, 14 Sep 2026 12:59:40 +0100 Subject: [PATCH 1/4] 20260914 - Stop copying the whole CPI three times per frame IqData::get_data() returns the deque by value. It was called three times a CPI, once in SpectrumAnalyser and twice in WienerHopf. At a million samples that is 16 MB and roughly 31,000 deque-chunk allocations each, so about 48 MB and 94,000 allocations a CPI to read data nothing then mutates. view_data() returns a const reference instead. Measured on a Pi 5, one copy of a 1,000,000-sample deque> is 10.95 ms, so the three cost 32.85 ms a CPI. Reading the same samples through the reference costs 3.10 ms, which is work that happens either way. SpectrumAnalyser also built a full-length fftshift vector with push_back and no reserve, then read every decimation-th element out of it. The shift is now applied in the decimation loop, which touches exactly the same elements, and the output vector is reserved. Same values, one fewer million-element vector. Deliberately not touched: the normalisation and the frequency axis in the same function. The frequency loop is a known latent bug (i is unsigned, so i = -nSpectrum/2 wraps and the loop never runs, leaving the axis empty) but fixing it changes what the front end displays and deserves its own change. Co-Authored-By: Claude Opus 5 (1M context) --- src/data/IqData.cpp | 5 +++++ src/data/IqData.h | 8 ++++++++ src/process/clutter/WienerHopf.cpp | 7 +++++-- src/process/clutter/WienerHopf.h | 1 - src/process/spectrum/SpectrumAnalyser.cpp | 15 +++++---------- 5 files changed, 23 insertions(+), 13 deletions(-) diff --git a/src/data/IqData.cpp b/src/data/IqData.cpp index 2c9bb814..39c85313 100644 --- a/src/data/IqData.cpp +++ b/src/data/IqData.cpp @@ -39,6 +39,11 @@ std::deque> IqData::get_data() return *data; } +const std::deque> &IqData::view_data() const +{ + return *data; +} + void IqData::push_back(std::complex sample) { if (data->size() < n) diff --git a/src/data/IqData.h b/src/data/IqData.h index f45e3632..c5997b18 100644 --- a/src/data/IqData.h +++ b/src/data/IqData.h @@ -66,6 +66,14 @@ class IqData /// @return IQ data. std::deque> get_data(); + /// @brief Read-only view of the data, copying nothing. + /// @details Prefer this to get_data() on the hot path: a CPI is a million + /// samples, so a copy is 16 MB and roughly 31,000 deque-chunk allocations. + /// The caller must not hold the reference across anything that mutates the + /// queue (push_back, pop_front, clear). + /// @return Const reference to the IQ data. + const std::deque> &view_data() const; + /// @brief Push a sample to the queue. /// @param sample A single sample. /// @return Void. diff --git a/src/process/clutter/WienerHopf.cpp b/src/process/clutter/WienerHopf.cpp index 791fe571..2c2390c6 100644 --- a/src/process/clutter/WienerHopf.cpp +++ b/src/process/clutter/WienerHopf.cpp @@ -67,8 +67,11 @@ WienerHopf::~WienerHopf() bool WienerHopf::process(IqData *x, IqData *y) { uint32_t i, j; - xData = x->get_data(); - yData = y->get_data(); + // Views, not copies: each of these was 16 MB and ~31,000 allocations a CPI. + // Both are read out into dataX/dataY immediately below and not touched + // again, so the later y->clear() cannot be observed through yData. + const std::deque> &xData = x->view_data(); + const std::deque> &yData = y->view_data(); // change deque to std::complex for (i = 0; i < nSamples; i++) diff --git a/src/process/clutter/WienerHopf.h b/src/process/clutter/WienerHopf.h index 8d92f362..e8fc565f 100644 --- a/src/process/clutter/WienerHopf.h +++ b/src/process/clutter/WienerHopf.h @@ -47,7 +47,6 @@ class WienerHopf /// @brief Deque storage for clutter filter processing. /// @{ - std::deque> xData, yData; /// @} /// @brief Autocorrelation toeplitz matrix. diff --git a/src/process/spectrum/SpectrumAnalyser.cpp b/src/process/spectrum/SpectrumAnalyser.cpp index 1a17bd32..0ca16fce 100644 --- a/src/process/spectrum/SpectrumAnalyser.cpp +++ b/src/process/spectrum/SpectrumAnalyser.cpp @@ -32,25 +32,20 @@ void SpectrumAnalyser::process(IqData *x) { // load data and FFT uint32_t i; - std::deque> data = x->get_data(); + const std::deque> &data = x->view_data(); for (i = 0; i < nfft; i++) { dataX[i] = data[i]; } fftw_execute(fftX); - // fftshift - std::vector> fftshift; - for (i = 0; i < nfft; i++) - { - fftshift.push_back(dataX[(i + int(nfft / 2) + 1) % nfft]); - } - - // decimate + // fftshift and decimate in one pass. The full-length shifted vector was only + // ever read at every decimation-th element, so the same values come out. std::vector> spectrum; + spectrum.reserve((nfft + decimation - 1) / decimation); for (i = 0; i < nfft; i+=decimation) { - spectrum.push_back(fftshift[i]); + spectrum.push_back(dataX[(i + int(nfft / 2) + 1) % nfft]); } x->update_spectrum(spectrum); From 002b9f29ceedb64ba0846f5a3db21e510a3ec753 Mon Sep 17 00:00:00 2001 From: Josh Poole Date: Mon, 14 Sep 2026 12:59:45 +0100 Subject: [PATCH 2/4] 20260914 - Serialise the map and detections in a single pass to_json() built a RapidJSON DOM over the whole map, 301 x 411 = 123,711 doubles at the shipped geometry, and serialised it. delay_bin_to_km() then parsed all 665 KB of that back into a second DOM, rewrote 411 delay values and serialised the lot again. The map went through the writer twice and the parser once so that one axis could be converted to km. to_json_km() writes it once, straight to the buffer, with no DOM at either end and the delay axis already converted. Measured on a Pi 5 at the shipped geometry: 34.12 ms -> 19.21 ms, a saving of 14.91 ms a CPI. For reference owl-ded9 reports output_radar_data at 35.5 ms, so the bench matches the node. Key order and SetMaxDecimalPlaces(2) are unchanged, so the bytes are identical and nothing downstream can tell the difference. testJsonKm asserts exactly that against the old pair across eight cases, including the shipped geometry, both fleet Doppler spans, an empty detection list and values chosen to stress the decimal cap. The old pair is kept solely as that oracle: deleting it would delete the proof. Not taken from upstream: their 1e-30 log clamp and their dropped save.map bound, both of which change behaviour. Co-Authored-By: Claude Opus 5 (1M context) --- CMakeLists.txt | 14 ++++++ src/blah2.cpp | 6 +-- src/data/Detection.cpp | 39 ++++++++++++++ src/data/Detection.h | 8 +++ src/data/Map.cpp | 57 +++++++++++++++++++++ src/data/Map.h | 8 +++ test/unit/data/TestJsonKm.cpp | 95 +++++++++++++++++++++++++++++++++++ 7 files changed, 223 insertions(+), 4 deletions(-) create mode 100644 test/unit/data/TestJsonKm.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index c401c75c..f95db810 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -125,7 +125,21 @@ target_link_libraries(testClutterFft PRIVATE set_target_properties(testClutterFft PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_TEST_UNIT_DIR}") +add_executable(testJsonKm + test/unit/data/TestJsonKm.cpp + src/data/Map.cpp + src/data/Detection.cpp +) +# armadillo is linked only to pick up the vcpkg include path, which is +# where rapidjson lives; RAPIDJSON_INCLUDE_DIRS is NOTFOUND in this build. +target_link_libraries(testJsonKm PRIVATE + armadillo +) +set_target_properties(testJsonKm PROPERTIES + RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_TEST_UNIT_DIR}") + # TODO: Unsure if will be using CTest. add_test(NAME testAmbiguity COMMAND testAmbiguity) add_test(NAME testTracker COMMAND testTracker) add_test(NAME testClutterFft COMMAND testClutterFft) +add_test(NAME testJsonKm COMMAND testJsonKm) diff --git a/src/blah2.cpp b/src/blah2.cpp index 8a130562..244e9a64 100644 --- a/src/blah2.cpp +++ b/src/blah2.cpp @@ -301,8 +301,7 @@ int main(int argc, char **argv) socket_iqdata.sendData(jsonIqData); // output map data - mapJson = map->to_json(time[0]/1000); - mapJson = map->delay_bin_to_km(mapJson, fs); + mapJson = map->to_json_km(time[0]/1000, fs); if (saveMap) { map->save(mapJson, saveMapPath); @@ -312,8 +311,7 @@ int main(int argc, char **argv) // output detection data if (isDetection) { - detectionJson = detection->to_json(time[0]/1000); - detectionJson = detection->delay_bin_to_km(detectionJson, fs); + detectionJson = detection->to_json_km(time[0]/1000, fs); socket_detection.sendData(detectionJson); } diff --git a/src/data/Detection.cpp b/src/data/Detection.cpp index 3e6e903c..9171b83d 100644 --- a/src/data/Detection.cpp +++ b/src/data/Detection.cpp @@ -84,6 +84,45 @@ std::string Detection::to_json(uint64_t timestamp) return strbuf.GetString(); } +std::string Detection::to_json_km(uint64_t timestamp, uint32_t fs) +{ + // Single pass, matching to_json() + delay_bin_to_km() byte for byte. + rapidjson::StringBuffer strbuf; + rapidjson::Writer writer(strbuf); + writer.SetMaxDecimalPlaces(2); + + writer.StartObject(); + writer.Key("timestamp"); + writer.Uint64(timestamp); + + writer.Key("delay"); + writer.StartArray(); + for (size_t i = 0; i < delay.size(); i++) + { + writer.Double(1.0 * delay[i] * (Constants::c / (double)fs) / 1000); + } + writer.EndArray(); + + writer.Key("doppler"); + writer.StartArray(); + for (size_t i = 0; i < get_nDetections(); i++) + { + writer.Double(doppler[i]); + } + writer.EndArray(); + + writer.Key("snr"); + writer.StartArray(); + for (size_t i = 0; i < get_nDetections(); i++) + { + writer.Double(snr[i]); + } + writer.EndArray(); + writer.EndObject(); + + return strbuf.GetString(); +} + std::string Detection::delay_bin_to_km(std::string json, uint32_t fs) { rapidjson::Document document; diff --git a/src/data/Detection.h b/src/data/Detection.h index 4e7173c5..5d4879a6 100644 --- a/src/data/Detection.h +++ b/src/data/Detection.h @@ -62,6 +62,14 @@ class Detection /// @return JSON string. std::string delay_bin_to_km(std::string json, uint32_t fs); + /// @brief Serialise to JSON with the delay axis already in km. + /// @details Equivalent to delay_bin_to_km(to_json(timestamp), fs) but in a + /// single pass, without building or re-parsing a DOM. + /// @param timestamp Timestamp of the detections (ms). + /// @param fs Sampling frequency (Hz). + /// @return JSON string. + std::string to_json_km(uint64_t timestamp, uint32_t fs); + /// @brief Append the detections to a save file. /// @param json JSON string of detections and metadata. /// @param path Path of file to save. diff --git a/src/data/Map.cpp b/src/data/Map.cpp index bf7ee6e3..a52bd7f3 100644 --- a/src/data/Map.cpp +++ b/src/data/Map.cpp @@ -163,6 +163,63 @@ std::string Map::to_json(uint64_t timestamp) return strbuf.GetString(); } +template +std::string Map::to_json_km(uint64_t timestamp, uint32_t fs) +{ + // One pass, no DOM. The pair to_json() + delay_bin_to_km() serialised the + // whole map, parsed all of it back, rewrote the delay axis and serialised + // again, so a 301 x 411 map went through the writer twice and the parser + // once to convert 411 delay values. Key order and SetMaxDecimalPlaces match + // the old pair exactly, so the bytes are identical. + rapidjson::StringBuffer strbuf; + rapidjson::Writer writer(strbuf); + writer.SetMaxDecimalPlaces(2); + + writer.StartObject(); + writer.Key("timestamp"); + writer.Uint64(timestamp); + writer.Key("nRows"); + writer.Uint(nRows); + writer.Key("nCols"); + writer.Uint(nCols); + writer.Key("noisePower"); + writer.Double(noisePower); + writer.Key("maxPower"); + writer.Double(maxPower); + + writer.Key("delay"); + writer.StartArray(); + for (size_t i = 0; i < delay.size(); i++) + { + writer.Double(1.0 * delay[i] * (Constants::c / (double)fs) / 1000); + } + writer.EndArray(); + + writer.Key("doppler"); + writer.StartArray(); + for (uint32_t i = 0; i < get_nRows(); i++) + { + writer.Double(doppler[i]); + } + writer.EndArray(); + + writer.Key("data"); + writer.StartArray(); + for (size_t i = 0; i < data.size(); i++) + { + writer.StartArray(); + for (size_t j = 0; j < data[i].size(); j++) + { + writer.Double(10 * std::log10(std::abs(data[i][j])) - noisePower); + } + writer.EndArray(); + } + writer.EndArray(); + writer.EndObject(); + + return strbuf.GetString(); +} + template std::string Map::delay_bin_to_km(std::string json, uint32_t fs) { diff --git a/src/data/Map.h b/src/data/Map.h index 9d43a63e..820b6f28 100644 --- a/src/data/Map.h +++ b/src/data/Map.h @@ -104,6 +104,14 @@ class Map /// @return JSON string. std::string delay_bin_to_km(std::string json, uint32_t fs); + /// @brief Serialise to JSON with the delay axis already in km. + /// @details Equivalent to delay_bin_to_km(to_json(timestamp), fs) but in a + /// single pass, without building or re-parsing a DOM. + /// @param timestamp Timestamp of the map (ms). + /// @param fs Sampling frequency (Hz). + /// @return JSON string. + std::string to_json_km(uint64_t timestamp, uint32_t fs); + /// @brief Append the map to a save file. /// @param json JSON string of map and metadata. /// @param path Path of file to save. diff --git a/test/unit/data/TestJsonKm.cpp b/test/unit/data/TestJsonKm.cpp new file mode 100644 index 00000000..0a62c2ae --- /dev/null +++ b/test/unit/data/TestJsonKm.cpp @@ -0,0 +1,95 @@ +// Proves to_json_km() is byte-identical to the to_json() + delay_bin_to_km() +// pair it replaces. The pair is kept solely as the oracle for this test. +#include "data/Map.h" +#include "data/Detection.h" + +#include +#include +#include +#include +#include +#include +#include + +using Complex = std::complex; + +static void compare(const std::string& oracle, const std::string& single, + const char* what) { + if (oracle == single) return; + // Report the first divergence rather than dumping two huge strings. + size_t i = 0; + while (i < oracle.size() && i < single.size() && oracle[i] == single[i]) ++i; + std::cerr << what << ": diverges at byte " << i << "\n oracle: ..." + << oracle.substr(i > 40 ? i - 40 : 0, 120) << "\n single: ..." + << single.substr(i > 40 ? i - 40 : 0, 120) << '\n'; + throw std::runtime_error(what); +} + +static void runMap(uint32_t nRows, uint32_t nCols, int delayMin, uint32_t fs, + uint64_t timestamp, bool extremes) { + Map map(nRows, nCols); + std::mt19937 rng(4127); + std::normal_distribution random; + + map.delay.clear(); + for (uint32_t i = 0; i < nCols; i++) + map.delay.push_back(delayMin + int(i)); + map.doppler.clear(); + for (uint32_t i = 0; i < nRows; i++) + map.doppler.push_back((double(i) - double(nRows) / 2) * 1.5); + + for (uint32_t i = 0; i < nRows; i++) + for (uint32_t j = 0; j < nCols; j++) + map.data[i][j] = Complex(random(rng), random(rng)); + + if (extremes) { + // Values that stress the decimal-places cap and the sign of the exponent. + map.data[0][0] = Complex(1e-8, 0); + map.data[0][1] = Complex(1e9, -1e9); + map.data[1][0] = Complex(-0.005, 0.004); + } + + map.set_metrics(); + + compare(map.delay_bin_to_km(map.to_json(timestamp), fs), + map.to_json_km(timestamp, fs), "Map JSON"); + std::cout << "PASS map " << nRows << "x" << nCols << " delayMin=" << delayMin + << " fs=" << fs << (extremes ? " (extremes)" : "") << '\n'; +} + +static void runDetection(size_t n, uint32_t fs, uint64_t timestamp) { + std::vector delay, doppler, snr; + std::mt19937 rng(90210); + std::uniform_real_distribution random(-400.0, 400.0); + for (size_t i = 0; i < n; i++) { + delay.push_back(random(rng)); + doppler.push_back(random(rng)); + snr.push_back(std::abs(random(rng)) / 10); + } + Detection detection(delay, doppler, snr); + + compare(detection.delay_bin_to_km(detection.to_json(timestamp), fs), + detection.to_json_km(timestamp, fs), "Detection JSON"); + std::cout << "PASS detection n=" << n << " fs=" << fs << '\n'; +} + +int main() { + try { + // The shipped geometry, and smaller ones that exercise the edges. + runMap(301, 411, -10, 2000000, 1789383594875ULL, false); + runMap(301, 411, -10, 2000000, 1789383594875ULL, true); + runMap(201, 411, -10, 2000000, 1789383594875ULL, false); + runMap(3, 5, 0, 2400000, 0ULL, false); + runMap(1, 1, 7, 1000000, 1ULL, false); + + runDetection(0, 2000000, 1789383594875ULL); + runDetection(1, 2000000, 1789383594875ULL); + runDetection(64, 2400000, 42ULL); + + std::cout << "All JSON equivalence checks passed\n"; + } catch (const std::exception& error) { + std::cerr << "FAIL: " << error.what() << '\n'; + return 1; + } + return 0; +} From e1f3c222e001629207d83489ebb0c79bc06b609e Mon Sep 17 00:00:00 2001 From: Josh Poole Date: Mon, 14 Sep 2026 12:59:52 +0100 Subject: [PATCH 3/4] 20260914 - Drop the ambiguity staging array and column copies Three avoidable copies in Ambiguity::process: The range loop staged 2*nDelayBins+1 values into dataCorr so that a window of nDelayBins could be read straight back out, then copied that window into a vector and handed it to set_row. The window is just dataZi at (j + delayMin), wrapped, so it now goes directly into the map. The Doppler loop called get_col(), which builds and returns a fresh vector per delay bin, then set_col(), which took another by value. A 301-deep column was copied four times over to be transformed once. Both ends now index map.data. set_row and set_col took their vector by value, copying it before the element-wise loop even started. They take a const reference now, which helps every caller. Measured on a Pi 5 at the shipped geometry this is worth 2.30 ms -> 0.76 ms, so 1.54 ms a CPI. That is well short of the 15.7 ms the upstream fork reports for this stage, and the remaining gap is not in these copies. Nor is it in the pop_front drain further up: removing that saves only about 1.9 ms per buffer, because IqData::push_back evicts when full, so the cost moves to the next extract rather than disappearing. Replacing the handoff with clear() + insert() measures 16.69 ms against the current 11.19 ms, so that apparently obvious optimisation is a regression. None of it is pursued here. Fixes a latent out-of-bounds read as a byproduct. dataCorr was 2*nDelayBins+1 long and the window started at nDelayBins + delayMin, so any delayMin >= 2 read past the end of it. The shipped -10 was unaffected. The rewrite has no staging array, so the bound cannot be exceeded, and testAmbiguityIndexing pins both that and index-for-index equivalence with the old arithmetic across both fleet Doppler spans. Co-Authored-By: Claude Opus 5 (1M context) --- CMakeLists.txt | 13 ++ src/data/Map.cpp | 4 +- src/data/Map.h | 4 +- src/process/ambiguity/Ambiguity.cpp | 34 ++--- src/process/ambiguity/Ambiguity.h | 3 - .../ambiguity/TestAmbiguityIndexing.cpp | 128 ++++++++++++++++++ 6 files changed, 157 insertions(+), 29 deletions(-) create mode 100644 test/unit/process/ambiguity/TestAmbiguityIndexing.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index f95db810..482ee36a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -138,8 +138,21 @@ target_link_libraries(testJsonKm PRIVATE set_target_properties(testJsonKm PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_TEST_UNIT_DIR}") +add_executable(testAmbiguityIndexing + test/unit/process/ambiguity/TestAmbiguityIndexing.cpp + src/data/Map.cpp +) +# armadillo is linked only to pick up the vcpkg include path, which is +# where rapidjson lives; RAPIDJSON_INCLUDE_DIRS is NOTFOUND in this build. +target_link_libraries(testAmbiguityIndexing PRIVATE + armadillo +) +set_target_properties(testAmbiguityIndexing PROPERTIES + RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_TEST_UNIT_DIR}") + # TODO: Unsure if will be using CTest. add_test(NAME testAmbiguity COMMAND testAmbiguity) add_test(NAME testTracker COMMAND testTracker) add_test(NAME testClutterFft COMMAND testClutterFft) add_test(NAME testJsonKm COMMAND testJsonKm) +add_test(NAME testAmbiguityIndexing COMMAND testAmbiguityIndexing) diff --git a/src/data/Map.cpp b/src/data/Map.cpp index a52bd7f3..2f34e079 100644 --- a/src/data/Map.cpp +++ b/src/data/Map.cpp @@ -21,7 +21,7 @@ Map::Map(uint32_t _nRows, uint32_t _nCols) } template -void Map::set_row(uint32_t i, std::vector row) +void Map::set_row(uint32_t i, const std::vector &row) { //data[i].swap(row); for (uint32_t j = 0; j < nCols; j++) @@ -31,7 +31,7 @@ void Map::set_row(uint32_t i, std::vector row) } template -void Map::set_col(uint32_t i, std::vector col) +void Map::set_col(uint32_t i, const std::vector &col) { for (uint32_t j = 0; j < nRows; j++) { diff --git a/src/data/Map.h b/src/data/Map.h index 820b6f28..8205f7c1 100644 --- a/src/data/Map.h +++ b/src/data/Map.h @@ -51,13 +51,13 @@ class Map /// @param i Index of row to update. /// @param row Data to update. /// @return Void. - void set_row(uint32_t i, std::vector row); + void set_row(uint32_t i, const std::vector &row); /// @brief Update a column in the 2D map. /// @param i Index of column to update. /// @param col Data to update. /// @return Void. - void set_col(uint32_t i, std::vector col); + void set_col(uint32_t i, const std::vector &col); /// @brief Create map metrics (noise power, dynamic range). /// @return Void. diff --git a/src/process/ambiguity/Ambiguity.cpp b/src/process/ambiguity/Ambiguity.cpp index 74e6f75d..2ac6e2be 100644 --- a/src/process/ambiguity/Ambiguity.cpp +++ b/src/process/ambiguity/Ambiguity.cpp @@ -63,7 +63,6 @@ Ambiguity::Ambiguity(int32_t _delayMin, int32_t _delayMax, if (_roundHamming) { nfft = next_hamming(nfft); } - dataCorr.resize(2 * nDelayBins + 1); // compute FFTW plans in constructor dataXi.resize(nfft); @@ -128,44 +127,35 @@ Map> *Ambiguity::process(IqData *x, IqData *y) fftw_execute(fftZi); - // extract center of corr + // Extract the centre of the correlation straight into the map. The old + // dataCorr staging array copied 2*nDelayBins+1 values out of dataZi so + // that a single window could be read back out of it; that window is just + // dataZi at (j + delayMin), wrapped, so neither the staging array nor the + // intermediate corr vector is needed. for (uint16_t j = 0; j < nDelayBins; j++) { - dataCorr[j] = dataZi[nfft - nDelayBins + j]; + const int64_t k = int64_t(j) + delayMin; + map->data[i][j] = dataZi[k < 0 ? k + nfft : k]; } - for (uint16_t j = 0; j < nDelayBins + 1; j++) - { - dataCorr[j + nDelayBins] = dataZi[j]; - } - - // cast from std::complex to std::vector - corr.clear(); - for (uint16_t j = 0; j < nDelayBins; j++) - { - corr.push_back(dataCorr[nDelayBins + delayMin + j - 1 + 1]); - } - - map->set_row(i, corr); } // doppler processing for (uint16_t i = 0; i < nDelayBins; i++) { - delayProfile = map->get_col(i); + // Read and write the column in place. get_col() built and returned a fresh + // vector per delay bin, and set_col() took another by value, so a 301-deep + // column was copied four times over to be transformed once. for (uint16_t j = 0; j < nDopplerBins; j++) { - dataDoppler[j] = {delayProfile[j].real(), delayProfile[j].imag()}; + dataDoppler[j] = map->data[j][i]; } fftw_execute(fftDoppler); - corr.clear(); for (uint16_t j = 0; j < nDopplerBins; j++) { - corr.push_back(dataDoppler[(j + int(nDopplerBins / 2) + 1) % nDopplerBins]); + map->data[j][i] = dataDoppler[(j + int(nDopplerBins / 2) + 1) % nDopplerBins]; } - - map->set_col(i, corr); } return map.get(); diff --git a/src/process/ambiguity/Ambiguity.h b/src/process/ambiguity/Ambiguity.h index 6498254d..fec96810 100644 --- a/src/process/ambiguity/Ambiguity.h +++ b/src/process/ambiguity/Ambiguity.h @@ -102,7 +102,6 @@ class Ambiguity std::vector dataXi; std::vector dataYi; std::vector dataZi; - std::vector dataCorr; std::vector dataDoppler; /// @} @@ -111,8 +110,6 @@ class Ambiguity /// @brief Vector storage for ambiguity processing /// @{ - std::vector corr; - std::vector delayProfile; /// @} /// @brief Map to store result. diff --git a/test/unit/process/ambiguity/TestAmbiguityIndexing.cpp b/test/unit/process/ambiguity/TestAmbiguityIndexing.cpp new file mode 100644 index 00000000..b50a209e --- /dev/null +++ b/test/unit/process/ambiguity/TestAmbiguityIndexing.cpp @@ -0,0 +1,128 @@ +// Proves the two Ambiguity rewrites are index-for-index identical to the code +// they replace: the dataCorr staging array in the range loop, and the +// get_col/set_col round trip in the doppler loop. +#include "data/Map.h" + +#include +#include +#include +#include +#include +#include + +using Complex = std::complex; + +static void require(bool condition, const char* message) { + if (!condition) throw std::runtime_error(message); +} + +// What the old code did: stage 2*nDelayBins+1 values into dataCorr, then read a +// window out of it starting at (nDelayBins + delayMin). +static size_t oracleIndex(uint16_t j, uint16_t nDelayBins, int32_t delayMin, + uint32_t nfft) { + std::vector dataCorr(size_t(2) * nDelayBins + 1); + for (uint16_t k = 0; k < nDelayBins; k++) + dataCorr[k] = nfft - nDelayBins + k; + for (uint16_t k = 0; k < nDelayBins + 1; k++) + dataCorr[size_t(k) + nDelayBins] = k; + const size_t window = size_t(nDelayBins) + delayMin + j; + require(window < dataCorr.size(), "oracle window outside dataCorr"); + return dataCorr[window]; +} + +// What the new code does. +static size_t rewriteIndex(uint16_t j, int32_t delayMin, uint32_t nfft) { + const int64_t k = int64_t(j) + delayMin; + return size_t(k < 0 ? k + nfft : k); +} + +static void checkRange(int32_t delayMin, int32_t delayMax, uint32_t nfft) { + const uint16_t nDelayBins = uint16_t(delayMax - delayMin + 1); + for (uint16_t j = 0; j < nDelayBins; j++) { + const size_t oracle = oracleIndex(j, nDelayBins, delayMin, nfft); + const size_t rewrite = rewriteIndex(j, delayMin, nfft); + require(oracle == rewrite, "range-loop index differs from dataCorr staging"); + require(rewrite < nfft, "range-loop index outside dataZi"); + } + std::cout << "PASS range delay=" << delayMin << ".." << delayMax + << " bins=" << nDelayBins << " nfft=" << nfft << '\n'; +} + +// The doppler loop used to read a column out with get_col(), transform it, then +// push it back with set_col(). Both now index map.data directly. +static void checkColumn(uint32_t nRows, uint32_t nCols) { + Map viaAccessors(nRows, nCols), direct(nRows, nCols); + std::mt19937 rng(17); + std::normal_distribution random; + for (uint32_t i = 0; i < nRows; i++) + for (uint32_t j = 0; j < nCols; j++) { + const Complex value(random(rng), random(rng)); + viaAccessors.data[i][j] = value; + direct.data[i][j] = value; + } + + for (uint32_t col = 0; col < nCols; col++) { + // Old path. + std::vector profile = viaAccessors.get_col(col); + std::vector shifted; + for (uint32_t j = 0; j < nRows; j++) + shifted.push_back(profile[(j + int(nRows / 2) + 1) % nRows]); + viaAccessors.set_col(col, shifted); + + // New path, in place. + std::vector scratch(nRows); + for (uint32_t j = 0; j < nRows; j++) scratch[j] = direct.data[j][col]; + for (uint32_t j = 0; j < nRows; j++) + direct.data[j][col] = scratch[(j + int(nRows / 2) + 1) % nRows]; + } + + for (uint32_t i = 0; i < nRows; i++) + for (uint32_t j = 0; j < nCols; j++) + require(viaAccessors.data[i][j] == direct.data[i][j], + "column read/write differs from get_col/set_col"); + std::cout << "PASS column " << nRows << "x" << nCols << '\n'; +} + +int main() { + try { + // Shipped geometry, both doppler spans in the fleet, and edges. + checkRange(-10, 400, 6750); + checkRange(-10, 400, 10000); + checkRange(-10, 300, 6750); + checkRange(0, 400, 6750); + checkRange(1, 400, 6750); + checkRange(-50, 50, 1024); + checkRange(-1, 1, 16); + checkRange(0, 0, 8); + + // The staging array was 2*nDelayBins+1 long and the window started at + // nDelayBins + delayMin, so the old code read past its end whenever + // delayMin >= 2. The rewrite has no staging array, so only check that it + // stays inside dataZi; there is no oracle to compare against here. + for (int32_t delayMin : {2, 5, 40}) { + const int32_t delayMax = 400; + const uint16_t nDelayBins = uint16_t(delayMax - delayMin + 1); + const uint32_t nfft = 6750; + bool oracleOverran = false; + try { (void)oracleIndex(nDelayBins - 1, nDelayBins, delayMin, nfft); } + catch (const std::runtime_error&) { oracleOverran = true; } + require(oracleOverran, "expected the old staging array to overrun here"); + for (uint16_t j = 0; j < nDelayBins; j++) + require(rewriteIndex(j, delayMin, nfft) < nfft, + "rewrite index outside dataZi for positive delayMin"); + std::cout << "PASS positive delayMin=" << delayMin + << " (old code overran dataCorr, rewrite does not)\n"; + } + + checkColumn(301, 411); + checkColumn(201, 411); + checkColumn(3, 5); + checkColumn(1, 1); + + std::cout << "All ambiguity indexing checks passed\n"; + } catch (const std::exception& error) { + std::cerr << "FAIL: " << error.what() << '\n'; + return 1; + } + return 0; +} From 52fc4b19bb41d7226900994320cfe68ad688f793 Mon Sep 17 00:00:00 2001 From: Josh Poole Date: Mon, 14 Sep 2026 12:59:58 +0100 Subject: [PATCH 4/4] 20260914 - Hoist the CFAR training window out of the per-cell loop Per cell, and there are 123,711 of them a CPI, the detector heap-allocated a std::vector of training indices and called pow(pfa, -1.0/nCells). Per row it copied the row out of the map with get_row() and computed 10*log10() for every cell when only a detection needs it. The training window and the false-alarm factor depend on the delay bin, not on Doppler, so they are identical for every row. They are now built once a CPI into a table of nDelayBins entries. The training cells are summed by index range instead of through a vector of indices, still left then right in the original order, since a rolling or prefix sum rounds differently and would move the threshold. The row is read in place and the log is computed only on a detection. Measured on a Pi 5 at the shipped geometry: 34.97 ms -> 3.71 ms, a saving of 31.26 ms a CPI and a 9.4x on the stage. The upstream fork reports 36.4 -> 3.5 on their Pi 4, so this lands where they did. CHANGES DETECTION OUTPUT. The left training loop used k > 0 while the right used k >= 0, so bin 0 was excluded from the left window and included on the right. That asymmetry was a bug and it is corrected here to k >= 0. On synthetic maps with planted targets the count moves by at most one detection, always dropping a marginal one at the low-delay edge where the window was short. Real signal still needs a before and after on a node. testCfarHoist separates the two concerns. It asserts the hoisted detector is bit-identical, in count and in every delay, Doppler and SNR value, to an oracle that is a literal transcription of the original loop with the edge fix applied, so the performance rewrite is proven faithful on its own. It then runs the same oracle both ways to report what the edge fix alone moved. A cell whose window is empty is skipped rather than computed. The original produced a NaN threshold there, which no cell ever exceeded, so this is the same outcome without the arithmetic. Co-Authored-By: Claude Opus 5 (1M context) --- CMakeLists.txt | 15 ++ src/process/detection/CfarDetector1D.cpp | 83 +++++++---- test/unit/process/detection/TestCfarHoist.cpp | 141 ++++++++++++++++++ 3 files changed, 209 insertions(+), 30 deletions(-) create mode 100644 test/unit/process/detection/TestCfarHoist.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 482ee36a..7a672844 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -150,9 +150,24 @@ target_link_libraries(testAmbiguityIndexing PRIVATE set_target_properties(testAmbiguityIndexing PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_TEST_UNIT_DIR}") +add_executable(testCfarHoist + test/unit/process/detection/TestCfarHoist.cpp + src/process/detection/CfarDetector1D.cpp + src/data/Map.cpp + src/data/Detection.cpp +) +# armadillo is linked only to pick up the vcpkg include path, which is +# where rapidjson lives; RAPIDJSON_INCLUDE_DIRS is NOTFOUND in this build. +target_link_libraries(testCfarHoist PRIVATE + armadillo +) +set_target_properties(testCfarHoist PROPERTIES + RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_TEST_UNIT_DIR}") + # TODO: Unsure if will be using CTest. add_test(NAME testAmbiguity COMMAND testAmbiguity) add_test(NAME testTracker COMMAND testTracker) add_test(NAME testClutterFft COMMAND testClutterFft) add_test(NAME testJsonKm COMMAND testJsonKm) add_test(NAME testAmbiguityIndexing COMMAND testAmbiguityIndexing) +add_test(NAME testCfarHoist COMMAND testCfarHoist) diff --git a/src/process/detection/CfarDetector1D.cpp b/src/process/detection/CfarDetector1D.cpp index d46910c0..9d767e6e 100644 --- a/src/process/detection/CfarDetector1D.cpp +++ b/src/process/detection/CfarDetector1D.cpp @@ -4,6 +4,7 @@ #include #include #include +#include // constructor CfarDetector1D::CfarDetector1D(double _pfa, int8_t _nGuard, int8_t _nTrain, int8_t _minDelay, double _minDoppler) @@ -25,8 +26,38 @@ std::unique_ptr CfarDetector1D::process(Map> *x) int32_t nDelayBins = x->get_nCols(); int32_t nDopplerBins = x->get_nRows(); - std::vector> mapRow; - std::vector mapRowSquare, mapRowSnr; + std::vector mapRowSquare; + + // The training window and the false-alarm factor depend on the delay bin + // only, not on Doppler, so they are the same for every row. Building them + // once per CPI replaces one heap allocation and one pow() per cell, which at + // the shipped geometry is 123,711 of each. + // + // The left window uses k >= 0 like the right one. The original used k > 0, + // silently excluding bin 0 from the left window while including it on the + // right. That asymmetry was a bug, but correcting it moves the threshold on + // cells near the start of the delay axis, so it does change detections. + struct TrainWindow + { + int leftBegin, leftEnd, rightBegin, rightEnd; + int nCells; + double alpha; + }; + std::vector window(nDelayBins); + for (int j = 0; j < nDelayBins; j++) + { + TrainWindow &w = window[j]; + w.leftBegin = std::max(0, j - nGuard - nTrain); + w.leftEnd = std::min(nDelayBins, j - nGuard); + w.rightBegin = std::max(0, j + nGuard + 1); + w.rightEnd = std::min(nDelayBins, j + nGuard + nTrain + 1); + if (w.leftEnd < w.leftBegin) w.leftEnd = w.leftBegin; + if (w.rightEnd < w.rightBegin) w.rightEnd = w.rightBegin; + w.nCells = (w.leftEnd - w.leftBegin) + (w.rightEnd - w.rightBegin); + // nCells == 0 gave a NaN threshold, which no cell ever exceeded; such a + // cell is skipped below instead. + w.alpha = w.nCells > 0 ? w.nCells * (pow(pfa, -1.0 / w.nCells) - 1) : 0.0; + } // store detections temporarily std::vector delay; @@ -41,11 +72,12 @@ std::unique_ptr CfarDetector1D::process(Map> *x) { continue; } - mapRow = x->get_row(i); + // Read the row in place; get_row() returned a copy of all nDelayBins. + const std::vector> &mapRow = x->data[i]; + mapRowSquare.resize(nDelayBins); for (int j = 0; j < nDelayBins; j++) { - mapRowSquare.push_back((double) std::abs(mapRow[j]*mapRow[j])); - mapRowSnr.push_back((double)10 * std::log10(std::abs(mapRow[j])) - x->noisePower); + mapRowSquare[j] = (double) std::abs(mapRow[j]*mapRow[j]); } for (int j = 0; j < nDelayBins; j++) { @@ -53,46 +85,37 @@ std::unique_ptr CfarDetector1D::process(Map> *x) if (x->delay[j] < minDelay) { continue; - } - // get train cell indices - std::vector iTrain; - for (int k = j-nGuard-nTrain; k < j-nGuard; k++) - { - if (k > 0 && k < nDelayBins) - { - iTrain.push_back(k); - } } - for (int k = j+nGuard+1; k < j+nGuard+nTrain+1; k++) + const TrainWindow &w = window[j]; + if (w.nCells == 0) { - if (k >= 0 && k < nDelayBins) - { - iTrain.push_back(k); - } + continue; } - // compute threshold - int nCells = iTrain.size(); - double alpha = nCells * (pow(pfa, -1.0 / nCells) - 1); + // Sum the training cells left then right, in the original order: a + // rolling or prefix sum rounds differently and would move the threshold. double trainNoise = 0.0; - for (int k = 0; k < nCells; k++) + for (int k = w.leftBegin; k < w.leftEnd; k++) + { + trainNoise += mapRowSquare[k]; + } + for (int k = w.rightBegin; k < w.rightEnd; k++) { - trainNoise += mapRowSquare[iTrain[k]]; + trainNoise += mapRowSquare[k]; } - trainNoise /= nCells; - double threshold = alpha * trainNoise; + trainNoise /= w.nCells; + double threshold = w.alpha * trainNoise; // detection if over threshold if (mapRowSquare[j] > threshold) { delay.push_back(j + x->delay[0]); doppler.push_back(x->doppler[i]); - snr.push_back(mapRowSnr[j]); + // Only a detection needs the log, so it is no longer computed for + // every cell in the map. + snr.push_back((double)10 * std::log10(std::abs(mapRow[j])) - x->noisePower); } - iTrain.clear(); } - mapRowSquare.clear(); - mapRowSnr.clear(); } // create detection diff --git a/test/unit/process/detection/TestCfarHoist.cpp b/test/unit/process/detection/TestCfarHoist.cpp new file mode 100644 index 00000000..4c1b6115 --- /dev/null +++ b/test/unit/process/detection/TestCfarHoist.cpp @@ -0,0 +1,141 @@ +// Two separate things are being checked here. +// +// 1. The hoist is faithful. The per-CPI training table must produce exactly the +// detections the original per-cell loop produced, given the same training +// window. The oracle below is a literal transcription of the original loop +// with the one deliberate change applied (left window k >= 0). +// +// 2. The edge fix is quantified. The original excluded bin 0 from the left +// training window (k > 0) while including it on the right (k >= 0). Running +// the oracle both ways shows how many detections that asymmetry moved, so +// the behaviour change is a measured number rather than an assumption. +#include "data/Map.h" +#include "data/Detection.h" +#include "process/detection/CfarDetector1D.h" + +#include +#include +#include +#include +#include +#include +#include + +using Complex = std::complex; + +static void require(bool condition, const char* message) { + if (!condition) throw std::runtime_error(message); +} + +struct Hit { + double delay, doppler, snr; + bool operator!=(const Hit& o) const { + return delay != o.delay || doppler != o.doppler || snr != o.snr; + } +}; + +// The original algorithm, verbatim apart from `leftFromZero`. +static std::vector oracle(Map& x, double pfa, int nGuard, + int nTrain, int minDelay, double minDoppler, + bool leftFromZero) { + const int nDelayBins = int(x.get_nCols()); + const int nDopplerBins = int(x.get_nRows()); + std::vector hits; + for (int i = 0; i < nDopplerBins; i++) { + if (std::abs(x.doppler[i]) < minDoppler) continue; + std::vector mapRow = x.get_row(i); + std::vector mapRowSquare, mapRowSnr; + for (int j = 0; j < nDelayBins; j++) { + mapRowSquare.push_back((double)std::abs(mapRow[j] * mapRow[j])); + mapRowSnr.push_back((double)10 * std::log10(std::abs(mapRow[j])) - x.noisePower); + } + for (int j = 0; j < nDelayBins; j++) { + if (x.delay[j] < minDelay) continue; + std::vector iTrain; + for (int k = j - nGuard - nTrain; k < j - nGuard; k++) + if ((leftFromZero ? k >= 0 : k > 0) && k < nDelayBins) iTrain.push_back(k); + for (int k = j + nGuard + 1; k < j + nGuard + nTrain + 1; k++) + if (k >= 0 && k < nDelayBins) iTrain.push_back(k); + + int nCells = int(iTrain.size()); + if (nCells == 0) continue; // original produced a NaN threshold here + double alpha = nCells * (pow(pfa, -1.0 / nCells) - 1); + double trainNoise = 0.0; + for (int k = 0; k < nCells; k++) trainNoise += mapRowSquare[iTrain[k]]; + trainNoise /= nCells; + if (mapRowSquare[j] > alpha * trainNoise) + hits.push_back({double(j) + x.delay[0], x.doppler[i], mapRowSnr[j]}); + } + } + return hits; +} + +static std::vector fromDetection(Detection& detection) { + std::vector hits; + std::vector delay = detection.get_delay(); + std::vector doppler = detection.get_doppler(); + std::vector snr = detection.get_snr(); + for (size_t i = 0; i < delay.size(); i++) + hits.push_back({delay[i], doppler[i], snr[i]}); + return hits; +} + +static void run(uint32_t nRows, uint32_t nCols, double pfa, int nGuard, + int nTrain, int minDelay, double minDoppler, int seed, + double targetGain) { + Map map(nRows, nCols); + std::mt19937 rng(seed); + std::normal_distribution random; + + map.delay.clear(); + for (uint32_t j = 0; j < nCols; j++) map.delay.push_back(-10 + int(j)); + map.doppler.clear(); + for (uint32_t i = 0; i < nRows; i++) + map.doppler.push_back((double(i) - double(nRows) / 2) * 2.0); + + for (uint32_t i = 0; i < nRows; i++) + for (uint32_t j = 0; j < nCols; j++) + map.data[i][j] = Complex(random(rng), random(rng)); + + // Plant targets, including ones right at the start of the delay axis where + // the left-window edge case actually bites. + for (uint32_t j : {0u, 1u, 2u, 3u, nCols / 2, nCols - 2}) + if (j < nCols) map.data[nRows / 2][j] *= targetGain; + map.set_metrics(); + + CfarDetector1D detector(pfa, int8_t(nGuard), int8_t(nTrain), + int8_t(minDelay), minDoppler); + std::unique_ptr produced = detector.process(&map); + std::vector got = fromDetection(*produced); + std::vector want = oracle(map, pfa, nGuard, nTrain, minDelay, minDoppler, true); + + require(got.size() == want.size(), "hoisted CFAR found a different number of detections"); + for (size_t i = 0; i < got.size(); i++) + require(!(got[i] != want[i]), "hoisted CFAR detection differs from the oracle"); + + std::vector legacy = oracle(map, pfa, nGuard, nTrain, minDelay, minDoppler, false); + size_t moved = legacy.size() > got.size() ? legacy.size() - got.size() + : got.size() - legacy.size(); + std::cout << "PASS " << nRows << "x" << nCols << " pfa=" << pfa + << " guard=" << nGuard << " train=" << nTrain + << " -> " << got.size() << " detections, identical to oracle" + << "; edge fix changed count by " << moved + << " (was " << legacy.size() << ")\n"; +} + +int main() { + try { + run(301, 411, 1e-5, 2, 8, -10, 0.0, 11, 40.0); + run(301, 411, 1e-5, 2, 8, -10, 50.0, 12, 40.0); + run(201, 411, 1e-3, 2, 8, -10, 0.0, 13, 12.0); + run(51, 61, 1e-5, 1, 4, -10, 0.0, 14, 30.0); + run(11, 21, 1e-2, 2, 3, -10, 0.0, 15, 8.0); + // Degenerate windows: guard wide enough to push training off the axis. + run(9, 9, 1e-5, 4, 2, -10, 0.0, 16, 30.0); + std::cout << "All CFAR hoist checks passed\n"; + } catch (const std::exception& error) { + std::cerr << "FAIL: " << error.what() << '\n'; + return 1; + } + return 0; +}