Changeset 143 in flair-src for trunk

Timestamp:

02/21/17 16:37:58 (7 years ago)

Author:

Bayard Gildas

Message:

Plop

Location:

trunk

Files:

• demos/OpticalFlow/simulator/build_x86_64/bin/simulator_x4.sh (modified) (1 diff)
• demos/OpticalFlow/uav/build_x86_64/bin/setup_x4.xml (modified) (7 diffs)
• demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp (modified) (3 diffs)
• lib/FlairVisionFilter/src/OpticalFlowSpeed.cpp (modified) (2 diffs)
• lib/FlairVisionFilter/src/OpticalFlowSpeed.h (modified) (2 diffs)

trunk/demos/OpticalFlow/simulator/build_x86_64/bin/simulator_x4.sh

@@ -2 +2 @@
 
 . $FLAIR_ROOT/flair-dev/scripts/ubuntu_cgroup_hack.sh
+
+export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6:/usr/lib/x86_64-linux-gnu/libdrm_intel.so.1:/usr/lib/x86_64-linux-gnu/libdrm_nouveau.so.2:/usr/lib/x86_64-linux-gnu/libdrm_radeon.so.1
 
 if [ -f /proc/xenomai/version ];then

trunk/demos/OpticalFlow/uav/build_x86_64/bin/setup_x4.xml

@@ -11 +11 @@
     </GroupBox>
     <GroupBox name="consignes joy">
-     <DoubleSpinBox value="-20" name="debattement roll"/>
-     <DoubleSpinBox value="-20" name="debattement pitch"/>
+     <DoubleSpinBox value="-10" name="debattement roll"/>
+     <DoubleSpinBox value="-10" name="debattement pitch"/>
      <DoubleSpinBox value="90" name="debattement wz"/>
      <DoubleSpinBox value="-0.7" name="debattement dz"/>
@@ -21 +21 @@
      <PushButton value="0" name="reset pitch trim"/>
     </GroupBox>
-   <GroupBox name="consignes fo"><DoubleSpinBox name="debattement x" value="-1"/><DoubleSpinBox name="debattement y" value="1"/></GroupBox></Tab>
+   <GroupBox name="consignes fo"><DoubleSpinBox name="debattement x" value="-1.0"/><DoubleSpinBox name="debattement y" value="1.0"/></GroupBox></Tab>
    <Tab name="Mesures">
     <DataPlot1D name="roll">
@@ -153 +153 @@
     </DataPlot1D>
    </Tab>
-  </TabWidget>
+  <Tab name="Quaternion"><DataPlot1D name="q0" period="100" enabled="0"/><DataPlot1D name="q1" period="100" enabled="0"/><DataPlot1D name="q2" period="100" enabled="0"/><DataPlot1D name="q3" period="100" enabled="0"/></Tab><Tab name="IMU"><DataPlot1D name="acc_x" period="100" enabled="0"/><DataPlot1D name="acc_y" period="100" enabled="0"/><DataPlot1D name="acc_z" period="100" enabled="0"/><DataPlot1D name="gyr_x" period="100" enabled="0"/><DataPlot1D name="gyr_y" period="100" enabled="0"/><DataPlot1D name="gyr_z" period="100" enabled="0"/><DataPlot1D name="mag_x" period="100" enabled="0"/><DataPlot1D name="mag_y" period="100" enabled="0"/><DataPlot1D name="mag_z" period="100" enabled="0"/></Tab></TabWidget>
  </Tab>
  <Tab name="battery">
@@ -175 +175 @@
       <DoubleSpinBox value="1" name="cutoff frequency"/>
      </GroupBox>
-    </GridLayout>
+    <GroupBox name="derivative"><DoubleSpinBox name="period, 0 for auto:" value="0.02"/></GroupBox><GroupBox name="Speed lowPass"><DoubleSpinBox name="period, 0 for auto" value="0.02"/><DoubleSpinBox name="cutoff frequency" value="2.00"/></GroupBox><GroupBox name="Acceleration lowPass"><DoubleSpinBox name="period, 0 for auto" value="0.02"/><DoubleSpinBox name="cutoff frequency" value="2.00"/></GroupBox></GridLayout>
    </Tab>
    <Tab name="Picture">
-    <Picture name="simu_cam_v">
+    <Picture name="simu_cam_v" period="200" enabled="0">
      <Setup period="50" enabled="1"/>
     </Picture>
@@ -191 +191 @@
    <PushButton value="1" name="take_off"/>
    <PushButton value="0" name="land"/>
-  </GridLayout>
+  <PushButton name="start optical flow"/></GridLayout>
   <GroupBox name="takeoff/landing">
    <DoubleSpinBox value="1" name="desired takeoff altitude"/>
@@ -239 +239 @@
     <GroupBox name="u_x">
      <DoubleSpinBox value="0" name="period, 0 for auto"/>
-     <DoubleSpinBox value="0.030" name="kp:"/>
-     <DoubleSpinBox value="0.020" name="ki:"/>
-     <DoubleSpinBox value="0.20" name="sat i:"/>
-     <DoubleSpinBox value="0.1" name="kd:"/>
+     <DoubleSpinBox value="0.20" name="kp:"/>
+     <DoubleSpinBox value="0.00" name="ki:"/>
+     <DoubleSpinBox value="1.0" name="sat i:"/>
+     <DoubleSpinBox value="0.005" name="kd:"/>
      <DoubleSpinBox value="1" name="sat:"/>
     </GroupBox>
     <GroupBox name="u_y">
      <DoubleSpinBox value="0" name="period, 0 for auto"/>
-     <DoubleSpinBox value="0.03" name="kp:"/>
-     <DoubleSpinBox value="0.020" name="ki:"/>
-     <DoubleSpinBox value="0.20" name="sat i:"/>
-     <DoubleSpinBox value="0.2" name="kd:"/>
+     <DoubleSpinBox value="0.20" name="kp:"/>
+     <DoubleSpinBox value="0.00" name="ki:"/>
+     <DoubleSpinBox value="1.0" name="sat i:"/>
+     <DoubleSpinBox value="0.005" name="kd:"/>
      <DoubleSpinBox value="1" name="sat:"/>
     </GroupBox>
@@ -283 +283 @@
    <Setup period="100" enabled="1"/>
   </DataPlot1D>
- </Tab>
-  </TabWidget>
-<TabWidget name="App_TabWidget"><Tab name="x4_0"><PushButton name="save config on target" value="1"/></Tab></TabWidget></root>
+ <DataPlot1D name="x_velocity_derivative" period="100" enabled="1"/><DataPlot1D name="y_velocity_derivative" period="100" enabled="1"/><DataPlot1D name="x_acceleration" period="100" enabled="1"/><DataPlot1D name="y_acceleration" period="100" enabled="1"/><DataPlot1D name="x speed (px/s)" period="100" enabled="1"/><DataPlot1D name="y speed (px/s)" period="100" enabled="1"/></Tab>
+  </TabWidget>
+<TabWidget name="App_TabWidget"><Tab name="x4_0"><PushButton name="save config on target" value="1"/></Tab></TabWidget><PushButton name="save config on target (x4_0)" value="1"/></root>

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);

trunk/lib/FlairVisionFilter/src/OpticalFlowSpeed.cpp

@@ -49 +49 @@
 
     input->GetMutex();
-    for(int i=0;i<input->NbFeatures();i++)
-    {
-        if(input->FoundFeature()[i]!=0)
-        {
+    for(int i=0;i<input->NbFeatures();i++) {
+        if(input->FoundFeature()[i]!=0) {
             deplx+=input->PointsB()[i].x-input->PointsA()[i].x;
             deply+=input->PointsB()[i].y-input->PointsA()[i].y;
@@ -59 +57 @@
     }
     input->ReleaseMutex();
+    Time deltaT=data->DataTime()-output->DataTime();
+    output->SetDataTime(data->DataTime());
 
-    if(nb_depl!=0)
-    {
-        output->SetValue(0,0,deplx/nb_depl);
-        output->SetValue(1,0,deply/nb_depl);
+    if(nb_depl!=0) {
+        output->SetValue(0,0,deplx/(nb_depl*deltaT)*1000*1000*1000);
+        output->SetValue(1,0,deply/(nb_depl*deltaT)*1000*1000*1000);
     }
 
-    output->SetDataTime(data->DataTime());
     ProcessUpdate(output);
 }

trunk/lib/FlairVisionFilter/src/OpticalFlowSpeed.h

@@ -31 +31 @@
     * \brief Optical flow speed calculation
     *
-    * Speed is the mean of all optical flow values.
+    * Speed is the mean of all optical flow values divided by the delta time between images.
     */
     class OpticalFlowSpeed : public core::IODevice
@@ -67 +67 @@
             * \brief Output matrix
             *
-            * Matrix is of sze (2,1). \n
+            * Matrix is of size (2,1). \n
             * First line is speed along x axis. \n
             * Second line is speed along y axis. \n