source: flair-src/trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp@ 147

Last change on this file since 147 was 147, checked in by Sanahuja Guillaume, 5 years ago

modif euler/lowpass

File size: 6.9 KB
Line 
1// created: 2011/05/01
2// filename: DemoOpticalFlow.cpp
3//
4// author: Guillaume Sanahuja
5// Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7// version: $Id: $
8//
9// purpose: demo optical flow
10//
11//
12/*********************************************************************/
13
14#include "DemoOpticalFlow.h"
15#include <Uav.h>
16#include <Camera.h>
17#include <CvtColor.h>
18#include <OpticalFlow.h>
19#include <OpticalFlowSpeed.h>
20#include <LowPassFilter.h>
21#include <EulerDerivative.h>
22#include <cvmatrix.h>
23#include <GridLayout.h>
24#include <DataPlot1D.h>
25#include <Tab.h>
26#include <TabWidget.h>
27#include <GroupBox.h>
28#include <DoubleSpinBox.h>
29#include <PushButton.h>
30#include <FrameworkManager.h>
31#include <MetaDualShock3.h>
32#include <Vector2D.h>
33#include <AhrsData.h>
34#include <Ahrs.h>
35#include <Pid.h>
36
37#include <stdio.h>
38
39using namespace std;
40using namespace flair::core;
41using namespace flair::gui;
42using namespace flair::filter;
43using namespace flair::meta;
44using namespace flair::sensor;
45
46DemoOpticalFlow::DemoOpticalFlow(TargetController *controller): UavStateMachine(controller) {
47 Uav* uav=GetUav();
48 if (uav->GetVerticalCamera() == NULL) {
49 Err("no vertical camera found\n");
50 exit(1);
51 }
52
53 startOpticalflow=new PushButton(GetButtonsLayout()->NewRow(),"start optical flow");
54
55 greyCameraImage=new CvtColor(uav->GetVerticalCamera(),"gray",CvtColor::Conversion_t::ToGray);
56
57 uav->GetVerticalCamera()->UseDefaultPlot(greyCameraImage->Output()); // Le defaultPlot de la caméra peut afficher n'importe quoi?
58
59 //optical flow stack
60 //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
61 opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
62 opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlow,"vitesse du flux optique");
63 //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;
69
70 getFrameworkManager()->AddDeviceToLog(opticalFlowSpeedRaw);
71
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);
77
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);
86
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());
91
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);
95
96 opticalFlowReference=new cvmatrix((const Thread*)this,2,1,floatType);
97 xVelocityPlot->AddCurve(opticalFlowReference->Element(0,0),DataPlot::Green,"consigne");
98 yVelocityPlot->AddCurve(opticalFlowReference->Element(1,0),DataPlot::Green,"consigne");
99
100 customReferenceOrientation= new AhrsData(this,"reference");
101 uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
102 AddDataToControlLawLog(customReferenceOrientation);
103}
104
105void DemoOpticalFlow::SignalEvent(Event_t event) {
106 switch(event) {
107 case Event_t::EnteringControlLoop:
108 opticalFlowReference->SetValue(0,0,GetJoystick()->GetAxisValue(1)*maxXSpeed->Value());//joy axis 0 maps to x displacement
109 opticalFlowReference->SetValue(1,0,GetJoystick()->GetAxisValue(0)*maxYSpeed->Value());//joy axis 1 maps to y displacement
110 break;
111 }
112}
113
114void DemoOpticalFlow::StartOpticalFlow(void) {
115 if (SetOrientationMode(OrientationMode_t::Custom)) {
116 Thread::Info("(Re)entering optical flow mode\n");
117 u_x->Reset();
118 u_y->Reset();
119 } else {
120 Thread::Warn("Could not enter optical flow mode\n");
121 }
122}
123
124void DemoOpticalFlow::ExtraCheckPushButton(void) {
125 if(startOpticalflow->Clicked()) {
126 StartOpticalFlow();
127 }
128}
129
130void DemoOpticalFlow::ExtraCheckJoystick(void) {
131 static bool wasOpticalFlowModeButtonPressed=false;
132 // controller button R1 enters optical flow mode
133 if(GetJoystick()->IsButtonPressed(9)) { // R1
134 if (!wasOpticalFlowModeButtonPressed) {
135 wasOpticalFlowModeButtonPressed=true;
136 StartOpticalFlow();
137 }
138 } else {
139 wasOpticalFlowModeButtonPressed=false;
140 }
141}
142
143const AhrsData *DemoOpticalFlow::GetReferenceOrientation(void) {
144 Euler refAngles=GetDefaultReferenceOrientation()->GetQuaternion().ToEuler();//to keep default yaw reference
145
146 // /!\ 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
147 Vector2D error, errorVariation; // in Uav coordinate system
148 float focal=271.76;
149 float z,dz;
150 AltitudeValues(z, dz);
151 float scale=z/focal;
152 error.x=opticalFlowSpeed->Output(0,0)*scale-opticalFlowReference->Value(0,0);
153 error.y=opticalFlowSpeed->Output(1,0)*scale-opticalFlowReference->Value(1,0);
154 errorVariation.x=opticalFlowAcceleration->Output(0,0)*scale;
155 errorVariation.y=opticalFlowAcceleration->Output(1,0)*scale;
156//Printf("Altitude=%f, Error=(%f,%f)\n",z,error.x,error.y);
157
158 u_x->SetValues(error.x, errorVariation.x);
159 u_x->Update(GetTime());
160 refAngles.pitch=u_x->Output();
161
162 u_y->SetValues(error.y, errorVariation.y);
163 u_y->Update(GetTime());
164 refAngles.roll=-u_y->Output();
165
166 customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3D(0,0,0));
167
168 return customReferenceOrientation;
169}
170
171DemoOpticalFlow::~DemoOpticalFlow() {
172}
Note: See TracBrowser for help on using the repository browser.