From ea76761275b68744f05550ddab612f551baad75a Mon Sep 17 00:00:00 2001
From: Achilleas Anastasopoulos
Date: Fri, 18 Feb 2011 20:35:23 -0500
Subject: core algorithms such as viterbi/siso were refactored and implemented
 using templates. Minor renaming of some files

---
 gr-trellis/src/lib/core_algorithms.cc | 710 ++++++++++++++++++++++++++++++++++
 1 file changed, 710 insertions(+)
 create mode 100644 gr-trellis/src/lib/core_algorithms.cc

(limited to 'gr-trellis/src/lib/core_algorithms.cc')

diff --git a/gr-trellis/src/lib/core_algorithms.cc b/gr-trellis/src/lib/core_algorithms.cc
new file mode 100644
index 000000000..41ecaf174
--- /dev/null
+++ b/gr-trellis/src/lib/core_algorithms.cc
@@ -0,0 +1,710 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2004 Free Software Foundation, Inc.
+ * 
+ * This file is part of GNU Radio
+ * 
+ * GNU Radio is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3, or (at your option)
+ * any later version.
+ * 
+ * GNU Radio is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ * 
+ * You should have received a copy of the GNU General Public License
+ * along with GNU Radio; see the file COPYING.  If not, write to
+ * the Free Software Foundation, Inc., 51 Franklin Street,
+ * Boston, MA 02110-1301, USA.
+ */
+
+#include <float.h>
+#include <stdexcept>
+#include "core_algorithms.h"
+#include "calc_metric.h"
+
+static const float INF = 1.0e9;
+
+template <class T> 
+void viterbi_algorithm(int I, int S, int O, 
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             const float *in, T *out)//,
+             //std::vector<int> &trace) 
+{
+  std::vector<int> trace(S*K);
+  std::vector<float> alpha(S*2);
+  int alphai;
+  float norm,mm,minm;
+  int minmi;
+  int st;
+
+
+  if(S0<0) { // initial state not specified
+      for(int i=0;i<S;i++) alpha[0*S+i]=0;
+  }
+  else {
+      for(int i=0;i<S;i++) alpha[0*S+i]=INF;
+      alpha[0*S+S0]=0.0;
+  }
+
+  alphai=0;
+  for(int k=0;k<K;k++) {
+      norm=INF;
+      for(int j=0;j<S;j++) { // for each next state do ACS
+          minm=INF;
+          minmi=0;
+          for(unsigned int i=0;i<PS[j].size();i++) {
+              //int i0 = j*I+i;
+              if((mm=alpha[alphai*S+PS[j][i]]+in[k*O+OS[PS[j][i]*I+PI[j][i]]])<minm)
+                  minm=mm,minmi=i;
+          }
+          trace[k*S+j]=minmi;
+          alpha[((alphai+1)%2)*S+j]=minm;
+          if(minm<norm) norm=minm;
+      }
+      for(int j=0;j<S;j++) 
+          alpha[((alphai+1)%2)*S+j]-=norm; // normalize total metrics so they do not explode
+      alphai=(alphai+1)%2;
+  }
+
+  if(SK<0) { // final state not specified
+      minm=INF;
+      minmi=0;
+      for(int i=0;i<S;i++)
+          if((mm=alpha[alphai*S+i])<minm) minm=mm,minmi=i;
+      st=minmi;
+  }
+  else {
+      st=SK;
+  }
+
+  for(int k=K-1;k>=0;k--) { // traceback
+      int i0=trace[k*S+st];
+      out[k]= (T) PI[st][i0];
+      st=PS[st][i0];
+  }
+
+}
+
+
+template
+void viterbi_algorithm<unsigned char>(int I, int S, int O,
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             const float *in, unsigned char *out);
+
+
+template
+void viterbi_algorithm<short>(int I, int S, int O,
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             const float *in, short *out);
+
+template
+void viterbi_algorithm<int>(int I, int S, int O,
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             const float *in, int *out);
+
+
+
+//==============================================
+
+template <class Ti, class To>
+void viterbi_algorithm_combined(int I, int S, int O,
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             int D,
+             const std::vector<Ti> &TABLE,
+             trellis_metric_type_t TYPE,
+             const Ti *in, To *out
+)
+{
+  std::vector<int> trace(S*K);
+  std::vector<float> alpha(S*2);
+  float *metric = new float[O];
+  int alphai;
+  float norm,mm,minm;
+  int minmi;
+  int st;
+
+  if(S0<0) { // initial state not specified
+      for(int i=0;i<S;i++) alpha[0*S+i]=0;
+  }
+  else {
+      for(int i=0;i<S;i++) alpha[0*S+i]=INF;
+      alpha[0*S+S0]=0.0;
+  }
+
+  alphai=0;
+  for(int k=0;k<K;k++) {
+      calc_metric(O, D, TABLE, &(in[k*D]), metric,TYPE); // calc metrics
+      norm=INF;
+      for(int j=0;j<S;j++) { // for each next state do ACS
+          minm=INF;
+          minmi=0;
+          for(unsigned int i=0;i<PS[j].size();i++) {
+              //int i0 = j*I+i;
+              if((mm=alpha[alphai*S+PS[j][i]]+metric[OS[PS[j][i]*I+PI[j][i]]])<minm)
+                  minm=mm,minmi=i;
+          }
+          trace[k*S+j]=minmi;
+          alpha[((alphai+1)%2)*S+j]=minm;
+          if(minm<norm) norm=minm;
+      }
+      for(int j=0;j<S;j++) 
+          alpha[((alphai+1)%2)*S+j]-=norm; // normalize total metrics so they do not explode
+      alphai=(alphai+1)%2;
+  }
+
+  if(SK<0) { // final state not specified
+      minm=INF;
+      minmi=0;
+      for(int i=0;i<S;i++)
+          if((mm=alpha[alphai*S+i])<minm) minm=mm,minmi=i;
+      st=minmi;
+  }
+  else {
+      st=SK;
+  }
+
+  for(int k=K-1;k>=0;k--) { // traceback
+      int i0=trace[k*S+st];
+      out[k]= (To) PI[st][i0];
+      st=PS[st][i0];
+  }
+  
+  delete [] metric;
+
+}
+
+// Ti = s i f c
+// To = b s i
+
+//---------------
+
+template
+void viterbi_algorithm_combined<short,unsigned char>(int I, int S, int O,
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             int D,
+             const std::vector<short> &TABLE,
+             trellis_metric_type_t TYPE,
+             const short *in, unsigned char *out
+);
+
+template
+void viterbi_algorithm_combined<int,unsigned char>(int I, int S, int O,
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             int D,
+             const std::vector<int> &TABLE,
+             trellis_metric_type_t TYPE,
+             const int *in, unsigned char *out
+);
+
+template
+void viterbi_algorithm_combined<float,unsigned char>(int I, int S, int O,
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             int D,
+             const std::vector<float> &TABLE,
+             trellis_metric_type_t TYPE,
+             const float *in, unsigned char *out
+);
+
+template
+void viterbi_algorithm_combined<gr_complex,unsigned char>(int I, int S, int O,
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             int D,
+             const std::vector<gr_complex> &TABLE,
+             trellis_metric_type_t TYPE,
+             const gr_complex *in, unsigned char *out
+);
+
+//---------------
+
+template
+void viterbi_algorithm_combined<short,short>(int I, int S, int O,
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             int D,
+             const std::vector<short> &TABLE,
+             trellis_metric_type_t TYPE,
+             const short *in, short *out
+);
+
+template
+void viterbi_algorithm_combined<int,short>(int I, int S, int O,
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             int D,
+             const std::vector<int> &TABLE,
+             trellis_metric_type_t TYPE,
+             const int *in, short *out
+);
+
+template
+void viterbi_algorithm_combined<float,short>(int I, int S, int O,
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             int D,
+             const std::vector<float> &TABLE,
+             trellis_metric_type_t TYPE,
+             const float *in, short *out
+);
+
+template
+void viterbi_algorithm_combined<gr_complex,short>(int I, int S, int O,
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             int D,
+             const std::vector<gr_complex> &TABLE,
+             trellis_metric_type_t TYPE,
+             const gr_complex *in, short *out
+);
+
+//--------------
+
+template
+void viterbi_algorithm_combined<short,int>(int I, int S, int O,
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             int D,
+             const std::vector<short> &TABLE,
+             trellis_metric_type_t TYPE,
+             const short *in, int *out
+);
+
+template
+void viterbi_algorithm_combined<int,int>(int I, int S, int O,
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             int D,
+             const std::vector<int> &TABLE,
+             trellis_metric_type_t TYPE,
+             const int *in, int *out
+);
+
+template
+void viterbi_algorithm_combined<float,int>(int I, int S, int O,
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             int D,
+             const std::vector<float> &TABLE,
+             trellis_metric_type_t TYPE,
+             const float *in, int *out
+);
+
+template
+void viterbi_algorithm_combined<gr_complex,int>(int I, int S, int O,
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             int D,
+             const std::vector<gr_complex> &TABLE,
+             trellis_metric_type_t TYPE,
+             const gr_complex *in, int *out
+);
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+//===============================================
+
+inline float min(float a, float b)
+{
+  return a <= b ? a : b;
+}
+
+inline float min_star(float a, float b)
+{
+  return (a <= b ? a : b)-log(1+exp(a <= b ? a-b : b-a));
+  //return 0;
+}
+
+void siso_algorithm(int I, int S, int O, 
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             bool POSTI, bool POSTO,
+             float (*p2mymin)(float,float),
+             const float *priori, const float *prioro, float *post//,
+             //std::vector<float> &alpha,
+             //std::vector<float> &beta
+             ) 
+{
+  float norm,mm,minm;
+  std::vector<float> alpha(S*(K+1));
+  std::vector<float> beta(S*(K+1));
+
+
+  if(S0<0) { // initial state not specified
+      for(int i=0;i<S;i++) alpha[0*S+i]=0;
+  }
+  else {
+      for(int i=0;i<S;i++) alpha[0*S+i]=INF;
+      alpha[0*S+S0]=0.0;
+  }
+
+  for(int k=0;k<K;k++) { // forward recursion
+      norm=INF;
+      for(int j=0;j<S;j++) {
+          minm=INF;
+          for(unsigned int i=0;i<PS[j].size();i++) {
+              //int i0 = j*I+i;
+              mm=alpha[k*S+PS[j][i]]+priori[k*I+PI[j][i]]+prioro[k*O+OS[PS[j][i]*I+PI[j][i]]];
+              minm=(*p2mymin)(minm,mm);
+          }
+          alpha[(k+1)*S+j]=minm;
+          if(minm<norm) norm=minm;
+      }
+      for(int j=0;j<S;j++) 
+          alpha[(k+1)*S+j]-=norm; // normalize total metrics so they do not explode
+  }
+
+  if(SK<0) { // final state not specified
+      for(int i=0;i<S;i++) beta[K*S+i]=0;
+  }
+  else {
+      for(int i=0;i<S;i++) beta[K*S+i]=INF;
+      beta[K*S+SK]=0.0;
+  }
+
+  for(int k=K-1;k>=0;k--) { // backward recursion
+      norm=INF;
+      for(int j=0;j<S;j++) { 
+          minm=INF;
+          for(int i=0;i<I;i++) {
+              int i0 = j*I+i;
+              mm=beta[(k+1)*S+NS[i0]]+priori[k*I+i]+prioro[k*O+OS[i0]];
+              minm=(*p2mymin)(minm,mm);
+          }
+          beta[k*S+j]=minm;
+          if(minm<norm) norm=minm;
+      }
+      for(int j=0;j<S;j++)
+          beta[k*S+j]-=norm; // normalize total metrics so they do not explode
+  }
+
+
+if (POSTI && POSTO)
+{
+  for(int k=0;k<K;k++) { // input combining
+      norm=INF;
+      for(int i=0;i<I;i++) {
+          minm=INF;
+          for(int j=0;j<S;j++) {
+              mm=alpha[k*S+j]+prioro[k*O+OS[j*I+i]]+beta[(k+1)*S+NS[j*I+i]];
+              minm=(*p2mymin)(minm,mm);
+          }
+          post[k*(I+O)+i]=minm;
+          if(minm<norm) norm=minm;
+      }
+      for(int i=0;i<I;i++)
+          post[k*(I+O)+i]-=norm; // normalize metrics
+  }
+
+
+  for(int k=0;k<K;k++) { // output combining
+      norm=INF;
+      for(int n=0;n<O;n++) {
+          minm=INF;
+          for(int j=0;j<S;j++) {
+              for(int i=0;i<I;i++) {
+                  mm= (n==OS[j*I+i] ? alpha[k*S+j]+priori[k*I+i]+beta[(k+1)*S+NS[j*I+i]] : INF);
+                  minm=(*p2mymin)(minm,mm);
+              }
+          }
+          post[k*(I+O)+I+n]=minm;
+          if(minm<norm) norm=minm;
+      }
+      for(int n=0;n<O;n++)
+          post[k*(I+O)+I+n]-=norm; // normalize metrics
+  }
+} 
+else if(POSTI) 
+{
+  for(int k=0;k<K;k++) { // input combining
+      norm=INF;
+      for(int i=0;i<I;i++) {
+          minm=INF;
+          for(int j=0;j<S;j++) {
+              mm=alpha[k*S+j]+prioro[k*O+OS[j*I+i]]+beta[(k+1)*S+NS[j*I+i]];
+              minm=(*p2mymin)(minm,mm);
+          }
+          post[k*I+i]=minm;
+          if(minm<norm) norm=minm;
+      }
+      for(int i=0;i<I;i++)
+          post[k*I+i]-=norm; // normalize metrics
+  }
+}
+else if(POSTO)
+{
+  for(int k=0;k<K;k++) { // output combining
+      norm=INF;
+      for(int n=0;n<O;n++) {
+          minm=INF;
+          for(int j=0;j<S;j++) {
+              for(int i=0;i<I;i++) {
+                  mm= (n==OS[j*I+i] ? alpha[k*S+j]+priori[k*I+i]+beta[(k+1)*S+NS[j*I+i]] : INF);
+                  minm=(*p2mymin)(minm,mm);
+              }
+          }
+          post[k*O+n]=minm;
+          if(minm<norm) norm=minm;
+      }
+      for(int n=0;n<O;n++)
+          post[k*O+n]-=norm; // normalize metrics
+  }
+}
+else
+    throw std::runtime_error ("Not both POSTI and POSTO can be false.");
+
+}
+
+
+//===========================================================
+
+void siso_algorithm_combined(int I, int S, int O, 
+             const std::vector<int> &NS,
+             const std::vector<int> &OS,
+             const std::vector< std::vector<int> > &PS,
+             const std::vector< std::vector<int> > &PI,
+             int K,
+             int S0,int SK,
+             bool POSTI, bool POSTO,
+             float (*p2mymin)(float,float),
+             int D,
+             const std::vector<float> &TABLE,
+             trellis_metric_type_t TYPE,
+             const float *priori, const float *observations, float *post//,
+             //std::vector<float> &alpha,
+             //std::vector<float> &beta
+             ) 
+{
+  float norm,mm,minm;
+  std::vector<float> alpha(S*(K+1));
+  std::vector<float> beta(S*(K+1));
+  float *prioro = new float[O*K];
+
+
+  if(S0<0) { // initial state not specified
+      for(int i=0;i<S;i++) alpha[0*S+i]=0;
+  }
+  else {
+      for(int i=0;i<S;i++) alpha[0*S+i]=INF;
+      alpha[0*S+S0]=0.0;
+  }
+
+  for(int k=0;k<K;k++) { // forward recursion
+      calc_metric(O, D, TABLE, &(observations[k*D]), &(prioro[k*O]),TYPE); // calc metrics
+      norm=INF;
+      for(int j=0;j<S;j++) {
+          minm=INF;
+          for(unsigned int i=0;i<PS[j].size();i++) {
+              //int i0 = j*I+i;
+              mm=alpha[k*S+PS[j][i]]+priori[k*I+PI[j][i]]+prioro[k*O+OS[PS[j][i]*I+PI[j][i]]];
+              minm=(*p2mymin)(minm,mm);
+          }
+          alpha[(k+1)*S+j]=minm;
+          if(minm<norm) norm=minm;
+      }
+      for(int j=0;j<S;j++) 
+          alpha[(k+1)*S+j]-=norm; // normalize total metrics so they do not explode
+  }
+
+  if(SK<0) { // final state not specified
+      for(int i=0;i<S;i++) beta[K*S+i]=0;
+  }
+  else {
+      for(int i=0;i<S;i++) beta[K*S+i]=INF;
+      beta[K*S+SK]=0.0;
+  }
+
+  for(int k=K-1;k>=0;k--) { // backward recursion
+      norm=INF;
+      for(int j=0;j<S;j++) { 
+          minm=INF;
+          for(int i=0;i<I;i++) {
+              int i0 = j*I+i;
+              mm=beta[(k+1)*S+NS[i0]]+priori[k*I+i]+prioro[k*O+OS[i0]];
+              minm=(*p2mymin)(minm,mm);
+          }
+          beta[k*S+j]=minm;
+          if(minm<norm) norm=minm;
+      }
+      for(int j=0;j<S;j++)
+          beta[k*S+j]-=norm; // normalize total metrics so they do not explode
+  }
+
+
+  if (POSTI && POSTO)
+  {
+    for(int k=0;k<K;k++) { // input combining
+        norm=INF;
+        for(int i=0;i<I;i++) {
+            minm=INF;
+            for(int j=0;j<S;j++) {
+                mm=alpha[k*S+j]+prioro[k*O+OS[j*I+i]]+beta[(k+1)*S+NS[j*I+i]];
+                minm=(*p2mymin)(minm,mm);
+            }
+            post[k*(I+O)+i]=minm;
+            if(minm<norm) norm=minm;
+        }
+        for(int i=0;i<I;i++)
+            post[k*(I+O)+i]-=norm; // normalize metrics
+    }
+
+
+    for(int k=0;k<K;k++) { // output combining
+        norm=INF;
+        for(int n=0;n<O;n++) {
+            minm=INF;
+            for(int j=0;j<S;j++) {
+                for(int i=0;i<I;i++) {
+                    mm= (n==OS[j*I+i] ? alpha[k*S+j]+priori[k*I+i]+beta[(k+1)*S+NS[j*I+i]] : INF);
+                    minm=(*p2mymin)(minm,mm);
+                }
+            }
+            post[k*(I+O)+I+n]=minm;
+            if(minm<norm) norm=minm;
+        }
+        for(int n=0;n<O;n++)
+            post[k*(I+O)+I+n]-=norm; // normalize metrics
+    }
+  } 
+  else if(POSTI) 
+  {
+    for(int k=0;k<K;k++) { // input combining
+        norm=INF;
+        for(int i=0;i<I;i++) {
+            minm=INF;
+            for(int j=0;j<S;j++) {
+                mm=alpha[k*S+j]+prioro[k*O+OS[j*I+i]]+beta[(k+1)*S+NS[j*I+i]];
+                minm=(*p2mymin)(minm,mm);
+            }
+            post[k*I+i]=minm;
+            if(minm<norm) norm=minm;
+        }
+        for(int i=0;i<I;i++)
+            post[k*I+i]-=norm; // normalize metrics
+    }
+  }
+  else if(POSTO)
+  {
+    for(int k=0;k<K;k++) { // output combining
+        norm=INF;
+        for(int n=0;n<O;n++) {
+            minm=INF;
+            for(int j=0;j<S;j++) {
+                for(int i=0;i<I;i++) {
+                    mm= (n==OS[j*I+i] ? alpha[k*S+j]+priori[k*I+i]+beta[(k+1)*S+NS[j*I+i]] : INF);
+                    minm=(*p2mymin)(minm,mm);
+                }
+            }
+            post[k*O+n]=minm;
+            if(minm<norm) norm=minm;
+        }
+        for(int n=0;n<O;n++)
+            post[k*O+n]-=norm; // normalize metrics
+    }
+  }
+  else
+    throw std::runtime_error ("Not both POSTI and POSTO can be false.");
+
+  delete [] prioro;
+
+}
+
-- 
cgit