예제 #1
0
static void discretise( double step, double tolerance )
{
    BOOST_STATIC_ASSERT( sizeof( Eigen::Vector3d ) == sizeof( double ) * 3 );
    comma::csv::input_stream< Eigen::Vector3d > istream( std::cin, csv );
    boost::optional< Eigen::Vector3d > previous_point;
    while( istream.ready() || ( std::cin.good() && !std::cin.eof() ) )
    {
        const Eigen::Vector3d* current_point = istream.read();
        if( !current_point ) { break; }
        if( previous_point )
        {
            output_points( *previous_point, *previous_point );
            double distance = ( *previous_point - *current_point ).norm();
            if( comma::math::less( step, distance ) )
            {
                Eigen::ParametrizedLine< double, 3 > line = Eigen::ParametrizedLine< double, 3 >::Through( *previous_point, *current_point );
                for( double t = step; comma::math::less( t + tolerance, distance ); t += step )
                {
                    Eigen::Vector3d point = line.pointAt( t );
                    output_points( *previous_point, point );
                }
            }
        }
        previous_point.reset( *current_point );
    }
    output_points( *previous_point, *previous_point );
}
/**
 * @brief The 'regression2D' method can be used to fit a line to a given point set.
 * @param points_begin      set begin iterator
 * @param points_end        set end iterator
 * @param fit_start         the start of the line fit
 * @param fit_end           the set termintating iterator position
 * @param line              the parameterized line to work with
*/
inline void regression2D(const std::vector<LaserBeam>::const_iterator &points_begin, const std::vector<LaserBeam>::const_iterator &points_end,
                         Eigen::ParametrizedLine<double, 2> &line)
{
    std::vector<LaserBeam>::const_iterator back_it = points_end;
    --back_it;
    Eigen::Vector2d front (points_begin->posX(), points_end->posY());
    Eigen::Vector2d back (back_it->posX(), back_it->posY());

    unsigned int size = std::distance(points_begin, points_end);
    Eigen::MatrixXd A(size, 2);
    Eigen::VectorXd b(size);
    A.setOnes();

    Eigen::Vector2d dxy = (front - back).cwiseAbs();
    bool solve_for_x = dxy.x() > dxy.y();
    if(solve_for_x) {
        /// SOLVE FOR X
        int i = 0;
        for(std::vector<LaserBeam>::const_iterator it = points_begin ; it != points_end ; ++it, ++i)
        {
            A(i,1) = it->posX();
            b(i)   = it->posY();
        }
    } else {
        /// SOLVE FOR Y
        int i = 0;
        for(std::vector<LaserBeam>::const_iterator it = points_begin ; it != points_end ; ++it, ++i)
        {
            A(i,1) = it->posY();
            b(i)   = it->posX();
        }
    }

    Eigen::VectorXd weights = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    double          alpha   = weights(0);
    double          beta    = weights(1);
    Eigen::Vector2d from;
    Eigen::Vector2d to;

    if(solve_for_x) {
        from(0) = 0.0;
        from(1) = alpha;
        to(0)   = 1.0;
        to(1)   = alpha + beta;
    } else {
        from(0) = alpha;
        from(1) = 0.0;
        to(0)   = alpha + beta;
        to(1)   = 1.0;
    }

    Eigen::Vector2d fit_start;
    Eigen::Vector2d fit_end;
    line = Eigen::ParametrizedLine<double, 2>(from, (to - from).normalized());
    fit_start = line.projection(front);
    fit_end   = line.projection(back);
    line = Eigen::ParametrizedLine<double, 2>(fit_start, (fit_end - fit_start));
}
예제 #3
0
/**
 * @brief drawFixation
 */
void drawFixation()
{
    double circleRadius = parameters.get("MaxCircleRadius");	// millimeters
    double zBoundary    = parameters.get("MaxZOscillation"); // millimeters
    // Projection of view normal on the focal plane
    Vector3d directionOfSight = (headEyeCoords.getRigidStart().getFullTransformation().linear()*Vector3d(0,0,-1)).normalized();
    Eigen::ParametrizedLine<double,3> lineOfSightRight = Eigen::ParametrizedLine<double,3>::Through( headEyeCoords.getRightEye() , headEyeCoords.getRightEye()+directionOfSight );
    Eigen::Hyperplane<double,3> focalPlane = Eigen::Hyperplane<double,3>::Through( Vector3d(1,0,focalDistance), Vector3d(0,1,focalDistance),Vector3d(0,0,focalDistance) );
    double lineOfSightRightDistanceToFocalPlane = lineOfSightRight.intersection(focalPlane);
    Vector3d opticalAxisToFocalPlaneIntersection = lineOfSightRightDistanceToFocalPlane *(directionOfSight)+ (headEyeCoords.getRightEye());

    switch ( headCalibrationDone )
    {
    case 1:
    {
        // STIM_FIXED stimulus at (0,0,focalDistance)
        glPushAttrib(GL_POINT_BIT);
        glColor3fv(glRed);
        glPointSize(5);
        glBegin(GL_POINTS);
        glVertex3d(0,0,focalDistance);
		glVertex3d(headEyeCoords.getRightEye().x(),headEyeCoords.getRightEye().y(),focalDistance);
        glEnd();
        glPopAttrib();
        break;
    }
    case 2:
    {
        // STIM_FIXED stimulus + projected points
        glPushAttrib( GL_ALL_ATTRIB_BITS );
        glPointSize(5);
		glLineWidth(2);
		
        glBegin(GL_POINTS);
        glColor3fv(glRed);
        glVertex3d(0,0,focalDistance);
		glColor3fv(glBlue);
		glVertex3dv(opticalAxisToFocalPlaneIntersection.data());
		glColor3fv(glWhite);
		glVertex3d(headEyeCoords.getRightEye().x(),headEyeCoords.getRightEye().y(),focalDistance);
        glEnd();

		double r2EyeRight = pow(headEyeCoords.getRightEye().x(),2)+pow(headEyeCoords.getRightEye().y(),2);
        // Draw the calibration circle
        if ( pow(opticalAxisToFocalPlaneIntersection.x(),2)+pow(opticalAxisToFocalPlaneIntersection.y(),2) <= circleRadius*circleRadius && abs(headEyeCoords.getRightEye().z()) < zBoundary && r2EyeRight<circleRadius*circleRadius )
        {
			readyToStart=true;
            drawCircle(circleRadius,0,0,focalDistance,glGreen);
        }
        else
        {
            drawCircle(circleRadius,0,0,focalDistance,glRed);
        }
        glPopAttrib();
		break;
	}
    }
}
예제 #4
0
Vector3d getEyeProjectionPoint()
{
    Eigen::Hyperplane<double,3> focalPlane = Eigen::Hyperplane<double,3>::Through   ( Vector3d(1,0,focalDistance), Vector3d(0,1,focalDistance),Vector3d(0,0,focalDistance) );

    // Projection of view normal on the focal plane
    Vector3d directionOfSight = (headEyeCoords.getRigidStart().getFullTransformation().rotation()*Vector3d(0,0,-1)).normalized();
    Eigen::ParametrizedLine<double,3> lineOfSightRight = Eigen::ParametrizedLine<double,3>::Through( eyeRight , eyeRight+directionOfSight );

    double dist = lineOfSightRight.intersection(focalPlane);

    return (dist*(directionOfSight)+ (eyeRight));
}
예제 #5
0
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);
}
/**
 * @brief Check if all points between first and second are close enough to the fitted line.
 * @param first             start iterator of point set
 * @param second            end   iterator of point set
 * @param line              the fitted line
 * @param sigma             the maximum distance to consider a point an inlyer
 * @return
 */
inline bool withinLineFit(const std::vector<LaserBeam>::const_iterator &first, const std::vector<LaserBeam>::const_iterator &second,
                          const Eigen::ParametrizedLine<double, 2> &line, const double sigma)
{
    assert(std::distance(first, second) > 0);

    bool fitting = true;
    for(std::vector<LaserBeam>::const_iterator it = first; it != second ; ++it) {
        fitting &= (line.distance(Eigen::Vector2d(it->posX(), it->posY())) < sigma);
    }

    return fitting;
}
예제 #7
0
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);
}
예제 #8
0
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);
}
예제 #9
0
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);
}
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);
}
예제 #11
0
int main( int argc, char** argv )
{
//     for( unsigned int i = 0; i < boost::lexical_cast< unsigned int >( argv[1] ); ++i )
//     {
//         double x = r() * 20 - 10;
//         double y = r() * 20 - 10;
//         double z = r() * 20 - 10;
//         std::cout << x << ',' << y << ',' << z << ',' << int( std::max( std::abs( x ), std::max( std::abs( y ), std::abs( z ) ) ) ) << std::endl;
//     }
//     return 0;

    try
    {
        std::string normal_string;
        std::string points_string;
        std::string point_outside;
        boost::program_options::options_description description( "options" );
        description.add_options()
            ( "help,h", "display help message" )
            ( "points,p", boost::program_options::value< std::string >( &points_string )->default_value( "0,0,0" ), "point(s) belonging to the plane, either 3 points, or 1 point, if --normal defined" )
            ( "point-outside", boost::program_options::value< std::string >( &point_outside ), "point on the side of the plane where the normal would point, a convenience option; 3 points are enough" )
            ( "normal,n", boost::program_options::value< std::string >( &normal_string ), "normal to the plane" )
            ( "intersections", "assume the input represents a trajectory, find all its intersections with the plane")
            ( "threshold", boost::program_options::value< double >(), "if --intersections present, any separation between contiguous points of trajectory greater than threshold will be treated as a gap in the trajectory (no intersections will lie in the gaps)");
        description.add( comma::csv::program_options::description( "x,y,z" ) );
        boost::program_options::variables_map vm;
        boost::program_options::store( boost::program_options::parse_command_line( argc, argv, description), vm );
        boost::program_options::notify( vm );
        if ( vm.count( "help" ) )
        {
            std::cerr << std::endl;
            std::cerr << "take points on stdin, append distance from a given plane" << std::endl;
            std::cerr << std::endl;
            std::cerr << "if --intersections is specified, assume the input represents a trajectory, find its intersections with the plane," << std::endl;
            std::cerr << "for each intersection, output adjacent points between which it occurs, the intersection point, and the direction of intersection (-1,0,+1)," << std::endl;
            std::cerr << "where 0 indicates that both adjacent points are in the plane, +1 if the trajectory's direction is the same as normal of the plane, and -1 otherwise" << std::endl;
            std::cerr << std::endl;
            std::cerr << "usage: " << std::endl;
            std::cerr << "    cat points.csv | points-slice [options] > points_with_distance.csv" << std::endl;
            std::cerr << "    cat trajectory.csv | points-slice [options] --intersections > intersections.csv" << std::endl;
            std::cerr << std::endl;
            std::cerr << description << std::endl;
            std::cerr << std::endl;
            std::cerr << "defining the plane" << std::endl;
            std::cerr << "    --points x1,y1,z1,x2,y2,z2,x3,y3,x3" << std::endl;
            std::cerr << "    --points x1,y1,z1,x2,y2,z2,x3,y3,x3 --point-outside x4,y4,z4" << std::endl;
            std::cerr << "    --points x,y,z --normal n1,n2,n3" << std::endl;
            std::cerr << "    --normal n1,n2,n3    (the plane passes through 0,0,0)" << std::endl;
            std::cerr << std::endl;
            std::cerr << "output" << std::endl;
            std::cerr << "    default: " << std::endl;
            std::cerr << "        x,y,z,distance, where distance is signed distance to the plane" << std::endl;
            std::cerr << std::endl;
            std::cerr << "    if --intersections is specified:" << std::endl;
            std::cerr << "        previous_input_line,input_line,intersection,direction" << std::endl;
            std::cerr << std::endl;
            std::cerr << "examples:" << std::endl;
            std::cerr << "   echo -e \"0,0,-1\\n0,0,0\\n0,0,1\" | points-slice --points 0,0,0,0,1,0,1,0,0" << std::endl;
            std::cerr << "   echo -e \"0,0,-1\\n0,0,0\\n0,0,1\" | points-slice --points 0,0,0,0,1,0,1,0,0 --point-outside 0,0,1" << std::endl;
            std::cerr << "   echo -e \"0,0,-1\\n0,0,0\\n0,0,1\" | points-slice --points 0,0,0 --normal 0,0,1" << std::endl;
            std::cerr << "   echo -e \"0,0,-1\\n0,0,0\\n0,0,1\" | points-slice --normal 0,0,1" << std::endl;
            std::cerr << "   echo -e \"0,0,-1\\n0,0,0\\n0,0,1\" | points-slice --normal 0,0,1 --intersections" << std::endl;
            std::cerr << "   echo -e \"0,0,-1\\n0,0,-0.5\\n0,0,0\\n0,0,1\\n0,0,1.5\" | points-slice --normal 0,0,1 --intersections --threshold=0.5" << std::endl;
            std::cerr << std::endl;
            return 1;
        }
        if( vm.count( "points" ) == 0 ) { std::cerr << "points-slice: please specify --points" << std::endl; return 1; }
        comma::csv::options csv = comma::csv::program_options::get( vm );
        Eigen::Vector3d normal;
        Eigen::Vector3d point;
        if( vm.count( "normal" ) )
        {
            normal = comma::csv::ascii< Eigen::Vector3d >( "x,y,z", ',' ).get( normal_string );
            point = comma::csv::ascii< Eigen::Vector3d >( "x,y,z", ',' ).get( points_string );
        }
        else
        {
            boost::array< Eigen::Vector3d, 3 > points; // quick and dirty
            std::vector< std::string > v = comma::split( points_string, ',' );
            if( v.size() != 9 ) { std::cerr << "points-slice: expected 3 points, got: \"" << points_string << "\"" << std::endl; return 1; }
            point = comma::csv::ascii< Eigen::Vector3d >( "x,y,z", ',' ).get( v[0] + ',' + v[1] + ',' + v[2] ); // quick and dirty
            Eigen::Vector3d a = comma::csv::ascii< Eigen::Vector3d >( "x,y,z", ',' ).get( v[3] + ',' + v[4] + ',' + v[5] ); // quick and dirty
            Eigen::Vector3d b = comma::csv::ascii< Eigen::Vector3d >( "x,y,z", ',' ).get( v[6] + ',' + v[7] + ',' + v[8] ); // quick and dirty
            a -= point;
            b -= point;
            if( comma::math::equal( std::abs( a.dot( b ) ), a.norm() * b.norm() ) ) { std::cerr << "points-slice: given points are not corners or a triangle: \"" << points_string << "\"" << std::endl; }
            normal = a.cross( b );
            if( vm.count( "point-outside" ) )
            {
                Eigen::Vector3d p = comma::csv::ascii< Eigen::Vector3d >( "x,y,z", ',' ).get( point_outside );
                if( comma::math::equal( ( p - point ).dot( normal ), 0 ) ) { std::cerr << "points-slice: expected a point outside of the plane, got: " << point_outside << ", which belongs to the plane" << std::endl; return 1; }
                normal *= normal.dot( p - point ) > 0 ? 1 : -1;
            }
        }
        normal.normalize();
        #ifdef WIN32
            _setmode( _fileno( stdout ), _O_BINARY ); /// @todo move to a library
        #endif
        comma::csv::input_stream< Eigen::Vector3d > istream( std::cin, csv, Eigen::Vector3d::Zero() );
        comma::signal_flag is_shutdown;
        comma::csv::ascii< Eigen::Vector3d > ascii( "x,y,z", csv.delimiter );
        comma::csv::binary< Eigen::Vector3d > binary( "3d", "x,y,z" );
        if( vm.count("intersections") )
        {
            Eigen::Hyperplane< double, 3 > plane( normal, point );
            boost::optional< Eigen::Vector3d > last;
            double d_last = 0;
            boost::optional< double > threshold;
            if( vm.count("threshold") ) { threshold.reset( vm["threshold"].as< double >() ); }
            std::string previous_line_ascii;
            std::vector< char > previous_data_binary;
            while( !is_shutdown && ( istream.ready() || ( !std::cin.eof() && std::cin.good() ) ) )
            {
                const Eigen::Vector3d* p = istream.read();
                if( !p ) { break; }
                double d = ( *p - point ).dot( normal );
                bool valid_intersection = false;
                if( last )
                {
                    bool intersects = d * d_last <= 0;
                    bool interval_within_threshold = !threshold || ( threshold && ( *p - *last ).norm() <= *threshold );
                    valid_intersection = intersects && interval_within_threshold;
                }
                if( valid_intersection )
                {
                    Eigen::Vector3d intersection_point;
                    BOOST_STATIC_ASSERT( sizeof( Eigen::Vector3d ) == sizeof( double ) * 3 );
                    bool lies_on_plane = ( d == 0 && d_last == 0 );
                    if( lies_on_plane )
                    {
                        intersection_point = *last; 
                    }
                    else
                    {
                        Eigen::ParametrizedLine< double, 3 > line = Eigen::ParametrizedLine< double, 3 >::Through( *last, *p );
                        intersection_point = line.intersectionPoint( plane );
                    }
                    comma::int32 direction;
                    if( d_last != 0 ) { direction = ( d_last < 0 ) ? 1 : -1; }
                    else if( d != 0 ) { direction = ( d > 0 ) ? 1 : -1; }
                    else { direction = 0; }
                    if( csv.binary() )
                    {
                        std::cout.write( &previous_data_binary[0], previous_data_binary.size() );
                        std::cout.write( istream.binary().last(), istream.binary().binary().format().size() );
                        std::cout.write( reinterpret_cast< const char* >( &intersection_point ), sizeof( double ) * 3 );
                        std::cout.write( reinterpret_cast< const char* >( &direction ), sizeof( comma::int32 ) );
                    }
                    else
                    {
                        std::cout << previous_line_ascii << csv.delimiter << comma::join( istream.ascii().last(), csv.delimiter ) << csv.delimiter
                                    << ascii.put( intersection_point ) << csv.delimiter << direction << std::endl;
                    }
                }
                last = *p;
                d_last = d;
                if( csv.binary() )
                {
                    if( previous_data_binary.size() != istream.binary().binary().format().size() )
                    {
                        previous_data_binary.resize( istream.binary().binary().format().size() );
                    }
                    ::memcpy( &previous_data_binary[0], istream.binary().last(), previous_data_binary.size() );
                }
                else
                {
                    previous_line_ascii = comma::join( istream.ascii().last(), csv.delimiter );
                }
            }
        }
        else
        {
            while( !is_shutdown && ( istream.ready() || ( !std::cin.eof() && std::cin.good() ) ) )
            {
                const Eigen::Vector3d* p = istream.read();
                if( !p ) { break; }
                double d = ( *p - point ).dot( normal );
                if( csv.binary() )
                {
                    std::cout.write( istream.binary().last(), istream.binary().binary().format().size() );
                    std::cout.write( reinterpret_cast< const char* >( &d ), sizeof( double ) );
                }
                else
                {
                    std::cout << comma::join( istream.ascii().last(), csv.delimiter ) << csv.delimiter << d << std::endl;
                }
            }
        }
        return 0;
    }
    catch( std::exception& ex )
    {
        std::cerr << "points-slice: " << ex.what() << std::endl;
    }
    catch( ... )
    {
        std::cerr << "points-slice: unknown exception" << std::endl;
    }
    return 1;
}
예제 #12
0
void drawGeneratedPlanes()
{
	Hyperplane<double,3> screenPlane,mirrorPlane;

	//mirror[3] = Vector3d(mirror[1].x(),mirror[2].y(),mirror[1].z());
	screen[3] = Vector3d(screen[1].x(), screen[2].y(),screen[1].z());


	screenPlane = Hyperplane<double,3>::Through( screen[0], screen[1],screen[2] );
	mirrorPlane = Hyperplane<double,3>::Through(mirror[1], mirror[0], mirror[2] );

	screenNormal = screenPlane.normal();
	mirrorNormal = mirrorPlane.normal();

	Eigen::ParametrizedLine<double,3> line = Eigen::ParametrizedLine<double,3>::Through( screenCenter , screenCenter + screenNormal*10);
	// Calcola il punto proiezione della normale uscente dallo schermo sullo specchio
	double p = line.intersection(mirrorPlane);
	screenProjectionOnMirror = p*((screenNormal).normalized()) + screenCenter;

	planesIntersectionBase = -getPlaneIntersection(mirrorPlane,screenPlane,0);
	planesIntersectionDirection = mirrorPlane.normal().cross(screenPlane.normal());

	planesIntersecEnd = planesIntersectionBase - 200*planesIntersectionDirection;

	// Draw the planes intersection
	glPushMatrix();
	glColor3fv(glBlue);
	glBegin(GL_LINES);
	glVertex3dv(planesIntersectionBase.data());
	glVertex3dv(planesIntersecEnd.data());
	glEnd();
	glPopMatrix();

	// Draw the mirror plane (only if calibrationPhase==0)
	if ( calibrationPhase==0 )
	{
		glPushMatrix();
		Grid gridMirror;
		gridMirror.setRowsAndCols(20,20);
		gridMirror.init(mirror[1],mirror[0],mirror[2],mirror[3]);	// invert the order to have 45 degrees yaw and 90 degree pitch when correct
		glColor3fv(glRed);
		gridMirror.draw();
		glPopMatrix();

		// Draw the mirror normal
		mirrorCenter = (mirror[0]+mirror[1]+mirror[2]+mirror[3])/4.0;
		Vector3d mirrorCenterDirection = mirrorCenter + 20*mirrorNormal;
		glPushMatrix();
		glColor3fv(glRed);
		glBegin(GL_LINES);
		glVertex3dv(mirrorCenter.data());
		glVertex3dv(mirrorCenterDirection.data());
		glEnd();
		glPopMatrix();
	}

	// Draw the screen plane
	if ( calibrationPhase!=0)
	{
		glPushMatrix();
		Grid gridScreen;
		gridScreen.setRowsAndCols(20,20);
		gridScreen.init(screen[0],screen[1],screen[2],screen[3]);
		glColor3fv(glGreen);
		gridScreen.draw();
		glPopMatrix();

		// Draw the screen normal
		screenCenter = (screen[0]+screen[1]+screen[2]+screen[3])/4.0;
		Vector3d screenCenterDirection = screenCenter + 20*screenNormal;
		glPushMatrix();
		glBegin(GL_LINES);
		glVertex3dv(screenCenter.data());
		glVertex3dv(screenCenterDirection.data());
		glEnd();
		glPopMatrix();
	}


	// Draw the vector from screen center to mirror center
	glPushMatrix();
	glColor3fv(glRed);
	glBegin(GL_LINES);
	glVertex3dv(screenCenter.data());
	glVertex3dv(mirrorCenter.data());
	glEnd();
	glPopMatrix();

	// Draw the vector from mirror center to X axis
	Vector3d orthoProjMirrorCenter(mirrorCenter);
	glPushMatrix();
	glColor3fv(glRed);
	glBegin(GL_LINES);
	glVertex3dv(mirrorCenter.data());
	glVertex3d(mirrorCenter.x(),mirrorCenter.y(),0);
	glEnd();
	glPopMatrix();

	// draw mirror saved center
	if ( calibrationPhase == 1)
	{
		glPushMatrix();
		glTranslatef(mirrorSavedCenter.x(),mirrorSavedCenter.y(),mirrorSavedCenter.z());
		glutSolidSphere(1,10,20);
		glPopMatrix();
	}

	mirrorYaw = mathcommon::toDegrees(acos(mirrorNormal.z()));
	mirrorPitch = mathcommon::toDegrees(acos(mirrorNormal.y()));

	screenYaw = mathcommon::toDegrees(acos(screenNormal.z()));
	screenPitch = mathcommon::toDegrees(acos(screenNormal.y()));
	glColor3fv(glWhite);

	double tolerance = 0.5; //degrees
	bool mirrorYawOK = abs(mirrorYaw - 45) <= tolerance;
	bool screenYawOK = abs(screenYaw - 90) <= tolerance;
	bool mirrorPitchOK = abs(mirrorPitch-90) <= tolerance;
	bool screenPitchOK = abs(screenPitch-90) <= tolerance;


	GLText text;
	text.init(SCREEN_WIDTH,SCREEN_HEIGHT,glWhite,GLUT_BITMAP_HELVETICA_18);
	text.enterTextInputMode();
	switch( calibrationPhase )
	{
	case 0:
		text.draw("==== INSTRUCTIONS ====");
		text.draw("Put the markers on the mirror vertices, try to keep them aligned. Press SPACEBAR to save their coordinates");
		text.draw("Press 2/8 to zoom out/in. Press '+/-' to go forward or backward");
		break;
	case 1:
		{
			text.draw("Now remove the mirror, mirror coordinates are saved to calibrationCoordinates.txt");
			text.draw("Now you must put the markers on the screen");
			text.draw("");
			text.draw("");
			text.draw("Mirror current center= " + util::stringify< Eigen::Matrix<double,1,3> >(mirrorSavedCenter.transpose() ) );
			text.draw("Distance from mirror center to screen center " + util::stringify<double>( (mirrorSavedCenter - screenCenter).norm()	 ));
			text.draw("Projection error of screen normal on mirror center  " + util::stringify<Eigen::Matrix<double,1,3> >( screenProjectionOnMirror.transpose() ) );
		}
		break;
	case 2:
		break;
	}

	text.draw("==== MIRROR INFO ====");
	mirrorPitchOK ? glColor3fv(glGreen) : glColor3fv(glRed);
	text.draw("Mirror Pitch= " + util::stringify<double>(mirrorPitch) );
	mirrorYawOK ? glColor3fv(glGreen) : glColor3fv(glRed);
	text.draw("Mirror Yaw=" + util::stringify<double>(mirrorYaw));
	glColor3fv(glWhite);
	text.draw("==== SCREEN INFO ====");
	screenPitchOK ? glColor3fv(glGreen) : glColor3fv(glRed);
	text.draw("Screen Pitch=" + util::stringify<double>(screenPitch));
	screenYawOK ? glColor3fv(glGreen) : glColor3fv(glRed);
	text.draw("Screen Yaw=" + util::stringify<double>(screenYaw));
	// Here print the relative orientation of screen and mirror
	if ( calibrationPhase==0)
	{   text.draw("Total distance (norm)= " + util::stringify<double>(abs(mirrorCenter.z())+(mirrorCenter - screenCenter).norm()) );
	text.draw("Total distance (only x+z)= " + util::stringify<double>( abs(mirrorCenter.x()-screenCenter.x()) + abs(mirrorCenter.z()) ));
	}
	else
	{
		text.draw("Total distance (norm)= " + util::stringify<double>(abs(mirrorSavedCenter.z())+(mirrorSavedCenter - screenCenter).norm()) );
		text.draw("Total distance (ignore y offset)= " + util::stringify<double>( abs(mirrorSavedCenter.x()-screenCenter.x()) + abs(mirrorSavedCenter.z()) ));
	}
	text.draw("Intersection of planes" + util::stringify<Eigen::Matrix<int,1,3> >(planesIntersectionBase.cast<int>()));
	text.leaveTextInputMode();
}