From 3982d051198ced76dcf4ed173098e5e344c0c8c0 Mon Sep 17 00:00:00 2001 From: Sam Moore Date: Sat, 14 Sep 2013 22:58:58 +0800 Subject: [PATCH] Interferometer Test Program Really ugly, concentrating on proof of concept, will clean up before merging into master branch. --- server/Makefile | 11 +- server/interferometer.c | 289 ++++++++++++++++++++++++++++++++++++++++ server/interferometer.h | 18 +++ server/intertest.sh | 15 +++ 4 files changed, 329 insertions(+), 4 deletions(-) create mode 100644 server/interferometer.c create mode 100644 server/interferometer.h create mode 100755 server/intertest.sh diff --git a/server/Makefile b/server/Makefile index bb70c4f..51dfd82 100644 --- a/server/Makefile +++ b/server/Makefile @@ -1,7 +1,7 @@ # Makefile for server software CXX = gcc -FLAGS = -std=c99 -Wall -Werror -pedantic -g -LIB = -lfcgi -lssl -lcrypto -lpthread -lm +FLAGS = -std=c99 -Wall -pedantic -g -I/usr/include/opencv -I/usr/include/opencv2/highgui -L/usr/lib +LIB = -lfcgi -lssl -lcrypto -lpthread -lm -lopencv_highgui -lopencv_core -lopencv_ml -lopencv_imgproc OBJ = log.o control.o data.o fastcgi.o main.o sensor.o actuator.o RM = rm -f @@ -10,8 +10,11 @@ BIN = server all : $(BIN) $(BIN2) -#stream : stream.c -# $(CXX) -o stream stream.c -I/usr/include/opencv -I/usr/include/opencv2/highgui -L/usr/lib -lopencv_highgui -lopencv_core -lopencv_ml -lopencv_imgproc -lm +stream : stream.c + $(CXX) $(FLAGS) -o stream stream.c $(LIB) + +intertest : interferometer.c + $(CXX) $(FLAGS) -o intertest interferometer.c $(LIB) $(BIN) : $(OBJ) diff --git a/server/interferometer.c b/server/interferometer.c new file mode 100644 index 0000000..136148b --- /dev/null +++ b/server/interferometer.c @@ -0,0 +1,289 @@ +/** + * @file interferometer.c + * @purpose Implementation of interferometer related functions + */ + +#include "cv.h" +#include "highgui_c.h" +#include "interferometer.h" +#include "analysis.h" +#include + +/** Buffer for storing image data. Stored as a single intensity value for the laser light **/ +static CvMat * g_data = NULL; + + +/** Camera capture pointer **/ +static CvCapture * g_capture = NULL; + + +struct timeval start; + +#define PI 3.141592 + +// For testing purposes +double test_omega = 0.05; +double test_angle = PI/2; +double test_noise[] = {0,0,0.02}; +double test_phase = 0; +double test_intensity = 1.0; + + +static void Interferometer_TestSinusoid() +{ + if (g_capture == NULL) + { + g_capture = cvCreateCameraCapture(0); + } + + // Get image from camera + IplImage * img = cvQueryFrame(g_capture); + + // Convert to CvMat + CvMat stub; + CvMat * background = cvGetMat(img, &stub, 0, 0); + // ... Honestly, I have no idea what the "stub" is for + + if (g_data == NULL) + { + g_data = cvCreateMat(background->rows, background->cols, CV_32FC3); + } + + //cvShowImage("background", background); + + for (int x = 0; x < g_data->cols-1; ++x) + { + for (int y = 0; y < g_data->rows-1; ++y) + { + // Calculate pure sine in test direction + double r = x*cos(test_angle) + y*sin(test_angle); + double value = 0.5*test_intensity*(1+sin(test_omega*r + test_phase)); + + CvScalar s; + s.val[0] = 0; s.val[1] = 0; s.val[2] = value; + CvScalar b = cvGet2D(background, y, x); + + + // Add noise & background image + + // Get the order the right way round + double t = b.val[0]; + b.val[0] = b.val[2]; + b.val[2] = t; + for (int i = 0; i < 3; ++i) + { + + s.val[i] += (rand() % 1000) * 1e-3 * test_noise[i]; + s.val[i] += b.val[i] / 255; // Camera image is 0-255 + } + + //printf("set %d,%d\n", x, y); + + + + cvSet2D(g_data,y, x, s); + } + } + + +} + + +/** + * Get an image from the Interferometer + */ +static void Interferometer_GetImage() +{ + + + Interferometer_TestSinusoid(); + //TODO: Implement camera + +} + +/** + * Initialise the Interferometer + */ +void Interferometer_Init() +{ + + // Make an initial reading (will allocate memory the first time only). + Interferometer_Read(1); +} + +/** + * Cleanup Interferometer stuff + */ +void Interferometer_Cleanup() +{ + if (g_data != NULL) + cvReleaseMat(&g_data); + + if (g_capture != NULL) + cvReleaseCapture(&g_capture); + +} + +/** + * Read the interferometer; gets the latest image, processes it, spits out a single number + * @param samples - Number of columns to scan (increasing will slow down performance!) + * @returns Value proportional to the change in interferometer path length since the last call to this function + */ +double Interferometer_Read(int samples) +{ + + + + + // Get the latest image + Interferometer_GetImage(); + // Frequency of the sinusoid + static double omega = 0; + // Stores locations of nodes + static int nodes[MAXNODES]; + // Current phase + static double phase = 0; + + // Phase + double cur_phase = 0; + + int xstep = g_data->cols / (samples+1); + + // Used for testing to see where the nodes are identified + // (Can't modify g_data) + static CvMat * test_overlay = NULL; + if (test_overlay == NULL) + { + // Creates a memory leak; don't do this in the final version! + test_overlay = cvCreateMat(g_data->rows, g_data->cols, CV_32FC3); + } + cvZero(test_overlay); + //cvCopy(g_data, test_overlay, NULL); + + // For each column to sample + for (int x = xstep; x < g_data->cols; x += xstep) + { + + double avg = 0.5; //TODO: Calculate from image + double threshold_dif = 0; //TODO: Pick this value + + int num_nodes = 0; + + // Find nodes + for (int y = 1; y < g_data->rows-2 && num_nodes < MAXNODES; ++y) + { + if (num_nodes == 0 || abs(nodes[num_nodes-1] - y) > 1) + { + // A "node" is defined where the ajacent points are on opposite sides of the avg + double ldif = INTENSITY(cvGet2D(g_data, y-1,x)) - avg; + double rdif = INTENSITY(cvGet2D(g_data, y+1,x)) - avg; + + // If that is the case, the product of the differences will be negative + if (ldif * rdif < -threshold_dif) + { + + nodes[num_nodes++] = y; + + // Put a white line on the overlay to indicate the node was found + for (int xx = 0; xx < g_data->cols; ++xx) + { + CvScalar s; // = cvGet2D(g_data, y, xx); + s.val[0] = 1; s.val[1] = 1; s.val[2] = 1; + cvSet2D(test_overlay, y, xx, s); + } + } + } + } + + // Insufficient nodes found to continue + if (num_nodes < 2) + { + --samples; + continue; + } + + // Estimate angular frequency from two nodes TODO: Average between nodes + double slice_omega = (PI*(num_nodes-1)) / (nodes[num_nodes-1] - nodes[0]); + //printf("SLICE: %f vs %f\n", slice_omega, test_omega); + + double slice_phase = 0; + for (int i = 0; i < num_nodes; ++i) + { + double this_phase = ((double)(i)*PI - slice_omega * nodes[i]); + //printf("Node %d gives phase %f\n", i, this_phase); + slice_phase += this_phase; + } + slice_phase /= num_nodes; + cur_phase += slice_phase; + } + + // Average over samples + if (samples == 0) + return 0; + + cur_phase /= samples; + + + + // Get phase change since last call, save current phase + double result = (cur_phase - phase); + + // HACK + if (abs(result) > 0.5*PI) + { + if (result > 0) + result -= PI; + else + result += PI; + } + phase = cur_phase; + + // Display the image with lines to indicate results of data processing + //cvShowImage("nodes", test_overlay); + //cvWaitKey(1); + cvAdd(test_overlay, g_data, test_overlay, NULL); + cvShowImage("overlay", test_overlay); + cvWaitKey(1); + return result; + +} + +/** + * For testing purposes + */ +int main(int argc, char ** argv) +{ + //cvNamedWindow( "display", CV_WINDOW_AUTOSIZE );// Create a window for display. + gettimeofday(&start, NULL); + + //Interferometer_Read(1); + //exit(EXIT_SUCCESS); + + Interferometer_Init(); + + double sum = 0; + double last_phase = test_phase; + + struct timeval now; + double time = 0; + + while (time < 20) + { + gettimeofday(&now, NULL); + time = TIMEVAL_DIFF(now, start); + + test_phase = 0.5*PI*sin(time); + + + double delta = Interferometer_Read(1); + sum += delta; + + printf("%f\t%f\t%f\t%f\t%f\n", time, test_phase - last_phase, test_phase, delta, sum); + + last_phase = test_phase; + //break; + } + +} + + diff --git a/server/interferometer.h b/server/interferometer.h new file mode 100644 index 0000000..f545463 --- /dev/null +++ b/server/interferometer.h @@ -0,0 +1,18 @@ +/** + * @file interferometer.h + * @purpose Declarations for functions to deal with interferometer + */ + +#include "common.h" + +//#define INTENSITY(rgb) ((rgb).val[0]*(rgb).val[0] + (rgb).val[1]*(rgb).val[1] + (rgb).val[2]*(rgb).val[2]) +#define INTENSITY(rgb) ((rgb).val[2]) + +#define MAXNODES 100 //TODO: Choose value based on real data + +extern void Interferometer_Init(); // Initialise the interferometer +extern void Interferometer_Cleanup(); // Cleanup +extern double Interferometer_Read(); // Read the interferometer + + + diff --git a/server/intertest.sh b/server/intertest.sh new file mode 100755 index 0000000..f3012ad --- /dev/null +++ b/server/intertest.sh @@ -0,0 +1,15 @@ +rm intertest +make intertest +./intertest > test.dat + +cmd="set title \"Interferometer Test\"" +cmd="$cmd; set xlabel \"Time (s)\"" +cmd="$cmd; set ylabel \"Phase (rad)\"" +cmd="$cmd; plot \"test.dat\" u 1:3 t \"Specified\" w l, \"test.dat\" u 1:5 t \"Calculated\" w p" +gnuplot --persist -e "$cmd" + +cmd="set title \"Interferometer Test\"" +cmd="$cmd; set xlabel \"Time (s)\"" +cmd="$cmd; set ylabel \"Delta Phase (rad)\"" +cmd="$cmd; plot \"test.dat\" u 1:2 t \"Specified\" w l, \"test.dat\" u 1:4 t \"Measured\" w p" +gnuplot --persist -e "$cmd" -- 2.20.1