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 <math.h>
  10
  11 // test positions
  12 static double test_left, test_right;
  13
  14 // Canny Edge algorithm variables
  15 int blur = 5;
  16 int lowThreshold = 30;
  17 int ratio = 3;
  18 int kernel_size = 3;
  19
  20 /** Buffers for storing image data.  **/
  21 static CvMat * g_srcRGB  = NULL;        // Source Image
  22 static CvMat * g_srcGray = NULL;        // Gray scale of source image
  23 static CvMat * g_edges   = NULL;        // Detected Edges
  24
  25 /** Pointers for capturing image **/
  26 static CvCapture * g_capture = NULL;
  27 static IplImage * frame  = NULL;        // This is required as you can not use capture with CvMat in C
  28
  29
  30 /**
  31  * Create a test image using left as left edge and right as right edge positions
  32  */
  33 void Dilatometer_TestImage()
  34 {
  35
  36         g_srcRGB = cvCreateMat(480, 640, CV_8UC3);
  37
  38         for( int x = 0; x < 640; ++x)
  39         {
  40                 for (int y = 0; y < 480; ++y)
  41                 {
  42                         CvScalar s;
  43                         for( int i = 0; i < 3; ++i)
  44                         {
  45                                 s.val[i]  =  210 + (rand() % 1000) * 1e-0 - (rand() % 1000) * 1e-0;
  46                                 // Produce an exponential decay around left edge
  47                                 if( x < test_left)
  48                                         s.val[i] *= exp( (x - test_left) / 25);
  49                                 else if( x < 320)
  50                                         s.val[i] *= exp( (test_left - x) / 25);
  51                                 // Produce an exponential decay around right edge
  52                                 else if( x < test_right)
  53                                         s.val[i] *= exp( (x - test_right) / 25);
  54                                 else
  55                                         s.val[i] *= exp( (test_right - x) / 25);
  56                         }
  57                         cvSet2D(g_srcRGB,y,x,s);
  58                 }
  59
  60         }
  61         if (g_srcGray == NULL)
  62         {
  63                 g_srcGray = cvCreateMat(g_srcRGB->rows,g_srcRGB->cols,CV_8UC1);
  64         }
  65         cvCvtColor(g_srcRGB,g_srcGray,CV_RGB2GRAY);
  66 }
  67
  68 /**
  69  * Cleanup Dilatometer pointers
  70  */
  71 bool Dilatometer_Cleanup(int id)
  72 {
  73         if (g_capture != NULL)
  74                 cvReleaseCapture(&g_capture);
  75         if (frame != NULL)
  76                 cvReleaseImageHeader(&frame);
  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
  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         if ( g_edges == NULL)
 126         {
 127                 g_edges = cvCreateMat(g_srcGray->rows,g_srcGray->cols,CV_8UC1);
 128         }
 129
 130         // Commented out lines are used during testing to show the image to screen, can also save the test images
 131         //cvShowImage("display", g_srcGray);
 132         //cvWaitKey(0);
 133
 134         // Reduce noise with a kernel blurxblur. Input the grayscale source image, output to edges. (0's mean it's determined from kernel sizes)
 135         cvSmooth( g_srcGray, g_edges, CV_GAUSSIAN, blur, blur ,0 ,0 );
 136
 137         //Save the image
 138         //cvSaveImage("test_blurred.jpg",g_edges,0);
 139
 140         //cvShowImage("display", g_edges);
 141         //cvWaitKey(0);
 142
 143         // Find the edges in the image
 144         cvCanny( g_edges, g_edges, lowThreshold, lowThreshold*ratio, kernel_size );
 145
 146         //Save the image
 147         //cvSaveImage("test_edge.jpg",g_edges,0);
 148
 149         //cvShowImage("display", g_edges);
 150         //cvWaitKey(0);
 151
 152 }
 153
 154  /**
 155  * Read the dilatometer image. The value changed will correspond to the new location of the edge.
 156  * @param val - Will store the read value if successful
 157  * @param samples - Number of rows to scan (increasing will slow down performance!)
 158  * @returns true on successful read
 159  */
 160 bool Dilatometer_GetEdge( double * value, int samples)
 161 {
 162         bool result = false;
 163         double average = 0;
 164         // Get the image from the camera
 165         result = Dilatometer_GetImage();
 166         // If an error occured when capturing image then return
 167         if (!result)
 168                 return result;
 169
 170         // Apply the Canny Edge theorem to the image
 171         CannyThreshold();
 172
 173         int width = g_edges->cols;
 174         int height = g_edges->rows;
 175
 176         // If the number of samples is greater than the image height, sample every row
 177         if( samples > height)
 178         {
 179                 samples = height;
 180         }
 181
 182         int sample_height;
 183         int num_edges = 0;      // Number of edges. if each sample location has an edge, then num_edges = samples
 184
 185         for (int i=0; i<samples; i++)
 186         {
 187                 // Determine the position in the rows to find the edges.
 188                 // This will give you a number of evenly spaced samples
 189                 sample_height = ceil(height * (i + 1) / samples) -1;
 190
 191                 // 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).
 192
 193                 int edge_location=0;
 194                 int num=0;
 195                 for ( int col = 0; col < width; col++)
 196                 {
 197                         // Count the number of points
 198                         // Get the threshold of the pixel at the current location
 199                         CvScalar value = cvGet2D(g_edges, sample_height, col);
 200                         if( value.val[0]> THRES)
 201                         {
 202                                 edge_location += col;
 203                                 num++;
 204                         }
 205                 }
 206                 if( num > 0)
 207                 {
 208                         average += ( edge_location / num );
 209                         num_edges++;
 210                 }
 211         }
 212         if (num_edges > 0)
 213                 average /= num_edges;
 214
 215         if( average > 0)
 216         {
 217                 result = true; //Successfully found an edge
 218                 *value = average;
 219         }
 220         return result;
 221 }
 222
 223  /**
 224  * Read the dilatometer image. The value changed will correspond to the new location of the edge.
 225  * @param val - Will store the read value if successful
 226  * @returns true on successful read
 227  */
 228 bool Dilatometer_Read(int id, double * value)
 229 {
 230         bool result = Dilatometer_GetEdge(value, SAMPLES);
 231         return result;
 232 }
 233
 234 /**
 235  * Initialise the dilatometer
 236  */
 237 bool Dilatometer_Init(const char * name, int id)
 238 {
 239         // Make an initial reading (will allocate memory the first time only).
 240         double val;
 241         Dilatometer_GetEdge(&val, 1);
 242         return true;
 243 }
 244
 245 // Overlays a line over the given edge position
 246 void Draw_Edge(double edge)
 247 {
 248         CvScalar value;
 249         value.val[0]=244;
 250         for( int i = 0; i < g_srcGray->rows; i++)
 251         {
 252                 cvSet2D(g_edges,i,edge,value);
 253         }
 254         cvShowImage("display", g_edges);
 255         cvWaitKey(0);
 256         //cvSaveImage("test_edge_avg.jpg",g_edges,0);
 257 }
 258
 259 /* // Test algorithm
 260 static void Dilatometer_GetImageTest( )
 261 {
 262         //Generates Test image
 263         //Dilatometer_TestImage();
 264
 265         //Load Test image
 266         g_srcGray = cvLoadImageM ("testimage4.jpg",CV_LOAD_IMAGE_GRAYSCALE );
 267 }*/
 268
 269 /**
 270  * For testing purposes
 271  */
 272 /*int main(int argc, char ** argv)
 273 {
 274         //cvNamedWindow( "display", CV_WINDOW_AUTOSIZE );// Create a window for display.
 275         //gettimeofday(&start, NULL);
 276         test_left = 100;
 277         test_right = 500;
 278         Dilatometer_Init();
 279
 280         cvNamedWindow( "display", CV_WINDOW_AUTOSIZE);
 281         //double width;
 282
 283         double edge;
 284         Dilatometer_GetEdge(&edge,20000);
 285         //For testing purposes, overlay the given average line over the image
 286         //Draw_Edge(edge);
 287
 288         cvDestroyWindow("display");
 289
 290         Dilatometer_Cleanup();
 291 }*/
 292