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); }
/*** Online operations ***/ void online_fingers() { allVisibleReferenceMarkers = isVisible(markers.at(calibration1).p) && isVisible(markers.at(calibration2).p) && isVisible(markers.at(calibration3).p) && isVisible(markers.at(calibration4).p); allVisibleIndex = isVisible(markers.at(ind1).p) && isVisible(markers.at(ind2).p) && isVisible(markers.at(ind3).p); allVisibleThumb = isVisible(markers.at(thu1).p) && isVisible(markers.at(thu2).p) && isVisible(markers.at(thu3).p); allVisibleFingers = allVisibleIndex && allVisibleThumb; allVisibleCalibrationMarkers = isVisible(markers.at(indexcal).p) && isVisible(markers.at(thumbcal).p); if(allVisibleCalibrationMarkers && fingerCalibrationDone == 0) fingerCalibrationDone = 1; // fingers coordinates, fingersOccluded and framesOccluded if ( fingerCalibrationDone >= 3) { if(allVisibleIndex) { indexCoords.update(markers.at(ind1).p, markers.at(ind2).p, markers.at(ind3).p ); #ifndef SIMULATION ind = indexCoords.getP1(); #endif } if(allVisibleThumb) { thumbCoords.update(markers.at(thu1).p, markers.at(thu2).p, markers.at(thu3).p ); #ifndef SIMULATION thu = thumbCoords.getP1(); #endif } } // what the program checks online during the grasp if (fingerCalibrationDone==5 ) { if(experiment) { // Write to responseFile markersFile << fixed << parameters.find("SubjectName") << "\t" << trialNumber << "\t" << timer.getElapsedTimeInMilliSec() << "\t" << //time frameN << "\t" << //frameN ind.transpose() << "\t" << //indexXraw, indexYraw, indexZraw thu.transpose() << "\t" << //thumbXraw, thumbYraw, thumbZraw fingersOccluded << "\t" << //fingersOccluded framesOccluded << "\t" << //framesOccluded iGrasped << endl; } } }
void initCalibration() { // Now wind the input points file while the head calibration isn't done while ( true ) { // Here we load the variables needed to keep track of the experiment status readline(inputStream, trialNumber, headCalibration, trialMode, pointMatrix ); //First phase of calibration (equivalent when spacebar is pressed first time ) if ( (headCalibration== 1) && (headCalibrationDone==0 )) { headEyeCoords.init(pointMatrix.col(3),pointMatrix.col(4), pointMatrix.col(0),pointMatrix.col(1),pointMatrix.col(2),interoculardistance ); headCalibrationDone=headCalibration; //cerr << headCalibrationDone << endl; } // Second phase of calibration (equivalent when space bar is pressed second time ) if ( (headCalibration== 2) && (headCalibrationDone==1 )) { headEyeCoords.init( headEyeCoords.getP1(),headEyeCoords.getP2(), pointMatrix.col(0),pointMatrix.col(1),pointMatrix.col(2),interoculardistance ); eyeCalibration=headEyeCoords.getRightEye(); headCalibrationDone=headCalibration; } // Third and final phase of calibration ( equivalent when spacebar is pressed third time ) if ((headCalibration==3)) { headEyeCoords.init( headEyeCoords.getP1(),headEyeCoords.getP2(), pointMatrix.col(0),pointMatrix.col(1),pointMatrix.col(2),interoculardistance ); eyeCalibration=headEyeCoords.getRightEye(); headCalibrationDone=3; break; // exit the while cycle } // simulates the update of head and eyes positions if ( headCalibration==headCalibrationDone) headEyeCoords.update(pointMatrix.col(0),pointMatrix.col(1),pointMatrix.col(2)); } }
void update(int value) { optotrak.updatePoints(); markers = optotrak.getAllPoints(); headEyeCoords.update(markers[1],markers[2],markers[3]); modelCoordinates.update(markers[10],markers[19],markers[20]); if ( isVisible(markers[20]+markers[19]+markers[10]) ) { rigidCurrent.setRigidBody(markers[20], markers[19],modelCoordinates.getFinger() ); rigidAux.computeTransformation(rigidCurrent,true); // true the scaling matrix } eyeLeft = headEyeCoords.getLeftEye(); eyeRight = headEyeCoords.getRightEye(); glutPostRedisplay(); glutTimerFunc(TIMER_MS, update, 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); }
/** * @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 idle() { Timer frameTimer; frameTimer.start(); // Timing things timeFrame+=1; double oscillationPeriod = factors.at("StimulusDuration")*TIMER_MS; switch (stimMotion) { case SAWTOOTH_MOTION: periodicValue = oscillationAmplitude*mathcommon::sawtooth(timeFrame,oscillationPeriod); break; case TRIANGLE_MOTION: periodicValue = oscillationAmplitude*mathcommon::trianglewave(timeFrame,oscillationPeriod); break; case SINUSOIDAL_MOTION: periodicValue = oscillationAmplitude*sin(3.14*timeFrame/(oscillationPeriod)); break; default: SAWTOOTH_MOTION; } timingFile << totalTimer.getElapsedTimeInMilliSec() << " " << periodicValue << endl; // Simulate head translation // Coordinates picker markers[1] = Vector3d(0,0,0); markers[2] = Vector3d(0,10,0); markers[3] = Vector3d(0,0,10); headEyeCoords.update(markers[1],markers[2],markers[3]); eyeLeft = headEyeCoords.getLeftEye(); eyeRight = headEyeCoords.getRightEye(); Vector3d fixationPoint = (headEyeCoords.getRigidStart().getFullTransformation() * ( Vector3d(0,0,focalDistance) ) ); // Projection of view normal on the focal plane Eigen::ParametrizedLine<double,3> pline = Eigen::ParametrizedLine<double,3>::Through(eyeRight,fixationPoint); projPoint = pline.intersection(focalPlane)*((fixationPoint - eyeRight).normalized()) + eyeRight; stimTransformation.matrix().setIdentity(); stimTransformation.translation() <<0,0,focalDistance; Timer sleepTimer; sleepTimer.sleep((TIMER_MS - frameTimer.getElapsedTimeInMilliSec())/2); }
// 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() { timeFile << globalTimer.getElapsedTimeInMilliSec() << endl; optotrak->updateMarkers(); markers = optotrak->getAllMarkers(); // Coordinates picker allVisiblePatch = isVisible(markers[5].p) && isVisible(markers[6].p) && isVisible(markers[7].p); headEyeCoords.update(markers[5],markers[6],markers[7]); eyeLeft = headEyeCoords.getLeftEye(); eyeRight = headEyeCoords.getRightEye(); eyeRight = Vector3d(interoculardistance/2,0,0); eyeLeft = -eyeRight; thumb = thumbCoords.getP1(); index = indexCoords.getP1(); }
// 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(); /*cerr << trial.getCurrent()["AbsDepth"] << "\t" << trial.getCurrent()["ObjHeight"] << "\t" << trial.getCurrent()["HapticFB"] << endl; */ //cerr << parameters.find("fObjHeight",1) << endl; markers = optotrak->getAllMarkers(); // Coordinates picker allVisiblePlatform = isVisible(markers[1].p); allVisibleIndex = isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p); allVisibleThumb = isVisible(markers[15].p) && isVisible(markers[17].p) && isVisible(markers[18].p); allVisibleFingers = allVisibleIndex && allVisibleThumb; allVisiblePatch = isVisible(markers[5].p) && isVisible(markers[6].p) && isVisible(markers[7].p); allVisibleHead = allVisiblePatch && isVisible(markers[1].p); mirrorAlignment = asin( abs((markers[6].p.z()-markers[7].p.z()))/ sqrt( pow(markers[6].p.x()-markers[7].p.x(), 2) + pow(markers[6].p.z()-markers[7].p.z(), 2) ) )*180/M_PI; if ( allVisiblePatch ) headEyeCoords.update(markers[5].p,markers[6].p,markers[7].p); if ( allVisibleFingers ) { indexCoords.update(markers[13].p, markers[14].p, markers[16].p ); thumbCoords.update(markers[15].p, markers[17].p, markers[18].p ); fingersOccluded = 0; } if (fingerCalibrationDone==3 ) { // check for finger occlusion if ( !allVisibleFingers ) { fingersOccluded=1; num_lost_frames += 1; } // advance frame number frameN++; // check that we're at the start // if so, keep resetting timer if(((-500) < index.y()) && (index.y() < (-315)) && ((-500) < thumb.y()) && (thumb.y() < (-315))) { handAtStart = true; timer.start(); } else // we've moved from the start, begin counting { handAtStart = false; } // if we're in the reaching area if(!handAtStart && ((-315) <= index.y()) && (index.y() < 200) && ((-315) <= thumb.y()) && (thumb.y() < 100)) { started=true; // we've started }else{ started=started; } // find y_dist and z_dist y_dist = (abs(index.y() - (-25)) + abs(thumb.y() - (-25))); z_dist = (abs(index.z() - (trial.getCurrent()["AbsDepth"])) + abs(thumb.z() - (trial.getCurrent()["AbsDepth"]))); within_time_limit = (timer.getElapsedTimeInMilliSec() <= str2num<double>(parameters.find("TimeLimit"))); if(!reachedObject && (stimPosn==LEFT) && started) { x_dist = (abs(index.x() - (-26)) + abs(thumb.x() - (-26))); if ( (x_dist <= 25) && (y_dist <= 70) && (z_dist <= 25) && within_time_limit) { reachedObject = true; TGA_frame = frameN; } }else if(!reachedObject && (stimPosn==RIGHT) && started) { x_dist = (abs(index.x() - (74)) + abs(thumb.x() - (74))); if ( (x_dist <= 25) && (y_dist <= 70) && (z_dist <= 25) && within_time_limit) { reachedObject = true; TGA_frame = frameN; } } } if(headCalibration) { eyeLeft = headEyeCoords.getLeftEye(); eyeRight = headEyeCoords.getRightEye(); } else { eyeRight = Vector3d(interoculardistance/2,0,0); eyeLeft = -eyeRight; } index = indexCoords.getP1(); thumb = thumbCoords.getP1(); // Write to trialFile if (fingerCalibrationDone==3 ) { trialFile << fixed << parameters.find("SubjectName") << "\t" << //subjName trialNumber << "\t" << //trialN timer.getElapsedTimeInMilliSec() << "\t" << //time frameN << "\t" << //frameN index.transpose() << "\t" << //indexXraw, indexYraw, indexZraw thumb.transpose() << "\t" << //thumbXraw, thumbYraw, thumbZraw //eyeRight.transpose() << "\t" << //eyeRXraw, eyeRYraw, eyeRZraw //eyeLeft.transpose() << "\t" << //eyeLXraw, eyeLYraw, eyeLZraw fingersOccluded << "\t" << //fingersOccluded attempt << "\t" << reachedObject << "\t" << num_lost_frames ; trialFile << endl; } }
// 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(); //cerr << deltaT << endl; markers = optotrak->getAllMarkers(); // Coordinates picker allVisiblePlatform = isVisible(markers[1].p); allVisibleIndex = isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p); allVisibleFingers = allVisibleIndex; allVisiblePatch = isVisible(markers[5].p) && isVisible(markers[6].p) && isVisible(markers[7].p); allVisibleHead = allVisiblePatch && isVisible(markers[1].p); if ( allVisiblePatch ) headEyeCoords.update(markers[5].p,markers[6].p,markers[7].p); if ( allVisibleIndex ) { indexCoords.update(markers[13].p, markers[14].p, markers[16].p ); viewingFrames=1; occludedFrames = occludedFrames; } if (fingerCalibrationDone==3 ) { if ( !allVisibleIndex ) { viewingFrames=0; occludedFrames++; } if(index.z() < (-180) && markers[14].v.norm() < 250) frontRodShown=true; else frontRodShown=frontRodShown; if(index.z() < (-180) && markers[14].v.norm() < 200) timeStim++; else timeStim=timeStim; } if(!frontRodShown) frame++; else frame=frame; if(headCalibration) { eyeLeft = headEyeCoords.getLeftEye(); eyeRight = headEyeCoords.getRightEye(); } else { eyeRight = Vector3d(interoculardistance/2,0,0); eyeLeft = -eyeRight; } index = indexCoords.getP1(); singleMarker = markers.at(4).p.transpose(); lastPos[frame%NFrames] = index; /* if(index.z() > (startingPoint - 40.0)) fingersAtStart=true; else fingersAtStart=false; if(fingersAtStart) timer.start(); if(endTrial) drawBlack(); if( fingersAtStart && endTrial ) { advanceTrial(); timer.start(); } */ // #ifdef WRITE // Write to file if (fingerCalibrationDone==3 ) { markersFile << fixed << trialNumber << "\t" << index.transpose() << "\t" << frame << "\t" << occludedFrames << "\t" << viewingFrames << "\t" << markers[14].v.norm() << "\t" << timeStim << "\t" << timer.getElapsedTimeInMilliSec() ; markersFile << endl; velocityFile << fixed << markers[14].p.transpose() << "\t" << markers[14].v.transpose() << "\t" << markers[14].v.norm() // sqrt( vx^2+vy^2+vz^2 ) << endl; //markers[14].v.z() } //#endif }
// 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(); //cerr << deltaT << endl; markers = optotrak->getAllMarkers(); // Coordinates picker allVisiblePlatform = isVisible(markers[1].p); allVisibleIndex = isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p); allVisibleThumb = isVisible(markers[15].p) && isVisible(markers[17].p) && isVisible(markers[18].p); allVisibleFingers = allVisibleIndex && allVisibleThumb; allVisiblePatch = isVisible(markers[5].p) && isVisible(markers[6].p) && isVisible(markers[7].p); allVisibleHead = allVisiblePatch && isVisible(markers[1].p); if ( allVisiblePatch ) headEyeCoords.update(markers[5].p,markers[6].p,markers[7].p); if ( allVisibleFingers ) { indexCoords.update(markers[13].p, markers[14].p, markers[16].p ); thumbCoords.update(markers[15].p, markers[17].p, markers[18].p ); fingersOccluded = 0; } if (fingerCalibrationDone==3 ) { if ( !allVisibleFingers ) { fingersOccluded=1; } frameN++; if(condition == 0) condition = unifRand(-1, 1); if(index.z() < (-180) && index.y() > -150 && markers[14].v.norm() < 90) pointing=true; else pointing=pointing; if(index.z() > (-80)) handAtStart = true; else handAtStart = false; if(handAtStart) checkHandAtStart = true; else checkHandAtStart = checkHandAtStart; if(!checkHandAtStart) timer.start(); if(pointing && !handRecorded) { recFHP = index.z(); tFHP = timer.getElapsedTimeInMilliSec(); handRecorded = true; } else { recFHP = recFHP; tFHP = tFHP; handRecorded = handRecorded; } if(handAtStart && !pointing) { timer.start(); started=false; } else started=true; if(condition < 0) // vision then pointing { if(timer.getElapsedTimeInMilliSec() > stepVisual) // move to drawing the cross { moveToPartII = true; step = stepVisual; } if(moveToPartII && timer.getElapsedTimeInMilliSec() > step + 500) // move to drawing the first stimulus { moveToPartIII = true; step = step + 500; } if(moveToPartIII && timer.getElapsedTimeInMilliSec() > step + 800) // move to drawing black { moveToPartIV = true; step = step + 800; } } else // pointing then vision { if(tFHP > 0) // move to drawing the cross { moveToPartII = true; timeToFHP = tFHP; step = tFHP; } if(moveToPartII && timer.getElapsedTimeInMilliSec() > step + 500) // move to drawing the first stimulus { moveToPartIII = true; step = step + 500; } if(moveToPartIII && timer.getElapsedTimeInMilliSec() > step + 800 && !training) // move to drawing black { moveToPartIV = true; step = step + 800; } } } if(headCalibration) { eyeLeft = headEyeCoords.getLeftEye(); eyeRight = headEyeCoords.getRightEye(); } else { eyeRight = Vector3d(interoculardistance/2,0,0); eyeLeft = -eyeRight; } index = indexCoords.getP1(); thumb = thumbCoords.getP1(); singleMarker = markers.at(4).p.transpose(); /* if(pause && !paused) { paused = true; } else if(paused && !pause) { initTrial(); paused = false; } */ // Write to trialFile if (fingerCalibrationDone==3 ) { trialFile << fixed << parameters.find("SubjectName") << "\t" << //subjName trialNumber << "\t" << //trialN timer.getElapsedTimeInMilliSec() << "\t" << //time frameN << "\t" << //frameN index.transpose() << "\t" << //indexXraw, indexYraw, indexZraw thumb.transpose() << "\t" << //thumbXraw, thumbYraw, thumbZraw eyeRight.transpose() << "\t" << //eyeRXraw, eyeRYraw, eyeRZraw eyeLeft.transpose() << "\t" << //eyeLXraw, eyeLYraw, eyeLZraw fingersOccluded << "\t" << //fingersOccluded markers[14].v.norm() << "\t" << //indexVelraw recFHP << "\t" << tFHP << "\t" << part << "\t" << step << "\t" << moveToPartII << "\t" << moveToPartIII << "\t" << moveToPartIV << "\t" << firstD << "\t" << handRecorded << "\t" << pointing << "\t" << stepVisual << "\t" << attempt ; trialFile << endl; } }
void idle() { // update optotrak optotrak->updateMarkers(); // fill markers with updated coords markers = optotrak->getAllMarkers(); // Coordinates picker allVisiblePlatform = isVisible(markers[1].p); allVisibleIndex = isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p); allVisibleThumb = isVisible(markers[15].p) && isVisible(markers[17].p) && isVisible(markers[18].p); allVisibleFingers = allVisibleIndex && allVisibleThumb; // mirror visible allVisiblePatch = isVisible(markers[5].p) && isVisible(markers[6].p) && isVisible(markers[7].p); allVisibleHead = allVisiblePatch && isVisible(markers[1].p); if ( allVisiblePatch ) headEyeCoords.update(markers[5].p,markers[6].p,markers[7].p); if ( allVisibleFingers ) { indexCoords.update(markers[13].p, markers[14].p, markers[16].p ); thumbCoords.update(markers[15].p, markers[17].p, markers[18].p ); fingersOccluded = 0; } // during the trial (after the calibratioon) if (fingerCalibrationDone==3 ) { // check for visibility of the fingers if ( !allVisibleFingers ) { fingersOccluded=1; noVisibleFingers++; } // check for the visibility of the object if(!isVisible(markers[3].p)) objectOccluded=1; else objectOccluded=0; // start the flow of the frames frameN++; // eye coords depend on whether the head calibration took place or not if(headCalibration) { // real time coords eyeLeft = headEyeCoords.getLeftEye(); eyeRight = headEyeCoords.getRightEye(); } else { // fixed coords eyeRight = Vector3d(interoculardistance/2,0,0); eyeLeft = -eyeRight; } // online position of fingers updated index = indexCoords.getP1(); thumb = thumbCoords.getP1(); // online posotion of the mirror updated mirrorAlignment = asin( abs((markers[6].p.z()-markers[7].p.z()))/ sqrt( pow(markers[6].p.x()-markers[7].p.x(), 2) + pow(markers[6].p.z()-markers[7].p.z(), 2) ) )*180/M_PI; // online info written in the trialFile trialFile << fixed << parameters.find("SubjectName") << "\t" << //subjName trialNumber << "\t" << //trialN timer.getElapsedTimeInMilliSec() << "\t" << //time frameN << "\t" << //frameN index.transpose() << "\t" << //indexXraw, indexYraw, indexZraw thumb.transpose() << "\t" << //thumbXraw, thumbYraw, thumbZraw eyeRight.transpose() << "\t" << //eyeRXraw, eyeRYraw, eyeRZraw eyeLeft.transpose() << "\t" << //eyeLXraw, eyeLYraw, eyeLZraw fingersOccluded << "\t" << //fingersOccluded markers[3].p.transpose() << "\t" << //objectPos isStimulusDrawn << "\t" << noVisibleFingers << "\t" << round << "\t" << objLocation <<"\t" << sizeNow <<"\t" << diameter <<"\t" << condition << "\t" << interoculardistance << "\t" << allTrials ; trialFile << endl; } }
// 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(); //cerr << deltaT << endl; markers = optotrak->getAllMarkers(); // Coordinates picker allVisiblePlatform = isVisible(markers[1].p); allVisibleIndex = isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p); allVisibleFingers = allVisibleIndex; allVisiblePatch = isVisible(markers[5].p) && isVisible(markers[6].p) && isVisible(markers[7].p); allVisibleHead = allVisiblePatch && isVisible(markers[1].p); if ( allVisiblePatch ) headEyeCoords.update(markers[5].p,markers[6].p,markers[7].p); if ( allVisibleIndex ) { indexCoords.update(markers[13].p, markers[14].p, markers[16].p ); viewingFrames=1; occludedFrames = occludedFrames; } if (fingerCalibrationDone==3 ) { if ( !allVisibleIndex ) { viewingFrames=0; occludedFrames++; } if(condition == 0) condition = unifRand(-1, 1); if(index.z() < (-180) && index.y() > -150 && markers[14].v.norm() < 90) pointing=true; else pointing=pointing; if(index.z() > (-80)) handAtStart = true; else handAtStart = false; if(pointing && !handRecorded) { recFHP = index.z(); tFHP = timer.getElapsedTimeInMilliSec(); handRecorded = true; } else { recFHP = recFHP; tFHP = tFHP; handRecorded = handRecorded; } if(condition < 0) // vision then pointing { if(timer.getElapsedTimeInMilliSec() > stepVisual) // move to drawing the cross { moveToPartII = true; step = stepVisual; } if(moveToPartII && timer.getElapsedTimeInMilliSec() > step + 500) // move to drawing the first stimulus { moveToPartIII = true; step = step + 500; } if(moveToPartIII && timer.getElapsedTimeInMilliSec() > step + 800) // move to drawing black { moveToPartIV = true; step = step + 800; } if(moveToPartIV) // move to drawing the letter { moveToPartV = true; } if(moveToPartV && tFHP > 0) // move to drawing the cross { moveToPartVI = true; timeToFHP = tFHP - step; step = tFHP; } if(moveToPartVI && timer.getElapsedTimeInMilliSec() > step + 500) // move to drawing the second stimulus { moveToPartVII = true; step = step + 500; } if(moveToPartVII && timer.getElapsedTimeInMilliSec() > step + 800) // move to drawing black { moveToPartVIII = true; } } else // pointing then vision { if(tFHP > 0) // move to drawing the cross { moveToPartII = true; timeToFHP = tFHP; step = tFHP; } if(moveToPartII && timer.getElapsedTimeInMilliSec() > step + 500) // move to drawing the first stimulus { moveToPartIII = true; step = step + 500; } if(moveToPartIII && timer.getElapsedTimeInMilliSec() > step + 800) // move to drawing black { moveToPartIV = true; step = step + 800; } if(moveToPartIV) // move to drawing the letter { moveToPartV = true; } if(moveToPartV && timer.getElapsedTimeInMilliSec() > step + stepVisual) //move to drawing the cross { if(handAtStart && !moveToPartVI) { stepT = timer.getElapsedTimeInMilliSec(); moveToPartVI = true; } } if(moveToPartVI) // move to drawing the second stimulus { step = stepT; if(timer.getElapsedTimeInMilliSec() > step + 500) { moveToPartVII = true; step = step + 500; } } if(moveToPartVII && timer.getElapsedTimeInMilliSec() > step + 800) // move to drawing black { moveToPartVIII = true; } } } if(headCalibration) { eyeLeft = headEyeCoords.getLeftEye(); eyeRight = headEyeCoords.getRightEye(); } else { eyeRight = Vector3d(interoculardistance/2,0,0); eyeLeft = -eyeRight; } index = indexCoords.getP1(); singleMarker = markers.at(4).p.transpose(); if(pause && !paused) { timer.stop(); paused = true; } else if(paused && !pause) { initTrial(); paused = false; } // Write to markersfile if (fingerCalibrationDone==3 ) { markersFile << fixed << trialNumber << "\t" << index.transpose() << "\t" << occludedFrames << "\t" << viewingFrames << "\t" << markers[14].v.norm() << "\t" << recFHP << "\t" << timer.getElapsedTimeInMilliSec() << "\t" << tFHP << "\t" << part << "\t" << step << "\t" << moveToPartII << "\t" << moveToPartIII << "\t" << moveToPartIV << "\t" << moveToPartV << "\t" << moveToPartVI << "\t" << moveToPartVII << "\t" << moveToPartVIII << "\t" << firstD << "\t" << secondD << "\t" << handRecorded << "\t" << pointing << "\t" << stepVisual ; markersFile << endl; } }
void update(int value) { // Conta i cicli di presentazione dello stimolo if ( (sumOutside > str2num<int>(parameters.find("StimulusCycles")) ) && (trialMode == STIMULUSMODE) ) { sumOutside=0; trialMode++; trialMode=trialMode%4; } if (conditionInside && (sumOutside*2 > str2num<int>(parameters.find("FixationCycles"))) && (trialMode ==FIXATIONMODE ) ) { sumOutside=0; trialMode++; trialMode=trialMode%4; stimulusDuration.start(); } if ( trialMode == STIMULUSMODE ) stimulusFrames++; if ( trialMode == FIXATIONMODE ) stimulusFrames=0; Screen screenPassive; screenPassive.setWidthHeight(SCREEN_WIDE_SIZE, SCREEN_WIDE_SIZE*SCREEN_HEIGHT/SCREEN_WIDTH); screenPassive.setOffset(alignmentX,alignmentY); screenPassive.setFocalDistance(0); screenPassive.transform(headEyeCoords.getRigidStart().getFullTransformation()*Translation3d(center)); camPassive.init(screenPassive); camPassive.setDrySimulation(true); camPassive.setEye(eyeRight); objectPassiveTransformation = ( camPassive.getModelViewMatrix()*objectActiveTransformation ); // Coordinates picker markers = optotrak.getAllPoints(); if ( isVisible(markers[1]) && isVisible(markers[2]) && isVisible(markers[3]) ) headEyeCoords.update(markers[1],markers[2],markers[3]); Affine3d active = headEyeCoords.getRigidStart().getFullTransformation(); eulerAngles.init( headEyeCoords.getRigidStart().getFullTransformation().rotation() ); eyeLeft = headEyeCoords.getLeftEye(); eyeRight = headEyeCoords.getRightEye(); cyclopeanEye = (eyeLeft+eyeRight)/2.0; // Projection of view normal on the focal plane Vector3d directionOfSight = (active.rotation()*Vector3d(0,0,-1)).normalized(); Eigen::ParametrizedLine<double,3> lineOfSightRight = Eigen::ParametrizedLine<double,3>::Through( eyeRight , eyeRight+directionOfSight ); Eigen::ParametrizedLine<double,3> lineOfSightLeft = Eigen::ParametrizedLine<double,3>::Through( eyeLeft, eyeLeft+directionOfSight ); double lineOfSightRightDistanceToFocalPlane = lineOfSightRight.intersection(focalPlane); double lineOfSightLeftDistanceToFocalPlane = lineOfSightLeft.intersection(focalPlane); //double lenghtOnZ = (active*(center-eyeCalibration )+eyeRight).z(); projPointEyeRight = lineOfSightRightDistanceToFocalPlane *(directionOfSight)+ (eyeRight); projPointEyeLeft= lineOfSightLeftDistanceToFocalPlane * (directionOfSight) + (eyeLeft); // second projection the fixation point computed with z non constant but perfectly parallel to projPointEyeRight lineOfSightRightDistanceToFocalPlane= (( active.rotation()*(center)) - eyeRight).norm(); Vector3d secondProjection = lineOfSightRightDistanceToFocalPlane *(directionOfSight)+ (eyeRight); if ( !zOnFocalPlane ) projPointEyeRight=secondProjection ; // Compute the translation to move the eye in order to avoid shear components Vector3d posAlongLineOfSight = (headEyeCoords.getRigidStart().getFullTransformation().rotation())*(eyeRight -eyeCalibration); switch ( (int)factors["Translation"] ) { case -1: case -2: translationFactor.setZero(); if ( trialMode == STIMULUSMODE ) projPointEyeRight=center; break; case 0: translationFactor.setZero(); break; case 1: translationFactor = factors["TranslationConstant"]*Vector3d(posAlongLineOfSight.z(),0,0); break; case 2: translationFactor = factors["TranslationConstant"]*Vector3d(0,posAlongLineOfSight.z(),0); break; } if ( passiveMode ) initProjectionScreen(0,headEyeCoords.getRigidStart().getFullTransformation()*Translation3d(Vector3d(0,0,focalDistance))); else initProjectionScreen(focalDistance,Affine3d::Identity()); checkBounds(); /**** Save to file part ****/ // Markers file save the used markers and time-depending experimental variable to a file // (Make sure that in passive experiment the list of variables has the same order) markersFile << trialNumber << " " << headCalibrationDone << " " << trialMode << " " ; markersFile <<markers[1].transpose() << " " << markers[2].transpose() << " " << markers[3].transpose() << " " << markers[17].transpose() << " " << markers[18].transpose() << " " ; markersFile << factors["Tilt"] << " " << factors["Slant"] << " " << factors["Translation"] << " " << factors["Onset"] << " " << factors["TranslationConstant"] << endl; ofstream outputfile; outputfile.open("data.dat"); outputfile << "Subject Name: " << parameters.find("SubjectName") << endl; outputfile << "Passive matrix:" << endl << objectPassiveTransformation.matrix() << endl; outputfile << "Yaw: " << toDegrees(eulerAngles.getYaw()) << endl <<"Pitch: " << toDegrees(eulerAngles.getPitch()) << endl; outputfile << "EyeLeft: " << headEyeCoords.getLeftEye().transpose() << endl; outputfile << "EyeRight: " << headEyeCoords.getRightEye().transpose() << endl << endl; outputfile << "Slant: " << instantPlaneSlant << endl; outputfile << "(Width,Height) [px]: " << getPlaneDimensions().transpose() << " " << endl; outputfile << "Factors:" << endl; for (map<string,double>::iterator iter=factors.begin(); iter!=factors.end(); ++iter) { outputfile << "\t\t" << iter->first << "= " << iter->second << endl; } outputfile << "Trial remaining: " << trial.getRemainingTrials()+1 << endl; outputfile << "Last response: " << probeAngle << endl; // Here we save plane projected width and height // now rewind the file outputfile.clear(); outputfile.seekp(0,ios::beg); // Write down frame by frame the trajectories and angles of eyes and head if ( trialMode == STIMULUSMODE && headCalibrationDone > 2 ) { trajFile << setw(6) << left << trialNumber << " " << stimulusFrames << " " << eyeRight.transpose() << endl; anglesFile << setw(6) << left << trialNumber << " " << stimulusFrames << " " << toDegrees(eulerAngles.getPitch()) << " " << toDegrees(eulerAngles.getRoll()) << " " << toDegrees(eulerAngles.getYaw()) << " " << instantPlaneSlant << endl; matrixFile << setw(6) << left << trialNumber << " " << stimulusFrames << " " ; for (int i=0; i<3; i++) matrixFile << objectPassiveTransformation.matrix().row(i) << " " ; matrixFile << endl; // Write the 13 special extremal points on stimFile stimFile << setw(6) << left << trialNumber << " " << stimulusFrames << " " ; double winx=0,winy=0,winz=0; for (PointsRandIterator iRand = redDotsPlane.specialPointsRand.begin(); iRand!=redDotsPlane.specialPointsRand.end(); ++iRand) { Point3D *p=(*iRand); Vector3d v = objectActiveTransformation*Vector3d( p->x, p->y, p->z); gluProject(v.x(),v.y(),v.z(), (&cam)->getModelViewMatrix().data(), (&cam)->getProjectiveMatrix().data(), (&cam)->getViewport().data(), &winx,&winy,&winz); stimFile << winx << " " << winy << " " << winz << " "; } stimFile << endl; } glutPostRedisplay(); glutTimerFunc(TIMER_MS, update, 0); }
void idle() { updateTheMarkers(); // Visibility check allVisiblePlatform = isVisible(markers[1].p) && isVisible(markers[2].p); allVisibleIndex = isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p); allVisibleThumb = isVisible(markers[15].p) && isVisible(markers[17].p) && isVisible(markers[18].p); allVisibleFingers = allVisibleIndex && allVisibleThumb; allVisibleObject = isVisible(markers[8].p) && isVisible(markers[11].p) && isVisible(markers[12].p); allVisiblePatch = isVisible(markers[10].p) && isVisible(markers[11].p) && isVisible(markers[12].p); allVisibleHead = allVisiblePatch && isVisible(markers[9].p); // mirror alignment check mirrorAlignment = asin( abs((markers[6].p.z()-markers[7].p.z()))/ sqrt( pow(markers[6].p.x()-markers[7].p.x(), 2) + pow(markers[6].p.z()-markers[7].p.z(), 2) ) )*180/M_PI; // screen Y alignment check screenAlignmentY = asin( abs((markers[19].p.y()-markers[21].p.y()))/ sqrt( pow(markers[19].p.x()-markers[21].p.x(), 2) + pow(markers[19].p.y()-markers[21].p.y(), 2) ) )*180/M_PI; // screen Z alignment check screenAlignmentZ = asin( abs(markers[19].p.z()-markers[20].p.z())/ sqrt( pow(markers[19].p.x()-markers[20].p.x(), 2) + pow(markers[19].p.z()-markers[20].p.z(), 2) ) )*180/M_PI* abs(markers[19].p.x()-markers[20].p.x())/ (markers[19].p.x()-markers[20].p.x()); // eye coordinates eyeRight = Vector3d(interoculardistance/2,0,0); eyeLeft = Vector3d(-interoculardistance/2,0,0); // fingers coordinates, fingersOccluded and framesOccluded if ( allVisibleFingers ) { indexCoords.update(markers[13].p, markers[14].p, markers[16].p ); thumbCoords.update(markers[15].p, markers[17].p, markers[18].p ); } // Record the calibration platform's position and home position if ( isVisible(markers[1].p) && allVisibleObject && fingerCalibrationDone==0 ) { fingerCalibrationDone=1; calibration_fingers(fingerCalibrationDone); } // Record the calibration platform's position and home position if ( isVisible(markers[2].p) && allVisibleObject && fingerCalibrationDone==1 ) { fingerCalibrationDone=2; calibration_fingers(fingerCalibrationDone); beepOk(2); } if ( allVisibleObject && fingerCalibrationDone==2 ) { upperPin.update(markers[8].p, markers[11].p, markers[12].p ); lowerPin.update(markers[8].p, markers[11].p, markers[12].p ); } #ifndef SIMULATION // index coordinates if(allVisibleIndex) ind = indexCoords.getP1(); // thumb coordinates if(allVisibleThumb) thu = thumbCoords.getP1(); #endif // what the program checks online during the grasp if (fingerCalibrationDone==4 ) { // frames counting frameN++; // fingersOccluded if ( allVisibleFingers ) fingersOccluded = 0; else { fingersOccluded=1; } if(experiment) timeForGrasping = 1500; if(training) timeForGrasping = 15000; if(timer.getElapsedTimeInMilliSec() > timeForGrasping) iGrasped = true; if(isHandHome && !iGrasped) { timer.start(); frames_at_start++; } if(iGrasped) frames_post_grasp++; // middle point between index and thumb (center of grasp) grip_position = (ind + thu)/2; // euclidean distance to home dist_to_home = sqrt( pow((grip_position.x() - home_position.x()),2) + pow((grip_position.y() - home_position.y()),2) + pow((grip_position.z() - home_position.z()),2) ); // is the hand at start? if( dist_to_home < 50.0 ) isHandHome = true; else isHandHome = false; // euclidean distance to target dist_to_target = sqrt( pow((grip_position.x() - 0.0),2) + pow((grip_position.y() - 0.0),2) + pow((grip_position.z() - trial.getCurrent()["AbsDepth"]),2) ); if(!isHandHome && !allVisibleFingers && !iGrasped) framesOccluded++; if(experiment) { // Write to responseFile markersFile << fixed << parameters.find("SubjectName") << "\t" << interoculardistance << "\t" << trialNumber << "\t" << timer.getElapsedTimeInMilliSec() << "\t" << //time frameN << "\t" << //frameN ind.transpose() << "\t" << //indexXraw, indexYraw, indexZraw thu.transpose() << "\t" << //thumbXraw, thumbYraw, thumbZraw eyeRight.transpose() << "\t" << //eyeRXraw, eyeRYraw, eyeRZraw eyeLeft.transpose() << "\t" << //eyeLXraw, eyeLYraw, eyeLZraw fingersOccluded << "\t" << //fingersOccluded framesOccluded << "\t" << //framesOccluded trial.getCurrent()["AbsDepth"] << "\t" << trial.getCurrent()["RelDepthObj"] << "\t" << frameToGrasp << endl; } } }
// 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(); //cerr << trial.getCurrent()["AbsDepth"] << endl; markers = optotrak->getAllMarkers(); // Coordinates picker allVisiblePlatform = isVisible(markers[1].p) && isVisible(markers[2].p); allVisibleIndex = isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p); allVisibleThumb = isVisible(markers[15].p) && isVisible(markers[17].p) && isVisible(markers[18].p); allVisibleFingers = allVisibleIndex && allVisibleThumb; allVisibleObject = isVisible(markers[8].p) && isVisible(markers[11].p) && isVisible(markers[12].p); allVisiblePatch = isVisible(markers[10].p) && isVisible(markers[11].p) && isVisible(markers[12].p); allVisibleHead = allVisiblePatch && isVisible(markers[9].p); mirrorAlignment = asin( abs((markers[6].p.z()-markers[7].p.z()))/ sqrt( pow(markers[6].p.x()-markers[7].p.x(), 2) + pow(markers[6].p.z()-markers[7].p.z(), 2) ) )*180/M_PI; if ( allVisiblePatch ) headEyeCoords.update(markers[10].p,markers[11].p,markers[12].p); if(headCalibration) { eyeLeft = headEyeCoords.getLeftEye(); eyeRight = headEyeCoords.getRightEye(); } else { eyeRight = Vector3d(interoculardistance/2,0,0); eyeLeft = -eyeRight; } if ( allVisibleFingers ) { indexCoords.update(markers[13].p, markers[14].p, markers[16].p ); thumbCoords.update(markers[15].p, markers[17].p, markers[18].p ); fingersOccluded = 0; } // Record the calibration platform's position and home position if ( isVisible(markers[1].p) && allVisibleObject && fingerCalibrationDone==0 ) { fingerCalibrationDone=1; calibration_fingers(fingerCalibrationDone); } // Record the calibration platform's position and home position if ( isVisible(markers[2].p) && allVisibleObject && fingerCalibrationDone==1 ) { fingerCalibrationDone=2; calibration_fingers(fingerCalibrationDone); beepOk(2); } if ( allVisibleObject && fingerCalibrationDone==2 ) { upperPin.update(markers[8].p, markers[11].p, markers[12].p ); lowerPin.update(markers[8].p, markers[11].p, markers[12].p ); } if (fingerCalibrationDone==4 ) { if ( !allVisibleFingers ) { fingersOccluded=1; } frameN++; if(indexDisappeared && thumbDisappeared) fingersDisappeared = true; else fingersDisappeared = fingersDisappeared; if(index.z() > (-80)) handAtStart = true; else handAtStart = false; index = indexCoords.getP1(); thumb = thumbCoords.getP1(); } }
void update(int value) { // Read the experiment from file, if the file is finished exit suddenly if ( inputStream.eof() ) { cleanup(); exit(0); } if ( isReading ) { // This reads a line (frame) in inputStream readline(inputStream, trialNumber, headCalibration, trialMode, pointMatrix ); headEyeCoords.update(pointMatrix.col(0),pointMatrix.col(1),pointMatrix.col(2)); Affine3d active = headEyeCoords.getRigidStart().getFullTransformation(); eulerAngles.init( headEyeCoords.getRigidStart().getFullTransformation().rotation() ); eyeLeft = headEyeCoords.getLeftEye(); eyeRight= headEyeCoords.getRightEye(); cyclopeanEye = (eyeLeft+eyeRight)/2.0; if ( trialMode == STIMULUSMODE ) stimulusFrames++; if ( trialMode == FIXATIONMODE ) stimulusFrames=0; // Projection of view normal on the focal plane Vector3d directionOfSight = (active.rotation()*Vector3d(0,0,-1)).normalized(); Eigen::ParametrizedLine<double,3> lineOfSightRight = Eigen::ParametrizedLine<double,3>::Through( eyeRight , eyeRight+directionOfSight ); Eigen::ParametrizedLine<double,3> lineOfSightLeft = Eigen::ParametrizedLine<double,3>::Through( eyeLeft, eyeLeft+directionOfSight ); double lineOfSightRightDistanceToFocalPlane = lineOfSightRight.intersection(focalPlane); double lineOfSightLeftDistanceToFocalPlane = lineOfSightLeft.intersection(focalPlane); //double lenghtOnZ = (active*(center-eyeCalibration )+eyeRight).z(); projPointEyeRight = lineOfSightRightDistanceToFocalPlane *(directionOfSight)+ (eyeRight); projPointEyeLeft= lineOfSightLeftDistanceToFocalPlane * (directionOfSight) + (eyeLeft); // second projection the fixation point computed with z non constant but perfectly parallel to projPointEyeRight lineOfSightRightDistanceToFocalPlane= (( active.rotation()*(center)) - eyeRight).norm(); Vector3d secondProjection = lineOfSightRightDistanceToFocalPlane *(directionOfSight)+ (eyeRight); if ( !zOnFocalPlane ) projPointEyeRight=secondProjection ; // Compute the translation to move the eye in order to avoid share components Vector3d posAlongLineOfSight = (headEyeCoords.getRigidStart().getFullTransformation().rotation())*(eyeRight -eyeCalibration); // GENERATION OF PASSIVE MODE. // HERE WE MOVE THE SCREEN TO FACE THE OBSERVER's EYE if ( passiveMode ) { initProjectionScreen(0, headEyeCoords.getRigidStart().getFullTransformation()*Translation3d(center)); } else initProjectionScreen(focalDistance, Affine3d::Identity()); objectPassiveTransformation = ( cam.getModelViewMatrix()*objectActiveTransformation ); ofstream outputfile; outputfile.open("data.dat"); outputfile << "Subject Name: " << parameters.find("SubjectName") << endl; outputfile << "Passive matrix:" << endl << objectPassiveTransformation.matrix() << endl; outputfile << "Yaw: " << toDegrees(eulerAngles.getYaw()) << endl <<"Pitch: " << toDegrees(eulerAngles.getPitch()) << endl; outputfile << "EyeLeft: " << headEyeCoords.getLeftEye().transpose() << endl; outputfile << "EyeRight: " << headEyeCoords.getRightEye().transpose() << endl << endl; outputfile << "Slant: " << instantPlaneSlant << endl; outputfile << "Factors:" << endl; for (map<string,double>::iterator iter=factors.begin(); iter!=factors.end(); ++iter) { outputfile << "\t\t" << iter->first << "= " << iter->second << endl; } } if ( trialMode == PROBEMODE ) isReading=false; glutPostRedisplay(); glutTimerFunc(TIMER_MS, update, 0); }
// 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 idle() { if (totalTimer.getElapsedTimeInSec() > initialAdaptationTimeInSeconds && trialMode == INITIALADAPTATION && experimentStarted ) { trialMode = STIMULUSMODE; totalTimer.start(); } if (totalTimer.getElapsedTimeInSec() > fixationDurationInSeconds && trialMode == FIXATIONMODE && experimentStarted ) { timeFrame=0.0; trialMode=STIMULUSMODE; totalTimer.start(); } if (totalTimer.getElapsedTimeInMilliSec() > stimulusDurationInMilliSeconds && trialMode == STIMULUSMODE && experimentStarted ) { timeFrame=0.0; trialMode=PROBEMODE; totalTimer.start(); } if (!experimentStarted) return; Timer frameTimer; frameTimer.start(); // Timing things double oscillationPeriod = stimulusDurationInMilliSeconds; switch (stimMotion) { case SAWTOOTH_MOTION: periodicValue = oscillationAmplitude*mathcommon::sawtooth(timeFrame,oscillationPeriod); break; case TRIANGLE_MOTION: periodicValue = oscillationAmplitude*mathcommon::trianglewave(timeFrame,oscillationPeriod); break; case SINUSOIDAL_MOTION: periodicValue = oscillationAmplitude*sin(3.14*timeFrame/(oscillationPeriod)); break; default: SAWTOOTH_MOTION; } timingFile << totalTimer.getElapsedTimeInMilliSec() << " " << periodicValue << endl; // Simulate head translation // Coordinates picker markers[1] = Vector3d(0,0,0); markers[2] = Vector3d(0,10,0); markers[3] = Vector3d(0,0,10); headEyeCoords.update(markers[1],markers[2],markers[3]); eyeLeft = headEyeCoords.getLeftEye(); eyeRight = headEyeCoords.getRightEye(); stimTransformation.matrix().setIdentity(); stimTransformation.translation() <<0,0,focalDistance; Timer sleepTimer; sleepTimer.sleep(abs(TIMER_MS - frameTimer.getElapsedTimeInMilliSec())); timeFrame+=1; }
void idle() { // get new marker positions from optotrak optotrak->updateMarkers(); markers = optotrak->getAllMarkers(); // check visibility if (triangulate){ allVisiblePlatform3 = isVisible(markers[8].p) && isVisible(markers[11].p) && isVisible(markers[12].p); allVisiblePlatform1 = isVisible(markers[1].p) && allVisiblePlatform3; allVisiblePlatform2 = isVisible(markers[2].p) && allVisiblePlatform3; allVisibleIndex = isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p); allVisibleThumb = isVisible(markers[15].p) && isVisible(markers[17].p) && isVisible(markers[18].p); allVisibleFingers = allVisibleIndex && allVisibleThumb;// && allVisibleWrist; allVisiblePatch = isVisible(markers[5].p) && isVisible(markers[6].p) && isVisible(markers[7].p); allVisibleHead = allVisiblePatch && isVisible(markers[1].p); } else { allVisibleIndex = isVisible(markers[9].p); allVisibleThumb = isVisible(markers[10].p); allVisibleFingers = allVisibleThumb && allVisibleIndex; allVisibleWrist = isVisible(markers[22].p); } // check equipment alignments mirrorAlignment = asin( abs((markers[6].p.z()-markers[7].p.z()))/ sqrt( pow(markers[6].p.x()-markers[7].p.x(), 2) + pow(markers[6].p.z()-markers[7].p.z(), 2) ) )*180/M_PI; screenAlignmentY = asin( abs((markers[19].p.y()-markers[21].p.y()))/ sqrt( pow(markers[19].p.x()-markers[21].p.x(), 2) + pow(markers[19].p.y()-markers[21].p.y(), 2) ) )*180/M_PI; screenAlignmentZ = asin( abs(markers[19].p.z()-markers[20].p.z())/ sqrt( pow(markers[19].p.x()-markers[20].p.x(), 2) + pow(markers[19].p.z()-markers[20].p.z(), 2) ) )*180/M_PI* abs(markers[19].p.x()-markers[20].p.x())/ (markers[19].p.x()-markers[20].p.x()); // update head coordinates if ( allVisiblePatch ) headEyeCoords.update(markers[5].p,markers[6].p,markers[7].p); // update finger coordinates (but we don't really use these directly!) if ( allVisibleFingers ) { if(triangulate){ indexCoords.update(markers[13].p, markers[14].p, markers[16].p ); thumbCoords.update(markers[15].p, markers[17].p, markers[18].p ); } //wristCoords = markers[wristMarkerNum].p; fingersOccluded = 0; } if (triangulate){ if ( allVisiblePlatform1 && fingerCalibration==0 ) { // get upper pin calibration_fingers(1); fingerCalibration=1; } if ( fingerCalibration==1 && allVisiblePlatform2 ) { // get lower pin calibration_fingers(2); fingerCalibration=2; beepOk(0); } if ( allVisiblePlatform3 && fingerCalibration<3 ) { upperPin.update(markers[8].p, markers[11].p, markers[12].p ); lowerPin.update(markers[8].p, markers[11].p, markers[12].p ); } // update the finger position in the objects we actually use if (allVisibleIndex) index = indexCoords.getP1(); if (allVisibleThumb) thumb = thumbCoords.getP1(); } if (!triangulate){ if (fingerCalibration==0) fingerCalibration=3; index = markers[9].p; thumb = markers[10].p; wrist = markers[22].p; } ////////////////////////////////////// // While the experiment is running! // ////////////////////////////////////// if (fingerCalibration==7 && !finished) { // Check for finger occlusion if ( !allVisibleFingers ) { fingersOccluded = 1; //if (!started) // beepOk(4); if (started && !reachedObject) // only increment if we're in flight { num_lost_frames += 1; } } // Advance frame number frameN++; // find distance from grip center to object center grip_Origin_X = (index.x()+thumb.x())/2; grip_Origin_Y = (index.y()+thumb.y())/2; grip_Origin_Z = (index.z()+thumb.z())/2; x_dist = abs(grip_Origin_X - targetOriginX); y_dist = abs(grip_Origin_Y - targetOriginY); z_dist = abs(grip_Origin_Z - targetOriginZ); distanceGripCenterToObject = sqrt((x_dist*x_dist)+(y_dist*y_dist)+(z_dist*z_dist)); x_dist_home = abs(grip_Origin_X - (startPosLeft + startPosRight)/2); y_dist_home = abs(grip_Origin_Y - (startPosTop + startPosBottom)/2); z_dist_home = abs(grip_Origin_Z - (startPosFront + startPosRear)/2); distanceGripCenterToHome = sqrt((x_dist_home*x_dist_home)+(y_dist_home*y_dist_home)+(z_dist_home*z_dist_home)); // compute grip aperture grip_aperture = sqrt( (index.x() - thumb.x())*(index.x() - thumb.x()) + (index.y() - thumb.y())*(index.y() - thumb.y()) + (index.z() - thumb.z())*(index.z() - thumb.z()) ); /* Check that both fingers are in the start position if( ( (index.y() < startPosTop) && // index below ceiling (index.y() > startPosBottom) && // index above floor (index.x() > startPosLeft) && // index right of left wall (index.x() < startPosRight) && // index left of right wall (index.z() > startPosFront) && // index behind front wall (index.z() < startPosRear) && // index in front of rear wall (thumb.y() < startPosTop) && // thumb below ceiling (index.y() > startPosBottom) && // thumb above floor (thumb.x() > startPosLeft) && // thumb right of left wall (thumb.x() < startPosRight) && // thumb left of right wall (thumb.z() > startPosFront) && // thumb behind front wall (thumb.z() < startPosRear) ) // thumb in front of rear wall || (estimate_given==0) )*/ if (handAtStart || (estimate_given==0)) { // if we already gave the estimate and are returning to the start position if (estimate_given==1){ estimate_given=2; beepOk(10); } // keep resetting timer //handAtStart = true; maxTime = timer.getElapsedTimeInMilliSec(); timer.start(); } else if (estimate_given==2) { // otherwise we are in flight, so set flags and let the timer run if (start_frame==0){ start_frame=frameN; start_dist = distanceGripCenterToHome; } //handAtStart = false; started = true; } // if we are still approaching object if (!reachedObject && started) { // when conditions for "end criterion" are satisfied (usually re: GA and distanceToObject) if ( (distanceGripCenterToObject<=10) && (grip_aperture<(cylHeight+10)) ){ // set flag and record the frame (for computing % missing frames) reachedObject = true; //spin = true; TGA_frame = frameN; } } } // recompute the eye coordinates for drawing so we can change IOD online if(headCalibration){ eyeLeft = headEyeCoords.getLeftEye(); eyeRight = headEyeCoords.getRightEye(); }else{ eyeRight = Vector3d(interoculardistance/2,0,0); eyeLeft = -eyeRight; } // Write to trialFile once calibration is over if (fingerCalibration==7 ) { trialFile << fixed << //parameters.find("SubjectName") << "\t" << //subjName parameters[0].find("SubjectName") << "\t" << //subjName trialNumber << "\t" << //trialN timer.getElapsedTimeInMilliSec() << "\t" << //time frameN << "\t" << //frameN index.transpose() << "\t" << //indexXraw, indexYraw, indexZraw thumb.transpose() << "\t" << //thumbXraw, thumbYraw, thumbZraw //wrist.transpose() << "\t" << distanceGripCenterToObject << "\t" << //distanceToObject fingersOccluded << "\t" << //fingersOccluded reachedObject << endl; //reachedObject } // conditions for trial advance if(handAtStart && started) advanceTrial(); }
void update(int value) { frameTimer.start(); // Read the experiment from file, if the file is finished exit suddenly if ( inputStream.eof() ) { exit(0); } if ( isReading ) { // This reads a line (frame) in inputStream readline(inputStream, trialNumber, headCalibration, trialMode, pointMatrix ); headEyeCoords.update(pointMatrix.col(0),pointMatrix.col(1),pointMatrix.col(2)); Affine3d active = headEyeCoords.getRigidStart().getFullTransformation(); eulerAngles.init( headEyeCoords.getRigidStart().getFullTransformation().rotation() ); eyeLeft = headEyeCoords.getLeftEye(); eyeRight= headEyeCoords.getRightEye(); //cerr << eyeRight.transpose() << endl; cyclopeanEye = (eyeLeft+eyeRight)/2.0; if ( trialMode == STIMULUSMODE ) stimulusFrames++; if ( trialMode == FIXATIONMODE ) stimulusFrames=0; // Projection of view normal on the focal plane Vector3d directionOfSight = (active.rotation()*Vector3d(0,0,-1)).normalized(); Eigen::ParametrizedLine<double,3> lineOfSightRight = Eigen::ParametrizedLine<double,3>::Through( eyeRight , eyeRight+directionOfSight ); double lineOfSightRightDistanceToFocalPlane = lineOfSightRight.intersection(focalPlane); //double lenghtOnZ = (active*(center-eyeCalibration )+eyeRight).z(); projPointEyeRight = lineOfSightRightDistanceToFocalPlane *(directionOfSight)+ (eyeRight); // second projection the fixation point computed with z non constant but perfectly parallel to projPointEyeRight lineOfSightRightDistanceToFocalPlane= (( active.rotation()*(center)) - eyeRight).norm(); Vector3d secondProjection = lineOfSightRightDistanceToFocalPlane *(directionOfSight)+ (eyeRight); projPointEyeRight=secondProjection ; // Compute the translation to move the eye in order to avoid share components Vector3d posAlongLineOfSight = (headEyeCoords.getRigidStart().getFullTransformation().rotation())*(eyeRight -eyeCalibration); // GENERATION OF PASSIVE MODE. // HERE WE MOVE THE SCREEN TO FACE THE OBSERVER's EYE if ( passiveMode ) { initProjectionScreen(0, headEyeCoords.getRigidStart().getFullTransformation()*Translation3d(center)); } else initProjectionScreen(focalDistance, Affine3d::Identity()); if ( trialMode == STIMULUSMODE ) { // IMPORTANT Reset the previous status of transformations objectActiveTransformation[0].setIdentity(); objectActiveTransformation[1].setIdentity(); // PLANE 0 Transformation QUELLO CHE STA SOTTO alpha = atan( eyeRight.x()/abs(projPointEyeRight.z()) ); if ( overallTilt ) { instantPlaneSlant = alphaMultiplier*alpha+toRadians(-factors.at("DeltaSlant")-factors.at("StillPlaneSlant")); AngleAxis<double> aa0( instantPlaneSlant,Vector3d::UnitY()); objectActiveTransformation[0]*=aa0; double planesYOffset = factors.at("PlanesCentersYDistance")*(whichPlaneDrawUp ? 1 : -1); objectActiveTransformation[0].translation() = Vector3d(0,planesYOffset,focalDistance); // PLANE 1 Transformation QUELLO CHE STA SOPRA AngleAxis<double> aa1(-toRadians(factors.at("StillPlaneSlant")),Vector3d::UnitY()); objectActiveTransformation[1]*=aa1; objectActiveTransformation[1].translation() = Vector3d(0,-planesYOffset,focalDistance); } else { instantPlaneSlant = alphaMultiplier*alpha+toRadians(factors.at("DeltaSlant")+factors.at("StillPlaneSlant")); AngleAxis<double> aa0( instantPlaneSlant,Vector3d::UnitY()); objectActiveTransformation[0]*=aa0; double planesYOffset = factors.at("PlanesCentersYDistance")*(whichPlaneDrawUp ? 1 : -1); objectActiveTransformation[0].translation() = Vector3d(0,planesYOffset,focalDistance); // PLANE 1 Transformation QUELLO CHE STA SOPRA AngleAxis<double> aa1(toRadians(factors.at("StillPlaneSlant")),Vector3d::UnitY()); objectActiveTransformation[1]*=aa1; objectActiveTransformation[1].translation() = Vector3d(0,-planesYOffset,focalDistance); } objectPassiveTransformation[0] = ( cam.getModelViewMatrix()*objectActiveTransformation[0] ); objectPassiveTransformation[1] = ( cam.getModelViewMatrix()*objectActiveTransformation[1] ); //cout << toDegrees(instantPlaneSlant) << endl; // **************** COMPUTE THE OPTIC FLOWS ************************** // 1) Project the points to screen by computing their coordinates on focalPlane in passive (quite complicate, see the specific method) // *********** FOR THE MOVING PLANE ************* vector<Vector3d> projPointsMovingPlane = stimDrawer[0].projectStimulusPoints(objectActiveTransformation[0],headEyeCoords.getRigidStart().getFullTransformation(),cam,focalDistance, screen, eyeCalibration,passiveMode,false); // 2) Get the angles formed by stimulus and observer // updating with the latest values Vector3d oldAlphaMoving = flowsAnglesAlphaMoving,oldBetaMoving=flowsAnglesBetaMoving; // alpha is the "pitch" angle, beta is the "yaw" angle // Here me must use the points 4,5,8 of the stimulus flowsAnglesAlphaMoving(0) = ( atan2(projPointsMovingPlane[4].x(), abs(focalDistance) ) ); flowsAnglesAlphaMoving(1) = ( atan2(projPointsMovingPlane[5].x(), abs(focalDistance) ) ); flowsAnglesAlphaMoving(2) = ( atan2(projPointsMovingPlane[8].x(), abs(focalDistance) ) ); flowsAnglesBetaMoving(0) = ( atan2(projPointsMovingPlane[4].y(), abs(focalDistance) ) ); flowsAnglesBetaMoving(1) = ( atan2(projPointsMovingPlane[5].y(), abs(focalDistance) ) ); flowsAnglesBetaMoving(2) = ( atan2(projPointsMovingPlane[8].y(), abs(focalDistance) ) ); // 3) Fill the matrix of derivatives MatrixXd angVelocitiesMoving(6,1); angVelocitiesMoving(0) = flowsAnglesAlphaMoving(0)-oldAlphaMoving(0); angVelocitiesMoving(1) = flowsAnglesBetaMoving(0)-oldBetaMoving(0); angVelocitiesMoving(2) = flowsAnglesAlphaMoving(1)-oldAlphaMoving(1); angVelocitiesMoving(3) = flowsAnglesBetaMoving(1)-oldBetaMoving(1); angVelocitiesMoving(4) = flowsAnglesAlphaMoving(2)-oldAlphaMoving(2); angVelocitiesMoving(5) = flowsAnglesBetaMoving(2)-oldBetaMoving(2); angVelocitiesMoving /= ((double)TIMER_MS/(double)1000); // 4) Fill the coefficient matrix, to solve the linear system MatrixXd coeffMatrixMoving(6,6); coeffMatrixMoving << 1, flowsAnglesAlphaMoving(0), flowsAnglesBetaMoving(0), 0, 0, 0, 0, 0, 0, 1,flowsAnglesAlphaMoving(0),flowsAnglesBetaMoving(0), 1, flowsAnglesAlphaMoving(1), flowsAnglesBetaMoving(1), 0, 0, 0, 0, 0, 0, 1,flowsAnglesAlphaMoving(1),flowsAnglesBetaMoving(1), 1, flowsAnglesAlphaMoving(2), flowsAnglesBetaMoving(2), 0, 0, 0, 0, 0, 0, 1,flowsAnglesAlphaMoving(2),flowsAnglesBetaMoving(2) ; // 5) Solve the linear system by robust fullPivHouseholderQR decomposition (see Eigen for details http://eigen.tuxfamily.org/dox/TutorialLinearAlgebra.html ) MatrixXd velocitiesMoving = coeffMatrixMoving.colPivHouseholderQr().solve(angVelocitiesMoving); // 6) Write the output to file flowsFileMoving flowsFileMoving << fixed << trialNumber << "\t" << //1 stimulusFrames << " " << factors.at("DeltaSlant")<< " " << factors.at("StillPlaneSlant") << " " << overallTilt << " " << projPointsMovingPlane[4].transpose() << " " << projPointsMovingPlane[5].transpose() << " " << projPointsMovingPlane[8].transpose() << " " << angVelocitiesMoving.transpose() << " " << velocitiesMoving.transpose() << endl; // ********************* FLOWS FOR THE STILL PLANE ************** // Here we must repeat the same things for the still plane vector<Vector3d> projPointsStillPlane = stimDrawer[1].projectStimulusPoints(objectActiveTransformation[1],headEyeCoords.getRigidStart().getFullTransformation(),cam,focalDistance, screen, eyeCalibration,passiveMode,false); // 2) Get the angles formed by stimulus and observer // updating with the latest values Vector3d oldAlphaStill = flowsAnglesAlphaStill,oldBetaStill=flowsAnglesBetaStill; // alpha is the "pitch" angle, beta is the "yaw" angle // Here me must use the points 4,5,8 of the stimulus flowsAnglesAlphaStill(0) = ( atan2(projPointsStillPlane[4].x(), abs(focalDistance) ) ); flowsAnglesAlphaStill(1) = ( atan2(projPointsStillPlane[5].x(), abs(focalDistance) ) ); flowsAnglesAlphaStill(2) = ( atan2(projPointsStillPlane[8].x(), abs(focalDistance) ) ); flowsAnglesBetaStill(0) = ( atan2(projPointsStillPlane[4].y(), abs(focalDistance) ) ); flowsAnglesBetaStill(1) = ( atan2(projPointsStillPlane[5].y(), abs(focalDistance) ) ); flowsAnglesBetaStill(2) = ( atan2(projPointsStillPlane[8].y(), abs(focalDistance) ) ); // 3) Fill the matrix of derivatives MatrixXd angVelocitiesStill(6,1); angVelocitiesStill(0) = flowsAnglesAlphaStill(0)-oldAlphaStill(0); angVelocitiesStill(1) = flowsAnglesBetaStill(0)-oldBetaStill(0); angVelocitiesStill(2) = flowsAnglesAlphaStill(1)-oldAlphaStill(1); angVelocitiesStill(3) = flowsAnglesBetaStill(1)-oldBetaStill(1); angVelocitiesStill(4) = flowsAnglesAlphaStill(2)-oldAlphaStill(2); angVelocitiesStill(5) = flowsAnglesBetaStill(2)-oldBetaStill(2); angVelocitiesStill /= ((double)TIMER_MS/(double)1000); // 4) Fill the coefficient matrix, to solve the linear system MatrixXd coeffMatrixStill(6,6); coeffMatrixStill << 1, flowsAnglesAlphaStill(0), flowsAnglesBetaStill(0), 0, 0, 0, 0, 0, 0, 1,flowsAnglesAlphaStill(0),flowsAnglesBetaStill(0), 1, flowsAnglesAlphaStill(1), flowsAnglesBetaStill(1), 0, 0, 0, 0, 0, 0, 1,flowsAnglesAlphaStill(1),flowsAnglesBetaStill(1), 1, flowsAnglesAlphaStill(2), flowsAnglesBetaStill(2), 0, 0, 0, 0, 0, 0, 1,flowsAnglesAlphaStill(2),flowsAnglesBetaStill(2) ; // 5) Solve the linear system by robust fullPivHouseholderQR decomposition (see Eigen for details http://eigen.tuxfamily.org/dox/TutorialLinearAlgebra.html ) MatrixXd velocitiesStill = coeffMatrixStill.colPivHouseholderQr().solve(angVelocitiesStill); // 6) Write the output to file flowsFileStill flowsFileStill << fixed << trialNumber << "\t" << // 1 stimulusFrames << " " << // 2 factors.at("DeltaSlant")<< " " << // 3 factors.at("StillPlaneSlant") << " " << // 4 overallTilt << " " << projPointsStillPlane[4].transpose() << " " << // 5,6,7 projPointsStillPlane[5].transpose() << " " << // 8,9,10 projPointsStillPlane[8].transpose() << " " << // 11,12,13 angVelocitiesStill.transpose() << " " << // 14, 15, 16, 17, 18, 19 velocitiesStill.transpose() << endl; // 20, 21, 22, 23, 24, 25 // **************** END OF OPTIC FLOWS COMPUTATION } /* ofstream outputfile; outputfile.open("data.dat"); outputfile << "Subject Name: " << parameters.find("SubjectName") << endl; outputfile << "Passive matrix:" << endl << objectPassiveTransformation.matrix() << endl; outputfile << "Yaw: " << toDegrees(eulerAngles.getYaw()) << endl <<"Pitch: " << toDegrees(eulerAngles.getPitch()) << endl; outputfile << "EyeLeft: " << headEyeCoords.getLeftEye().transpose() << endl; outputfile << "EyeRight: " << headEyeCoords.getRightEye().transpose() << endl << endl; outputfile << "Factors:" << endl; for (map<string,double>::iterator iter=factors.begin(); iter!=factors.end(); ++iter) { outputfile << "\t\t" << iter->first << "= " << iter->second << endl; } */ } if ( trialMode == PROBEMODE ) isReading=false; glutPostRedisplay(); glutTimerFunc(TIMER_MS, update, 0); }
// 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(); /*cerr << trial.getCurrent()["AbsDepth"] << "\t" << trial.getCurrent()["ObjHeight"] << "\t" << trial.getCurrent()["HapticFB"] << endl; */ //cerr << parameters.find("fObjHeight",1) << endl; markers = optotrak->getAllMarkers(); // Coordinates picker allVisiblePlatform = isVisible(markers[1].p); allVisibleIndex = isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p); allVisibleThumb = isVisible(markers[15].p) && isVisible(markers[17].p) && isVisible(markers[18].p); allVisibleFingers = allVisibleIndex && allVisibleThumb; allVisiblePatch = isVisible(markers[5].p) && isVisible(markers[6].p) && isVisible(markers[7].p); allVisibleHead = allVisiblePatch && isVisible(markers[1].p); if ( allVisiblePatch ) headEyeCoords.update(markers[5].p,markers[6].p,markers[7].p); if ( allVisibleFingers ) { indexCoords.update(markers[13].p, markers[14].p, markers[16].p ); thumbCoords.update(markers[15].p, markers[17].p, markers[18].p ); fingersOccluded = 0; } if (fingerCalibrationDone==3 ) { if ( !allVisibleFingers ) { fingersOccluded=1; } frameN++; if(((-500) < index.y()) && (index.y() < (-315)) && ((-500) < thumb.y()) && (thumb.y() < (-315))) { handAtStart = true; timer.start(); } else { handAtStart = false; } if(!handAtStart && ((-315) <= index.y()) && (index.y() < 200) && ((-315) <= thumb.y()) && (thumb.y() < 100)) { started=true; } else started=started; } if(headCalibration) { eyeLeft = headEyeCoords.getLeftEye(); eyeRight = headEyeCoords.getRightEye(); } else { eyeRight = Vector3d(interoculardistance/2,0,0); eyeLeft = -eyeRight; } index = indexCoords.getP1(); thumb = thumbCoords.getP1(); // Write to trialFile if (fingerCalibrationDone==3 ) { trialFile << fixed << parameters.find("SubjectName") << "\t" << //subjName trialNumber << "\t" << //trialN timer.getElapsedTimeInMilliSec() << "\t" << //time frameN << "\t" << //frameN index.transpose() << "\t" << //indexXraw, indexYraw, indexZraw thumb.transpose() << "\t" << //thumbXraw, thumbYraw, thumbZraw //eyeRight.transpose() << "\t" << //eyeRXraw, eyeRYraw, eyeRZraw //eyeLeft.transpose() << "\t" << //eyeLXraw, eyeLYraw, eyeLZraw fingersOccluded << "\t" << //fingersOccluded attempt ; trialFile << endl; } }
// 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(); //cerr << deltaT << endl; markers = optotrak->getAllMarkers(); // Coordinates picker allVisiblePlatform = isVisible(markers[1].p); allVisibleThumb = isVisible(markers[15].p) && isVisible(markers[17].p) && isVisible(markers[18].p); allVisibleIndex = isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p); allVisibleFingers = allVisibleThumb && allVisibleIndex; allVisiblePatch = isVisible(markers[5].p) && isVisible(markers[6].p) && isVisible(markers[7].p); allVisibleHead = allVisiblePatch && isVisible(markers[1].p); if ( allVisiblePatch ) headEyeCoords.update(markers[5].p,markers[6].p,markers[7].p); if ( allVisibleThumb ) thumbCoords.update(markers[15].p,markers[17].p,markers[18].p); if ( allVisibleIndex ) indexCoords.update(markers[13].p, markers[14].p, markers[16].p ); if ( headCalibrationDone==3 && fingerCalibrationDone==3 ) { if ( !allVisibleIndex ) occludedFrames++; if ( !allVisibleThumb ) occludedFrames++; } if(headCalibration) { eyeLeft = headEyeCoords.getLeftEye(); eyeRight = headEyeCoords.getRightEye(); } else { eyeRight = Vector3d(interoculardistance/2,0,0); eyeLeft = -eyeRight; } thumb = thumbCoords.getP1(); index = indexCoords.getP1(); singleMarker = markers.at(4).p.transpose(); if(!letStimTimer) stimTimer.start(); if( endTrial && fingersAtStart) { advanceTrial(); } #ifdef WRITE // Write to file if ( headCalibrationDone==3 && fingerCalibrationDone==3 ) { markersFile << fixed << trialNumber << "\t" << eyeLeft.transpose() << "\t" << eyeRight.transpose() << "\t" << //markers.at(4).p.transpose() << "\t" << index.transpose() << "\t" << thumb.transpose() << "\t" << fingersAtStart << "\t" << isStimulusDrawn ; markersFile << endl; } #endif }
void idle() { updateTheMarkers(); // Visibility check allVisiblePlatform = isVisible(markers[1].p) && isVisible(markers[2].p); allVisibleIndex = isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p); allVisibleThumb = isVisible(markers[15].p) && isVisible(markers[17].p) && isVisible(markers[18].p); allVisibleFingers = allVisibleIndex && allVisibleThumb; allVisibleObject = isVisible(markers[8].p) && isVisible(markers[11].p) && isVisible(markers[12].p); allVisiblePatch = isVisible(markers[10].p) && isVisible(markers[11].p) && isVisible(markers[12].p); allVisibleHead = allVisiblePatch && isVisible(markers[9].p); test = Vector3d(markers.at(1).p.x(), markers.at(2).p.y(), markers.at(3).p.z()); // mirror alignment check mirrorAlignment = asin( abs((markers[6].p.z()-markers[7].p.z()))/ sqrt( pow(markers[6].p.x()-markers[7].p.x(), 2) + pow(markers[6].p.z()-markers[7].p.z(), 2) ) )*180/M_PI; platformXZtheta = asin( abs((markers[1].p.z()-markers[2].p.z()))/ sqrt( pow(markers[1].p.x()-markers[2].p.x(), 2) + pow(markers[1].p.z()-markers[2].p.z(), 2) ) )*180/M_PI; // screen Y alignment check screenAlignmentY = asin( abs((markers[19].p.y()-markers[21].p.y()))/ sqrt( pow(markers[19].p.x()-markers[21].p.x(), 2) + pow(markers[19].p.y()-markers[21].p.y(), 2) ) )*180/M_PI; // screen Z alignment check screenAlignmentZ = asin( abs(markers[19].p.z()-markers[20].p.z())/ sqrt( pow(markers[19].p.x()-markers[20].p.x(), 2) + pow(markers[19].p.z()-markers[20].p.z(), 2) ) )*180/M_PI* abs(markers[19].p.x()-markers[20].p.x())/ (markers[19].p.x()-markers[20].p.x()); // eye coordinates eyeRight = Vector3d(interoculardistance/2,0,0); eyeLeft = Vector3d(-interoculardistance/2,0,0); // fingers coordinates, fingersOccluded and framesOccluded if ( allVisibleFingers ) { indexCoords.update(markers[13].p, markers[14].p, markers[16].p ); thumbCoords.update(markers[15].p, markers[17].p, markers[18].p ); } // Record the calibration platform's position and home position if ( isVisible(markers[1].p) && allVisibleObject && fingerCalibrationDone==0 ) { calibration_fingers(1); fingerCalibrationDone=1; } // Record the calibration platform's position and home position if ( isVisible(markers[2].p) && allVisibleObject && fingerCalibrationDone==1 ) { calibration_fingers(2); fingerCalibrationDone=2; } if ( allVisibleObject && fingerCalibrationDone==2 ) { upperPin.update(markers[8].p, markers[11].p, markers[12].p ); lowerPin.update(markers[8].p, markers[11].p, markers[12].p ); } #ifndef SIMULATION // index coordinates if(allVisibleIndex) ind = indexCoords.getP1(); // thumb coordinates if(allVisibleThumb) thu = thumbCoords.getP1(); #endif }
void idle() { #ifndef SIMULATION optotrak->updateMarkers(); markers = optotrak->getAllMarkers(); #endif // Visibility check allVisiblePlatform = isVisible(markers[1].p) && isVisible(markers[2].p); allVisibleIndex = isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p); allVisibleThumb = isVisible(markers[15].p) && isVisible(markers[17].p) && isVisible(markers[18].p); allVisibleFingers = allVisibleIndex && allVisibleThumb; allVisiblePatch = isVisible(markers[10].p) && isVisible(markers[11].p) && isVisible(markers[12].p); allVisibleHead = allVisiblePatch && isVisible(markers[9].p); // mirror alignment check mirrorAlignment = asin( abs((markers[6].p.z()-markers[7].p.z()))/ sqrt( pow(markers[6].p.x()-markers[7].p.x(), 2) + pow(markers[6].p.z()-markers[7].p.z(), 2) ) )*180/M_PI; // screen Y alignment check screenAlignmentY = asin( abs((markers[19].p.y()-markers[21].p.y()))/ sqrt( pow(markers[19].p.x()-markers[21].p.x(), 2) + pow(markers[19].p.y()-markers[21].p.y(), 2) ) )*180/M_PI; // screen Z alignment check screenAlignmentZ = asin( abs(markers[19].p.z()-markers[20].p.z())/ sqrt( pow(markers[19].p.x()-markers[20].p.x(), 2) + pow(markers[19].p.z()-markers[20].p.z(), 2) ) )*180/M_PI* abs(markers[19].p.x()-markers[20].p.x())/ (markers[19].p.x()-markers[20].p.x()); // vertical translation (due to physical stimulus being lower than usual settings) Vtranslation = whereTop.y()-(stimulus_height[whichStimulus]/2); // eye coordinates eyeRight = Vector3d(interoculardistance/2,Vtranslation,0); eyeLeft = Vector3d(-interoculardistance/2,Vtranslation,0); // fingers coordinates if ( allVisibleIndex ) { indexCoords.update(markers[13].p, markers[14].p, markers[16].p ); thumbCoords.update(markers[15].p, markers[17].p, markers[18].p ); fingersOccluded = 0; }else { fingersOccluded=1; framesOccluded++; } // what the program checks online during the actual experiment // if it's haptic condition if(condition == 1) { if (fingerCalibrationDone==3 ) { // frames counting frameN++; #ifndef SIMULATION // index coordinates if(allVisibleIndex) ind = indexCoords.getP1(); // thumb coordinates if(allVisibleThumb) thu = thumbCoords.getP1(); #endif if(ind.y() > whereTop.y() - 5 && ind.y() < whereTop.y() + 5 && ind.z() < absDepth + 20 && markers[14].v.norm() < 35) indexOnPlace = true; else { indexOnPlace = false; timer.start(); } // Write to responseFile responseFile << fixed << parameters.find("SubjectName") << "\t" << interoculardistance << "\t" << trialNumber << "\t" << timer.getElapsedTimeInMilliSec() << "\t" << //time frameN << "\t" << //frameN ind.transpose() << "\t" << //indexXraw, indexYraw, indexZraw thu.transpose() << "\t" << //thumbXraw, thumbYraw, thumbZraw eyeRight.transpose() << "\t" << //eyeRXraw, eyeRYraw, eyeRZraw eyeLeft.transpose() << "\t" << //eyeLXraw, eyeLYraw, eyeLZraw fingersOccluded << "\t" << //fingersOccluded framesOccluded << "\t" << //framesOccluded absDepth <<"\t" << trial.getCurrent().second->getCurrentStaircase()->getID() << "\t" << trial.getCurrent().second->getCurrentStaircase()->getState() << "\t" << trial.getCurrent().second->getCurrentStaircase()->getInversions() << "\t" << trial.getCurrent().second->getCurrentStaircase()->getAscending() << "\t" << trial.getCurrent().second->getCurrentStaircase()->getStepsDone() << "\t" << stimulus_height[first_interval] << "\t" << stimulus_height[second_interval] << "\t" << markers[4].p.y() - markers[3].p.y() << "\t" << responsekey << endl; } } }
void update(int value) { // Timing things if ( trialMode != PROBEMODE ) { oldvariable = variable; variable = -factors["Onset"]*mathcommon::trianglewave( timeFrame , factors["StimulusDuration"]/(TIMER_MS*factors["FollowingSpeed"]) ); timeFrame+=1; bool isInside = ((projPoint - Vector3d(0,0,focalDistance) ).norm()) <= (circleRadius); // permette di avanzare se siamo in stimulusMode e lo stimolo ha fatto un semiciclo ( una passata da dx a sx o da su a giù ) bool nextMode = ( sumOutside == 0 ) && (trialMode==STIMULUSMODE); if ( isInside && ( sumOutside > stimulusEmiCycles ) || (nextMode) ) { sumOutside=-1; advanceTrial(); //cerr << "stim time= " << stimulusTimer.getElapsedTimeInMilliSec() << endl; } } // Simulate head translation // Coordinates picker markers[1] = Vector3d(0,0,0); markers[2] = Vector3d(0,10,0); markers[3] = Vector3d(0,0,10); Vector3d translation(0,0,0); switch ( (int) factors["Anchored"] ) { case 0: translation = Vector3d((circleRadius+1)*variable,0,0); break; case 1: translation = Vector3d((circleRadius+1)*variable,0,0); break; case 2: translation = Vector3d(0,(circleRadius+1)*variable,0); break; } markers[1]+=translation; markers[2]+=translation; markers[3]+=translation; headEyeCoords.update(markers[1],markers[2],markers[3]); eyeLeft = headEyeCoords.getLeftEye(); eyeRight = headEyeCoords.getRightEye(); fixationPoint = (headEyeCoords.getRigidStart().getFullTransformation() * ( Vector3d(0,0,focalDistance) ) ); // Projection of view normal on the focal plane pline = Eigen::ParametrizedLine<double,3>::Through(eyeRight,fixationPoint); projPoint = pline.intersection(focalPlane)*((fixationPoint - eyeRight).normalized()) + eyeRight; checkBounds(); glutPostRedisplay(); glutTimerFunc(TIMER_MS, update, 0); }