calibrateFG.cpp

/*
 * Copyright 2010 Andrew Leifer et al <leifer@fas.harvard.edu>
 * This file is part of MindControl.
 *
 * MindControl is free software: you can redistribute it and/or modify
 * it under the terms of the GNU  General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * MindControl is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with MindControl. If not, see <http://www.gnu.org/licenses/>.
 *
 * For the most up to date version of this software, see:
 * http://github.com/samuellab/mindcontrol
 *
 *
 *
 * NOTE: If you use any portion of this code in your research, kindly cite:
 * Leifer, A.M., Fang-Yen, C., Gershow, M., Alkema, M., and Samuel A. D.T.,
 * 	"Optogenetic manipulation of neural activity with high spatial resolution in
 *	freely moving Caenorhabditis elegans," Nature Methods, Submitted (2010).
 */

/*
 *
 *  Calibrate the position of the mirrors relative to the pixels of the camera.
 *  This replaces the now obsolete calibrate.c file.
 *
 *  This routine works by flipping the mirrors so as to scan a point
 *  across the camera. The software records the location of the mirror
 *  and the corresponding light on the camera and then opens MATLAB
 *  to generate a lookup table to transform between camera space and
 *  mirror space based on the measured points. The calibration is stored in
 *  calib.dat
 *
 */


//Standard C headers
#include <unistd.h>
#include <stdio.h>
#include <ctime>
#include <time.h>
#include <conio.h>
#include <math.h>


//Windows Header

//C++ header
#include <iostream>
#include <limits>

using namespace std;


//OpenCV Headers
#include "opencv2/highgui/highgui_c.h"
#include "opencv/cv.h"
#include "opencv/cxcore.h"


#include <assert.h>

//Andy's Personal Headers
#include "MyLibs/AndysOpenCVLib.h"
#include "MyLibs/Talk2FrameGrabber.h"
#include "MyLibs/Talk2DLP.h"
#include "MyLibs/AndysComputations.h"
#include "version.h"



typedef struct CalibrationSessionStruct {
	/** Calibration Transfomration **/
	int *CCD2DLPLookUp;

	/** Input Output **/
	long myDLP;
	FrameGrabber* fg;

	/** Frames **/
	Frame* fromCCD;
	Frame* toDLP;
	Frame* temp;
	Frame* temp2;
	Frame* background;

	/** Internal Variables **/
	CvPoint MinPoint;
	CvPoint MaxPoint;
	double minvalue;
	double maxvalue;
	CvScalar mean;
	CvScalar stdev;

	/** Circle Drawing properties**/
	int CircRadius;
	int gauss_radius;
	int rel_intensity_thresh;
	int minIntensityAboveMean; // Number of intensity units that a pt must have above the mean to be valid.

	/** Size of Objects **/
	CvSize DLPsize;
	CvSize Camsize;


	/** Calibrated Points CvSeq Stuff **/
	CvMemStorage* calibstorage;
	CvSeq* CalibSeq;

	/** Parameters **/
	int StepSize;
	int LoopsPerPt;

}CalibrationSession;



/*
 * Create the calibration session structure
 *
 */
CalibrationSession* CreateCalibrationSession(){
	CalibrationSession* c = (CalibrationSession* ) malloc(sizeof(CalibrationSession) );

	/** Set Everything to Zero **/

	/** Input/Output **/
	c->myDLP=0;
	c->fg=NULL;


	/*** Frames **/
	c->fromCCD=NULL;
	c->toDLP=NULL;
	c->temp=NULL;
	c->temp2=NULL;
	c->background=NULL;

	/** Internal Variables **/
	c->MinPoint=cvPoint(0,0);
	c->MaxPoint=cvPoint(0,0);
	c->minvalue=0;
	c->maxvalue=0;

	/** Circle properties **/
	c->CircRadius=4;
	c->gauss_radius=6;
	c->rel_intensity_thresh=0;
	c->minIntensityAboveMean=1;

	/** Sizing Info **/
	c->DLPsize=cvSize(0,0);
	c->Camsize=cvSize(0,0);

	/** Lookup Table **/
	c->CCD2DLPLookUp=NULL;

	/** Calibrated Points CvSeq Stuff **/
	c->calibstorage=NULL;
	c->CalibSeq=NULL;

	/** Parameters **/
	c->StepSize=0;
	c->LoopsPerPt=0;

	return c;


}




/*
 * Allocate memory for the members of the calibration session structure
 */
void InitializeCalibrationSession(CalibrationSession* c){

	if (!(c->Camsize.width==c->DLPsize.width && c->Camsize.height==c->DLPsize.height) || c->Camsize.width==0 || c->DLPsize.width==0 || c->Camsize.height==0 || c->DLPsize.height==0){
		printf("The dimensions of the Camera or the DLP are invalid in IntializeCalibtraionSession()\n");
		assert(0);
	}

	/** Calibration Lookup Table **/
	c->	CCD2DLPLookUp = (int *) malloc(2 * c->Camsize.width * c->Camsize.height * sizeof(int));

	/** Frames **/
	c->fromCCD=CreateFrame(c->Camsize);
	c->toDLP=CreateFrame(c->DLPsize);
	c->temp=CreateFrame(c->Camsize);
	c->temp2=CreateFrame(c->Camsize);
	c->background=CreateFrame(c->Camsize);

	/** Calibration Data Sequences **/

	c->calibstorage = cvCreateMemStorage(0);
	c->CalibSeq = cvCreateSeq(0, sizeof(CvSeq), sizeof(PairOfPoints), c->calibstorage);

	return;
}

void DestroyCalibrationSession(CalibrationSession* c){
	DestroyFrame(&(c->fromCCD));
	DestroyFrame(&(c->toDLP));

	free(c);
	return;

}

/*
 * Set size of camera and DLP
 */
void SetHardwareDimensions(CalibrationSession* c, CvSize DLPsize, CvSize Camsize){
	c->Camsize=Camsize;
	c->DLPsize=DLPsize;
	printf("For now the camera and the DLP must have the same dimensions.");
	assert(Camsize.width==DLPsize.width && Camsize.height==DLPsize.height);
	return;
}




void SetupGUI(CalibrationSession* c){
	cvNamedWindow("FromCamera", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("ToDLP", CV_WINDOW_AUTOSIZE);
	cvCreateTrackbar("InputRad", "FromCamera", &(c->CircRadius), 10, NULL);
	cvCreateTrackbar("GaussRad", "FromCamera", &(c->gauss_radius), 30,			NULL);
	cvCreateTrackbar("RelIntThresh", "FromCamera",
				&(c->rel_intensity_thresh), 10, NULL);
	cvCreateTrackbar("MinIntAboveMean","FromCamera", &(c->minIntensityAboveMean),10,NULL);
	return;

}

void DrawCircleOnDLP(CvPoint center, CalibrationSession* c){

	cvSetZero(c->toDLP->iplimg);
	cvCircle(c->toDLP->iplimg, center, c->CircRadius+ 1,
			CV_RGB(255,255,255), CV_FILLED, 8);
	copyIplImageToCharArray(c->toDLP->iplimg,c->toDLP->binary);
	T2DLP_SendFrame((unsigned char *) c->toDLP->binary, c->myDLP);
	cvShowImage("SentToDLP",c->toDLP->iplimg);
	return;
}

void AnalyzePointInFrame(CalibrationSession* c){
	if (c->fromCCD==NULL || c->temp == NULL || c->temp2==NULL ) printf("ERROR! c->fromCCD, c->temp, or c->temp2 are NULL!\n");


	/** Subtract background **/
	cvSub(c->fromCCD->iplimg, c->background->iplimg,c->temp2->iplimg);

	/** Apply Gaussian Smooth **/
	cvSmooth(c->temp2->iplimg, c->temp->iplimg, CV_GAUSSIAN, (c->gauss_radius) * 2
			+ 1, (c->gauss_radius) * 2 + 1);



	//Find the pixel with the maximal intensity
	cvMinMaxLoc(c->temp->iplimg, &(c->minvalue), &(c->maxvalue), &(c->MinPoint), &(c->MaxPoint),
			NULL);
	cvAvgSdv(c->temp->iplimg, &(c->mean), &(c->stdev));

	printf("x: %d, y: %d,\t value: %d \t mean: %f, stdev: %f\n",
			c->MaxPoint.x, c->MaxPoint.y, (int) c->maxvalue, c->mean.val[0],
					c->stdev.val[0]);


}

void CheckFGSizeMatch(IplImage* img, FrameGrabber* fg){
	if ((int) fg->xsize != img->width   ||  (int) fg->ysize != img->height){
		printf("In CheckFGSizeMAtch, size from framegrabber does not match size in IplImage fromCCD!\n");
		printf(" fg->xsize=%d\n",(int) fg->xsize);
		printf(" img->width=%d\n",img->width);
		printf(" fg->ysize=%d\n",(int)  fg->ysize);
		printf(" img->height=%d\n",img->height);
	}


	return;
}

/*
 * Draws a circle and sends it to the DLP. Acquires a frame from the camera
 * and then conducts an analysis of the frame from the camera.
 */
void SendPt2DLPAndObserve(CvPoint pt, CalibrationSession* c ){
	/** Draw Circle on DLP **/
	DrawCircleOnDLP(pt,c);


	/** Grab new frame from camera **/
	AcquireFrame(c->fg);

	/** Check that our image is properly sized **/
	CheckFGSizeMatch(c->fromCCD->iplimg,c->fg);

	LoadFrameWithBin(c->fg->HostBuf,c->fromCCD);

	/** Analyze the image from the camera **/
	AnalyzePointInFrame(c);
	return;

}


/*
 * Takes the background image
 */
void TakeBakgroundImage(CalibrationSession* c){
	/** Let's take a background image so that we can subtract it off **/
	printf("\nTaking a background image. \n");
	T2DLP_clear(c->myDLP);
	cvWaitKey(10);
	AcquireFrame(c->fg);

	/** Check that our image is properly sized **/
	CheckFGSizeMatch(c->background->iplimg,c->fg);

	/** Load the binary image data from the frame grabber into our background variable **/
	LoadFrameWithBin(c->fg->HostBuf,c->background);
	printf("\nBackground image acquried.\n");
	return;

}



void CalibrateAPoint(CvPoint pt, CalibrationSession* c){
	int k;


	/** Create sequence to store the max point for each frame **/
	CvSeq* Pts= cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint), c->calibstorage);
	cvClearSeq(Pts);


	/** For each frame find the location of the brightest pixel **/
	for (k = 0; k <	c->LoopsPerPt; ++k) {

		/** Make pt on DLP, observe on Camera**/
		SendPt2DLPAndObserve(pt,c);

		/** Draw a square on the image and display it **/
		SafeDrawSquare(&(c->fromCCD->iplimg) , &(c->MaxPoint), 7);

		cvShowImage("ToDLP",c->toDLP->iplimg);
		cvShowImage("FromCamera",c->fromCCD->iplimg);
		cvWaitKey(3);



		/** Is the Point Valid? **/
		if ((c->maxvalue > ((c->rel_intensity_thresh) * c->stdev.val[0])
					+ c->mean.val[0]) &&  c->maxvalue-c->mean.val[0] > c->minIntensityAboveMean   ) {
				cvSeqPush(Pts, &(c->MaxPoint));
			} else {
				printf("Tossing frame. Spot fails relative intensity threshold.\n");
		}
	}


	/** Find the median point **/
	PairOfPoints pair;
	pair.alpha=pt; // known DLP point
	pair.beta= GetMedianOfPoints(Pts); // observed point on CCD

	//If both points in the are greater than zero AND if the number of valid points is greater than half of the expected number of points
	if ((pair.beta.x >0 && pair.beta.y > 0) && Pts->total > c->LoopsPerPt / 2) {
		cvSeqPush(c->CalibSeq, &pair);
		printf("Median Found ( %d, %d )\n", pair.beta.x, pair.beta.y);
	} else {
		printf("Tossing out the calibration for this point.\n");
	}

	cvClearSeq(Pts);

}

void WriteCalibrationToFile(int* CCD2DLPLookup, CvSize size, const char * filename ){

	/** Open File **/
	FILE *fp;
	if ((fp = fopen(filename, "wb+")) == NULL) {
		printf("ERROR: Cannot open file to write calibration\n");
		return;
	}

	/** Write lookup table to disk **/
	int result=fwrite(CCD2DLPLookup, sizeof(int)* 2 * size.width* size.height,1, fp);

	if ( result != 1) {
		printf("Write error!\n");
	} else{
		printf("Write was successful.\n");
	}

	fclose(fp);

	return;
}



/*
/* Write out the raw calibrated points to disk as a YAML fil
/* MATLAB will later read this file in and generated a lookup table that is to be provided
/* to the main colbert soffware.
/*
/*  (This is is all to correctly register the DLP and CCD */
int WriteOutCalibPointPairs(CvSeq *CalibSeq, int nsizex, int nsizey,int CCDsizex, int CCDsizey){


	printf("In WriteOutCalibPointPairs()..\n");

	/* Create writing data object and memory */
	CvFileStorage* fs;
	CvMemStorage* writerMem;
	writerMem=cvCreateMemStorage(0);
	
	/** Open YAML Data File for writing**/
	fs=cvOpenFileStorage("calibPoints.yaml",writerMem,CV_STORAGE_WRITE);
	
	if (fs==0) {
		printf("DataWriter->fs is zero! Could you have specified the wrong directory?\n");
		/** If there were errors, return immediately **/
		return -1;
	}



	/** Write the header for the YAML data file **/
	cvWriteComment(fs, "Raw Instrument Calibration Data\nMade by OpticalMindControl software\nleifer@princeton.edu\n",0);
	cvWriteString(fs, "gitHash", build_git_sha,0);
	cvWriteString(fs, "gitBuildTime",build_git_time,0);

	/** Write Out Current Time**/
	  struct tm *local;
	  time_t t;

	  t = time(NULL);
	  local = localtime(&t);
	cvWriteString(fs, "ExperimentTime",asctime(local),0);
	
	/** Write size info **/
	cvWriteInt(fs,"DLPwidth", nsizex);
	cvWriteInt(fs,"DLPheight", nsizey);
	cvWriteInt(fs,"CCDwidth", CCDsizex);
	cvWriteInt(fs,"CCDheight", CCDsizey);
	cvWriteInt(fs,"SizeOfInt",sizeof(int));

	/** Begin Writing out Pairs of Points **/
	int PairCount; // Current pair 
	PairOfPoints tuple;  //A pair consisting of a CCD (x,y) and a DLP (x,y) point

	int NumberOfCalibPoints= CalibSeq->total; //How many pairs of points (CCD & DLP) do we have?
	
	cvStartWriteStruct(fs,"PairOfPoints",CV_NODE_SEQ,NULL);
	
	for (PairCount = 0; PairCount < NumberOfCalibPoints; ++PairCount) {
		printf("Calibrated Pair %d of %d\n", PairCount, CalibSeq->total);
		cvSeqPopFront(CalibSeq, &tuple);
		printf("DLP ( %d , %d ) -> CCD ( %d , %d )\n", tuple.alpha.x, tuple.alpha.y, tuple.beta.x, tuple.beta.y);
		
		cvStartWriteStruct(fs,NULL,CV_NODE_MAP,NULL);

			cvStartWriteStruct(fs,"DLP",CV_NODE_MAP,NULL);
				cvWriteInt(fs,"x", tuple.alpha.x);
				cvWriteInt(fs,"y",tuple.alpha.y);
			cvEndWriteStruct(fs);
		
			cvStartWriteStruct(fs,"CCD",CV_NODE_MAP,NULL);
				cvWriteInt(fs,"x", tuple.beta.x);
				cvWriteInt(fs,"y",tuple.beta.y);
			cvEndWriteStruct(fs);	

		cvEndWriteStruct(fs);

	}
	cvEndWriteStruct(fs);
	
	/* Close up things */
	cvReleaseFileStorage(&fs);
	
	printf("End WriteOutCalibPointPairs()\n");
	return 0;


}




int main (int argc, char** argv){


	/** Display output about the OpenCV setup currently installed **/
	DisplayOpenCVInstall();

	/** Create session object **/
	CalibrationSession* c = CreateCalibrationSession();

	/** Set the size of the objects **/
	SetHardwareDimensions(c,cvSize(NSIZEX,NSIZEY),cvSize(NSIZEX,NSIZEY));

	/** Allocate memory for the variables we will be using this session **/
	InitializeCalibrationSession(c);


	/** Set Parameters **/
	c->StepSize=100; //pixels
	c->LoopsPerPt=20; // Number of frames we use to calibrate a given point

	/** Start Camera **/
	c->fg= TurnOnFrameGrabber();

	/** Set the acquisition timeout to be very long to give the camera time to take long exposures, as is typical with calibration **/
	setAcquisitionTimeout(c->fg,200);

	/** Prepare DLP ***/
	c->myDLP= T2DLP_on();

	/** Setup GUI **/
	SetupGUI(c);



	/** Let's take a background image so that we can subtract it off **/
	TakeBakgroundImage(c);


	/** Draw A circle on the DLP so we can focus on the camera **/
	printf("Showing camera.\n Press any key to continue!\n");
	while (!kbhit()){

		/** Make pt on DLP, observe on Camera**/
		SendPt2DLPAndObserve(cvPoint(c->Camsize.width/2,c->Camsize.height/2 ),c);

		/** Draw a square on the image and display it **/
		SafeDrawSquare(&(c->fromCCD->iplimg) , &(c->MaxPoint), 7);
		cvShowImage("ToDLP",c->toDLP->iplimg);
		cvWaitKey(2);
		cvShowImage("FromCamera",c->fromCCD->iplimg);
		cvWaitKey(2);

	}
	T2DLP_clear(c->myDLP);



	/*** Build Up a set of calibrated points ***/
	int calx = 0;
	int caly = 0;


	printf(" Beginning calibration..\n");

	while (caly < c->DLPsize.height ) {
		calx = 0;

		while (calx < c->DLPsize.width) {

			CalibrateAPoint(cvPoint(calx,caly),c);

			calx = calx + c->StepSize;
		}
		caly = caly + c->StepSize;
	}

	T2DLP_clear(c->myDLP);

	cvDestroyAllWindows();
	
	/** Generate Look Up Table in Matlab **/
	printf("Writing out Calibrated Pair of Points to YAML for later processing by MATLAB...\n");
	WriteOutCalibPointPairs(c->CalibSeq,c->DLPsize.width, c->DLPsize.height, c->Camsize.width, c->Camsize.height);
	printf("YAML file written.\n");
	
	
	//T2Matlab_GenLookUpTable(c->CalibSeq, c->CCD2DLPLookUp, c->DLPsize.width, c->DLPsize.height, c->Camsize.width, c->Camsize.height);

	/** Write calibration to file **/
	//WriteCalibrationToFile(c->CCD2DLPLookUp,c->Camsize,"calib.dat");

	/** Turn everything Off **/
	T2DLP_off(c->myDLP);
	CloseFrameGrabber(c->fg);

	DestroyCalibrationSession(c);

	printf("\n Good bye.\n");
	return 0;
}