void idle()
{
double deltaT=1E-2;
optotrak.updateMarkers();
markers = optotrak.getAllMarkers();
allVisiblePlatform = isVisible(markers.at(15).p) && isVisible(markers.at(16).p);
allVisibleThumb = isVisible(markers.at(11).p) && isVisible(markers.at(12).p) && isVisible(markers.at(13).p);
allVisibleIndex = isVisible(markers.at(7).p) && isVisible(markers.at(8).p) && isVisible(markers.at(9).p);
allVisibleFingers = allVisibleThumb && allVisibleIndex;
allVisiblePatch = isVisible(markers.at(1).p) && isVisible(markers.at(2).p) && isVisible(markers.at(3).p);
allVisibleHead = allVisiblePatch && isVisible(markers.at(17).p) && isVisible(markers.at(18).p);
headEyeCoords.update(markers.at(1).p,markers.at(2).p,markers.at(3).p);
// update thumb coordinates
thumbCoords.update(markers.at(11).p,markers.at(12).p,markers.at(13).p);
// update index coordinates
indexCoords.update(markers.at(7).p, markers.at(8).p, markers.at(9).p);
eyeLeft = headEyeCoords.getLeftEye();
eyeRight = headEyeCoords.getRightEye();
realThumb = thumbCoords.getP1();
realIndex = indexCoords.getP1();
reflectedIndex = getReflected(realIndex,mirrorPlane);
reflectedThumb = getReflected(realThumb,mirrorPlane);
}
//Called every TIMER_MS milliseconds
void update(int value)
{
optotrak.updateMarkers();
markers = optotrak.getAllMarkers();
// Set the screen and mirror points
screen[0] = markers.at(5).p;
screen[1] = markers.at(6).p;
screen[2] = markers.at(7).p;
screen[3] = markers.at(8).p;
mirror[0] = markers.at(9).p;
mirror[1] = markers.at(10).p;
mirror[2] = markers.at(11).p;
mirror[3] = markers.at(12).p;
double markersOffset=3.0;
double mirrorThickness=4.0;
for (int i=0; i<4; i++)
{
screen[i].x()-=markersOffset;
mirror[i] -= (markersOffset+mirrorThickness)*Vector3d(sqrt(2.0)/2,0,-sqrt(2.0)/2);
}
glutPostRedisplay();
glutTimerFunc(TIMER_MS, update, 0);
}
void updateTheMarkers()
{
optotrak.updateMarkers();
markers = optotrak.getAllMarkers();
if(system_calibrated)
online_calibrate_markers();
}
void initOptotrak()
{
optotrak.setTranslation(frameOrigin);
optotrak.init(LastAlignedFile,TS_N_MARKERS,TS_FRAMERATE,TS_MARKER_FREQ,TS_DUTY_CYCLE,TS_VOLTAGE);
for (int i=0; i<100; i++)
{ optotrak.updateMarkers();
markers = optotrak.getAllMarkers();
}
}
void initOptotrak()
{ optotrak.setTranslation(calibration);
if ( optotrak.init(LastAlignedFile) != 0)
{ exit(0);
}
for (int i=0; i<100; i++)
{ optotrak.updateMarkers();
markers = optotrak.getAllMarkers();
}
}
void initVariables()
{
coordinateFile.open("calibrationCoordinates.txt");
grid.setRowsAndCols(20,20); //here we set the number of columns and rows per edge
// here we measure in millimeters
grid.init(Vector3d(-500, -500, 0.0),Vector3d(500, -500, 0.0),Vector3d(-500, 500, 0.0),Vector3d(500, 500, 0.0));
optotrak.setTranslation(frameOrigin);
optotrak.init("C:/cncsvisiondata/camerafiles/Aligned20111014",TS_N_MARKERS,TS_FRAMERATE,TS_MARKER_FREQ,TS_DUTY_CYCLE,TS_VOLTAGE);
}
void initOptotrak()
{ optotrak.setTranslation(calibration);
if ( optotrak.init("cameraFiles/Aligned20110823") != 0)
{ cleanup();
exit(0);
}
for (int i=0; i<10; i++)
{ optotrak.updateMarkers();
markers = optotrak.getAllMarkers();
}
}
/**
* @brief initializeOptotrakMonitor
*/
void initializeOptotrakMonitor()
{
// Move the monitor in the positions
RoveretoMotorFunctions::homeMirror(3500);
RoveretoMotorFunctions::homeScreen(3500);
RoveretoMotorFunctions::homeObjectAsynchronous(3500);
optotrak.setTranslation(calibration);
if ( optotrak.init(LastAlignedFile) != 0)
{
plato_stop();
exit(0);
}
}
/**
* @brief idle
*/
void idle()
{
// Update markers
optotrak.updateMarkers();
markers = optotrak.getAllMarkers();
allVisiblePatch = isVisible(markers[1].p) && isVisible(markers[2].p) && isVisible(markers[3].p);
allVisibleHead = isVisible(markers[17].p) && isVisible(markers[18].p) && allVisiblePatch;
headEyeCoords.update(markers.at(1).p,markers.at(2).p,markers.at(3).p);
if ( trialTimer.getElapsedTimeInMilliSec() >= parameters.get("WaitTime") && trialMode==BLACK_MODE )
{
frame=0.0;
trialMode++;
trialTimer.start();
}
if ( trialTimer.getElapsedTimeInMilliSec() >= parameters.get("ProbeTime") && trialMode==PROBE_MODE )
{
frame=0.0;
trialMode++;
trialTimer.start();
}
if ( trialTimer.getElapsedTimeInMilliSec() >= parameters.get("StimulusTime") && trialMode==STIMULUS_MODE )
{
Beep(660,660);
frame=0.0;
trialMode=RESPONSE_MODE;
trialTimer.start();
}
markersFile << fixed << trialNumber << " " << headCalibrationDone << " " << trialMode << " " ;
markersFile << setprecision(3) <<
( isVisible(markers[1].p) ? markers[1].p.transpose() : junk ) << " " <<
( isVisible(markers[2].p) ? markers[2].p.transpose() : junk ) << " " <<
( isVisible(markers[3].p) ? markers[3].p.transpose() : junk ) << " " <<
( isVisible(markers[17].p) ? markers[17].p.transpose() : junk ) << " " <<
( isVisible(markers[18].p) ? markers[18].p.transpose() : junk ) << " " <<
( isVisible(markers[18].p) ? markers[18].p.transpose() : junk ) << " " <<
( isVisible(headEyeCoords.getLeftEye()) ? headEyeCoords.getLeftEye().transpose() : junk ) << " " <<
( isVisible(headEyeCoords.getRightEye()) ? headEyeCoords.getRightEye().transpose() : junk ) << " " ;
markersFile << setprecision(1) <<
trial.getCurrent().at("ZWidth") << " " <<
trial.getCurrent().at("Slant") << " " <<
trial.getCurrent().at("Tilt") << " " <<
trial.getCurrent().at("StimulusAnchored") << " " <<
endl;
}
void initOptotrak()
{
optotrak.setTranslation(calibration);
chdir("/usr/NDIoapi/ndigital/realtime/");
if ( optotrak.init(LastAlignedFile, OPTO_NUM_MARKERS, OPTO_FRAMERATE, OPTO_MARKER_FREQ, OPTO_DUTY_CYCLE,OPTO_VOLTAGE) != 0)
{ cerr << "Something during Optotrak initialization failed, press ENTER to continue. A error log has been generated, look \"opto.err\" in this folder" << endl;
cin.ignore(1E6,'\n');
exit(0);
}
// Read 10 frames of coordinates and fill the markers vector
for (int i=0; i<10; i++)
{
updateTheMarkers();
}
}
void cleanup()
{ // Stop the optotrak (Optotrak class should be RAII but for every evenience we stop it...)
optotrak.stopCollection();
for (int i=0; i<3; i++)
Beep(880,220);
boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
}
int main(int argc, char*argv[])
{
optotrak.setTranslation(calibration);
if ( optotrak.init(LastAlignedFile) != 0)
{ cleanup();
exit(0);
}
screen.setWidthHeight(SCREEN_WIDE_SIZE, SCREEN_WIDE_SIZE*SCREEN_HEIGHT/SCREEN_WIDTH);
screen.setOffset(alignmentX,alignmentY);
screen.setFocalDistance(focalDistance);
cam.init(screen);
recordHeadEyeRelativePositions();
glutInit(&argc, argv);
if (stereo)
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH | GLUT_STEREO);
else
glutInitDisplayMode( GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
if ( gameMode )
{
glutGameModeString(ROVERETO_GAME_MODE_STRING);
glutEnterGameMode();
}
else
{
glutInitWindowSize(SCREEN_WIDTH, SCREEN_HEIGHT);
glutCreateWindow("CNCSVISION Example 2 HappyBuddha");
glutFullScreen();
}
initRendering();
model.load("../../data/objmodels/happyBuddha.obj");
glutDisplayFunc(drawGLScene);
glutKeyboardFunc(handleKeypress);
glutReshapeFunc(handleResize);
glutTimerFunc(TIMER_MS, update, 0);
glutSetCursor(GLUT_CURSOR_NONE);
/* Application main loop */
glutMainLoop();
cleanup();
return 0;
}
void recordHeadEyeRelativePositions()
{ bool allVisible=false;
for (int i=0; i<10; i++)
{ optotrak.updateMarkers();
markers=optotrak.getAllMarkers();
allVisible= isVisible(markers[17].p) && isVisible(markers[18].p) && isVisible(markers[1].p) && isVisible(markers[2].p) && isVisible(markers[3].p) ;
cerr << "Move the head such that all markers are visible: Trial num " << i << endl;
cin.ignore( std::numeric_limits <std::streamsize> ::max(), '\n' );
cerr << allVisible << endl;
cerr << markers[17].p.transpose() << " " << markers[18].p.transpose() << endl;
if ( allVisible )
{ headEyeCoords.init(markers[17].p,markers[18].p,
markers[1].p,markers[2].p,markers[3].p,interoculardistance);
break;
}
}
}
// Questa funzione inizializza l'optotrak passandogli una traslazione di default "calibration" che
// in questo caso rappresenta la coordinata dell'occhio ciclopico (in realta' andrebbe bene qualsiasi valore ma questo
// allinea meglio coordinate optotrak e coordinate opengl cosicche abbiano lo zero molto vicino. IMPORTANTE: vedi di
// tenere "calibration" lo stesso in tutti gli esperimenti perche' altrimenti devi modificare anche
// alignmentX e alignmentY
// Se qualcosa nell'inizializzazione dell'optotrak non va hai due possibilita'
// 1) Leggerti il log che sta qui sotto
// 2) Leggerti il log generato nel file opto.err che normalmente dovrebbe essere nella cartella dove lanci l'eseguibile // altrimenti fatti una ricerca file.
// La funzione initOptotrak deve stare all'inizio di tutto.
void initMotorsOptotrak()
{
// Move the monitor in the positions
RoveretoMotorFunctions::homeMirror(3500);
RoveretoMotorFunctions::homeScreen(3000);
if (!quickStart)
{
RoveretoMotorFunctions::homeObjectAsynchronous(4500);
}
optotrak.setTranslation(calibration);
if ( optotrak.init(LastAlignedFile,24) != 0)
{
exit(0);
}
}
void update(int value)
{ markers = optotrak.getAllMarkers();
headEyeCoords.update(markers[1].p,markers[2].p,markers[3].p);
eyeLeft = headEyeCoords.getLeftEye();
eyeRight = headEyeCoords.getRightEye();
glutPostRedisplay();
glutTimerFunc(TIMER_MS, update, 0);
}
void handleKeypress(unsigned char key, int x, int y)
{
switch (key)
{ //Quit program
case 'q':
case 27:
{
optotrak.stopCollection();
Sleep(1000);
exit(0);
}
break;
case 'i':
infoDrawn=!infoDrawn;
break;
case ' ':
{
// Here we record the head shape - coordinates of eyes and markers, but centered in (0,0,0)
if ( headCalibrationDone==0 && allVisiblePatch )
{
headEyeCoords.init(markers[17].p,markers[18].p, markers[1].p,markers[2].p,markers[3].p,interoculardistance );
headCalibrationDone=1;
beepOk();
break;
}
// Second calibration, you must look a fixed fixation point
if ( headCalibrationDone==1 && allVisiblePatch)
{
headEyeCoords.init( headEyeCoords.getP1().p,headEyeCoords.getP2().p, markers[1].p, markers[2].p,markers[3].p,interoculardistance );
headCalibrationDone=2;
beepOk();
break;
}
if ( headCalibrationDone==2 && allVisiblePatch )
{ headEyeCoords.init( headEyeCoords.getP1().p,headEyeCoords.getP2().p, markers[1].p, markers[2].p,markers[3].p,interoculardistance );
beepOk();
break;
}
}
break;
// Enter key: press to make the final calibration
case 13:
{
if ( canCalibrate && headCalibrationDone == 2 && allVisiblePatch )
{
headEyeCoords.init( headEyeCoords.getP1().p,headEyeCoords.getP2().p, markers[1].p, markers[2].p,markers[3].p,interoculardistance );
headCalibrationDone=3;
infoDrawn=false;
for (int i=0; i<3; i++)
beepOk();
}
}
break;
}
}
/**
* @brief idle
*/
void idle()
{
double deltaT = (double)TIMER_MS;
optotrak.updateMarkers(deltaT);
markers = optotrak.getAllMarkers();
// Coordinates picker
allVisiblePlatform = isVisible(markers.at(15).p) && isVisible(markers.at(16).p);
allVisibleThumb = isVisible(markers.at(11).p) && isVisible(markers.at(12).p) && isVisible(markers.at(13).p);
allVisibleIndex = isVisible(markers.at(7).p) && isVisible(markers.at(8).p) && isVisible(markers.at(9).p);
allVisibleFingers = allVisibleThumb && allVisibleIndex;
allVisiblePatch = isVisible(markers.at(1).p) && isVisible(markers.at(2).p) && isVisible(markers.at(3).p);
allVisibleHead = allVisiblePatch && isVisible(markers.at(17).p) && isVisible(markers.at(18).p);
//if ( allVisiblePatch )
headEyeCoords.update(markers.at(1).p,markers.at(2).p,markers.at(3).p,deltaT);
// update thumb coordinates
thumbCoords.update(markers.at(11).p,markers.at(12).p,markers.at(13).p,deltaT);
// update index coordinates
indexCoords.update(markers.at(7).p, markers.at(8).p, markers.at(9).p,deltaT);
// Compute the coordinates of visual thumb
thumb = thumbCoords.getP1().p;
index = indexCoords.getP1().p;
eyeLeft = headEyeCoords.getLeftEye().p;
eyeRight = headEyeCoords.getRightEye().p;
rigidCurrentThumb.setRigidBody(markers.at(11).p,markers.at(12).p,markers.at(13).p);
rigidCurrentIndex.setRigidBody(markers.at(7).p,markers.at(8).p,markers.at(9).p);
rigidStartThumb.computeTransformation(rigidCurrentThumb);
rigidStartIndex.computeTransformation(rigidCurrentIndex);
if (headCalibrationDone==3 && fingerCalibrationDone==4)
{
if ( !allVisibleIndex || !allVisibleThumb || !isVisible(markers.at(6).p))
boost::thread invisibleBeep( beepInvisible);
Vector2d thumbProjection = cam.computeProjected(thumb);
thumbProjectedInside = ( thumbProjection.x() >= 0 && thumbProjection.x() <= SCREEN_WIDTH ) && ( thumbProjection.y() >= 0 && thumbProjection.y() <= SCREEN_HEIGHT );
Vector2d indexProjection = cam.computeProjected(index);
indexProjectedInside = ( indexProjection.x() >= 0 && indexProjection.x() <= SCREEN_WIDTH ) && ( indexProjection.y() >= 0 && indexProjection.y() <= SCREEN_HEIGHT );
trialFrame++;
// Work on fingers coordinates
double wThumb=0.5, wIndex=0.5;
Vector3d m = (wThumb*thumb+wIndex*index)/(wThumb+wIndex);
Vector3d d = thumb-index;
visualThumb = thumb;
visualIndex = index;
double clamp= 2*factors.getCurrent().at("StimulusRadius");
double gain = 1.0;//in quello di Bob era invece: factors.getCurrent().at("Gain");
if ( d.norm() > clamp )
{
visualThumb = m + 0.5*d.normalized()*(gain*d.blueNorm() - clamp*(gain-1));
visualIndex = m - 0.5*d.normalized()*(gain*d.blueNorm() - clamp*(gain-1));
}
// Check when to advance trial
double stimRadius = factors.getCurrent().at("StimulusRadius");
double radiusTolerance = util::str2num<double>(parameters.find("RadiusTolerance"));
fingNow = fingersInSphericalVolume(thumb,index, visualStimCenter, stimRadius-radiusTolerance, stimRadius+radiusTolerance, selectedFinger );
// SubjectName\tFingerDist\tTrialNumber\tTrialFrame\tTotTime\tVisualStimX\tVisualStimY\tVisualStimZ\tfStimulusRadius\tfDistances\tfGain\tEyeLeftXraw\tEyeLeftYraw\tEyeLeftZraw\tEyeRightXraw\tEyeRightYraw\tEyeRightZraw\tWristXraw\tWristYraw\tWristZraw\tThumbXraw\tThumbYraw\tThumbZraw\tIndexXraw\tIndexYraw\tIndexZraw\tVisualThumbXraw\tVisualThumbYraw\tVisualThumbZraw\tVisualIndexXraw\tVisualIndexYraw\tVisualIndexZraw\tIsDrawing\tIsThumbProjected\tIsIndexProjected
if (!isSaving)
return;
RowVector3d junk(9999,9999,9999);
markersFile << fixed << setprecision(3) <<
parameters.find("SubjectName") << "\t" <<
fingerDistance << "\t" <<
totalTrialNumber << "\t" <<
trialFrame << "\t" <<
globalTimer.getElapsedTimeInMilliSec() << "\t" <<
visualStimCenter.x() << "\t" <<
visualStimCenter.y() << "\t" <<
visualStimCenter.z() << "\t" <<
factors.getCurrent().at("StimulusRadius") << "\t" <<
factors.getCurrent().at("Distances") << "\t" <<
factors.getCurrent().at("Gain") << "\t" <<
factors.getCurrent().at("DisappearRadius") << "\t" <<
( isVisible(eyeLeft) ? eyeLeft.transpose() : junk ) << "\t" <<
( isVisible(eyeRight) ? eyeRight.transpose() : junk ) << "\t" <<
( isVisible(markers.at(6).p) ? markers.at(6).p.transpose() : junk ) << "\t" <<
( isVisible(thumb) ? thumb.transpose() : junk ) << "\t" <<
( isVisible(index) ? index.transpose() : junk ) << "\t" <<
( isVisible(visualThumb) ? visualThumb.transpose() : junk ) << "\t" <<
( isVisible(visualIndex) ? visualIndex.transpose() : junk ) << "\t" <<
isDrawing << "\t" <<
thumbProjectedInside << "\t" <<
indexProjectedInside << "\t" <<
fingNow <<
endl;
// Switch to this function if you want cylindrical stimulus
//fingersInCylindricalVolume( thumb,index, Vector3d(0,0,factors.getCurrent().at("Distances")), util::str2num<double>(parameters.find("StimulusHeight")), stimRadius,stimRadius+20, selectedFinger );
if (trialMode == STIMULUSMODE )
{
if ( !fingNow ) // always starts counting the time if the finger is outside
{
fingersTimer.start();
}
// only if the finger is inside the volume the counter has counted more that 1 second
if ( fingersTimer.getElapsedTimeInMilliSec() > util::str2num<double>(parameters.find("LeaningTime")) )
{
plato_write(PLATO_LEFT_RIGHT_CLOSED);
advanceTrial();
plato_write(PLATO_LEFT_RIGHT_OPEN);
}
trialTimer.start(); // continue keeping it starting during stimulus
drawingTrialFrame++;
invisibleIndexFrames+=!allVisibleIndex;
invisibleThumbFrames+=!allVisibleThumb;
invisibleWristFrames+=!isVisible(markers.at(6).p);
}
else
{
if ( trialTimer.getElapsedTimeInMilliSec() > util::str2num<double>(parameters.find("IntervalTime")) )
{
boost::thread trialBeep( beepTrial );
trialMode=STIMULUSMODE;
}
}
isDrawing = (trialMode == STIMULUSMODE);
}
}
void idle()
{
if (trialNumber >= maxTotalTrials )
exit(0);
double elapsedFrameTime = totalTimer.getTimeIntervalInMilliSec();
optotrak.updateMarkers(elapsedFrameTime);
markers = optotrak.getAllMarkers();
headEyeCoords.update(markers[1],markers[2],markers[3],TIMER_MS);
allVisiblePatch = markers[1].isVisible() && markers[2].isVisible()
&& markers[3].isVisible();
allVisibleHead = markers[17].isVisible() && markers[18].isVisible() && allVisibleHead;
eyeLeft = headEyeCoords.getLeftEye().p;
eyeRight = headEyeCoords.getRightEye().p;
cyclopeanEye = (eyeLeft+eyeRight)/2.0;
projPointEyeRight = getEyeProjectionPoint();
checkBounds(nOscillationsFixation,
eyeRight.x(),
trialMode,
headCalibrationDone,
minOscTime,
maxOscTime,
maxXOscillation,
translationTimer,
beepOk,
tweeter,
woofer,tweeter);
if ( trialMode == STIMULUSMODE )
deltaT+=TIMER_MS;
else
deltaT=0;
if (headCalibrationDone == 3 && trialMode != PROBEMODE )
{
// Questo rende conto del fatto che lo stimolo appare solo quando l'occhio è quasi in centro
int actualTrialMode = trialMode;
if ( trialMode == STIMULUSMODE && ( eyeRight.x()) > centerTolerance )
actualTrialMode=FIXATIONMODE;
markersFile << fixed << trialNumber << " " << actualTrialMode << " " ;
markersFile << fixed << setprecision(3) << eyeRight.transpose() << " " << eyeLeft.transpose() << " " << toDegrees(headEyeCoords.getPitch()) << " " << toDegrees(headEyeCoords.getYaw()) << " " << toDegrees(headEyeCoords.getRoll()) << " " ;
markersFile << fixed << setprecision(0)<<
factors["OmegaY"] << " " <<
factors["Binocular"] << " " <<
factors["Tilt"] << " " <<
factors["Slant"] << " " <<
totalTimer.getElapsedTimeInMilliSec() << endl;
//objectPassiveTransformation.setIdentity();
if ( actualTrialMode == STIMULUSMODE )
{
objectPassiveTransformation = getPassiveMatrix();
matrixFile << setw(6) << left <<
trialNumber << " " ;
for ( int i=0; i<3; i++)
matrixFile << objectPassiveTransformation.matrix().row(i) << " " ;
matrixFile << endl;
}
if ( actualTrialMode == STIMULUSMODE )
{
vector< Vector3d> projPoints = stimDrawer.projectStimulusPoints(objectActiveTransformation,headEyeCoords.getRigidStart().getFullTransformation(),cam,focalDistance,screen,Vector3d(0,0,0),false,false);
MatrixXd a1toa6 = stimDrawer.computeOpticFlow(projPoints, focalDistance, elapsedFrameTime/1000);
flowsFile << trialNumber << " " << a1toa6.transpose() << endl;
}
}
writeContinuosDataFile();
}
void idle()
{ // Set the time during which the stimulus is drawn, it depents on the phase (adaption or test)
double drawStimTime = 0;
if ( block.at("Phase") == 1 )
drawStimTime = str2num<double>(parameters.find("AdaptStimulusDuration"));
else
drawStimTime = str2num<double>(parameters.find("TestStimulusDuration"));
double deltaT = (double)TIMER_MS;
optotrak.updateMarkers(deltaT);
markers = optotrak.getAllMarkers();
// Coordinates picker
allVisiblePlatform = isVisible(markers.at(15).p) && isVisible(markers.at(16).p);
allVisibleThumb = isVisible(markers.at(11).p) && isVisible(markers.at(12).p) && isVisible(markers.at(13).p);
allVisibleIndex = isVisible(markers.at(7).p) && isVisible(markers.at(8).p) && isVisible(markers.at(9).p);
allVisibleFingers = allVisibleThumb && allVisibleIndex;
allVisiblePatch = isVisible(markers.at(1).p) && isVisible(markers.at(2).p) && isVisible(markers.at(3).p);
allVisibleHead = allVisiblePatch && isVisible(markers.at(18).p);
//cerr << markers.at(15).p.transpose() << endl; // stampa la coordinata del marker 15 preso come oggetto
//if ( allVisiblePatch )
headEyeCoords.update(markers.at(1).p,markers.at(2).p,markers.at(3).p,deltaT);
// update thumb coordinates
thumbCoords.update(markers.at(11).p,markers.at(12).p,markers.at(13).p,deltaT);
// update index coordinates
indexCoords.update(markers.at(7).p, markers.at(8).p, markers.at(9).p,deltaT);
eyeLeft = headEyeCoords.getLeftEye().p;
eyeRight = headEyeCoords.getRightEye().p;
// cerr << "Idle 888" << endl;
checkBounds();
indexInside[1]=indexInside[0];
// Controlla i frames occlusi, se il dito di test è fuori dalla sfera
// ed ha superato una percentuale di tempo maggiore di
// TestPercentOccludedFrames rispetto al tempo di presentazione dello
// stimolo allora fa un beep
if ( headCalibrationDone==3 && fingerCalibrationDone==3 && !indexInside[0] && (int) block.at("Phase")!=1 && isDrawing )
{
bool visibleFinger=true;
switch ( str2num<int>(parameters.find("TestFinger")) )
{
case 0:
visibleFinger=isVisible(thumbCoords.getP1().p);
break;
case 1:
visibleFinger=isVisible(indexCoords.getP1().p);
break;
case 2:
visibleFinger=isVisible(thumbCoords.getP1().p) && isVisible(indexCoords.getP1().p);
break;
}
// XXX scommentare per far si che anche il polso venga controllato per missing frames
//visibleFinger = visibleFinger && isVisible(markers.at(6).p);
if ( !visibleFinger )
{ occludedFrames++;
if ( str2num<int>(parameters.find("AudioFeedback") ) )
boost::thread invisibleBeep( beepInvisible);
}
trialOk=true; // mentre disegna lo stimolo
}
// Qui durante la fase di delay dopo la scomparsa dello stimolo controlla che il numero
// di frames occlusi non abbia superato il limite consentito
if ( isDrawing==0 && headCalibrationDone==3 && (block.at("Phase")!=1) && !indexInside[0] )
{
double percentOccludedFrames = ((double)occludedFrames/(double)drawingTrialFrame )*100.0;
trialOk=(percentOccludedFrames < str2num<double>(parameters.find("TestPercentOccludedFrames")));
}
// Calcola le coordinate delle dita offsettate durante la fase di adattamento
if ( block.at("Phase") == 1 )
{ double fingerOffset = adaptOffsets.at(block1TrialNumber) ;
switch ( str2num<int>(parameters.find("AdaptFinger")) )
{
case 0: //index
visualThumb = thumbCoords.getP1().p + Vector3d(0,0,fingerOffset);
break;
case 1: //thumb
visualIndex = indexCoords.getP1().p + Vector3d(0,0,fingerOffset);
break;
case 2:
{ visualThumb = thumbCoords.getP1().p + Vector3d(0,0,fingerOffset);
visualIndex = indexCoords.getP1().p + Vector3d(0,0,fingerOffset);
}
break;
}
drawingTrialFrame=0;
occludedFrames=0;
}
if ( headCalibrationDone==3 && fingerCalibrationDone==3 && trialMode==STIMULUSMODE )
{
if ( globalTimer.getElapsedTimeInMilliSec() <= drawStimTime )
{ isDrawing=1;
delayedTimer.start();
}
else
{ // non disegna nulla
if ( delayedTimer.getElapsedTimeInMilliSec() < str2num<int>(parameters.find("DelayTime")) )
isDrawing=0;
else
{ delayedTimer.start();
advanceTrial();
}
}
}
if ( (int) block.at("Phase") != 1 && isDrawing==1 )
{ drawingTrialFrame++;
}
#define WRITE
int thisBlockTrialNumber=0;
switch ( block.at("Phase") )
{
case 0:
thisBlockTrialNumber=block0TrialNumber;
break;
case 1:
thisBlockTrialNumber=block1TrialNumber;
break;
case 2:
thisBlockTrialNumber=block2TrialNumber;
break;
}
if ( !isMovingRod )
{
RowVector3d junk(9999,9999,9999);
// Write to file
if ( headCalibrationDone==3 && fingerCalibrationDone==3 )
{
if ( totalFrame==0)
{
markersFile << fixed << setprecision(3) <<
"SubjectName\tAdaptFinger\tTestFinger\tAdaptOffset\tAdaptHapticFeedback\tFingerDist\tTotalTrial\tPhase\tTrialNumber\tTrialFrame\tdT\tTotTime\tIndexInside\tIsDrawing\tTrialOk\tVisualStimX\tVisualStimY\tVisualStimZ\tHapticRodX\tHapticRodY\tHapticRodZ\tfRelDepth\tfStimulusHeight\tfDistances\tDeltaXRods\tEyeLeftXraw\tEyeLeftYraw\tEyeLeftZraw\tEyeRightXraw\tEyeRightYraw\tEyeRightZraw\tWristXraw\tWristYraw\tWristZraw\tThumbXraw\tThumbYraw\tThumbZraw\tIndexXraw\tIndexYraw\tIndexZraw" << endl;
totalFrame++;
}
markersFile << fixed << setprecision(3) <<
parameters.find("SubjectName") << "\t" <<
parameters.find("AdaptFinger") << "\t" <<
parameters.find("TestFinger") << "\t" <<
adaptOffsets.at(block1TrialNumber) << "\t" <<
parameters.find("AdaptHapticFeedback") << "\t" <<
fingerDistance << "\t" <<
totalTrialNumber << "\t" <<
block.at("Phase") << "\t" <<
thisBlockTrialNumber << "\t" <<
trialFrame << "\t" <<
deltaT*1000 << "\t" <<
totalTime.getElapsedTimeInMilliSec() << "\t" <<
indexInside[0] << "\t" <<
isDrawing << "\t" <<
trialOk << "\t" <<
visualRodCenter.transpose() << "\t" <<
hapticRodCenter.transpose() << "\t" <<
((block.at("Phase")!=1) ? factors.at("RelDepth") : 9999) << "\t" <<
((block.at("Phase")!=1) ? factors.at("StimulusHeight") : 9999) << "\t" <<
((block.at("Phase")!=1) ? factors.at("Distances") : 9999) << "\t" <<
deltaXRods << "\t" <<
( isVisible(eyeLeft) ? eyeLeft.transpose() : junk ) << "\t" <<
( isVisible(eyeRight) ? eyeRight.transpose() : junk ) << "\t" <<
( isVisible(markers.at(6).p) ? markers.at(6).p.transpose() : junk ) << "\t" <<
( isVisible(thumbCoords.getP1().p) ? thumbCoords.getP1().p.transpose() : junk ) << "\t" <<
( isVisible(indexCoords.getP1().p) ? indexCoords.getP1().p.transpose() : junk ) << endl;
}
trialFrame++;
}
#ifdef SAVEDATADAT
eulerAngles.init( headEyeCoords.getRigidStart().getFullTransformation().rotation() );
// Write datafile to real-time streaming of data
ofstream outputfile;
outputfile.open("data.dat");
outputfile << "Subject Name: " << parameters.find("SubjectName") << endl;
outputfile << "AdaptHapticFeedback: " << parameters.find("AdaptHapticFeedback") << endl;
outputfile << "AdaptFinger: " << parameters.find("AdaptFinger") << endl;
outputfile << "TestFinger: " << parameters.find("TestFinger") << endl;
outputfile << "IndexInside: " << indexInside[0] << endl;
//outputfile << "Yaw: " << toDegrees(eulerAngles.getYaw()) << endl <<"Pitch: " << toDegrees(eulerAngles.getPitch()) << endl << "Roll: " << toDegrees(eulerAngles.getRoll()) << endl;
outputfile << "EyeLeft: " << headEyeCoords.getLeftEye().p.transpose() << endl;
outputfile << "EyeRight: " << headEyeCoords.getRightEye().p.transpose() << endl << endl;
outputfile << "Thumb: " << thumbCoords.getP1().p.transpose() << endl;
outputfile << "Index: " << indexCoords.getP1().p.transpose() << endl;
outputfile << "Wrist: " << markers.at(6).p.transpose() << endl;
outputfile << "IsDrawing: " << isDrawing << " TrialOk: " << trialOk << " OccludedFrames: " << occludedFrames << " DrawingTrialFrames: " << drawingTrialFrame << endl;
outputfile << "HapticRodCenter: " << hapticRodCenter.transpose() << endl;
outputfile << "VisualRodCenter: " << visualRodCenter.transpose() << endl;
outputfile << "Markers(5): " << markers.at(5).p.transpose() << endl;
outputfile << "AdaptOffset: " << adaptOffsets.at(block1TrialNumber) << endl;
outputfile << "Phase: " << block.at("Phase") << endl;
outputfile << "TrialNumber: " << thisBlockTrialNumber << endl;
outputfile << "Total trials: " << totalTrialNumber << endl;
#endif
frameTimer.start();
}
void cleanup()
{
// Stop the optotrak
optotrak.stopCollection();
}
// Questa funzione e' quella che in background fa tutti i conti matematici, quindi qui devi inserire
// 1) Scrittura su file continua delle coordinate che vuoi salvare
// 2) Estrazione delle coordinate a partire dai corpi rigidi precedentemente definiti vedi ad esempio
// come e' fatto per eyeLeft e eyeRight oppure per thumb ed index
void idle()
{
optotrak.updateMarkers();
markers = optotrak.getAllMarkers();
// Coordinates picker //is this correct? (are the marker identities the same?)
allVisiblePlatform = isVisible(markers[15].p) && isVisible(markers[16].p);
allVisibleThumb = isVisible(markers[20].p) && isVisible(markers[21].p) && isVisible(markers[22].p);
allVisibleIndex = isVisible(markers[22].p) && isVisible(markers[23].p) && isVisible(markers[24].p);
allVisibleFingers = allVisibleIndex && allVisibleThumb;
if ( allVisibleFingers )
{
thumbCoords.update(markers[20].p, markers[21].p, markers[22].p );
indexCoords.update(markers[19].p, markers[23].p, markers[24].p );
}
eyeRight = Vector3d(0,0,0);
index = indexCoords.getP1();
thumb = thumbCoords.getP1();
//////////////////////////
/// Real Stuff
//////////////////////////
if (fingerCalibrationDone==3) { // After we've calibrated
if ( !allVisibleFingers ) { // Check for finger occlusion
beepOk(0);
if (started && !reachedObject) // Increment counter if hand is in flight
numLostFrames += 1;
}
frameNumber++; // Advance frame number
// Check that we're at the start position
if( (index.y() < startPos_top) && // index below ceiling
(index.y() > startPos_bottom) && // index above floor
(index.x() < startPos_right) && // index left of right wall
(index.x() > startPos_left) && // index right of left wall
(index.z() < startPos_rear) && // index in front of rear wall
(index.z() > startPos_front) ) { // index behind front wall
// if so, keep resetting timer (need this outside the "fingers together" loop!)
timer.start();
// distance from index to thumb less than 1.5cm in each direction
if ((abs(index.y() - thumb.y()) < 15) &&
(abs(index.x() - thumb.x()) < 15) &&
(abs(index.z() - thumb.z()) < 15) ) {
// if so, we are in the start position
handAtStart = true;
}
} else if (allVisibleFingers) { // we've moved from the start, begin counting
handAtStart = false;
started = true;
start_frame = frameNumber;
if (!reachedObject) //(trial.getCurrent().at("OpenLoop") && !reachedObject)
plato_write(3);
}
// Find component distances
grip_X = (index.x()+thumb.x())/2;
grip_Y = (index.y()+thumb.y())/2;
grip_Z = (index.z()+thumb.z())/2;
x_dist = abs(grip_X - target_X);
y_dist = abs(grip_Y - target_Y);
z_dist = abs(grip_Z - target_Z);
plato_trigger = index.z() - (target_Z+20);
// Distance formulas
grip_aperture = sqrt(
(abs(index.x() - thumb.x())*abs(index.x() - thumb.x())) +
(abs(index.y() - thumb.y())*abs(index.y() - thumb.y())) +
(abs(index.z() - thumb.z())*abs(index.z() - thumb.z())));
grip_distanceToTarget = sqrt((x_dist*x_dist)+(y_dist*y_dist)+(z_dist*z_dist));
// If hand is in flight
if (!reachedObject && started && allVisibleFingers) {
// PLATO Threshold
if ( (plato_trigger < 0) && (plato_trigger > -1000) )
{
reachedObject = true;
//if (trial.getCurrent().at("OpenLoop"))
plato_write(0);
TGA_frame = frameNumber - start_frame;
TGA_time = timer.getElapsedTimeInMilliSec();
}
}
if(handAtStart && started)
advanceTrial();
}
// Write to trialFile
if (fingerCalibrationDone==3 )
{
int TN;
if (controlCondition)
TN = controlTrial;
else
TN = trialNumber;
trialFile << fixed <<
parameters.find("SubjectName") << "\t" << //subjName
TN << "\t" << //trialN
timer.getElapsedTimeInMilliSec() << "\t" << //time
frameNumber << "\t" << //frameN
index.transpose() << "\t" << //indexXraw, indexYraw, indexZraw
thumb.transpose() << "\t" << //thumbXraw, thumbYraw, thumbZraw
grip_distanceToTarget << "\t" <<
allVisibleFingers << "\t" << //allVisibleFingers
reachedObject << endl;
}
}
void cleanup()
{ optotrak.stopCollection();
}