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

Timestamp:

Feb 21, 2017, 4:37:58 PM (8 years ago)

Author:

Bayard Gildas

Message:

Plop

File:

• trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp (modified) (3 diffs)

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

@@ -58 +58 @@
 
         //optical flow stack
+        //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");
+        //opticalFlowSpeed=vitesse de déplacement en pixel par seconde (moyenne sur tous les points et division par le delta T)
         cvmatrix* twoByOneSpeed=new cvmatrix((const Thread*)this,2,1,floatType);
         opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOneSpeed);
@@ -70 +72 @@
 
         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);
+        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);
 
         xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0));
@@ -143 +145 @@
     Euler refAngles=GetDefaultReferenceOrientation()->GetQuaternion().ToEuler();//to keep default yaw reference
 
-    // /!\ in this demo, the target value is a speed (in pixel/s). As a consequence the error is the difference between the current speed and the target speed
+    // /!\ in this demo, the target value is a speed (in m/s). As a consequence the error is the difference between the current speed and the target speed
     Vector2D error, errorVariation; // in Uav coordinate system
-
-    //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=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);
+    float focal=271.76;
+    float z,dz;
+    AltitudeValues(z, dz);
+    float scale=z/focal;
+    error.x=opticalFlowSpeed->Output(0,0)*scale-opticalFlowReference->Value(0,0);
+    error.y=opticalFlowSpeed->Output(1,0)*scale-opticalFlowReference->Value(1,0);
+    errorVariation.x=opticalFlowAcceleration->Output(0,0)*scale;
+    errorVariation.y=opticalFlowAcceleration->Output(1,0)*scale;
+//Printf("Altitude=%f, Error=(%f,%f)\n",z,error.x,error.y);
 
     u_x->SetValues(error.x, errorVariation.x);