Ignore:
Timestamp:
02/21/17 16:37:58 (5 years ago)
Author:
Bayard Gildas
Message:

Plop

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp

    r142 r143  
    5858
    5959        //optical flow stack
     60        //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
    6061        opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
    6162        opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlow,"vitesse du flux optique");
     63        //opticalFlowSpeed=vitesse de déplacement en pixel par seconde (moyenne sur tous les points et division par le delta T)
    6264        cvmatrix* twoByOneSpeed=new cvmatrix((const Thread*)this,2,1,floatType);
    6365        opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOneSpeed);
     
    7072
    7173        Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
    72         DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_velocity",-5,5);
    73         DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_velocity",-5,5);
    74         DataPlot1D* xAccelerationPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_acceleration",-5,5);
    75         DataPlot1D* yAccelerationPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_acceleration",-5,5);
     74        DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x speed (px/s)",-250,250);
     75        DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y speed (px/s)",-250,250);
     76        DataPlot1D* xAccelerationPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_acceleration",-250,250);
     77        DataPlot1D* yAccelerationPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_acceleration",-250,250);
    7678
    7779        xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0));
     
    143145    Euler refAngles=GetDefaultReferenceOrientation()->GetQuaternion().ToEuler();//to keep default yaw reference
    144146
    145     // /!\ in this demo, the target value is a speed (in pixel/s). As a consequence the error is the difference between the current speed and the target speed
     147    // /!\ in this demo, the target value is a speed (in m/s). As a consequence the error is the difference between the current speed and the target speed
    146148    Vector2D error, errorVariation; // in Uav coordinate system
    147 
    148     //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
    149     //opticalFlowSpeed=vitesse de déplacement en pixel par seconde (moyenne sur tous les points et division par le delta T)
    150     error.x=opticalFlowSpeed->Output(0,0)-opticalFlowReference->Value(0,0);
    151     error.y=opticalFlowSpeed->Output(1,0)-opticalFlowReference->Value(1,0);
    152     errorVariation.x=opticalFlowAcceleration->Output(0,0);
    153     errorVariation.y=opticalFlowAcceleration->Output(1,0);
     149    float focal=271.76;
     150    float z,dz;
     151    AltitudeValues(z, dz);
     152    float scale=z/focal;
     153    error.x=opticalFlowSpeed->Output(0,0)*scale-opticalFlowReference->Value(0,0);
     154    error.y=opticalFlowSpeed->Output(1,0)*scale-opticalFlowReference->Value(1,0);
     155    errorVariation.x=opticalFlowAcceleration->Output(0,0)*scale;
     156    errorVariation.y=opticalFlowAcceleration->Output(1,0)*scale;
     157//Printf("Altitude=%f, Error=(%f,%f)\n",z,error.x,error.y);
    154158
    155159    u_x->SetValues(error.x, errorVariation.x);
Note: See TracChangeset for help on using the changeset viewer.