Add usleep to strain.c just in case...
[matches/MCTX3420.git] / 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

UCC git Repository :: git.ucc.asn.au