comparison src/EM.cpp @ 127:df05f855f63b

Merge from branch bqvec-openmp

 #include <cstdlib>
 #include <cmath>
 
 #include <iostream>
 
-#include <vector>
+#include "VectorOps.h"
+#include "Allocators.h"
 
 using std::vector;
 using std::cerr;
 using std::endl;
 
+using namespace breakfastquay;
+
 static double epsilon = 1e-16;
 
 EM::EM(bool useShifts) :
-    m_useShifts(useShifts),
     m_noteCount(SILVET_TEMPLATE_NOTE_COUNT),
     m_shiftCount(useShifts ? SILVET_TEMPLATE_MAX_SHIFT * 2 + 1 : 1),
     m_binCount(SILVET_TEMPLATE_HEIGHT),
-    m_instrumentCount(SILVET_TEMPLATE_COUNT),
+    m_sourceCount(SILVET_TEMPLATE_COUNT),
     m_pitchSparsity(1.1),
-    m_sourceSparsity(1.3)
-{
-    m_lowestPitch = silvet_templates_lowest_note;
-    m_highestPitch = silvet_templates_highest_note;
-
-    m_pitches = V(m_noteCount);
+    m_sourceSparsity(1.3),
+    m_lowestPitch(silvet_templates_lowest_note),
+    m_highestPitch(silvet_templates_highest_note)
+{
+    m_pitches = allocate<double>(m_noteCount);
+    m_updatePitches = allocate<double>(m_noteCount);
     for (int n = 0; n < m_noteCount; ++n) {
         m_pitches[n] = drand48();
     }
 
-    m_shifts = Grid(m_shiftCount);
-    for (int f = 0; f < m_shiftCount; ++f) {
-        m_shifts[f] = V(m_noteCount);
-        for (int n = 0; n < m_noteCount; ++n) {
-            if (m_useShifts) {
+    if (useShifts) {
+        m_shifts = allocate_channels<double>(m_shiftCount, m_noteCount);
+        m_updateShifts = allocate_channels<double>(m_shiftCount, m_noteCount);
+        for (int f = 0; f < m_shiftCount; ++f) {
+            for (int n = 0; n < m_noteCount; ++n) {
                 m_shifts[f][n] = drand48();
-            } else {
-                m_shifts[f][n] = 1.0;
-            }
-        }
+            }
+        }
+    } else {
+        m_shifts = 0;
+        m_updateShifts = 0;
     }
     
-    m_sources = Grid(m_instrumentCount);
-        for (int i = 0; i < m_instrumentCount; ++i) {
-        m_sources[i] = V(m_noteCount);
+    m_sources = allocate_channels<double>(m_sourceCount, m_noteCount);
+    m_updateSources = allocate_channels<double>(m_sourceCount, m_noteCount);
+    for (int i = 0; i < m_sourceCount; ++i) {
         for (int n = 0; n < m_noteCount; ++n) {
             m_sources[i][n] = (inRange(i, n) ? 1.0 : 0.0);
         }
     }
 
-    m_estimate = V(m_binCount);
-    m_q = V(m_binCount);
+    m_estimate = allocate<double>(m_binCount);
+    m_q = allocate<double>(m_binCount);
 }
 
 EM::~EM()
 {
+    deallocate(m_q);
+    deallocate(m_estimate);
+    deallocate_channels(m_sources, m_sourceCount);
+    deallocate_channels(m_updateSources, m_sourceCount);
+    deallocate_channels(m_shifts, m_shiftCount);
+    deallocate_channels(m_updateShifts, m_shiftCount);
+    deallocate(m_pitches);
+    deallocate(m_updatePitches);
 }
 
 void
 EM::rangeFor(int instrument, int &minPitch, int &maxPitch)
 {

     rangeFor(instrument, minPitch, maxPitch);
     return (pitch >= minPitch && pitch <= maxPitch);
 }
 
 void
-EM::normaliseColumn(V &column)
-{
-    double sum = 0.0;
-    for (int i = 0; i < (int)column.size(); ++i) {
-        sum += column[i];
-    }
-    for (int i = 0; i < (int)column.size(); ++i) {
-        column[i] /= sum;
-    }
-}
-
-void
-EM::normaliseGrid(Grid &grid)
-{
-    V denominators(grid[0].size());
-
-    for (int i = 0; i < (int)grid.size(); ++i) {
-        for (int j = 0; j < (int)grid[i].size(); ++j) {
+EM::normaliseColumn(double *column, int size)
+{
+    double sum = v_sum(column, size);
+    v_scale(column, 1.0 / sum, size);
+}
+
+void
+EM::normaliseGrid(double **grid, int size1, int size2)
+{
+    double *denominators = allocate_and_zero<double>(size2);
+
+    for (int i = 0; i < size1; ++i) {
+        for (int j = 0; j < size2; ++j) {
             denominators[j] += grid[i][j];
         }
     }
 
-    for (int i = 0; i < (int)grid.size(); ++i) {
-        for (int j = 0; j < (int)grid[i].size(); ++j) {
-            grid[i][j] /= denominators[j];
-        }
-    }
-}
-
-void
-EM::iterate(V column)
-{
-    normaliseColumn(column);
-    expectation(column);
-    maximisation(column);
-}
-
-const float *
+    for (int i = 0; i < size1; ++i) {
+        v_divide(grid[i], denominators, size2);
+    }
+
+    deallocate(denominators);
+}
+
+void
+EM::iterate(const double *column)
+{
+    double *norm = allocate<double>(m_binCount);
+    v_copy(norm, column, m_binCount);
+    normaliseColumn(norm, m_binCount);
+    expectation(norm);
+    maximisation(norm);
+    deallocate(norm);
+}
+
+const double *
 EM::templateFor(int instrument, int note, int shift)
 {
-    if (m_useShifts) {
+    if (m_shifts) {
         return silvet_templates[instrument].data[note] + shift;
     } else {
         return silvet_templates[instrument].data[note] + 
             SILVET_TEMPLATE_MAX_SHIFT;
     }
 }
 
 void
-EM::expectation(const V &column)
+EM::expectation(const double *column)
 {
 //    cerr << ".";
 
-    for (int i = 0; i < m_binCount; ++i) {
-        m_estimate[i] = epsilon;
-    }
-
-    for (int i = 0; i < m_instrumentCount; ++i) {
-        for (int n = 0; n < m_noteCount; ++n) {
+    v_set(m_estimate, epsilon, m_binCount);
+
+    for (int i = 0; i < m_sourceCount; ++i) {
+        for (int n = 0; n < m_noteCount; ++n) {
+            const double pitch = m_pitches[n];
+            const double source = m_sources[i][n];
             for (int f = 0; f < m_shiftCount; ++f) {
-                const float *w = templateFor(i, n, f);
-                double pitch = m_pitches[n];
-                double source = m_sources[i][n];
-                double shift = m_shifts[f][n];
-                for (int j = 0; j < m_binCount; ++j) {
-                    m_estimate[j] += w[j] * pitch * source * shift;
-                }
+                const double *w = templateFor(i, n, f);
+                const double shift = m_shifts ? m_shifts[f][n] : 1.0;
+                const double factor = pitch * source * shift;
+                v_add_with_gain(m_estimate, w, factor, m_binCount);
             }
         }
     }
 
     for (int i = 0; i < m_binCount; ++i) {
         m_q[i] = column[i] / m_estimate[i];
     }
 }
 
 void
-EM::maximisation(const V &column)
-{
-    V newPitches = m_pitches;
+EM::maximisation(const double *column)
+{
+    v_set(m_updatePitches, epsilon, m_noteCount);
+
+    for (int i = 0; i < m_sourceCount; ++i) {
+        v_set(m_updateSources[i], epsilon, m_noteCount);
+    }
+
+    if (m_shifts) {
+        for (int i = 0; i < m_shiftCount; ++i) {
+            v_set(m_updateShifts[i], epsilon, m_noteCount);
+        }
+    }
+
+    double *contributions = allocate<double>(m_binCount);
 
     for (int n = 0; n < m_noteCount; ++n) {
-        newPitches[n] = epsilon;
-        if (n >= m_lowestPitch && n <= m_highestPitch) {
-            for (int i = 0; i < m_instrumentCount; ++i) {
-                for (int f = 0; f < m_shiftCount; ++f) {
-                    const float *w = templateFor(i, n, f);
-                    double pitch = m_pitches[n];
-                    double source = m_sources[i][n];
-                    double shift = m_shifts[f][n];
-                    for (int j = 0; j < m_binCount; ++j) {
-                        newPitches[n] += w[j] * m_q[j] * pitch * source * shift;
+
+        const double pitch = m_pitches[n];
+
+        for (int f = 0; f < m_shiftCount; ++f) {
+
+            const double shift = m_shifts ? m_shifts[f][n] : 1.0;
+
+            for (int i = 0; i < m_sourceCount; ++i) {
+
+                const double source = m_sources[i][n];
+                const double factor = pitch * source * shift;
+                const double *w = templateFor(i, n, f);
+
+                v_copy(contributions, w, m_binCount);
+                v_multiply(contributions, m_q, m_binCount);
+
+                double total = factor * v_sum(contributions, m_binCount);
+
+                if (n >= m_lowestPitch && n <= m_highestPitch) {
+
+                    m_updatePitches[n] += total;
+
+                    if (inRange(i, n)) {
+                        m_updateSources[i][n] += total;
                     }
                 }
-            }
-        }
-        if (m_pitchSparsity != 1.0) {
-            newPitches[n] = pow(newPitches[n], m_pitchSparsity);
-        }
-    }
-    normaliseColumn(newPitches);
-
-    Grid newShifts = m_shifts;
-
-    if (m_useShifts) {
-        for (int f = 0; f < m_shiftCount; ++f) {
-            for (int n = 0; n < m_noteCount; ++n) {
-                newShifts[f][n] = epsilon;
-                for (int i = 0; i < m_instrumentCount; ++i) {
-                    const float *w = templateFor(i, n, f);
-                    double pitch = m_pitches[n];
-                    double source = m_sources[i][n];
-                    double shift = m_shifts[f][n];
-                    for (int j = 0; j < m_binCount; ++j) {
-                        newShifts[f][n] += w[j] * m_q[j] * pitch * source * shift;
-                    }
+
+                if (m_shifts) {
+                    m_updateShifts[f][n] += total;
                 }
             }
         }
-        normaliseGrid(newShifts);
-    }
-
-    Grid newSources = m_sources;
-
-    for (int i = 0; i < m_instrumentCount; ++i) {
-        for (int n = 0; n < m_noteCount; ++n) {
-            newSources[i][n] = epsilon;
-            if (inRange(i, n)) {
-                for (int f = 0; f < m_shiftCount; ++f) {
-                    const float *w = templateFor(i, n, f);
-                    double pitch = m_pitches[n];
-                    double source = m_sources[i][n];
-                    double shift = m_shifts[f][n];
-                    for (int j = 0; j < m_binCount; ++j) {
-                        newSources[i][n] += w[j] * m_q[j] * pitch * source * shift;
-                    }
-                }
-            }
-            if (m_sourceSparsity != 1.0) {
-                newSources[i][n] = pow(newSources[i][n], m_sourceSparsity);
-            }
-        }
-    }
-    normaliseGrid(newSources);
-
-    m_pitches = newPitches;
-    m_shifts = newShifts;
-    m_sources = newSources;
-}
-
-
+    }
+
+    if (m_pitchSparsity != 1.0) {
+        for (int n = 0; n < m_noteCount; ++n) {
+            m_updatePitches[n] = 
+                pow(m_updatePitches[n], m_pitchSparsity);
+        }
+    }
+
+    if (m_sourceSparsity != 1.0) {
+        for (int n = 0; n < m_noteCount; ++n) {
+            for (int i = 0; i < m_sourceCount; ++i) {
+                m_updateSources[i][n] =
+                    pow(m_updateSources[i][n], m_sourceSparsity);
+            }
+        }
+    }
+
+    normaliseColumn(m_updatePitches, m_noteCount);
+    std::swap(m_pitches, m_updatePitches);
+
+    normaliseGrid(m_updateSources, m_sourceCount, m_noteCount);
+    std::swap(m_sources, m_updateSources);
+
+    if (m_shifts) {
+        normaliseGrid(m_updateShifts, m_shiftCount, m_noteCount);
+        std::swap(m_shifts, m_updateShifts);
+    }
+}
+
+