Ignore:
Timestamp:
02/20/17 18:28:22 (5 years ago)
Author:
Bayard Gildas
Message:

With EulerDerivative acceleration estimation (noisy...)

File:
1 edited

Legend:

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

    r131 r142  
    1919#include <OpticalFlowSpeed.h>
    2020#include <LowPassFilter.h>
     21#include <EulerDerivative.h>
    2122#include <cvmatrix.h>
    2223#include <GridLayout.h>
     
    4950        exit(1);
    5051    }
    51 uav->GetVerticalCamera()->SetLogFormat(Camera::LogFormat::JPG);
    52 getFrameworkManager()->AddDeviceToLog(uav->GetVerticalCamera());
    5352       
    5453        startOpticalflow=new PushButton(GetButtonsLayout()->NewRow(),"start optical flow");
     
    5958
    6059        //optical flow stack
    61         opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
    62         opticalFlowSpeed=new OpticalFlowSpeed(opticalFlow,"vitesse du flux optique");
     60        opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
     61        opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlow,"vitesse du flux optique");
     62        cvmatrix* twoByOneSpeed=new cvmatrix((const Thread*)this,2,1,floatType);
     63        opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOneSpeed);
     64        cvmatrix* twoByOneAccelerationRaw=new cvmatrix((const Thread*)this,2,1,floatType);
     65        opticalFlowAccelerationRaw=new EulerDerivative(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"derivative",twoByOneAccelerationRaw);
     66        cvmatrix* twoByOneAcceleration=new cvmatrix((const Thread*)this,2,1,floatType);
     67        opticalFlowAcceleration=new LowPassFilter(opticalFlowAccelerationRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Acceleration lowPass",twoByOneAcceleration);
    6368
    64         cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,1);
    65         for(int i=0;i<2;i++) {
    66                         desc->SetElementName(i,0,opticalFlowSpeed->Output()->Name(i,0));
    67         }
    68         cvmatrix* prev_value=new cvmatrix((const Thread*)this,desc,floatType,uav->ObjectName()); // diamond inheritance
    69         for(int i=0;i<2;i++) {
    70                         prev_value->SetValue(i,0,0);
    71         }
    72 
    73         opticalFlowSpeedFiltered=new LowPassFilter(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"passe bas",prev_value);
    74 // delete prev_value?
    75 
    76         getFrameworkManager()->AddDeviceToLog(opticalFlowSpeed);
     69        getFrameworkManager()->AddDeviceToLog(opticalFlowSpeedRaw);
    7770
    7871        Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
    7972        DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_velocity",-5,5);
    8073        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);
    8176
    82         xVelocityPlot->AddCurve(opticalFlowSpeed->Output()->Element(0,0));
    83         xVelocityPlot->AddCurve(opticalFlowSpeedFiltered->Matrix()->Element(0,0),DataPlot::Blue);
    84         yVelocityPlot->AddCurve(opticalFlowSpeed->Output()->Element(1,0));
    85         yVelocityPlot->AddCurve(opticalFlowSpeedFiltered->Matrix()->Element(1,0),DataPlot::Blue);
     77        xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0));
     78        xVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(0,0),DataPlot::Blue);
     79        yVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(1,0));
     80        yVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(1,0),DataPlot::Blue);
     81        xAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(0,0));
     82        xAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(0,0),DataPlot::Blue);
     83        yAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(1,0));
     84        yAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(1,0),DataPlot::Blue);
    8685
    8786        u_x=new Pid(setupLawTab->At(1,0),"u_x");
     
    124123void DemoOpticalFlow::ExtraCheckPushButton(void) {
    125124        if(startOpticalflow->Clicked()) {
    126         //StartOpticalFlow();
    127                                 GetUav()->GetVerticalCamera()->SavePictureToFile("./toto.jpg");
    128                                
     125            StartOpticalFlow();
    129126        }
    130127}
     
    151148    //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
    152149    //opticalFlowSpeed=vitesse de déplacement en pixel par seconde (moyenne sur tous les points et division par le delta T)
    153     error.x=opticalFlowSpeedFiltered->Output(0,0)-opticalFlowReference->Value(0,0);
    154     error.y=opticalFlowSpeedFiltered->Output(1,0)-opticalFlowReference->Value(1,0);
    155 
    156     //la dérivée est à la fréquence de la loi de commande ("rapide") alors que le flux optique est à la fréquence de la caméra
    157     // fréquemment la dérivée car le signal n'a pas bougé -> dérivée super crade
    158     //gsanahuj: brancher un eulerderivative derriere le opticalFlowSpeedFiltered pour avoir la derivee
    159     //opticalFlowSpeed doit etre renomme car finalement ce n'est pas une vitesse mais un deplacement
    160     errorVariation.x=0;
    161     errorVariation.y=0;
     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);
    162154
    163155    u_x->SetValues(error.x, errorVariation.x);
Note: See TracChangeset for help on using the changeset viewer.