Add subsample_alignment and it's test

3 files changed, 80 insertions, 22 deletions

diff --git a/src/dab_util.py b/src/dab_util.py
index 617bd9a..3187036 100644
--- a/src/dab_util.py
+++ b/src/dab_util.py
@@ -2,6 +2,7 @@ import numpy as np
 import scipy
 import matplotlib.pyplot as plt
 import src.dabconst as dabconst
+import src.subsample_align as sa
 from scipy import signal
 
 c = {}
@@ -76,22 +77,26 @@ def lag_upsampling(sig_orig, sig_rec, n_up):
     l_orig = float(l) / n_up
     return l_orig
 
-def fftlag(sig_orig, sig_rec, n_upsampling = 1):
+def subsample_align(sig1, sig2):
     """
-    Efficient way to find lag between two signals
-    Args:
-        sig_orig: The signal that has been sent
-        sig_rec: The signal that has been recored
+    Returns an aligned version of sig1 and sig2 by cropping and subsample alignment
     """
-    #off = sig_rec.shape[0]
-    #fft1 = np.fft.fft(sig_orig, n=sig_orig.shape[0])
-    #fft2 = np.fft.fft(np.flipud(sig_rec), n=sig_rec.shape[0])
-    #fftc = fft1 * fft2
-    #c = np.fft.ifft(fftc)
-    c = signal.convolve(sig_orig, np.flipud(sig_rec))
-    #c = signal.correlate(sig_orig, sig_rec)
-    return c
-    return np.argmax(c) - off + 1
+    off_meas = lag_upsampling(sig2, sig1, n_up=1)
+    off = int(abs(off_meas))
+
+    if off_meas > 0:
+        sig1 = sig1[:-off]
+        sig2 = sig2[off:]
+    elif off_meas < 0:
+        sig1 = sig1[off:]
+        sig2 = sig2[:-off]
+
+    if off % 2 == 1:
+        sig1 = sig1[:-1]
+        sig2 = sig2[:-1]
+
+    sig2 = sa.subsample_align(sig2, sig1)
+    return sig1, sig2
 
 def get_amp_ratio(ampl_1, ampl_2, a_out_abs, a_in_abs):
     idxs = (a_in_abs > ampl_1) & (a_in_abs < ampl_2)
@@ -108,5 +113,8 @@ def get_transmission_frame_indices(n_frames, offset, rate = 2048000):
     indices = [tm1.S_F * i + offset for i in range(n_frames)]
     return indices
 
-def fromfile(filename, offset, length):
-    return np.memmap(filename, dtype=np.complex64, mode='r', offset=64/8*offset, shape=length)
+def fromfile(filename, offset=0, length=None):
+    if length is None:
+        return np.memmap(filename, dtype=np.complex64, mode='r', offset=64/8*offset)
+    else:
+        return np.memmap(filename, dtype=np.complex64, mode='r', offset=64/8*offset, shape=length)
diff --git a/src/dab_util_test.py b/src/dab_util_test.py
index be36d53..3f9e941 100644
--- a/src/dab_util_test.py
+++ b/src/dab_util_test.py
@@ -1,5 +1,7 @@
 from scipy import signal
 import numpy as np
+import pandas as pd
+from tqdm import tqdm
 import src.gen_source as gs
 import src.dab_util as du
 
@@ -28,7 +30,54 @@ def test_phase_offset(lag_function, tol):
         res.append(np.abs(off-off_meas)<tol)
     return np.mean(res)
 
+
+def test_using_aligned_pair(sample_orig=r'../data/orig_rough_aligned.dat', sample_rec =r'../data/recored_rough_aligned.dat', length = 10240, max_size = 1000000):
+    res = []
+    for i in tqdm(range(100)):
+        start = np.random.randint(50, max_size)
+        r = np.random.randint(-50, 50)
+
+        s1 = du.fromfile(sample_orig, offset=start+r, length=length)
+        s2 = du.fromfile(sample_rec, offset=start, length=length)
+
+        res.append({'offset':r,
+                    '1':r - du.lag_upsampling(s2, s1, n_up=1),
+                    '2':r - du.lag_upsampling(s2, s1, n_up=2),
+                    '3':r - du.lag_upsampling(s2, s1, n_up=3),
+                    '4':r - du.lag_upsampling(s2, s1, n_up=4),
+                    '8':r - du.lag_upsampling(s2, s1, n_up=8),
+                    '16':r - du.lag_upsampling(s2, s1, n_up=16),
+                    '32':r - du.lag_upsampling(s2, s1, n_up=32),
+                    })
+    df = pd.DataFrame(res)
+    df = df.reindex_axis(sorted(df.columns), axis=1)
+    print(df.describe())
+
+def test_subsample_alignment(sample_orig=r'../data/orig_rough_aligned.dat',
+        sample_rec =r'../data/recored_rough_aligned.dat', length = 10240, max_size = 1000000):
+    res1 = []
+    res2 = []
+    for i in tqdm(range(10)):
+        start = np.random.randint(50, max_size)
+        r = np.random.randint(-50, 50)
+
+        s1 = du.fromfile(sample_orig, offset=start+r, length=length)
+        s2 = du.fromfile(sample_rec, offset=start, length=length)
+
+        res1.append(du.lag_upsampling(s2, s1, 32))
+
+        s1_aligned, s2_aligned = du.subsample_align(s1,s2)
+
+        res2.append(du.lag_upsampling(s2_aligned, s1_aligned, 32))
+
+    print("Before subsample alignment: lag_std = %.2f, lag_abs_mean = %.2f" % (np.std(res1), np.mean(np.abs(res1))))
+    print("After subsample alignment: lag_std = %.2f, lag_abs_mean = %.2f" % (np.std(res2), np.mean(np.abs(res2))))
+
+print("Align using upsampling")
 for n_up in [1, 2, 3, 4, 7, 8, 16]:
    correct_ratio = test_phase_offset(lambda x,y: du.lag_upsampling(x,y,n_up), tol=1./n_up)
    print("%.1f%% of the tested offsets were measured within tolerance %.4f for n_up = %d" % (correct_ratio * 100, 1./n_up, n_up))
+test_using_aligned_pair()
 
+print("Phase alignment")
+test_subsample_alignment()
diff --git a/src/subsample_align.py b/src/subsample_align.py
index 376058c..1657131 100755
--- a/src/subsample_align.py
+++ b/src/subsample_align.py
@@ -3,6 +3,7 @@ import numpy as np
 from scipy import signal, optimize
 import sys
 import matplotlib.pyplot as plt
+import dab_util as du
 
 def gen_omega(length):
     if (length % 2) == 1:
@@ -59,29 +60,29 @@ def subsample_align(sig, ref_sig):
     optim_result = optimize.minimize_scalar(correlate_for_delay, bounds=(-1,1), method='bounded', options={'disp': True})
 
     if optim_result.success:
-        print("x:")
-        print(optim_result.x)
+        #print("x:")
+        #print(optim_result.x)
 
         best_tau = optim_result.x
 
-        print("Found subsample delay = {}".format(best_tau))
+        #print("Found subsample delay = {}".format(best_tau))
 
         # Prepare rotate_vec = fft_sig with rotated phase
         rotate_vec = np.exp(1j * best_tau * omega)
         rotate_vec[halflen] = np.cos(np.pi * best_tau)
         return np.fft.ifft(rotate_vec * fft_sig)
     else:
-        print("Could not optimize: " + optim_result.message)
+        #print("Could not optimize: " + optim_result.message)
         return np.zeros(0, dtype=np.complex64)
 
 if __name__ == "__main__":
-    phaseref_filename = "/home/bram/dab/aux/odr-dab-cir/phasereference.2048000.fc64.iq"
+    phaseref_filename = "/home/andreas/dab/ODR-StaticPrecorrection/data/samples/sample_orig_0.iq"
     phase_ref = np.fromfile(phaseref_filename, np.complex64)
 
     delay = 15
     n_up = 32
 
-    print("Generate signal with delay {}/{} = {}".format(delay, n_up, delay/n_up))
+    print("Generate signal with delay {}/{} = {}".format(delay, n_up, float(delay)/n_up))
     phase_ref_up = signal.resample(phase_ref, phase_ref.shape[0] * n_up)
     phase_ref_up_late = np.append(np.zeros(delay, dtype=np.complex64), phase_ref_up[:-delay])
     phase_ref_late = signal.resample(phase_ref_up_late, phase_ref.shape[0])