Index: /branches/czw_branch/20110406/psModules/src/imcombine/pmStack.c
===================================================================
--- /branches/czw_branch/20110406/psModules/src/imcombine/pmStack.c	(revision 31606)
+++ /branches/czw_branch/20110406/psModules/src/imcombine/pmStack.c	(revision 31607)
@@ -400,4 +400,5 @@
 	CHECKPIX(x, y, "keep: %d : %x (badMask = %x)\n", i, mask->data.PS_TYPE_IMAGE_MASK_DATA[yIn][xIn], *badMask);
     }
+    
     pixelData->n = numGood;
     if (variance) {
@@ -976,4 +977,182 @@
 }
 
+// KMM functions to do bimodality rejection of pixels
+
+float gaussian(float x, float m, float s) {
+  return(pow(s * sqrt(2 * M_PI),-1) * exp(-0.5 * pow( (x - m) / s, 2)));
+}
+
+static void KMMcalculate(const psVector *values,
+			 float *Punimodal,
+			 float *pi1, float *m1, float *s1,
+			 float *pi2, float *m2, float *s2) {
+  double logL_bimodal = 0, logL_unimodal;
+  float mU,sU;
+  psVector *P1 = psVectorAlloc(values->n,PS_TYPE_F32);
+  psVector *P2 = psVectorAlloc(values->n,PS_TYPE_F32);
+  int i;
+  
+  // Calculate unimodal properties
+  mU = 0;
+  sU = 0;
+  logL_unimodal = 0;
+  for (i = 0; i < values->n; i++) { // Calculate mean
+    mU += values->data.F32[i];
+  }
+  mU /= values->n;
+  for (i = 0; i < values->n; i++) { // Calculate sigma
+    sU += pow(values->data.F32[i] - mU,2);
+  }
+  sU = sqrt(sU / values->n);
+  for (i = 0; i < values->n; i++) { // Calculate log likelihood
+    logL_unimodal += log(gaussian(values->data.F32[i],mU,sU));
+  }
+
+  // Do EM loop
+  float dL = 0;
+  float oldL = -999;
+  int k = 0;
+  logL_bimodal = logL_unimodal;
+  *m1 = mU - 3 * sU;
+  *m2 = mU + 3 * sU;
+  *s1 = sU / 2;
+  *s2 = sU / 2;
+  *pi1 = 0.5;
+  *pi2 = 0.5;
+
+  float g1,g2,norm;
+  float w1,w2;
+
+  while (((dL > KMM_TOLERANCE)||(k < 3))&&(k < KMM_MAX_ITERATIONS)) {
+    k++;
+    dL = fabs(logL_bimodal - oldL);
+    oldL = logL_bimodal;
+
+    // Expectation/P-stage
+    for (i = 0; i < values->n; i++) { // Calculate probabilities for each mode
+      g1 = gaussian(values->data.F32[i],*m1,*s1);
+      g2 = gaussian(values->data.F32[i],*m2,*s2);
+      norm = (*pi1 * g1 + *pi2 * g2);
+      P1->data.F32[i] = (*pi1 * g1) / norm;
+      P2->data.F32[i] = (*pi2 * g2) / norm;
+    }
+    // Maximization/M-stage
+    logL_bimodal = 0;
+    w1 = 0;
+    w2 = 0;
+    for (i = 0; i < values->n; i++) { // Calculate log likelihood
+      if (!((*pi1 == 0)||(*pi2 == 0))) {
+	logL_bimodal += log(*pi1 * gaussian(values->data.F32[i],*m1,*s1) +
+			    *pi2 * gaussian(values->data.F32[i],*m2,*s2));
+      }
+    }
+    *m1 = 0;
+    *m2 = 0;
+    *s1 = 0;
+    *s2 = 0;
+    for (i = 0; i < values->n; i++) { // Calculate new means
+      *m1 += values->data.F32[i] * P1->data.F32[i];
+      *m2 += values->data.F32[i] * P2->data.F32[i];
+
+      w1 += P1[i];
+      w2 += P2[i];
+    }
+    *m1 /= w1;
+    *m2 /= w2;
+    for (i = 0; i < values->n; i++) { // Calculate new sigmas
+      *s1 += pow(values->data.F32[i] - *m1,2) * P1->data.F32[i];
+      *s2 += pow(values->data.F32[i] - *m2,2) * P2->data.F32[i];
+    }
+    *s1 = sqrt(*s1 / w1);
+    *s2 = sqrt(*s2 / w2);
+
+    *pi1 = w1 / values->n;
+    *pi2 = w2 / values->n;
+
+    if (!isfinite(*pi1)) { // finite checks
+      *pi1 = 0.0;
+    }
+    if (!isfinite(*pi2)) { // finite checks
+      *pi2 = 0.0;
+    }
+    if (*s1 == 0) { // sigma may not be zero
+      *s1 = KMM_SMALL_NUMBER * *m1;
+    }
+    if (*s2 == 0) { // sigma may not be zero
+      *s2 = KMM_SMALL_NUMBER * *m2;
+    }
+  } // End EM phase
+
+  // Calculate Punimodal
+  double lambda = -2.0 * (logL_unimodal - logL_bimodal);
+  int    df     = 2 + 2 * 1;
+  if (lambda > 0) {
+    *Punimodal = gsl_cdf_chisq_Q(lambda,df);
+  }
+  else {
+    *Punimodal = 1.0;
+  }  
+}
+
+static void KMMrejectUnpopular(const psVector *values, psArray *reject) {
+  float Punimodal,pi1,m1,s1,pi2,m2,s2;
+  KMMcalculate(values,&Punimodal,
+	       &pi1,&m1,&s1,
+	       &pi2,&m2,&s2);
+  if (Punimodal < KMM_MINIMUM_PVALUE) {
+    int i;
+    float g1,g2;
+    float P1,P2;
+
+    for (i = 0; i < values->n; i++) { // Calculate probabilities for each mode
+      g1 = gaussian(values->data.F32[i],m1,s1);
+      g2 = gaussian(values->data.F32[i],m2,s2);
+      norm = (pi1 * g1 + pi2 * g2);
+      P1 = (pi1 * g1) / norm;
+      P2 = (pi2 * g2) / norm;
+
+      if ((pi1 > pi2)&&(P1 < P2)) { // mode 1 is more popular, but this element belongs to mode 2
+	reject_input(reject,i);
+      }
+      if ((pi1 < pi2)&&(P1 > P2)) { // mode 2 is more popular, but this element belongs to mode 1
+	reject_input(reject,i);
+      }
+    }
+  }
+  // else do nothing.
+}
+
+static void KMMrejectBright(const psVector *values, psArray *reject) {
+  KMMcalculate(values,&Punimodal,
+	       &pi1,&m1,&s1,
+	       &pi2,&m2,&s2);
+  if (Punimodal < KMM_MINIMUM_PVALUE) {
+    int i;
+    float g1,g2;
+    float P1,P2;
+
+    for (i = 0; i < values->n; i++) { // Calculate probabilities for each mode
+      g1 = gaussian(values->data.F32[i],m1,s1);
+      g2 = gaussian(values->data.F32[i],m2,s2);
+      norm = (pi1 * g1 + pi2 * g2);
+      P1 = (pi1 * g1) / norm;
+      P2 = (pi2 * g2) / norm;
+
+      if ((m1 > m2)&&(P1 > P2)) { // m1 is larger, and this element belongs to mode 1
+	reject_input(reject,i);
+      }
+      if ((m1 < m2)&&(P1 < P2)) { // m2 is larger, and this element belongs to mode 2
+	reject_input(reject,i);
+      }
+    }
+  }
+  // else do nothing.
+}
+			       
+			       
+    
+  
+  
+  
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////