server/sensors/dilatometer.c

   1 /**
   2  * @file dilatometer.c
   3  * @purpose Implementation of dilatometer related functions
   4  */
   5
   6 #include "cv.h"
   7 #include "highgui_c.h"
   8 #include "dilatometer.h"
   9 #include "../image.h"
  10 #include <math.h>
  11
  12 // test positions
  13 static double test_left, test_right;
  14
  15 // Remembers the last position to measure rate of expansion
  16 static double lastPosition;
  17
  18
  19 /** Buffers for storing image data.  **/
  20 static CvMat * g_srcRGB  = NULL;        // Source Image
  21 static CvMat * g_srcGray = NULL;        // Gray scale of source image
  22 static CvMat * g_edges   = NULL;        // Detected Edges
  23
  24 /** Pointers for capturing image **/
  25 //static CvCapture * g_capture = NULL;
  26 //static IplImage * frame  = NULL;      // This is required as you can not use capture with CvMat in C
  27
  28
  29 /**
  30  * Create a test image using left as left edge and right as right edge positions
  31  */
  32 void Dilatometer_TestImage()
  33 {
  34
  35         g_srcRGB = cvCreateMat(480, 640, CV_8UC3);
  36
  37         for( int x = 0; x < 640; ++x)
  38         {
  39                 for (int y = 0; y < 480; ++y)
  40                 {
  41                         CvScalar s;
  42                         for( int i = 0; i < 3; ++i)
  43                         {
  44                                 s.val[i]  =  210 + (rand() % 1000) * 1e-0 - (rand() % 1000) * 1e-0;
  45                                 // Produce an exponential decay around left edge
  46                                 if( x < test_left)
  47                                         s.val[i] *= exp( (x - test_left) / 25);
  48                                 else if( x < 320)
  49                                         s.val[i] *= exp( (test_left - x) / 25);
  50                                 // Produce an exponential decay around right edge
  51                                 else if( x < test_right)
  52                                         s.val[i] *= exp( (x - test_right) / 25);
  53                                 else
  54                                         s.val[i] *= exp( (test_right - x) / 25);
  55                         }
  56                         cvSet2D(g_srcRGB,y,x,s);
  57                 }
  58
  59         }
  60         if (g_srcGray == NULL)
  61         {
  62                 g_srcGray = cvCreateMat(g_srcRGB->rows,g_srcRGB->cols,CV_8UC1);
  63         }
  64         cvCvtColor(g_srcRGB,g_srcGray,CV_RGB2GRAY);
  65 }
  66
  67 /**
  68  * Cleanup Dilatometer pointers
  69  */
  70 bool Dilatometer_Cleanup(int id)
  71 {
  72         //if (g_capture != NULL)
  73         //      cvReleaseCapture(&g_capture);
  74         //if (frame != NULL)
  75         //      cvReleaseImageHeader(&frame);
  76
  77         //if (g_srcRGB != NULL)
  78         //      cvReleaseMat(&g_srcRGB);        // Causing run time error in cvReleaseMat
  79         if (g_srcGray != NULL)
  80                 cvReleaseMat(&g_srcGray);
  81         if (g_edges != NULL)
  82                 cvReleaseMat(&g_edges);
  83         return true;
  84 }
  85
  86 /**
  87  * Get an image from the Dilatometer. Replaced by Camera_GetImage in image.c
  88  */
  89 /*static bool Dilatometer_GetImage()
  90 {
  91         bool result = true;
  92         // If more than one camera is connected, then input needs to be determined, however the camera ID may change after being unplugged
  93         if( g_capture == NULL)
  94         {
  95                 g_capture = cvCreateCameraCapture(0);
  96                 //If cvCreateCameraCapture returns NULL there is an error with the camera
  97                 if( g_capture == NULL)
  98                 {
  99                         result = false;
 100                         return;
 101                 }
 102         }
 103
 104         // Get the frame and convert it to CvMat
 105         frame =  cvQueryFrame(g_capture);
 106         CvMat stub;
 107         g_srcRGB = cvGetMat(frame,&stub,0,0);
 108
 109         if( g_srcRGB == NULL)
 110                 result = false;
 111
 112         // Convert the image to grayscale
 113         if (g_srcGray == NULL)
 114         {
 115                 g_srcGray = cvCreateMat(g_srcRGB->rows,g_srcRGB->cols,CV_8UC1);
 116         }
 117
 118         cvCvtColor(g_srcRGB,g_srcGray,CV_RGB2GRAY);
 119
 120         return result;
 121 }*/
 122
 123 void CannyThreshold()
 124 {
 125
 126         // Create greyscale array
 127         if (g_srcGray == NULL)
 128         {
 129                 Log(LOGDEBUG, "%d %d %d", g_srcRGB->rows, g_srcRGB->cols, CV_8UC1);
 130                 g_srcGray = cvCreateMat(g_srcRGB->rows,g_srcRGB->cols,CV_8UC1);
 131         }
 132
 133         // Convert the RGB source file to grayscale
 134         Log(LOGDEBUG, "About to cvCvtColor(%p, %p, %d)", g_srcRGB, g_srcGray, CV_RGB2GRAY);
 135         cvCvtColor(g_srcRGB,g_srcGray,CV_RGB2GRAY);
 136
 137
 138         Log(LOGDEBUG, "About to cvCreateMat");
 139         if ( g_edges == NULL)
 140         {
 141                 g_edges = cvCreateMat(g_srcGray->rows,g_srcGray->cols,CV_8UC1);
 142         }
 143
 144         // Commented out lines are used during testing to show the image to screen, can also save the test images
 145         //cvShowImage("display", g_srcGray);
 146         //cvWaitKey(0);
 147
 148         // Reduce noise with a kernel blurxblur. Input the grayscale source image, output to edges. (0's mean it's determined from kernel sizes)
 149         cvSmooth( g_srcGray, g_edges, CV_GAUSSIAN, BLUR, BLUR ,0 ,0 );
 150
 151         //Save the image
 152         //cvSaveImage("test_blurred.jpg",g_edges,0);
 153
 154         //cvShowImage("display", g_edges);
 155         //cvWaitKey(0);
 156
 157         // Find the edges in the image
 158         cvCanny( g_edges, g_edges, LOWTHRESHOLD, LOWTHRESHOLD*RATIO, KERNELSIZE );
 159
 160         //Save the image
 161         //cvSaveImage("test_edge.jpg",g_edges,0);
 162
 163         //cvShowImage("display", g_edges);
 164         //cvWaitKey(0);
 165
 166 }
 167
 168  /**
 169  * Read the dilatometer image. The value changed will correspond to the rate of expansion. If no edge is found then
 170  * @param val - Will store the read value if successful
 171  * @param samples - Number of rows to scan (increasing will slow down performance!)
 172  * @returns true on successful read
 173  */
 174 bool Dilatometer_GetExpansion( int id, double * value, int samples)
 175 {
 176         bool result = false;
 177         double average = 0;
 178         // Get the image from the camera
 179         Log(LOGDEBUG, "GET IMAGE?");
 180
 181         IplImage * frame = NULL;
 182         result = Camera_GetImage( 0, 1600, 1200 ,&frame); // Get a 1600x1200 image and place it into src
 183         Log(LOGDEBUG, "Got image...");
 184
 185         // If an error occured when capturing image then return
 186
 187
 188         if (result)
 189         {
 190                 CvMat stub;
 191                 g_srcRGB = cvGetMat(frame,&stub,0,0);
 192                 result = (g_srcRGB != NULL);
 193                 Log(LOGDEBUG, "Converted image %d %p", result, g_srcRGB);
 194         }
 195         cvReleaseImageHeader(&frame);
 196
 197         if (!result)
 198                 return result;
 199
 200
 201         Log(LOGDEBUG, "GOT IMAGE (without error)!");
 202
 203         // Apply the Canny Edge theorem to the image
 204         CannyThreshold();
 205
 206         Log(LOGDEBUG, "Got past CannyThreshold()");
 207
 208         int width = g_edges->cols;
 209         int height = g_edges->rows;
 210
 211         // If the number of samples is greater than the image height, sample every row
 212         if( samples > height)
 213         {
 214                 samples = height;
 215         }
 216
 217         int sample_height;
 218         int num_edges = 0;      // Number of edges found. if each sample location has an edge, then num_edges = samples
 219
 220         for (int i=0; i<samples; i++)
 221         {
 222                 // Determine the position in the rows to find the edges.
 223                 // This will give you a number of evenly spaced samples
 224                 sample_height = ceil(height * (i + 1) / samples) -1;
 225
 226                 // Need to go through each pixel of a row and find all the locations of a line. If there is more than one pixel, average it. note this only works if the canny edge algorithm returns lines about the actual line (no outliers).
 227
 228                 int edge_location=0;
 229                 int num=0;
 230                 for ( int col = 0; col < width; col++)
 231                 {
 232                         // Count the number of points
 233                         // Get the threshold of the pixel at the current location
 234                         CvScalar value = cvGet2D(g_edges, sample_height, col);
 235                         if( value.val[0]> THRES)
 236                         {
 237                                 edge_location += col;
 238                                 num++;
 239                         }
 240                 }
 241                 if( num > 0)
 242                 {
 243                         average += ( edge_location / num );
 244                         num_edges++;
 245                 }
 246         }
 247         if (num_edges > 0)
 248                 average /= num_edges;
 249         else
 250                 return result;  // As no edges were found
 251
 252         if( average > 0)
 253         {
 254                 result = true; // Successfully found an edge
 255                 // If the experiment has already been initialised
 256                 switch (id)
 257                 {
 258                         case DIL_POS:
 259                                 *value = average*SCALE;
 260                                 return result;
 261                         case DIL_DIFF:
 262                                 if( lastPosition > 0)
 263                                 {
 264                                         // Find the rate of expansion and convert to mm. Will give a negative result for compression.
 265                                         *value = (average - lastPosition) * SCALE *2;
 266                                         lastPosition = average; // Current position now becomes the last position
 267                                 }
 268                                 return result;
 269                         default:
 270                                 return false;           }
 271         }
 272         return result;
 273 }
 274
 275  /**
 276  * Read the dilatometer image. The value changed will correspond to the new location of the edge.
 277  * @param val - Will store the read value if successful
 278  * @returns true on successful read
 279  */
 280 bool Dilatometer_Read(int id, double * value)
 281 {
 282         bool result = Dilatometer_GetExpansion(id, value, SAMPLES);
 283         return result;
 284 }
 285
 286 /**
 287  * Initialise the dilatometer
 288  */
 289 bool Dilatometer_Init(const char * name, int id)
 290 {
 291         // Make an initial reading (will allocate memory the first time only).
 292         double val;
 293         lastPosition = 0;  // Reset the last position
 294         Dilatometer_GetExpansion(DIL_POS, &val, 1);
 295         return true;
 296 }
 297
 298 // Overlays a line over the given edge position
 299 void Draw_Edge(double edge)
 300 {
 301         CvScalar value;
 302         value.val[0]=244;
 303         for( int i = 0; i < g_srcGray->rows; i++)
 304         {
 305                 cvSet2D(g_edges,i,edge,value);
 306         }
 307         cvShowImage("display", g_edges);
 308         cvWaitKey(0);
 309         //cvSaveImage("test_edge_avg.jpg",g_edges,0);
 310 }
 311
 312 /* // Test algorithm
 313 static void Dilatometer_GetImageTest( )
 314 {
 315         //Generates Test image
 316         //Dilatometer_TestImage();
 317
 318         //Load Test image
 319         g_srcGray = cvLoadImageM ("testimage4.jpg",CV_LOAD_IMAGE_GRAYSCALE );
 320 }*/
 321
 322 /**
 323  * For testing purposes
 324  */
 325 /*int main(int argc, char ** argv)
 326 {
 327         //cvNamedWindow( "display", CV_WINDOW_AUTOSIZE );// Create a window for display.
 328         //gettimeofday(&start, NULL);
 329         test_left = 100;
 330         test_right = 500;
 331         Dilatometer_Init();
 332
 333         cvNamedWindow( "display", CV_WINDOW_AUTOSIZE);
 334         //double width;
 335
 336         double edge;
 337         Dilatometer_GetEdge(&edge,20000);
 338         //For testing purposes, overlay the given average line over the image
 339         //Draw_Edge(edge);
 340
 341         cvDestroyWindow("display");
 342
 343         Dilatometer_Cleanup();
 344 }*/
 345