Ignore:
Timestamp:
03/03/17 12:26:05 (7 years ago)
Author:
Bayard Gildas
Message:

Compensation rotations

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp

    r154 r155  
    1717#include <CvtColor.h>
    1818#include <OpticalFlow.h>
     19#include <OpticalFlowCompensated.h>
    1920#include <OpticalFlowSpeed.h>
    2021#include <LowPassFilter.h>
    … …  
    6061  //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
    6162  opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
    62   opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlow,"vitesse du flux optique");
     63  opticalFlowCompensated=new OpticalFlowCompensated(opticalFlow,uav->GetAhrs(),"flux optique compensé");
     64  opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlowCompensated,"vitesse du flux optique");
    6365  //opticalFlowSpeed=vitesse de déplacement en pixels par seconde (moyenne sur tous les points et division par le delta T)
    64   cvmatrix* twoByOne=new cvmatrix((const Thread*)this,2,1,floatType);
    65   opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOne);
    66   opticalFlowAccelerationRaw=new EulerDerivative(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"derivative",twoByOne);
    67   opticalFlowAcceleration=new LowPassFilter(opticalFlowAccelerationRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Acceleration lowPass",twoByOne);
    68   delete twoByOne;
     66  cvmatrix* twoByOneOFS=new cvmatrix((const Thread*)this,2,1,floatType);
     67  cvmatrix* twoByOneOFAR=new cvmatrix((const Thread*)this,2,1,floatType);
     68  cvmatrix* twoByOneOFA=new cvmatrix((const Thread*)this,2,1,floatType);
     69  opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOneOFS);
     70  opticalFlowAccelerationRaw=new EulerDerivative(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"derivative",twoByOneOFAR);
     71  opticalFlowAcceleration=new LowPassFilter(opticalFlowAccelerationRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Acceleration lowPass",twoByOneOFA);
    6972 
    7073  getFrameworkManager()->AddDeviceToLog(opticalFlowSpeedRaw);
    7174
    72         Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
    73         DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x speed (px/s)",-250,250);
    74         DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y speed (px/s)",-250,250);
    75   DataPlot1D* xAccelerationPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_acceleration",-250,250);
    76   DataPlot1D* yAccelerationPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_acceleration",-250,250);
     75  Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
     76  DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x speed (px/s)",-250,250);
     77  DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y speed (px/s)",-250,250);
     78  DataPlot1D* xFirstPointPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x movement first point",-25,25);
     79  DataPlot1D* yFirstPointPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y movement first point",-25,25);
     80//  DataPlot1D* xAccelerationPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_acceleration",-250,250);
     81//  DataPlot1D* yAccelerationPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_acceleration",-250,250);
    7782
    78         xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0));
    79         xVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(0,0),DataPlot::Blue);
    80         yVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(1,0));
    81         yVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(1,0),DataPlot::Blue);
    82   xAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(0,0));
    83   xAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(0,0),DataPlot::Blue);
    84   yAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(1,0));
    85   yAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(1,0),DataPlot::Blue);
     83  xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0));
     84  xVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(0,0),DataPlot::Blue);
     85  yVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(1,0));
     86  yVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(1,0),DataPlot::Blue);
     87  xFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(0,0));
     88  xFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(1,0),DataPlot::Blue);
     89  xFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(2,0),DataPlot::Green);
     90//  xAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(0,0));
     91//  xAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(0,0),DataPlot::Blue);
     92//  yAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(1,0));
     93//  yAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(1,0),DataPlot::Blue);
    8694
    87         u_x=new Pid(setupLawTab->At(1,0),"u_x");
    88         u_x->UseDefaultPlot(graphLawTab->NewRow());
    89         u_y=new Pid(setupLawTab->At(1,1),"u_y");
    90         u_y->UseDefaultPlot(graphLawTab->LastRowLastCol());
     95  u_x=new Pid(setupLawTab->At(1,0),"u_x");
     96  u_x->UseDefaultPlot(graphLawTab->NewRow());
     97  u_y=new Pid(setupLawTab->At(1,1),"u_y");
     98  u_y->UseDefaultPlot(graphLawTab->LastRowLastCol());
    9199
    92         opticalFlowGroupBox=new GroupBox(GetJoystick()->GetTab()->NewRow(),"consignes fo");
    93         maxXSpeed=new DoubleSpinBox(opticalFlowGroupBox->NewRow(),"debattement x"," m/s",-5,5,0.1,1);
    94         maxYSpeed=new DoubleSpinBox(opticalFlowGroupBox->LastRowLastCol(),"debattement y"," m/s",-5,5,0.1,1);
     100  opticalFlowGroupBox=new GroupBox(GetJoystick()->GetTab()->NewRow(),"consignes fo");
     101  maxXSpeed=new DoubleSpinBox(opticalFlowGroupBox->NewRow(),"debattement x"," m/s",-5,5,0.1,1);
     102  maxYSpeed=new DoubleSpinBox(opticalFlowGroupBox->LastRowLastCol(),"debattement y"," m/s",-5,5,0.1,1);
    95103 
    96104  Tab* opticalFlowRealTab=new Tab(getFrameworkManager()->GetTabWidget(),"real speed");
    … …  
    99107  opticalFlowRealAcceleration=new cvmatrix((const Thread*)this,2,1,floatType);
    100108  DataPlot1D* xRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->NewRow(),"x speed (m/s)",-2,2);
    101         DataPlot1D* yRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y speed (m/s)",-2,2);
     109  DataPlot1D* yRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y speed (m/s)",-2,2);
    102110  DataPlot1D* xRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->NewRow(),"x acceleration (m/s2)",-2,2);
    103         DataPlot1D* yRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y acceleration (m/s2)",-2,2);
     111  DataPlot1D* yRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y acceleration (m/s2)",-2,2);
    104112  xRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(0));
    105         xRealVelocityPlot->AddCurve(opticalFlowReference->Element(0),DataPlot::Blue,"consigne");
    106         yRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(1));
     113  xRealVelocityPlot->AddCurve(opticalFlowReference->Element(0),DataPlot::Blue,"consigne");
     114  yRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(1));
    107115  yRealVelocityPlot->AddCurve(opticalFlowReference->Element(1),DataPlot::Blue,"consigne");
    108116  xRealAccelerationPlot->AddCurve(opticalFlowRealAcceleration->Element(0));
    109117  yRealAccelerationPlot->AddCurve(opticalFlowRealAcceleration->Element(1));
    110118
    111         customReferenceOrientation= new AhrsData(this,"reference");
    112         uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
    113         AddDataToControlLawLog(customReferenceOrientation);
     119  customReferenceOrientation= new AhrsData(this,"reference");
     120  uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
     121  AddDataToControlLawLog(customReferenceOrientation);
    114122}
    115123
Note: See TracChangeset for help on using the changeset viewer.