server/interferometer.c

   1 /**
   2  * @file interferometer.c
   3  * @purpose Implementation of interferometer related functions
   4  */
   5
   6 #include "cv.h"
   7 #include "highgui_c.h"
   8 #include "interferometer.h"
   9 #include "analysis.h"
  10 #include <math.h>
  11
  12 /** Buffer for storing image data. Stored as a single intensity value for the laser light **/
  13 static CvMat * g_data = NULL;
  14
  15
  16 /** Camera capture pointer **/
  17 static CvCapture * g_capture = NULL;
  18
  19
  20 struct timeval start;
  21
  22 #define PI 3.141592
  23
  24 // For testing purposes
  25 double test_omega = 0.05;
  26 double test_angle = PI/2;
  27 double test_noise[] = {0,0,0.02};
  28 double test_phase = 0;
  29 double test_intensity = 1.0;
  30
  31
  32 static void Interferometer_TestSinusoid()
  33 {
  34         if (g_capture == NULL)
  35         {
  36                 g_capture = cvCreateCameraCapture(0);
  37         }
  38
  39         // Get image from camera
  40         IplImage * img = cvQueryFrame(g_capture);
  41
  42         // Convert to CvMat
  43         CvMat stub;
  44         CvMat * background = cvGetMat(img, &stub, 0, 0);
  45         // ... Honestly, I have no idea what the "stub" is for
  46
  47         if (g_data == NULL)
  48         {
  49                 g_data = cvCreateMat(background->rows, background->cols, CV_32FC3);
  50         }
  51
  52         //cvShowImage("background", background);
  53
  54         for (int x = 0; x < g_data->cols-1; ++x)
  55         {
  56                 for (int y = 0; y < g_data->rows-1; ++y)
  57                 {
  58                         // Calculate pure sine in test direction
  59                         double r = x*cos(test_angle) + y*sin(test_angle);
  60                         double value = 0.5*test_intensity*(1+sin(test_omega*r + test_phase));
  61
  62                         CvScalar s;
  63                         s.val[0] = 0; s.val[1] = 0; s.val[2] = value;
  64                         CvScalar b = cvGet2D(background, y, x);
  65
  66
  67                         // Add noise & background image
  68
  69                         // Get the order the right way round
  70                         double t = b.val[0];
  71                         b.val[0] = b.val[2];
  72                         b.val[2] = t;
  73                         for (int i = 0; i < 3; ++i)
  74                         {
  75
  76                                 s.val[i] += (rand() % 1000) * 1e-3 * test_noise[i];
  77                                 s.val[i] += b.val[i] / 255; // Camera image is 0-255
  78                         }
  79
  80                         //printf("set %d,%d\n", x, y);
  81
  82
  83
  84                         cvSet2D(g_data,y, x, s);
  85                 }
  86         }
  87
  88
  89 }
  90
  91
  92 /**
  93  * Get an image from the Interferometer
  94  */
  95 static void Interferometer_GetImage()
  96 {
  97
  98
  99         Interferometer_TestSinusoid();
 100         //TODO: Implement camera
 101
 102 }
 103
 104 /**
 105  * Initialise the Interferometer
 106  */
 107 void Interferometer_Init()
 108 {
 109
 110         // Make an initial reading (will allocate memory the first time only).
 111         Interferometer_Read(1);
 112 }
 113
 114 /**
 115  * Cleanup Interferometer stuff
 116  */
 117 void Interferometer_Cleanup()
 118 {
 119         if (g_data != NULL)
 120                 cvReleaseMat(&g_data);
 121
 122         if (g_capture != NULL)
 123                 cvReleaseCapture(&g_capture);
 124
 125 }
 126
 127 /**
 128  * Read the interferometer; gets the latest image, processes it, spits out a single number
 129  * @param samples - Number of columns to scan (increasing will slow down performance!)
 130  * @returns Value proportional to the change in interferometer path length since the last call to this function
 131  */
 132 double Interferometer_Read(int samples)
 133 {
 134
 135
 136
 137
 138         // Get the latest image
 139         Interferometer_GetImage();
 140         // Frequency of the sinusoid
 141         static double omega = 0;
 142         // Stores locations of nodes
 143         static int nodes[MAXNODES];
 144         // Current phase
 145         static double phase = 0;
 146
 147         // Phase
 148         double cur_phase = 0;
 149
 150         int xstep = g_data->cols / (samples+1);
 151
 152         // Used for testing to see where the nodes are identified
 153         // (Can't modify g_data)
 154         static CvMat * test_overlay = NULL;
 155         if (test_overlay == NULL)
 156         {
 157                 // Creates a memory leak; don't do this in the final version!
 158                 test_overlay = cvCreateMat(g_data->rows, g_data->cols, CV_32FC3);
 159         }
 160         cvZero(test_overlay);
 161         //cvCopy(g_data, test_overlay, NULL);
 162
 163         // For each column to sample
 164         for (int x = xstep; x < g_data->cols; x += xstep)
 165         {
 166
 167                 double avg = 0.5; //TODO: Calculate from image
 168                 double threshold_dif = 0; //TODO: Pick this value
 169
 170                 int num_nodes = 0;
 171
 172                 // Find nodes
 173                 for (int y = 1; y < g_data->rows-2 && num_nodes < MAXNODES; ++y)
 174                 {
 175                         if (num_nodes == 0 || abs(nodes[num_nodes-1] - y) > 1)
 176                         {
 177                                 // A "node" is defined where the ajacent points are on opposite sides of the avg
 178                                 double ldif = INTENSITY(cvGet2D(g_data, y-1,x)) - avg;
 179                                 double rdif = INTENSITY(cvGet2D(g_data, y+1,x)) - avg;
 180
 181                                 // If that is the case, the product of the differences will be negative
 182                                 if (ldif * rdif < -threshold_dif)
 183                                 {
 184
 185                                         nodes[num_nodes++] = y;
 186
 187                                         // Put a white line on the overlay to indicate the node was found
 188                                         for (int xx = 0; xx < g_data->cols; ++xx)
 189                                         {
 190                                                 CvScalar s; // = cvGet2D(g_data, y, xx);
 191                                                 s.val[0] = 1; s.val[1] = 1; s.val[2] = 1;
 192                                                 cvSet2D(test_overlay, y, xx, s);
 193                                         }
 194                                 }
 195                         }
 196                 }
 197
 198                 // Insufficient nodes found to continue
 199                 if (num_nodes < 2)
 200                 {
 201                         --samples;
 202                         continue;
 203                 }
 204
 205                 // Estimate angular frequency from two nodes TODO: Average between nodes
 206                 double slice_omega = (PI*(num_nodes-1)) / (nodes[num_nodes-1] - nodes[0]);
 207                 //printf("SLICE: %f vs %f\n", slice_omega, test_omega);
 208
 209                 double slice_phase = 0;
 210                 for (int i = 0; i < num_nodes; ++i)
 211                 {
 212                         double this_phase = ((double)(i)*PI - slice_omega * nodes[i]);
 213                         //printf("Node %d gives phase %f\n", i, this_phase);
 214                         slice_phase += this_phase;
 215                 }
 216                 slice_phase /= num_nodes;
 217                 cur_phase += slice_phase;
 218         }
 219
 220         // Average over samples
 221         if (samples == 0)
 222                 return 0;
 223
 224         cur_phase /= samples;
 225
 226
 227
 228         // Get phase change since last call, save current phase
 229         double result = (cur_phase - phase);
 230
 231         // HACK
 232         if (abs(result) > 0.5*PI)
 233         {
 234                 if (result > 0)
 235                         result -= PI;
 236                 else
 237                         result += PI;
 238         }
 239         phase = cur_phase;
 240
 241         // Display the image with lines to indicate results of data processing
 242         //cvShowImage("nodes", test_overlay);
 243         //cvWaitKey(1);
 244         cvAdd(test_overlay, g_data, test_overlay, NULL);
 245         cvShowImage("overlay", test_overlay);
 246         cvWaitKey(1);
 247         return result;
 248
 249 }
 250
 251 /**
 252  * For testing purposes
 253  */
 254 int main(int argc, char ** argv)
 255 {
 256         //cvNamedWindow( "display", CV_WINDOW_AUTOSIZE );// Create a window for display.
 257         gettimeofday(&start, NULL);
 258
 259         //Interferometer_Read(1);
 260         //exit(EXIT_SUCCESS);
 261
 262         Interferometer_Init();
 263
 264         double sum = 0;
 265         double last_phase = test_phase;
 266
 267         struct timeval now;
 268         double time = 0;
 269
 270         while (time < 20)
 271         {
 272                 gettimeofday(&now, NULL);
 273                 time = TIMEVAL_DIFF(now, start);
 274
 275                 test_phase = 0.5*PI*sin(time);
 276
 277
 278                 double delta = Interferometer_Read(1);
 279                 sum += delta;
 280
 281                 printf("%f\t%f\t%f\t%f\t%f\n", time, test_phase - last_phase, test_phase, delta, sum);
 282
 283                 last_phase = test_phase;
 284                 //break;
 285         }
 286
 287 }
 288
 289