Changeset 142 in flair-src for trunk/demos/OpticalFlow/uav

Timestamp:

Feb 20, 2017, 6:28:22 PM (8 years ago)

Author:

Bayard Gildas

Message:

With EulerDerivative acceleration estimation (noisy...)

Location:

trunk/demos/OpticalFlow/uav/src

Files:

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

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

@@ -19 +19 @@
 #include <OpticalFlowSpeed.h>
 #include <LowPassFilter.h>
+#include <EulerDerivative.h>
 #include <cvmatrix.h>
 #include <GridLayout.h>
@@ -49 +50 @@
         exit(1);
     }
-uav->GetVerticalCamera()->SetLogFormat(Camera::LogFormat::JPG);
-getFrameworkManager()->AddDeviceToLog(uav->GetVerticalCamera());
         
         startOpticalflow=new PushButton(GetButtonsLayout()->NewRow(),"start optical flow");
@@ -59 +58 @@
 
         //optical flow stack
-        opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
-        opticalFlowSpeed=new OpticalFlowSpeed(opticalFlow,"vitesse du flux optique");
+        opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
+        opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlow,"vitesse du flux optique");
+        cvmatrix* twoByOneSpeed=new cvmatrix((const Thread*)this,2,1,floatType);
+        opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOneSpeed);
+        cvmatrix* twoByOneAccelerationRaw=new cvmatrix((const Thread*)this,2,1,floatType);
+        opticalFlowAccelerationRaw=new EulerDerivative(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"derivative",twoByOneAccelerationRaw);
+        cvmatrix* twoByOneAcceleration=new cvmatrix((const Thread*)this,2,1,floatType);
+        opticalFlowAcceleration=new LowPassFilter(opticalFlowAccelerationRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Acceleration lowPass",twoByOneAcceleration);
 
-        cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,1);
-        for(int i=0;i<2;i++) {
-                        desc->SetElementName(i,0,opticalFlowSpeed->Output()->Name(i,0));
-        }
-        cvmatrix* prev_value=new cvmatrix((const Thread*)this,desc,floatType,uav->ObjectName()); // diamond inheritance
-        for(int i=0;i<2;i++) {
-                        prev_value->SetValue(i,0,0);
-        }
-
-        opticalFlowSpeedFiltered=new LowPassFilter(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"passe bas",prev_value);
-// delete prev_value?
-
-        getFrameworkManager()->AddDeviceToLog(opticalFlowSpeed);
+        getFrameworkManager()->AddDeviceToLog(opticalFlowSpeedRaw);
 
         Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
         DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_velocity",-5,5);
         DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_velocity",-5,5);
+        DataPlot1D* xAccelerationPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_acceleration",-5,5);
+        DataPlot1D* yAccelerationPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_acceleration",-5,5);
 
-        xVelocityPlot->AddCurve(opticalFlowSpeed->Output()->Element(0,0));
-        xVelocityPlot->AddCurve(opticalFlowSpeedFiltered->Matrix()->Element(0,0),DataPlot::Blue);
-        yVelocityPlot->AddCurve(opticalFlowSpeed->Output()->Element(1,0));
-        yVelocityPlot->AddCurve(opticalFlowSpeedFiltered->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);
+        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");
@@ -124 +123 @@
 void DemoOpticalFlow::ExtraCheckPushButton(void) {
         if(startOpticalflow->Clicked()) {
-        //StartOpticalFlow();
-                                GetUav()->GetVerticalCamera()->SavePictureToFile("./toto.jpg");
-                                
+            StartOpticalFlow();
         }
 }
@@ -151 +148 @@
     //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
     //opticalFlowSpeed=vitesse de déplacement en pixel par seconde (moyenne sur tous les points et division par le delta T)
-    error.x=opticalFlowSpeedFiltered->Output(0,0)-opticalFlowReference->Value(0,0);
-    error.y=opticalFlowSpeedFiltered->Output(1,0)-opticalFlowReference->Value(1,0);
-
-    //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
-    // fréquemment la dérivée car le signal n'a pas bougé -> dérivée super crade
-    //gsanahuj: brancher un eulerderivative derriere le opticalFlowSpeedFiltered pour avoir la derivee
-    //opticalFlowSpeed doit etre renomme car finalement ce n'est pas une vitesse mais un deplacement
-    errorVariation.x=0;
-    errorVariation.y=0;
+    error.x=opticalFlowSpeed->Output(0,0)-opticalFlowReference->Value(0,0);
+    error.y=opticalFlowSpeed->Output(1,0)-opticalFlowReference->Value(1,0);
+    errorVariation.x=opticalFlowAcceleration->Output(0,0);
+    errorVariation.y=opticalFlowAcceleration->Output(1,0);
 
     u_x->SetValues(error.x, errorVariation.x);

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

@@ -31 +31 @@
         class OpticalFlowSpeed;
         class LowPassFilter;
+        class EulerDerivative;
         class CvtColor;
     }
@@ -54 +55 @@
         flair::core::cvmatrix *opticalFlowReference;
         flair::filter::Pid *u_x, *u_y;
-        flair::filter::LowPassFilter* opticalFlowSpeedFiltered;
+        flair::filter::LowPassFilter* opticalFlowSpeed;
+        flair::filter::LowPassFilter* opticalFlowAcceleration;
         flair::filter::CvtColor* greyCameraImage;
         flair::core::AhrsData *customReferenceOrientation;
@@ -60 +62 @@
     private:
         flair::filter::OpticalFlow *opticalFlow;
-        flair::filter::OpticalFlowSpeed *opticalFlowSpeed;
+        flair::filter::OpticalFlowSpeed *opticalFlowSpeedRaw;
+        flair::filter::EulerDerivative *opticalFlowAccelerationRaw;
                                 flair::gui::PushButton *startOpticalflow,*stopOpticalflow;
                                 void StartOpticalFlow(void);