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