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