Changeset 155 in flair-src for trunk

Timestamp:

Mar 3, 2017, 12:26:05 PM (8 years ago)

Author:

Bayard Gildas

Message:

Compensation rotations

Location:

trunk/demos/OpticalFlow/uav/src

Files:

• DemoOpticalFlow.cpp (modified) (3 diffs)
• DemoOpticalFlow.h (modified) (3 diffs)

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

@@ -17 +17 @@
 #include <CvtColor.h>
 #include <OpticalFlow.h>
+#include <OpticalFlowCompensated.h>
 #include <OpticalFlowSpeed.h>
 #include <LowPassFilter.h>
@@ -60 +61 @@
   //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
   opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
-  opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlow,"vitesse du flux optique");
+  opticalFlowCompensated=new OpticalFlowCompensated(opticalFlow,uav->GetAhrs(),"flux optique compensé");
+  opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlowCompensated,"vitesse du flux optique");
   //opticalFlowSpeed=vitesse de déplacement en pixels par seconde (moyenne sur tous les points et division par le delta T)
-  cvmatrix* twoByOne=new cvmatrix((const Thread*)this,2,1,floatType);
-  opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOne);
-  opticalFlowAccelerationRaw=new EulerDerivative(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"derivative",twoByOne);
-  opticalFlowAcceleration=new LowPassFilter(opticalFlowAccelerationRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Acceleration lowPass",twoByOne);
-  delete twoByOne;
+  cvmatrix* twoByOneOFS=new cvmatrix((const Thread*)this,2,1,floatType);
+  cvmatrix* twoByOneOFAR=new cvmatrix((const Thread*)this,2,1,floatType);
+  cvmatrix* twoByOneOFA=new cvmatrix((const Thread*)this,2,1,floatType);
+  opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOneOFS);
+  opticalFlowAccelerationRaw=new EulerDerivative(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"derivative",twoByOneOFAR);
+  opticalFlowAcceleration=new LowPassFilter(opticalFlowAccelerationRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Acceleration lowPass",twoByOneOFA);
   
   getFrameworkManager()->AddDeviceToLog(opticalFlowSpeedRaw);
 
-        Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
-        DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x speed (px/s)",-250,250);
-        DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y speed (px/s)",-250,250);
-  DataPlot1D* xAccelerationPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_acceleration",-250,250);
-  DataPlot1D* yAccelerationPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_acceleration",-250,250);
+  Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
+  DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x speed (px/s)",-250,250);
+  DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y speed (px/s)",-250,250);
+  DataPlot1D* xFirstPointPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x movement first point",-25,25);
+  DataPlot1D* yFirstPointPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y movement first point",-25,25);
+//  DataPlot1D* xAccelerationPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_acceleration",-250,250);
+//  DataPlot1D* yAccelerationPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_acceleration",-250,250);
 
-        xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0));
-        xVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(0,0),DataPlot::Blue);
-        yVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(1,0));
-        yVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(1,0),DataPlot::Blue);
-  xAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(0,0));
-  xAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(0,0),DataPlot::Blue);
-  yAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(1,0));
-  yAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(1,0),DataPlot::Blue);
+  xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0));
+  xVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(0,0),DataPlot::Blue);
+  yVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(1,0));
+  yVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(1,0),DataPlot::Blue);
+  xFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(0,0));
+  xFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(1,0),DataPlot::Blue);
+  xFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(2,0),DataPlot::Green);
+//  xAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(0,0));
+//  xAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(0,0),DataPlot::Blue);
+//  yAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(1,0));
+//  yAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(1,0),DataPlot::Blue);
 
-        u_x=new Pid(setupLawTab->At(1,0),"u_x");
-        u_x->UseDefaultPlot(graphLawTab->NewRow());
-        u_y=new Pid(setupLawTab->At(1,1),"u_y");
-        u_y->UseDefaultPlot(graphLawTab->LastRowLastCol());
+  u_x=new Pid(setupLawTab->At(1,0),"u_x");
+  u_x->UseDefaultPlot(graphLawTab->NewRow());
+  u_y=new Pid(setupLawTab->At(1,1),"u_y");
+  u_y->UseDefaultPlot(graphLawTab->LastRowLastCol());
 
-        opticalFlowGroupBox=new GroupBox(GetJoystick()->GetTab()->NewRow(),"consignes fo");
-        maxXSpeed=new DoubleSpinBox(opticalFlowGroupBox->NewRow(),"debattement x"," m/s",-5,5,0.1,1);
-        maxYSpeed=new DoubleSpinBox(opticalFlowGroupBox->LastRowLastCol(),"debattement y"," m/s",-5,5,0.1,1);
+  opticalFlowGroupBox=new GroupBox(GetJoystick()->GetTab()->NewRow(),"consignes fo");
+  maxXSpeed=new DoubleSpinBox(opticalFlowGroupBox->NewRow(),"debattement x"," m/s",-5,5,0.1,1);
+  maxYSpeed=new DoubleSpinBox(opticalFlowGroupBox->LastRowLastCol(),"debattement y"," m/s",-5,5,0.1,1);
   
   Tab* opticalFlowRealTab=new Tab(getFrameworkManager()->GetTabWidget(),"real speed");
@@ -99 +107 @@
   opticalFlowRealAcceleration=new cvmatrix((const Thread*)this,2,1,floatType);
   DataPlot1D* xRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->NewRow(),"x speed (m/s)",-2,2);
-        DataPlot1D* yRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y speed (m/s)",-2,2);
+  DataPlot1D* yRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y speed (m/s)",-2,2);
   DataPlot1D* xRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->NewRow(),"x acceleration (m/s2)",-2,2);
-        DataPlot1D* yRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y acceleration (m/s2)",-2,2);
+  DataPlot1D* yRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y acceleration (m/s2)",-2,2);
   xRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(0));
-        xRealVelocityPlot->AddCurve(opticalFlowReference->Element(0),DataPlot::Blue,"consigne");
-        yRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(1));
+  xRealVelocityPlot->AddCurve(opticalFlowReference->Element(0),DataPlot::Blue,"consigne");
+  yRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(1));
   yRealVelocityPlot->AddCurve(opticalFlowReference->Element(1),DataPlot::Blue,"consigne");
   xRealAccelerationPlot->AddCurve(opticalFlowRealAcceleration->Element(0));
   yRealAccelerationPlot->AddCurve(opticalFlowRealAcceleration->Element(1));
 
-        customReferenceOrientation= new AhrsData(this,"reference");
-        uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
-        AddDataToControlLawLog(customReferenceOrientation);
+  customReferenceOrientation= new AhrsData(this,"reference");
+  uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
+  AddDataToControlLawLog(customReferenceOrientation);
 }
 

trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.h

@@ -29 +29 @@
     namespace filter {
         class OpticalFlow;
+        class OpticalFlowCompensated;
         class OpticalFlowSpeed;
         class LowPassFilter;
@@ -48 +49 @@
         void SignalEvent(Event_t event);
         void ExtraCheckJoystick(void);
-                                void ExtraCheckPushButton(void);
+    void ExtraCheckPushButton(void);
         const flair::core::AhrsData *GetReferenceOrientation(void);
 
@@ -62 +63 @@
     private:
         flair::filter::OpticalFlow *opticalFlow;
+        flair::filter::OpticalFlowCompensated *opticalFlowCompensated;
         flair::filter::OpticalFlowSpeed *opticalFlowSpeedRaw;
         flair::filter::EulerDerivative *opticalFlowAccelerationRaw;
-                                flair::gui::PushButton *startOpticalflow,*stopOpticalflow;
-                                void StartOpticalFlow(void);
+    flair::gui::PushButton *startOpticalflow,*stopOpticalflow;
+    void StartOpticalFlow(void);
         flair::core::cvmatrix *opticalFlowRealSpeed,*opticalFlowRealAcceleration;
 };