diff src/Matcher.cpp @ 75:e1a5f3095ba6 cheap_diagonals

Merge from branch refactors

--- a/src/Matcher.cpp	Tue Nov 18 10:33:12 2014 +0000
+++ b/src/Matcher.cpp	Wed Nov 19 12:13:28 2014 +0000
@@ -21,38 +21,13 @@
 #include <cstdlib>
 #include <cassert>
 
+using namespace std;
+
 //#define DEBUG_MATCHER 1
 
-Matcher::Matcher(Parameters parameters,
-                 FeatureExtractor::Parameters feParams,
-                 Matcher *p) :
-    m_params(parameters),
-    m_featureExtractor(feParams),
-    m_metric(parameters.distanceNorm)
-{
-#ifdef DEBUG_MATCHER
-    cerr << "Matcher::Matcher(" << m_params.sampleRate << ", " << p << ")" << endl;
-#endif
-
-    m_otherMatcher = p;	// the first matcher will need this to be set later
-    m_firstPM = (!p);
-    m_frameCount = 0;
-    m_runCount = 0;
-    m_featureSize = m_featureExtractor.getFeatureSize();
-    m_blockSize = 0;
-
-    m_blockSize = lrint(m_params.blockTime / m_params.hopTime);
-#ifdef DEBUG_MATCHER
-    cerr << "Matcher: m_blockSize = " << m_blockSize << endl;
-#endif
-
-    m_initialised = false;
-}
-
 Matcher::Matcher(Parameters parameters, Matcher *p, int m_featureSize_) :
     m_params(parameters),
     m_featureSize(m_featureSize_),
-    m_featureExtractor(FeatureExtractor::Parameters(m_params.sampleRate, m_params.fftSize)), // unused default config
     m_metric(parameters.distanceNorm)
 {
 #ifdef DEBUG_MATCHER
@@ -86,7 +61,7 @@
     if (m_initialised) return;
 
     m_frames = vector<vector<double> >
-        (m_blockSize, vector<double>(m_featureSize, 0));
+        (m_blockSize, vector<double>(m_featureSize, -1.0));
 
     m_distXSize = m_blockSize * 2;
 
@@ -102,31 +77,15 @@
 Matcher::size()
 {
     int distSize = (m_params.maxRunCount + 1) * m_blockSize;
-    m_bestPathCost.resize(m_distXSize, vector<double>(distSize, 0));
-    m_distance.resize(m_distXSize, vector<float>(distSize, 0));
+    m_bestPathCost.resize(m_distXSize, vector<double>(distSize, -1));
+    m_distance.resize(m_distXSize, vector<float>(distSize, -1));
     m_advance.resize(m_distXSize, vector<Advance>(distSize, AdvanceNone));
-    m_distYSizes.resize(m_distXSize, distSize);
     m_first.resize(m_distXSize, 0);
     m_last.resize(m_distXSize, 0);
 }
 
-vector<double>
-Matcher::consumeFrame(double *reBuffer, double *imBuffer)
-{
-    if (!m_initialised) init();
-
-    vector<double> real(reBuffer, reBuffer + m_params.fftSize/2 + 1);
-    vector<double> imag(imBuffer, imBuffer + m_params.fftSize/2 + 1);
-    vector<double> feature = m_featureExtractor.process(real, imag);
-    int frameIndex = m_frameCount % m_blockSize;
-    m_frames[frameIndex] = feature;
-    calcAdvance();
-
-    return feature;
-}
-
 void
-Matcher::consumeFeatureVector(std::vector<double> feature)
+Matcher::consumeFeatureVector(vector<double> feature)
 {
     if (!m_initialised) init();
     int frameIndex = m_frameCount % m_blockSize; 
@@ -153,22 +112,30 @@
         // distance[m_frameCount], and then truncate
         // distance[m_frameCount-m_blockSize] to its first len elements.
         // Same for bestPathCost.
-/*
-        std::cerr << "Matcher(" << this << "): moving " << distYSizes[m_frameCount - m_blockSize] << " from " << m_frameCount - m_blockSize << " to "
-                  << m_frameCount << ", allocating " << len << " for "
-                  << m_frameCount - m_blockSize << std::endl;
-*/
-        m_distance[m_frameCount] = m_distance[m_frameCount - m_blockSize];
-        m_distance[m_frameCount - m_blockSize].resize(len, 0);
 
-        m_bestPathCost[m_frameCount] = m_bestPathCost[m_frameCount - m_blockSize];
-        m_bestPathCost[m_frameCount - m_blockSize].resize(len, 0);
+        vector<float> dOld = m_distance[m_frameCount - m_blockSize];
+        vector<float> dNew(len, -1.f);
 
-        m_advance[m_frameCount] = m_advance[m_frameCount - m_blockSize];
-        m_advance[m_frameCount - m_blockSize].resize(len, AdvanceNone);
+        vector<double> bpcOld = m_bestPathCost[m_frameCount - m_blockSize];
+        vector<double> bpcNew(len, -1.0);
+
+        vector<Advance> adOld = m_advance[m_frameCount - m_blockSize];
+        vector<Advance> adNew(len, AdvanceNone);
+
+        for (int i = 0; i < len; ++i) {
+            dNew[i] = dOld[i];
+            bpcNew[i] = bpcOld[i];
+            adNew[i] = adOld[i];
+        }
         
-        m_distYSizes[m_frameCount] = m_distYSizes[m_frameCount - m_blockSize];
-        m_distYSizes[m_frameCount - m_blockSize] = len;
+        m_distance[m_frameCount] = dOld;
+        m_distance[m_frameCount - m_blockSize] = dNew;
+
+        m_bestPathCost[m_frameCount] = bpcOld;
+        m_bestPathCost[m_frameCount - m_blockSize] = bpcNew;
+
+        m_advance[m_frameCount] = adOld;
+        m_advance[m_frameCount - m_blockSize] = adNew;
     }
 
     int stop = m_otherMatcher->m_frameCount;
@@ -194,43 +161,43 @@
             mn = dMN;
 
         if ((m_frameCount == 0) && (index == 0))    // first element
-            setValue(0, 0, AdvanceNone, 0, dMN);
+            updateValue(0, 0, AdvanceNone, 0, dMN);
         else if (m_frameCount == 0)                 // first row
-            setValue(0, index, AdvanceOther,
-                     getValue(0, index-1, true), dMN);
+            updateValue(0, index, AdvanceOther,
+                        getPathCost(0, index-1), dMN);
         else if (index == 0)                      // first column
-            setValue(m_frameCount, index, AdvanceThis,
-                     getValue(m_frameCount - 1, 0, true), dMN);
+            updateValue(m_frameCount, index, AdvanceThis,
+                        getPathCost(m_frameCount - 1, 0), dMN);
         else if (index == m_otherMatcher->m_frameCount - m_blockSize) {
             // missing value(s) due to cutoff
             //  - no previous value in current row (resp. column)
             //  - no diagonal value if prev. dir. == curr. dirn
-            double min2 = getValue(m_frameCount - 1, index, true);
+            double min2 = getPathCost(m_frameCount - 1, index);
             //	if ((m_firstPM && (first[m_frameCount - 1] == index)) ||
             //			(!m_firstPM && (m_last[index-1] < m_frameCount)))
             if (m_first[m_frameCount - 1] == index)
-                setValue(m_frameCount, index, AdvanceThis, min2, dMN);
+                updateValue(m_frameCount, index, AdvanceThis, min2, dMN);
             else {
-                double min1 = getValue(m_frameCount - 1, index - 1, true);
+                double min1 = getPathCost(m_frameCount - 1, index - 1);
                 if (min1 + dMN <= min2)
-                    setValue(m_frameCount, index, AdvanceBoth, min1,dMN);
+                    updateValue(m_frameCount, index, AdvanceBoth, min1,dMN);
                 else
-                    setValue(m_frameCount, index, AdvanceThis, min2,dMN);
+                    updateValue(m_frameCount, index, AdvanceThis, min2,dMN);
             }
         } else {
-            double min1 = getValue(m_frameCount, index-1, true);
-            double min2 = getValue(m_frameCount - 1, index, true);
-            double min3 = getValue(m_frameCount - 1, index-1, true);
+            double min1 = getPathCost(m_frameCount, index-1);
+            double min2 = getPathCost(m_frameCount - 1, index);
+            double min3 = getPathCost(m_frameCount - 1, index-1);
             if (min1 <= min2) {
                 if (min3 + dMN <= min1)
-                    setValue(m_frameCount, index, AdvanceBoth, min3,dMN);
+                    updateValue(m_frameCount, index, AdvanceBoth, min3,dMN);
                 else
-                    setValue(m_frameCount, index, AdvanceOther,min1,dMN);
+                    updateValue(m_frameCount, index, AdvanceOther,min1,dMN);
             } else {
                 if (min3 + dMN <= min2)
-                    setValue(m_frameCount, index, AdvanceBoth, min3,dMN);
+                    updateValue(m_frameCount, index, AdvanceBoth, min3,dMN);
                 else
-                    setValue(m_frameCount, index, AdvanceThis, min2,dMN);
+                    updateValue(m_frameCount, index, AdvanceThis, min2,dMN);
             }
         }
         m_otherMatcher->m_last[index]++;
@@ -242,55 +209,178 @@
     m_otherMatcher->m_runCount = 0;
 }
 
+bool
+Matcher::isInRange(int i, int j)
+{
+    if (m_firstPM) {
+        return ((i >= 0) &&
+                (i < int(m_first.size())) &&
+                (j >= m_first[i]) &&
+                (j < int(m_first[i] + m_bestPathCost[i].size())));
+    } else {
+        return m_otherMatcher->isInRange(j, i);
+    }
+}
+
+bool
+Matcher::isAvailable(int i, int j)
+{
+    if (m_firstPM) {
+        if (isInRange(i, j)) {
+            return (m_bestPathCost[i][j - m_first[i]] >= 0);
+        } else {
+            return false;
+        }
+    } else {
+        return m_otherMatcher->isAvailable(j, i);
+    }
+}
+
+pair<int, int>
+Matcher::getColRange(int i)
+{
+    if (i < 0 || i >= int(m_first.size())) {
+        cerr << "ERROR: Matcher::getColRange(" << i << "): Index out of range"
+             << endl;
+        throw "Index out of range";
+    } else {
+        return pair<int, int>(m_first[i], m_last[i]);
+    }
+}
+
+pair<int, int>
+Matcher::getRowRange(int i)
+{
+    return m_otherMatcher->getColRange(i);
+}
+
+float
+Matcher::getDistance(int i, int j)
+{
+    if (m_firstPM) {
+        if (!isInRange(i, j)) {
+            cerr << "ERROR: Matcher::getDistance(" << i << ", " << j << "): "
+                 << "Location is not in range" << endl;
+            throw "Distance not available";
+        }
+        float dist = m_distance[i][j - m_first[i]];
+        if (dist < 0) {
+            cerr << "ERROR: Matcher::getDistance(" << i << ", " << j << "): "
+                 << "Location is in range, but distance ("
+                 << dist << ") is invalid or has not been set" << endl;
+            throw "Distance not available";
+        }
+        return dist;
+    } else {
+        return m_otherMatcher->getDistance(j, i);
+    }
+}
+                
+void
+Matcher::setDistance(int i, int j, float distance)
+{
+    if (m_firstPM) {
+        if (!isInRange(i, j)) {
+            cerr << "ERROR: Matcher::setDistance(" << i << ", " << j << ", "
+                 << distance << "): Location is out of range" << endl;
+            throw "Indices out of range";
+        }
+        m_distance[i][j - m_first[i]] = distance;
+    } else {
+        m_otherMatcher->setDistance(j, i, distance);
+    }
+}
+
 double
-Matcher::getValue(int i, int j, bool firstAttempt)
+Matcher::getPathCost(int i, int j)
 {
-    if (m_firstPM)
+    if (m_firstPM) {
+        if (!isAvailable(i, j)) {
+            if (!isInRange(i, j)) {
+                cerr << "ERROR: Matcher::getPathCost(" << i << ", " << j << "): "
+                     << "Location is not in range" << endl;
+            } else {
+                cerr << "ERROR: Matcher::getPathCost(" << i << ", " << j << "): "
+                     << "Location is in range, but pathCost ("
+                     << m_bestPathCost[i][j - m_first[i]]
+                     << ") is invalid or has not been set" << endl;
+            }
+            throw "Path cost not available";
+        }
         return m_bestPathCost[i][j - m_first[i]];
-    else
-        return m_otherMatcher->m_bestPathCost[j][i - m_otherMatcher->m_first[j]];
-} // getValue()
-
+    } else {
+        return m_otherMatcher->getPathCost(j, i);
+    }
+}
+                
 void
-Matcher::setValue(int i, int j, Advance dir, double value, float dMN)
+Matcher::setPathCost(int i, int j, Advance dir, double pathCost)
 {
-    float diagonalWeight = sqrtf(2.f);
-    
-    if (dir == AdvanceBoth) {
-        dMN *= diagonalWeight;
-    }
-    
     if (m_firstPM) {
-
-        int jdx = j - m_first[i];
-        m_distance[i][jdx] = dMN;
-        m_advance[i][jdx] = dir;
-        m_bestPathCost[i][jdx] = value + dMN;
-
+        if (!isInRange(i, j)) {
+            cerr << "ERROR: Matcher::setPathCost(" << i << ", " << j << ", "
+                 << dir << ", " << pathCost
+                 << "): Location is out of range" << endl;
+            throw "Indices out of range";
+        }
+        m_advance[i][j - m_first[i]] = dir;
+        m_bestPathCost[i][j - m_first[i]] = pathCost;
     } else {
-
         if (dir == AdvanceThis) {
             dir = AdvanceOther;
         } else if (dir == AdvanceOther) {
             dir = AdvanceThis;
         }
+        m_otherMatcher->setPathCost(j, i, dir, pathCost);
+    }
+
+}
+
+void
+Matcher::updateValue(int i, int j, Advance dir, double value, float dMN)
+{
+    float diagonalWeight = sqrtf(2.f);
+
+    float weight = 1.f;
+    if (dir == AdvanceBoth) {
+        weight *= diagonalWeight;
+    }
+    
+    if (m_firstPM) {
+
+        m_distance[i][j - m_first[i]] = dMN;
+        setPathCost(i, j, dir, value + weight * dMN);
+
+    } else {
 
         int idx = i - m_otherMatcher->m_first[j];
         
-        if (idx == (int)m_otherMatcher->m_distYSizes[j]) {
+        if (idx == (int)m_otherMatcher->m_distance[j].size()) {
             // This should never happen, but if we allow arbitrary
             // pauses in either direction, and arbitrary lengths at
             // end, it is better than a segmentation fault.
-            std::cerr << "Emergency resize: " << idx << " -> " << idx * 2 << std::endl;
-            m_otherMatcher->m_distYSizes[j] = idx * 2;
-            m_otherMatcher->m_bestPathCost[j].resize(idx * 2, 0);
-            m_otherMatcher->m_distance[j].resize(idx * 2, 0);
+            cerr << "Emergency resize: " << idx << " -> " << idx * 2 << endl;
+            m_otherMatcher->m_bestPathCost[j].resize(idx * 2, -1);
+            m_otherMatcher->m_distance[j].resize(idx * 2, -1);
             m_otherMatcher->m_advance[j].resize(idx * 2, AdvanceNone);
         }
 
         m_otherMatcher->m_distance[j][idx] = dMN;
-        m_otherMatcher->m_advance[j][idx] = dir;
-        m_otherMatcher->m_bestPathCost[j][idx] = value + dMN;
+        m_otherMatcher->setPathCost(j, i, dir, value + weight * dMN);
     }
-} // setValue()
+}
 
+Matcher::Advance
+Matcher::getAdvance(int i, int j)
+{
+    if (m_firstPM) {
+        if (!isInRange(i, j)) {
+            cerr << "ERROR: Matcher::getAdvance(" << i << ", " << j << "): "
+                 << "Location is not in range" << endl;
+            throw "Advance not available";
+        }
+        return m_advance[i][j - m_first[i]];
+    } else {
+        return m_otherMatcher->getAdvance(j, i);
+    }
+}