C++ Optotrak2::updateMarkers方法代码示例

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 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;
        }
    }
}

/**
 * @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;

//.........这里部分代码省略.........

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
//.........这里部分代码省略.........

// 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)
//.........这里部分代码省略.........