diff --git a/CMakeLists.txt b/CMakeLists.txt index 51f92509..2a7326f1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -127,7 +127,49 @@ 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}") + +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}") + +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/blah2.cpp b/src/blah2.cpp index 121b0398..9e52879f 100644 --- a/src/blah2.cpp +++ b/src/blah2.cpp @@ -302,8 +302,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); @@ -313,8 +312,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/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/data/Map.cpp b/src/data/Map.cpp index bf7ee6e3..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++) { @@ -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..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. @@ -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/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/src/process/clutter/WienerHopf.cpp b/src/process/clutter/WienerHopf.cpp index 5f028d57..cf06a513 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 e215df84..d7e6ccdb 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/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/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); 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; +} 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; +} 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; +}