feat: add comparable localization algorithms
This commit is contained in:
@@ -0,0 +1,783 @@
|
||||
import {
|
||||
createBinCalibration,
|
||||
createDistanceMatrix,
|
||||
finalizeDasBands,
|
||||
} from "./das.js";
|
||||
import { cooperativeYield } from "./cooperative-yield.js";
|
||||
import { fftInPlace } from "./fft.js";
|
||||
import {
|
||||
binsForBand,
|
||||
createStftWorkspace,
|
||||
extractStftBins,
|
||||
getFrameCount,
|
||||
} from "./stft.js";
|
||||
|
||||
export const GCC_PHAT_ANALYSIS_PLAN_KIND = "micarrsim.gcc-phat-analysis-plan/1";
|
||||
export const DEFAULT_GCC_PHAT_FLOOR_RATIO = 1e-6;
|
||||
export const DEFAULT_MAXIMUM_GCC_PHAT_PLAN_BYTES = 512 * 1024 * 1024;
|
||||
|
||||
function createAbortError(signal) {
|
||||
if (signal?.reason instanceof Error && signal.reason.name === "AbortError") {
|
||||
return signal.reason;
|
||||
}
|
||||
const error = new Error("GCC-PHAT localization was canceled.");
|
||||
error.name = "AbortError";
|
||||
return error;
|
||||
}
|
||||
|
||||
function throwIfAborted(signal) {
|
||||
if (signal?.aborted) throw createAbortError(signal);
|
||||
}
|
||||
|
||||
function requirePositiveFinite(value, label) {
|
||||
if (!Number.isFinite(value) || value <= 0) {
|
||||
throw new RangeError(label + " must be a positive finite number.");
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
function requirePositiveInteger(value, label) {
|
||||
if (!Number.isInteger(value) || value < 1) {
|
||||
throw new RangeError(label + " must be a positive integer.");
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
function validateArrayDefinition(arrayDefinition) {
|
||||
if (!Array.isArray(arrayDefinition?.microphones) || arrayDefinition.microphones.length < 2) {
|
||||
throw new RangeError("GCC-PHAT requires at least two microphones.");
|
||||
}
|
||||
arrayDefinition.microphones.forEach((microphone, index) => {
|
||||
if (
|
||||
!Array.isArray(microphone?.position) ||
|
||||
microphone.position.length !== 3 ||
|
||||
!microphone.position.every(Number.isFinite)
|
||||
) {
|
||||
throw new TypeError(
|
||||
"arrayDefinition.microphones[" + index + "].position must be a finite [x, y, z] array.",
|
||||
);
|
||||
}
|
||||
});
|
||||
return arrayDefinition;
|
||||
}
|
||||
|
||||
function validateConfiguration(options) {
|
||||
const sampleRate = requirePositiveFinite(options.sampleRate, "sampleRate");
|
||||
const arrayDefinition = validateArrayDefinition(options.arrayDefinition);
|
||||
const speedOfSound = requirePositiveFinite(options.scene?.speedOfSound, "scene.speedOfSound");
|
||||
const experiment = options.experiment;
|
||||
if (!experiment?.stft || !experiment?.scan || !Array.isArray(experiment.frequencyBands)) {
|
||||
throw new TypeError("experiment must contain stft, scan, and frequencyBands.");
|
||||
}
|
||||
const fftSize = requirePositiveInteger(experiment.stft.fftSize, "experiment.stft.fftSize");
|
||||
if ((fftSize & (fftSize - 1)) !== 0) {
|
||||
throw new RangeError("experiment.stft.fftSize must be a power of two.");
|
||||
}
|
||||
const hopSize = requirePositiveInteger(experiment.stft.hopSize, "experiment.stft.hopSize");
|
||||
if (hopSize > fftSize) {
|
||||
throw new RangeError("experiment.stft.hopSize must not exceed fftSize.");
|
||||
}
|
||||
const pointCount =
|
||||
requirePositiveInteger(experiment.scan.columns, "experiment.scan.columns") *
|
||||
requirePositiveInteger(experiment.scan.rows, "experiment.scan.rows");
|
||||
if (!Number.isSafeInteger(pointCount)) {
|
||||
throw new RangeError("scan point count exceeds the safe integer range.");
|
||||
}
|
||||
if (experiment.frequencyBands.length === 0) {
|
||||
throw new RangeError("experiment.frequencyBands must not be empty.");
|
||||
}
|
||||
return { sampleRate, arrayDefinition, speedOfSound, experiment, fftSize, hopSize, pointCount };
|
||||
}
|
||||
|
||||
function validateChannels(channels, microphoneCount) {
|
||||
if (!Array.isArray(channels) || channels.length !== microphoneCount) {
|
||||
throw new Error(
|
||||
"channel count " + (channels?.length ?? 0) +
|
||||
" does not match microphone count " + microphoneCount + ".",
|
||||
);
|
||||
}
|
||||
if (!channels.every((channel) => channel instanceof Float32Array)) {
|
||||
throw new TypeError("every channel must be a Float32Array.");
|
||||
}
|
||||
const sampleCount = channels[0].length;
|
||||
if (sampleCount === 0) throw new RangeError("channels must contain at least one sample.");
|
||||
if (channels.some((channel) => channel.length !== sampleCount)) {
|
||||
throw new Error("all channels must have the same sample count.");
|
||||
}
|
||||
return sampleCount;
|
||||
}
|
||||
|
||||
function referenceTieBreak(left, right) {
|
||||
if (left.id < right.id) return -1;
|
||||
if (left.id > right.id) return 1;
|
||||
return left.index - right.index;
|
||||
}
|
||||
|
||||
/**
|
||||
* Select a stable reference without depending on microphone input order.
|
||||
*
|
||||
* A peripheral microphone generally provides a longer TDOA baseline. Exact
|
||||
* geometric ties are resolved by microphone id and finally by input index.
|
||||
*/
|
||||
export function selectGccPhatReference(arrayDefinition) {
|
||||
validateArrayDefinition(arrayDefinition);
|
||||
const centroid = [0, 0, 0];
|
||||
for (const microphone of arrayDefinition.microphones) {
|
||||
for (let axis = 0; axis < 3; axis += 1) centroid[axis] += microphone.position[axis];
|
||||
}
|
||||
for (let axis = 0; axis < 3; axis += 1) {
|
||||
centroid[axis] /= arrayDefinition.microphones.length;
|
||||
}
|
||||
|
||||
let selected;
|
||||
arrayDefinition.microphones.forEach((microphone, index) => {
|
||||
const descriptor = {
|
||||
index,
|
||||
id: String(microphone.id ?? ""),
|
||||
microphone,
|
||||
distanceSquared:
|
||||
(microphone.position[0] - centroid[0]) ** 2 +
|
||||
(microphone.position[1] - centroid[1]) ** 2 +
|
||||
(microphone.position[2] - centroid[2]) ** 2,
|
||||
};
|
||||
if (!selected) {
|
||||
selected = descriptor;
|
||||
return;
|
||||
}
|
||||
if (
|
||||
descriptor.distanceSquared > selected.distanceSquared ||
|
||||
(
|
||||
descriptor.distanceSquared === selected.distanceSquared &&
|
||||
referenceTieBreak(descriptor, selected) < 0
|
||||
)
|
||||
) {
|
||||
selected = descriptor;
|
||||
}
|
||||
});
|
||||
|
||||
return Object.freeze({
|
||||
index: selected.index,
|
||||
id: selected.id,
|
||||
microphone: selected.microphone,
|
||||
});
|
||||
}
|
||||
|
||||
function createBandGeometry(configuration) {
|
||||
const bandBins = configuration.experiment.frequencyBands.map((band) =>
|
||||
binsForBand(band, configuration.sampleRate, configuration.fftSize)
|
||||
);
|
||||
const selectedBins = [...new Set(bandBins.flat())].sort((left, right) => left - right);
|
||||
return { bandBins, selectedBins };
|
||||
}
|
||||
|
||||
function maximumGeometricLagSamples({
|
||||
distances,
|
||||
pointCount,
|
||||
microphoneCount,
|
||||
referenceIndex,
|
||||
sampleRate,
|
||||
speedOfSound,
|
||||
}) {
|
||||
let maximumLagSamples = 0;
|
||||
for (let pointIndex = 0; pointIndex < pointCount; pointIndex += 1) {
|
||||
const distanceOffset = pointIndex * microphoneCount;
|
||||
const referenceDistance = distances[distanceOffset + referenceIndex];
|
||||
for (let microphoneIndex = 0; microphoneIndex < microphoneCount; microphoneIndex += 1) {
|
||||
if (microphoneIndex === referenceIndex) continue;
|
||||
const lagSamples =
|
||||
Math.abs(distances[distanceOffset + microphoneIndex] - referenceDistance) *
|
||||
sampleRate / speedOfSound;
|
||||
maximumLagSamples = Math.max(maximumLagSamples, lagSamples);
|
||||
}
|
||||
}
|
||||
return maximumLagSamples;
|
||||
}
|
||||
|
||||
function assertUnaliasedLag(maximumLagSamples, fftSize) {
|
||||
const maximumUnaliasedLag = fftSize / 2;
|
||||
if (maximumLagSamples > maximumUnaliasedLag) {
|
||||
throw new RangeError(
|
||||
"GCC-PHAT maximum absolute geometric lag " + maximumLagSamples.toFixed(3) +
|
||||
" samples exceeds fftSize/2 (" + maximumUnaliasedLag +
|
||||
"); increase fftSize or reduce the array aperture or scan range.",
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
function createGeometry(configuration, distances) {
|
||||
const microphoneCount = configuration.arrayDefinition.microphones.length;
|
||||
const reference = selectGccPhatReference(configuration.arrayDefinition);
|
||||
const pairMicrophoneIndexes = [];
|
||||
for (let microphoneIndex = 0; microphoneIndex < microphoneCount; microphoneIndex += 1) {
|
||||
if (microphoneIndex !== reference.index) pairMicrophoneIndexes.push(microphoneIndex);
|
||||
}
|
||||
const resolvedDistances =
|
||||
distances ?? createDistanceMatrix(configuration.arrayDefinition, configuration.experiment.scan);
|
||||
const maximumLagSamples = maximumGeometricLagSamples({
|
||||
distances: resolvedDistances,
|
||||
pointCount: configuration.pointCount,
|
||||
microphoneCount,
|
||||
referenceIndex: reference.index,
|
||||
sampleRate: configuration.sampleRate,
|
||||
speedOfSound: configuration.speedOfSound,
|
||||
});
|
||||
assertUnaliasedLag(maximumLagSamples, configuration.fftSize);
|
||||
return {
|
||||
microphoneCount,
|
||||
reference,
|
||||
pairMicrophoneIndexes,
|
||||
pairCount: pairMicrophoneIndexes.length,
|
||||
distances: resolvedDistances,
|
||||
maximumLagSamples,
|
||||
...createBandGeometry(configuration),
|
||||
};
|
||||
}
|
||||
|
||||
function requirePhatFloorRatio(value) {
|
||||
const ratio = value ?? DEFAULT_GCC_PHAT_FLOOR_RATIO;
|
||||
if (!Number.isFinite(ratio) || ratio < 0 || ratio > 1) {
|
||||
throw new RangeError("phatFloorRatio must be a finite number from 0 through 1.");
|
||||
}
|
||||
return ratio;
|
||||
}
|
||||
|
||||
function estimateWithGeometry(options, configuration, geometry) {
|
||||
const sampleCount = validateChannels(options.channels, geometry.microphoneCount);
|
||||
const frameCount = getFrameCount(
|
||||
sampleCount,
|
||||
configuration.fftSize,
|
||||
configuration.hopSize,
|
||||
);
|
||||
const pairCount = geometry.pairCount;
|
||||
const bandCount = geometry.bandBins.length;
|
||||
const bandBinCount = geometry.bandBins.reduce((sum, bins) => sum + bins.length, 0);
|
||||
const selectedBinCount = geometry.selectedBins.length;
|
||||
const stftTransformCount = frameCount * geometry.microphoneCount;
|
||||
const inverseFftCount = pairCount * bandCount;
|
||||
const fftButterfliesPerTransform =
|
||||
(configuration.fftSize / 2) * Math.log2(configuration.fftSize);
|
||||
const stftButterflyCount = stftTransformCount * fftButterfliesPerTransform;
|
||||
const inverseFftButterflyCount = inverseFftCount * fftButterfliesPerTransform;
|
||||
const crossSpectrumSampleCount = pairCount * frameCount * bandBinCount;
|
||||
// The relative-floor pass finds the maximum magnitude, then the PHAT pass
|
||||
// recomputes and accumulates each complex cross-spectrum sample.
|
||||
const crossSpectrumComplexOpCount = crossSpectrumSampleCount * 2;
|
||||
const scanLookupCount = configuration.pointCount * pairCount * bandCount;
|
||||
const dasEquivalentComplexMacCount =
|
||||
configuration.pointCount * geometry.microphoneCount * frameCount * selectedBinCount;
|
||||
const distanceBytes =
|
||||
configuration.pointCount * geometry.microphoneCount * Float32Array.BYTES_PER_ELEMENT;
|
||||
const stftWorkspaceBytes =
|
||||
configuration.fftSize * Float64Array.BYTES_PER_ELEMENT * 3;
|
||||
const correlationWorkspaceBytes =
|
||||
configuration.fftSize * Float64Array.BYTES_PER_ELEMENT * 2;
|
||||
const spectrumBytes =
|
||||
selectedBinCount * frameCount * geometry.microphoneCount *
|
||||
Float32Array.BYTES_PER_ELEMENT * 2;
|
||||
const calibrationPhaseBytes =
|
||||
selectedBinCount * geometry.microphoneCount * Float64Array.BYTES_PER_ELEMENT;
|
||||
const outputBytes =
|
||||
bandCount * configuration.pointCount * Float32Array.BYTES_PER_ELEMENT * 2;
|
||||
const retainedPlanBytes =
|
||||
options.analysisPlan?.retainedBytes ??
|
||||
distanceBytes + stftWorkspaceBytes + correlationWorkspaceBytes;
|
||||
const estimatedWorkingBytes =
|
||||
retainedPlanBytes + spectrumBytes + calibrationPhaseBytes + outputBytes;
|
||||
|
||||
return {
|
||||
bandBins: geometry.bandBins,
|
||||
selectedBins: geometry.selectedBins,
|
||||
selectedBinCount,
|
||||
bandBinCount,
|
||||
frameCount,
|
||||
microphoneCount: geometry.microphoneCount,
|
||||
pointCount: configuration.pointCount,
|
||||
bandCount,
|
||||
pairCount,
|
||||
referenceMicrophoneId: geometry.reference.id,
|
||||
referenceMicrophoneIndex: geometry.reference.index,
|
||||
maximumLagSamples: geometry.maximumLagSamples,
|
||||
stftTransformCount,
|
||||
inverseFftCount,
|
||||
stftButterflyCount,
|
||||
inverseFftButterflyCount,
|
||||
crossSpectrumSampleCount,
|
||||
crossSpectrumComplexOpCount,
|
||||
crossSpectrumCount: crossSpectrumComplexOpCount,
|
||||
complexMacCount: crossSpectrumComplexOpCount,
|
||||
scanLookupCount,
|
||||
operationCount: crossSpectrumComplexOpCount + scanLookupCount,
|
||||
dasEquivalentComplexMacCount,
|
||||
distanceBytes,
|
||||
stftWorkspaceBytes,
|
||||
correlationWorkspaceBytes,
|
||||
spectrumBytes,
|
||||
calibrationPhaseBytes,
|
||||
outputBytes,
|
||||
retainedPlanBytes,
|
||||
estimatedWorkingBytes,
|
||||
};
|
||||
}
|
||||
|
||||
/**
|
||||
* Estimate CPU work for one reference-pair GCC-PHAT analysis.
|
||||
*/
|
||||
export function estimateGccPhatWorkload(options) {
|
||||
if (!options || typeof options !== "object") {
|
||||
throw new TypeError("options must be an object.");
|
||||
}
|
||||
const configuration = validateConfiguration(options);
|
||||
let geometry;
|
||||
if (options.analysisPlan) {
|
||||
assertCompatibleAnalysisPlan(options.analysisPlan, options);
|
||||
geometry = options.analysisPlan;
|
||||
} else {
|
||||
geometry = createGeometry(configuration);
|
||||
}
|
||||
return estimateWithGeometry(options, configuration, geometry);
|
||||
}
|
||||
|
||||
/**
|
||||
* Retain geometry and FFT workspaces for repeated analyses with one immutable
|
||||
* array/scan/band configuration. Plans are stateful and cannot run concurrently.
|
||||
*/
|
||||
export function createGccPhatAnalysisPlan(options) {
|
||||
if (!options || typeof options !== "object") {
|
||||
throw new TypeError("options must be an object.");
|
||||
}
|
||||
const configuration = validateConfiguration(options);
|
||||
const maximumPlanBytes =
|
||||
options.maximumPlanBytes ?? DEFAULT_MAXIMUM_GCC_PHAT_PLAN_BYTES;
|
||||
if (!Number.isSafeInteger(maximumPlanBytes) || maximumPlanBytes <= 0) {
|
||||
throw new RangeError("maximumPlanBytes must be a positive safe integer.");
|
||||
}
|
||||
|
||||
const microphoneCount = configuration.arrayDefinition.microphones.length;
|
||||
const pointMicrophonePairs = configuration.pointCount * microphoneCount;
|
||||
const distanceBytes = pointMicrophonePairs * Float32Array.BYTES_PER_ELEMENT;
|
||||
const stftWorkspaceBytes =
|
||||
configuration.fftSize * Float64Array.BYTES_PER_ELEMENT * 3;
|
||||
const correlationWorkspaceBytes =
|
||||
configuration.fftSize * Float64Array.BYTES_PER_ELEMENT * 2;
|
||||
const retainedBytes = distanceBytes + stftWorkspaceBytes + correlationWorkspaceBytes;
|
||||
if (
|
||||
!Number.isSafeInteger(pointMicrophonePairs) ||
|
||||
!Number.isSafeInteger(retainedBytes) ||
|
||||
retainedBytes > maximumPlanBytes
|
||||
) {
|
||||
throw new RangeError(
|
||||
"GCC-PHAT analysis plan requires " + retainedBytes.toLocaleString() +
|
||||
" bytes, above maximumPlanBytes " + maximumPlanBytes.toLocaleString() + ".",
|
||||
);
|
||||
}
|
||||
|
||||
const distances = createDistanceMatrix(
|
||||
configuration.arrayDefinition,
|
||||
configuration.experiment.scan,
|
||||
);
|
||||
const geometry = createGeometry(configuration, distances);
|
||||
const plan = {
|
||||
kind: GCC_PHAT_ANALYSIS_PLAN_KIND,
|
||||
sampleRate: configuration.sampleRate,
|
||||
arrayDefinition: configuration.arrayDefinition,
|
||||
scene: options.scene,
|
||||
experiment: configuration.experiment,
|
||||
fftSize: configuration.fftSize,
|
||||
hopSize: configuration.hopSize,
|
||||
pointCount: configuration.pointCount,
|
||||
maximumPlanBytes,
|
||||
retainedBytes,
|
||||
distanceBytes,
|
||||
stftWorkspaceBytes,
|
||||
correlationWorkspaceBytes,
|
||||
stftWorkspace: createStftWorkspace(configuration.fftSize),
|
||||
correlationReal: new Float64Array(configuration.fftSize),
|
||||
correlationImaginary: new Float64Array(configuration.fftSize),
|
||||
busy: false,
|
||||
statistics: {
|
||||
analysisAttempts: 0,
|
||||
completedAnalyses: 0,
|
||||
distanceMatrixBuilds: 1,
|
||||
distanceEvaluations: pointMicrophonePairs,
|
||||
avoidedDistanceEvaluations: 0,
|
||||
stftWorkspaceAllocations: 1,
|
||||
stftWorkspaceUses: 0,
|
||||
avoidedStftWorkspaceAllocations: 0,
|
||||
correlationWorkspaceAllocations: 1,
|
||||
correlationWorkspaceUses: 0,
|
||||
avoidedCorrelationWorkspaceAllocations: 0,
|
||||
stftTransforms: 0,
|
||||
inverseFftTransforms: 0,
|
||||
crossSpectrumOperations: 0,
|
||||
scanLookups: 0,
|
||||
},
|
||||
...geometry,
|
||||
};
|
||||
return plan;
|
||||
}
|
||||
|
||||
function assertCompatibleAnalysisPlan(plan, options) {
|
||||
if (plan?.kind !== GCC_PHAT_ANALYSIS_PLAN_KIND) {
|
||||
throw new TypeError(
|
||||
"analysisPlan must be created by createGccPhatAnalysisPlan().",
|
||||
);
|
||||
}
|
||||
if (
|
||||
plan.sampleRate !== options.sampleRate ||
|
||||
plan.arrayDefinition !== options.arrayDefinition ||
|
||||
plan.scene !== options.scene ||
|
||||
plan.experiment !== options.experiment
|
||||
) {
|
||||
throw new Error(
|
||||
"analysisPlan must be used with the exact configuration objects that created it.",
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
function analysisCacheSummary(plan) {
|
||||
const statistics = plan.statistics;
|
||||
return {
|
||||
analysisAttempts: statistics.analysisAttempts,
|
||||
completedAnalyses: statistics.completedAnalyses,
|
||||
distanceMatrixBuilds: statistics.distanceMatrixBuilds,
|
||||
distanceEvaluations: statistics.distanceEvaluations,
|
||||
avoidedDistanceEvaluations: statistics.avoidedDistanceEvaluations,
|
||||
stftWorkspaceAllocations: statistics.stftWorkspaceAllocations,
|
||||
stftWorkspaceUses: statistics.stftWorkspaceUses,
|
||||
avoidedStftWorkspaceAllocations: statistics.avoidedStftWorkspaceAllocations,
|
||||
correlationWorkspaceAllocations: statistics.correlationWorkspaceAllocations,
|
||||
correlationWorkspaceUses: statistics.correlationWorkspaceUses,
|
||||
avoidedCorrelationWorkspaceAllocations:
|
||||
statistics.avoidedCorrelationWorkspaceAllocations,
|
||||
stftTransforms: statistics.stftTransforms,
|
||||
inverseFftTransforms: statistics.inverseFftTransforms,
|
||||
crossSpectrumOperations: statistics.crossSpectrumOperations,
|
||||
scanLookups: statistics.scanLookups,
|
||||
retainedBytes: plan.retainedBytes,
|
||||
maximumPlanBytes: plan.maximumPlanBytes,
|
||||
};
|
||||
}
|
||||
|
||||
function circularLinearSample(values, lagSamples) {
|
||||
const length = values.length;
|
||||
const wrapped = ((lagSamples % length) + length) % length;
|
||||
const firstIndex = Math.floor(wrapped);
|
||||
const fraction = wrapped - firstIndex;
|
||||
const secondIndex = (firstIndex + 1) % length;
|
||||
return values[firstIndex] * (1 - fraction) + values[secondIndex] * fraction;
|
||||
}
|
||||
|
||||
function fillBandPairCorrelation({
|
||||
correlationReal,
|
||||
correlationImaginary,
|
||||
bins,
|
||||
stft,
|
||||
microphoneIndex,
|
||||
referenceIndex,
|
||||
phasesByBin,
|
||||
phatFloorRatio,
|
||||
}) {
|
||||
correlationReal.fill(0);
|
||||
correlationImaginary.fill(0);
|
||||
const microphoneCount = stft.microphoneCount;
|
||||
let maximumMagnitude = 0;
|
||||
|
||||
for (const bin of bins) {
|
||||
const spectrum = stft.spectra.get(bin);
|
||||
for (let frameIndex = 0; frameIndex < stft.frameCount; frameIndex += 1) {
|
||||
const offset = frameIndex * microphoneCount;
|
||||
const microphoneReal = spectrum.real[offset + microphoneIndex];
|
||||
const microphoneImaginary = spectrum.imaginary[offset + microphoneIndex];
|
||||
const referenceReal = spectrum.real[offset + referenceIndex];
|
||||
const referenceImaginary = spectrum.imaginary[offset + referenceIndex];
|
||||
const crossReal =
|
||||
microphoneReal * referenceReal + microphoneImaginary * referenceImaginary;
|
||||
const crossImaginary =
|
||||
microphoneImaginary * referenceReal - microphoneReal * referenceImaginary;
|
||||
maximumMagnitude = Math.max(
|
||||
maximumMagnitude,
|
||||
Math.hypot(crossReal, crossImaginary),
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
const magnitudeFloor = maximumMagnitude * phatFloorRatio;
|
||||
if (!(maximumMagnitude > 0)) {
|
||||
fftInPlace(correlationReal, correlationImaginary, true);
|
||||
return;
|
||||
}
|
||||
|
||||
for (const bin of bins) {
|
||||
const spectrum = stft.spectra.get(bin);
|
||||
const phases = phasesByBin.get(bin);
|
||||
const correctionPhase = phases[referenceIndex] - phases[microphoneIndex];
|
||||
const correctionReal = Math.cos(correctionPhase);
|
||||
const correctionImaginary = Math.sin(correctionPhase);
|
||||
let accumulatedReal = 0;
|
||||
let accumulatedImaginary = 0;
|
||||
|
||||
for (let frameIndex = 0; frameIndex < stft.frameCount; frameIndex += 1) {
|
||||
const offset = frameIndex * microphoneCount;
|
||||
const microphoneReal = spectrum.real[offset + microphoneIndex];
|
||||
const microphoneImaginary = spectrum.imaginary[offset + microphoneIndex];
|
||||
const referenceReal = spectrum.real[offset + referenceIndex];
|
||||
const referenceImaginary = spectrum.imaginary[offset + referenceIndex];
|
||||
const crossReal =
|
||||
microphoneReal * referenceReal + microphoneImaginary * referenceImaginary;
|
||||
const crossImaginary =
|
||||
microphoneImaginary * referenceReal - microphoneReal * referenceImaginary;
|
||||
const magnitude = Math.hypot(crossReal, crossImaginary);
|
||||
const denominator = Math.max(magnitude, magnitudeFloor);
|
||||
if (!(denominator > 0)) continue;
|
||||
const normalizedReal = crossReal / denominator;
|
||||
const normalizedImaginary = crossImaginary / denominator;
|
||||
accumulatedReal +=
|
||||
normalizedReal * correctionReal - normalizedImaginary * correctionImaginary;
|
||||
accumulatedImaginary +=
|
||||
normalizedReal * correctionImaginary + normalizedImaginary * correctionReal;
|
||||
}
|
||||
|
||||
const averagedReal = accumulatedReal / stft.frameCount;
|
||||
const averagedImaginary = accumulatedImaginary / stft.frameCount;
|
||||
correlationReal[bin] = averagedReal;
|
||||
correlationImaginary[bin] =
|
||||
bin === 0 || bin === correlationReal.length / 2 ? 0 : averagedImaginary;
|
||||
if (bin > 0 && bin < correlationReal.length / 2) {
|
||||
const negativeBin = correlationReal.length - bin;
|
||||
correlationReal[negativeBin] = averagedReal;
|
||||
correlationImaginary[negativeBin] = -averagedImaginary;
|
||||
}
|
||||
}
|
||||
|
||||
fftInPlace(correlationReal, correlationImaginary, true);
|
||||
}
|
||||
|
||||
function shiftAndSquareScores(rawMaps) {
|
||||
for (const rawMap of rawMaps) {
|
||||
let minimum = Infinity;
|
||||
for (const value of rawMap) minimum = Math.min(minimum, value);
|
||||
for (let pointIndex = 0; pointIndex < rawMap.length; pointIndex += 1) {
|
||||
const shifted = rawMap[pointIndex] - minimum;
|
||||
rawMap[pointIndex] = shifted * shifted;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
async function computeGccPhatMapInternal(options, configuration, geometry, plan) {
|
||||
const {
|
||||
channels,
|
||||
signal,
|
||||
onProgress,
|
||||
maximumWorkingBytes = 512 * 1024 * 1024,
|
||||
maximumFftButterflies = 1_000_000_000,
|
||||
pointBatchSize = 256,
|
||||
stftTransformBatchSize = 128,
|
||||
} = options;
|
||||
requirePositiveInteger(pointBatchSize, "pointBatchSize");
|
||||
requirePositiveInteger(stftTransformBatchSize, "stftTransformBatchSize");
|
||||
if (!Number.isSafeInteger(maximumWorkingBytes) || maximumWorkingBytes <= 0) {
|
||||
throw new RangeError("maximumWorkingBytes must be a positive safe integer.");
|
||||
}
|
||||
const phatFloorRatio = requirePhatFloorRatio(
|
||||
options.phatFloorRatio ??
|
||||
options.phatRelativeFloor ??
|
||||
configuration.experiment.beamforming?.phatFloorRatio,
|
||||
);
|
||||
const workload = estimateWithGeometry(options, configuration, geometry);
|
||||
if (workload.estimatedWorkingBytes > maximumWorkingBytes) {
|
||||
throw new RangeError(
|
||||
"GCC-PHAT estimated working set " +
|
||||
(workload.estimatedWorkingBytes / 1024 ** 2).toFixed(1) +
|
||||
" MiB exceeds maximumWorkingBytes " +
|
||||
(maximumWorkingBytes / 1024 ** 2).toFixed(1) + " MiB.",
|
||||
);
|
||||
}
|
||||
const totalFftButterflies =
|
||||
workload.stftButterflyCount + workload.inverseFftButterflyCount;
|
||||
if (totalFftButterflies > maximumFftButterflies) {
|
||||
throw new RangeError(
|
||||
"GCC-PHAT estimated FFT butterfly count " +
|
||||
Math.ceil(totalFftButterflies).toLocaleString() +
|
||||
" exceeds maximumFftButterflies " +
|
||||
maximumFftButterflies.toLocaleString() + ".",
|
||||
);
|
||||
}
|
||||
|
||||
throwIfAborted(signal);
|
||||
if (plan) {
|
||||
plan.statistics.stftWorkspaceUses += 1;
|
||||
plan.statistics.avoidedStftWorkspaceAllocations = Math.max(
|
||||
0,
|
||||
plan.statistics.stftWorkspaceUses - 1,
|
||||
);
|
||||
plan.statistics.correlationWorkspaceUses += 1;
|
||||
plan.statistics.avoidedCorrelationWorkspaceAllocations = Math.max(
|
||||
0,
|
||||
plan.statistics.correlationWorkspaceUses - 1,
|
||||
);
|
||||
}
|
||||
const stft = await extractStftBins(channels, {
|
||||
fftSize: configuration.fftSize,
|
||||
hopSize: configuration.hopSize,
|
||||
bins: geometry.selectedBins,
|
||||
workspace: plan?.stftWorkspace,
|
||||
transformBatchSize: stftTransformBatchSize,
|
||||
signal,
|
||||
onProgress: ({ completedTransforms, totalTransforms }) =>
|
||||
onProgress?.({
|
||||
stage: "stft",
|
||||
algorithm: "gcc-phat",
|
||||
completedTransforms,
|
||||
totalTransforms,
|
||||
}),
|
||||
});
|
||||
if (plan) {
|
||||
plan.statistics.stftTransforms += workload.stftTransformCount;
|
||||
}
|
||||
await cooperativeYield();
|
||||
throwIfAborted(signal);
|
||||
|
||||
const phasesByBin = new Map();
|
||||
for (const bin of geometry.selectedBins) {
|
||||
const calibration = createBinCalibration({
|
||||
arrayDefinition: configuration.arrayDefinition,
|
||||
bin,
|
||||
sampleRate: configuration.sampleRate,
|
||||
fftSize: configuration.fftSize,
|
||||
compensateFrequencyResponse:
|
||||
configuration.experiment.beamforming?.compensateFrequencyResponse !== false,
|
||||
maxCompensationDb:
|
||||
configuration.experiment.beamforming?.maxCompensationDb,
|
||||
includeAmplitude: false,
|
||||
});
|
||||
phasesByBin.set(bin, calibration.phases);
|
||||
}
|
||||
|
||||
const correlationReal =
|
||||
plan?.correlationReal ?? new Float64Array(configuration.fftSize);
|
||||
const correlationImaginary =
|
||||
plan?.correlationImaginary ?? new Float64Array(configuration.fftSize);
|
||||
const rawMaps = geometry.bandBins.map(() => new Float32Array(configuration.pointCount));
|
||||
const totalPairs = geometry.pairCount * geometry.bandBins.length;
|
||||
let completedPairs = 0;
|
||||
|
||||
for (let bandIndex = 0; bandIndex < geometry.bandBins.length; bandIndex += 1) {
|
||||
const bins = geometry.bandBins[bandIndex];
|
||||
const rawMap = rawMaps[bandIndex];
|
||||
for (const microphoneIndex of geometry.pairMicrophoneIndexes) {
|
||||
throwIfAborted(signal);
|
||||
fillBandPairCorrelation({
|
||||
correlationReal,
|
||||
correlationImaginary,
|
||||
bins,
|
||||
stft,
|
||||
microphoneIndex,
|
||||
referenceIndex: geometry.reference.index,
|
||||
phasesByBin,
|
||||
phatFloorRatio,
|
||||
});
|
||||
if (plan) {
|
||||
plan.statistics.inverseFftTransforms += 1;
|
||||
plan.statistics.crossSpectrumOperations +=
|
||||
bins.length * stft.frameCount * 2;
|
||||
}
|
||||
|
||||
for (let pointIndex = 0; pointIndex < configuration.pointCount; pointIndex += 1) {
|
||||
const distanceOffset = pointIndex * geometry.microphoneCount;
|
||||
const predictedLagSamples =
|
||||
(
|
||||
geometry.distances[distanceOffset + microphoneIndex] -
|
||||
geometry.distances[distanceOffset + geometry.reference.index]
|
||||
) *
|
||||
configuration.sampleRate / configuration.speedOfSound;
|
||||
rawMap[pointIndex] += circularLinearSample(
|
||||
correlationReal,
|
||||
predictedLagSamples,
|
||||
);
|
||||
if (
|
||||
(pointIndex + 1) % pointBatchSize === 0 &&
|
||||
pointIndex + 1 < configuration.pointCount
|
||||
) {
|
||||
await cooperativeYield();
|
||||
throwIfAborted(signal);
|
||||
}
|
||||
}
|
||||
if (plan) plan.statistics.scanLookups += configuration.pointCount;
|
||||
completedPairs += 1;
|
||||
onProgress?.({
|
||||
stage: "localization",
|
||||
algorithm: "gcc-phat",
|
||||
completedPairs,
|
||||
totalPairs,
|
||||
});
|
||||
if (completedPairs < totalPairs) await cooperativeYield();
|
||||
}
|
||||
}
|
||||
|
||||
throwIfAborted(signal);
|
||||
shiftAndSquareScores(rawMaps);
|
||||
const bands = finalizeDasBands(
|
||||
rawMaps,
|
||||
workload,
|
||||
configuration.experiment,
|
||||
"score",
|
||||
);
|
||||
return {
|
||||
backend: "cpu",
|
||||
algorithm: "gcc-phat",
|
||||
frameCount: workload.frameCount,
|
||||
microphoneCount: workload.microphoneCount,
|
||||
pointCount: workload.pointCount,
|
||||
referenceMicrophoneId: geometry.reference.id,
|
||||
referenceMicrophoneIndex: geometry.reference.index,
|
||||
pairCount: geometry.pairCount,
|
||||
phatFloorRatio,
|
||||
maximumLagSamples: geometry.maximumLagSamples,
|
||||
workload,
|
||||
bands,
|
||||
...(plan ? { analysisCache: analysisCacheSummary(plan) } : {}),
|
||||
};
|
||||
}
|
||||
|
||||
/**
|
||||
* Build band-limited reference-pair GCC-PHAT maps on the CPU.
|
||||
*
|
||||
* For each pair and band, corrected PHAT cross spectra are averaged over STFT
|
||||
* frames, transformed once to circular correlation, and sampled at the
|
||||
* near-field geometric delay of each scan point.
|
||||
*/
|
||||
export async function computeGccPhatMap(options) {
|
||||
if (!options || typeof options !== "object") {
|
||||
throw new TypeError("options must be an object.");
|
||||
}
|
||||
const configuration = validateConfiguration(options);
|
||||
const plan = options.analysisPlan;
|
||||
if (!plan) {
|
||||
const geometry = createGeometry(configuration);
|
||||
return computeGccPhatMapInternal(options, configuration, geometry, undefined);
|
||||
}
|
||||
|
||||
assertCompatibleAnalysisPlan(plan, options);
|
||||
if (plan.busy) {
|
||||
throw new Error("analysisPlan is already in use by another GCC-PHAT analysis.");
|
||||
}
|
||||
plan.busy = true;
|
||||
plan.statistics.analysisAttempts += 1;
|
||||
if (plan.statistics.analysisAttempts > 1) {
|
||||
plan.statistics.avoidedDistanceEvaluations +=
|
||||
plan.pointCount * plan.microphoneCount;
|
||||
}
|
||||
try {
|
||||
const result = await computeGccPhatMapInternal(
|
||||
options,
|
||||
configuration,
|
||||
plan,
|
||||
plan,
|
||||
);
|
||||
plan.statistics.completedAnalyses += 1;
|
||||
result.analysisCache = analysisCacheSummary(plan);
|
||||
return result;
|
||||
} finally {
|
||||
plan.busy = false;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user