From: Sam Moore <matches@ucc.asn.au>
Date: Sat, 14 Sep 2013 14:58:58 +0000 (+0800)
Subject: Interferometer Test Program
X-Git-Url: https://git.ucc.asn.au/?a=commitdiff_plain;h=3982d051198ced76dcf4ed173098e5e344c0c8c0;p=matches%2FMCTX3420.git

Interferometer Test Program

Really ugly, concentrating on proof of concept, will clean up before merging into master branch.
---

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 <math.h>
+
+/** 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"