diff --git a/src/drone-software/src/acoustic/build/extract_mel_cpp.exe b/src/drone-software/src/acoustic/build/extract_mel_cpp.exe new file mode 100644 index 0000000..bc788b2 Binary files /dev/null and b/src/drone-software/src/acoustic/build/extract_mel_cpp.exe differ diff --git a/src/drone-software/src/acoustic/build/test_classifier_cpp.exe b/src/drone-software/src/acoustic/build/test_classifier_cpp.exe new file mode 100644 index 0000000..ec14197 Binary files /dev/null and b/src/drone-software/src/acoustic/build/test_classifier_cpp.exe differ diff --git a/src/drone-software/src/acoustic/build/test_core_lib.exe b/src/drone-software/src/acoustic/build/test_core_lib.exe new file mode 100644 index 0000000..30d3590 Binary files /dev/null and b/src/drone-software/src/acoustic/build/test_core_lib.exe differ diff --git a/src/drone-software/src/acoustic/tests/extract_mel_cpp.cpp b/src/drone-software/src/acoustic/tests/extract_mel_cpp.cpp index d7dcdf2..d5052f4 100644 --- a/src/drone-software/src/acoustic/tests/extract_mel_cpp.cpp +++ b/src/drone-software/src/acoustic/tests/extract_mel_cpp.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include "acoustic_analyzer/core/feature_extractor.h" #include "acoustic_analyzer/io/wav_file_source.h" @@ -26,22 +27,24 @@ int main(int argc, char** argv) { return 1; } - FeatureExtractorParams p; - p.sample_rate = wav.sample_rate(); - FeatureExtractor ext(p); - std::vector mel; - if (!ext.extract(audio, mel)) { - std::cerr << "Extraction failed" << std::endl; - return 1; - } + FeatureExtractor ext(wav.sample_rate(), 2048, 512, 64); + Eigen::MatrixXf mel = ext.MelSpectrogram(audio[0]); - std::cout << "Extracted Mel shape: " << p.n_mels << " x " << (mel.size() / p.n_mels) << std::endl; - std::cout << "Min: " << *std::min_element(mel.begin(), mel.end()) - << " Max: " << *std::max_element(mel.begin(), mel.end()) << std::endl; + std::cout << "Extracted Mel shape: " << mel.rows() << " x " << mel.cols() << std::endl; + + // Flatten for min/max stats + std::vector mel_flat(mel.size()); + for (int i = 0; i < mel.rows(); ++i) { + for (int j = 0; j < mel.cols(); ++j) { + mel_flat[i * mel.cols() + j] = mel(i, j); + } + } + std::cout << "Min: " << *std::min_element(mel_flat.begin(), mel_flat.end()) + << " Max: " << *std::max_element(mel_flat.begin(), mel_flat.end()) << std::endl; if (argc >= 3) { std::ofstream out(argv[2], std::ios::binary); - out.write(reinterpret_cast(mel.data()), mel.size() * sizeof(float)); + out.write(reinterpret_cast(mel_flat.data()), mel_flat.size() * sizeof(float)); std::cout << "Saved to " << argv[2] << std::endl; } return 0; diff --git a/src/drone-software/src/acoustic/tests/test_classifier_cpp.cpp b/src/drone-software/src/acoustic/tests/test_classifier_cpp.cpp index d47586a..f402f03 100644 --- a/src/drone-software/src/acoustic/tests/test_classifier_cpp.cpp +++ b/src/drone-software/src/acoustic/tests/test_classifier_cpp.cpp @@ -46,31 +46,22 @@ int main(int argc, char** argv) { return 1; } - FeatureExtractorParams feat_p; - feat_p.sample_rate = wav.sample_rate(); - FeatureExtractor extractor(feat_p); - std::vector features; - if (!extractor.extract(audio, features)) { - std::cerr << "Feature extraction failed" << std::endl; - return 1; - } + FeatureExtractor extractor(wav.sample_rate(), 2048, 512, 64); + Eigen::MatrixXf mel = extractor.MelSpectrogram(audio[0]); - GunshotClassifier classifier(model_path, label_map_path, 0.7f, 3); - if (!classifier.init()) { + ClassifierConfig cc; + cc.model_path = model_path; + cc.label_map_path = label_map_path; + cc.threshold = 0.7f; + cc.smoothing_window = 3; + GunshotClassifier classifier(cc); + if (!classifier.IsLoaded()) { std::cerr << "Classifier init failed" << std::endl; return 1; } - ClassificationResult result; - if (!classifier.classify(features, result)) { - std::cerr << "Classification failed" << std::endl; - return 1; - } - - std::cout << "Label: " << result.label << std::endl; - std::cout << "Confidence: " << result.confidence << std::endl; - for (const auto& kv : result.probs) { - std::cout << " " << kv.first << ": " << kv.second << std::endl; - } + auto [label, confidence] = classifier.Predict(mel); + std::cout << "Label: " << label << std::endl; + std::cout << "Confidence: " << confidence << std::endl; return 0; } diff --git a/src/drone-software/src/acoustic/tests/test_core_lib.cpp b/src/drone-software/src/acoustic/tests/test_core_lib.cpp index 6c81f8c..14d912a 100644 --- a/src/drone-software/src/acoustic/tests/test_core_lib.cpp +++ b/src/drone-software/src/acoustic/tests/test_core_lib.cpp @@ -8,6 +8,7 @@ #include "acoustic_analyzer/core/gcc_phat_localizer.h" #include "acoustic_analyzer/core/distance_estimator.h" #include "acoustic_analyzer/core/threat_tracker.h" +#include "acoustic_analyzer/core/fft_utils.h" using namespace acoustic; @@ -15,18 +16,19 @@ bool near(float a, float b, float eps = 1e-3f) { return std::abs(a - b) < eps; } -void test_audio_buffer_wraparound() { +void test_audio_buffer() { AudioBuffer buf(100, 2); std::vector data(200); for (int i = 0; i < 200; ++i) data[i] = static_cast(i); - buf.push_interleaved(data.data(), 200); - assert(buf.available(0) == 100); - float out[10]; - buf.get_latest_frame(out, 10, 0); - // Latest 10 samples of channel 0 should be 190, 192, 194, ... (interleaved) - assert(near(out[0], 190.0f)); - assert(near(out[9], 208.0f)); - std::cout << "[PASS] audio_buffer_wraparound" << std::endl; + buf.Push(data); + assert(buf.Size() == 100); + assert(buf.NumChannels() == 2); + + auto popped = buf.Pop(10); + assert(popped.size() == 10 * 2); + assert(near(popped[0], 0.0f)); // ch0, sample 0 (oldest) + assert(near(popped[1], 1.0f)); // ch1, sample 0 + std::cout << "[PASS] audio_buffer" << std::endl; } void test_preemphasis() { @@ -39,57 +41,56 @@ void test_preemphasis() { } void test_gcc_phat_cross_array() { - MicArrayGeometry geom; - geom.num_mics = 4; - geom.layout = "cross"; - geom.spacing = 0.15f; - GccPhatLocalizer loc(geom, 16000, 4); - loc.set_max_tdoa(0.00044f); + MicArrayConfig config; + config.num_mics = 4; + config.layout = "cross"; + config.spacing = 0.15f; + GccPhatLocalizer loc(config, 16000, 0.00044f, 4); - // Simulate a signal arriving from 90 degrees (right), delay mic2 ahead + // Simulate a signal arriving from 90 degrees (right) int n = 512; - std::vector> audio(4, std::vector(n)); + Eigen::MatrixXf audio(n, 4); for (int i = 0; i < n; ++i) { - float s = std::sin(2.0f * M_PI * 1000.0f * i / 16000.0f); - audio[0][i] = s; // front - audio[1][i] = s; // right (lead) - audio[2][i] = s; // back - audio[3][i] = s; // left (lag) + float s = std::sin(2.0f * static_cast(M_PI) * 1000.0f * i / 16000.0f); + audio(i, 0) = s; + audio(i, 1) = s; + audio(i, 2) = s; + audio(i, 3) = s; } - // Add small delay to simulate direction float delay_samples = 0.15f / 343.0f * 16000.0f; for (int i = 0; i < n; ++i) { int idx = static_cast(i + delay_samples); - if (idx < n) audio[1][idx] = audio[0][i]; + if (idx < n) audio(idx, 1) = audio(i, 0); idx = static_cast(i - delay_samples); - if (idx >= 0) audio[3][idx] = audio[0][i]; + if (idx >= 0) audio(idx, 3) = audio(i, 0); } - float azimuth, elevation; - bool ok = loc.localize(audio, azimuth, elevation); - assert(ok); - // For this simplified test, just assert it returns a valid angle + auto [azimuth, elevation] = loc.Localize(audio); assert(azimuth >= 0.0f && azimuth < 360.0f); std::cout << "[PASS] gcc_phat_cross_array (azimuth=" << azimuth << ")" << std::endl; } void test_distance_estimator() { - DistanceEstimator::Params p; - p.reference_spl["gunshot"] = 150.0f; - p.attenuation_alpha = 0.6f; - DistanceEstimator est(p); - float dist, conf; - bool ok = est.estimate(120.0f, "gunshot", dist, conf); - assert(ok); - assert(dist > 0.0f); + DistanceConfig config; + config.ref_spl_gunshot = 150.0f; + config.attenuation_alpha = 0.6f; + DistanceEstimator est(config); + float dist = est.Estimate(120.0f, "gunshot"); + assert(dist > 0.0f && dist < 5000.0f); std::cout << "[PASS] distance_estimator (dist=" << dist << "m)" << std::endl; } void test_threat_tracker() { - ThreatTracker tracker(15.0f, 5, 0.0f, 10.0f); - Threat t1{"gunshot", 0.9f, 45.0f, 0.0f, 100.0f, 0.8f}; - std::vector out; - tracker.update({t1}, out); + ThreatTracker tracker(0.3f, 15.0f, 5); + AcousticThreat t1; + t1.sound_type = "gunshot"; + t1.confidence = 0.9f; + t1.azimuth = 45.0f; + t1.elevation = 0.0f; + t1.distance = 100.0f; + t1.distance_confidence = 0.8f; + + auto out = tracker.Update({t1}); assert(!out.empty()); assert(out[0].sound_type == "gunshot"); assert(out[0].threat_id == "THREAT-0001"); @@ -97,26 +98,20 @@ void test_threat_tracker() { } void test_feature_extractor_shape() { - FeatureExtractorParams p; - p.sample_rate = 16000; - p.n_mels = 64; - p.n_fft = 2048; - p.hop_length = 512; - FeatureExtractor ext(p); + FeatureExtractor ext(16000, 2048, 512, 64); std::vector audio(16000 * 2, 0.0f); // 2 seconds for (int i = 0; i < 32000; ++i) { - audio[i] = std::sin(2.0f * M_PI * 500.0f * i / 16000.0f) * 0.1f; + audio[i] = std::sin(2.0f * static_cast(M_PI) * 500.0f * i / 16000.0f) * 0.1f; } - std::vector mel; - bool ok = ext.extract(audio.data(), audio.size(), mel); - assert(ok); - assert(mel.size() == static_cast(64 * 63)); // n_mels * pad_frames - std::cout << "[PASS] feature_extractor_shape (" << mel.size() << " floats)" << std::endl; + Eigen::MatrixXf mel = ext.MelSpectrogram(audio); + assert(mel.rows() == 64); + assert(mel.cols() == 63); // padded/truncated to 63 frames + std::cout << "[PASS] feature_extractor_shape (" << mel.rows() << " x " << mel.cols() << ")" << std::endl; } int main() { std::cout << "Running core library tests..." << std::endl; - test_audio_buffer_wraparound(); + test_audio_buffer(); test_preemphasis(); test_gcc_phat_cross_array(); test_distance_estimator();