Changeset 142 in flair-src


Ignore:
Timestamp:
Feb 20, 2017, 6:28:22 PM (4 years ago)
Author:
Bayard Gildas
Message:

With EulerDerivative? acceleration estimation (noisy...)

Location:
trunk/demos/OpticalFlow/uav/src
Files:
2 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);
  • trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.h

    r124 r142  
    3131        class OpticalFlowSpeed;
    3232        class LowPassFilter;
     33        class EulerDerivative;
    3334        class CvtColor;
    3435    }
     
    5455        flair::core::cvmatrix *opticalFlowReference;
    5556        flair::filter::Pid *u_x, *u_y;
    56         flair::filter::LowPassFilter* opticalFlowSpeedFiltered;
     57        flair::filter::LowPassFilter* opticalFlowSpeed;
     58        flair::filter::LowPassFilter* opticalFlowAcceleration;
    5759        flair::filter::CvtColor* greyCameraImage;
    5860        flair::core::AhrsData *customReferenceOrientation;
     
    6062    private:
    6163        flair::filter::OpticalFlow *opticalFlow;
    62         flair::filter::OpticalFlowSpeed *opticalFlowSpeed;
     64        flair::filter::OpticalFlowSpeed *opticalFlowSpeedRaw;
     65        flair::filter::EulerDerivative *opticalFlowAccelerationRaw;
    6366                                flair::gui::PushButton *startOpticalflow,*stopOpticalflow;
    6467                                void StartOpticalFlow(void);
Note: See TracChangeset for help on using the changeset viewer.