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

Last change on this file was 338, checked in by Sanahuja Guillaume, 3 years ago

remove opencv dep

File size: 8.4 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 <V4LCamera.h>
18#include <CvtColor.h>
19#include <OpticalFlow.h>
20#include <OpticalFlowCompensated.h>
21#include <OpticalFlowSpeed.h>
22#include <LowPassFilter.h>
23#include <EulerDerivative.h>
24#include <Matrix.h>
25#include <GridLayout.h>
26#include <DataPlot1D.h>
27#include <Tab.h>
28#include <TabWidget.h>
29#include <GroupBox.h>
30#include <DoubleSpinBox.h>
31#include <PushButton.h>
32#include <FrameworkManager.h>
33#include <MetaDualShock3.h>
34#include <Vector2D.h>
35#include <AhrsData.h>
36#include <Ahrs.h>
37#include <Pid.h>
38
39#include <stdio.h>
40
41using namespace std;
42using namespace flair::core;
43using namespace flair::gui;
44using namespace flair::filter;
45using namespace flair::meta;
46using namespace flair::sensor;
47
48DemoOpticalFlow::DemoOpticalFlow(TargetController *controller): UavStateMachine(controller) {
49 Uav* uav=GetUav();
50 if (uav->GetVerticalCamera() == NULL) {
51 Err("no vertical camera found\n");
52 exit(1);
53 }
54
55 startOpticalflow=new PushButton(GetButtonsLayout()->NewRow(),"start optical flow");
56
57 greyCameraImage=new CvtColor(uav->GetVerticalCamera(),"gray",CvtColor::Conversion_t::ToGray);
58
59 uav->GetVerticalCamera()->UseDefaultPlot(greyCameraImage->Output());
60
61 //optical flow stack
62 //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
63 opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
64 opticalFlowCompensated=new OpticalFlowCompensated(opticalFlow,uav->GetAhrs(),uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique compense");
65 opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlowCompensated,uav->GetVerticalCamera()->GetLayout()->NewRow(),"vitesse du Flux Optique");
66 //opticalFlowSpeed=vitesse de déplacement en pixels par seconde (moyenne sur tous les points et division par le delta T)
67 Matrix* twoByOneOFS=new Matrix((const Thread*)this,2,1,floatType);
68 Matrix* twoByOneOFAR=new Matrix((const Thread*)this,2,1,floatType);
69 Matrix* twoByOneOFA=new Matrix((const Thread*)this,2,1,floatType);
70 opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOneOFS);
71 opticalFlowAccelerationRaw=new EulerDerivative(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"derivative",twoByOneOFAR);
72 opticalFlowAcceleration=new LowPassFilter(opticalFlowAccelerationRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Acceleration lowPass",twoByOneOFA);
73
74 getFrameworkManager()->AddDeviceToLog(opticalFlowSpeedRaw);
75
76 Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
77 DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x speed (px/s)",-250,250);
78 DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y speed (px/s)",-250,250);
79
80 xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0));
81 xVelocityPlot->AddCurve(opticalFlowSpeed->GetMatrix()->Element(0,0),DataPlot::Blue);
82 yVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(1,0));
83 yVelocityPlot->AddCurve(opticalFlowSpeed->GetMatrix()->Element(1,0),DataPlot::Blue);
84
85 u_x=new Pid(setupLawTab->At(1,0),"u_x");
86 u_x->UseDefaultPlot(graphLawTab->NewRow());
87 u_y=new Pid(setupLawTab->At(1,1),"u_y");
88 u_y->UseDefaultPlot(graphLawTab->LastRowLastCol());
89
90 opticalFlowGroupBox=new GroupBox(GetTargetController()->GetTab()->NewRow(),"consignes fo");
91 maxXSpeed=new DoubleSpinBox(opticalFlowGroupBox->NewRow(),"debattement x"," m/s",-5,5,0.1,1);
92 maxYSpeed=new DoubleSpinBox(opticalFlowGroupBox->LastRowLastCol(),"debattement y"," m/s",-5,5,0.1,1);
93
94 Tab* opticalFlowRealTab=new Tab(getFrameworkManager()->GetTabWidget(),"real speed");
95 opticalFlowRealSpeed=new Matrix((const Thread*)this,2,1,floatType);
96 opticalFlowReference=new Matrix((const Thread*)this,2,1,floatType);
97 opticalFlowRealAcceleration=new Matrix((const Thread*)this,2,1,floatType);
98 DataPlot1D* xRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->NewRow(),"x speed (m/s)",-2,2);
99 DataPlot1D* yRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y speed (m/s)",-2,2);
100 DataPlot1D* xRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->NewRow(),"x acceleration (m/s2)",-2,2);
101 DataPlot1D* yRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y acceleration (m/s2)",-2,2);
102 xRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(0));
103 xRealVelocityPlot->AddCurve(opticalFlowReference->Element(0),DataPlot::Blue,"consigne");
104 yRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(1));
105 yRealVelocityPlot->AddCurve(opticalFlowReference->Element(1),DataPlot::Blue,"consigne");
106 xRealAccelerationPlot->AddCurve(opticalFlowRealAcceleration->Element(0));
107 yRealAccelerationPlot->AddCurve(opticalFlowRealAcceleration->Element(1));
108
109 customReferenceOrientation= new AhrsData(this,"reference");
110 uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
111 AddDataToControlLawLog(customReferenceOrientation);
112
113 flagCameraLost=false;
114}
115
116void DemoOpticalFlow::SignalEvent(Event_t event) {
117 UavStateMachine::SignalEvent(event);
118 switch(event) {
119 case Event_t::EnteringControlLoop:
120 opticalFlowReference->SetValue(0,0,GetTargetController()->GetAxisValue(1)*maxXSpeed->Value());//joy axis 0 maps to x displacement
121 opticalFlowReference->SetValue(1,0,GetTargetController()->GetAxisValue(0)*maxYSpeed->Value());//joy axis 1 maps to y displacement
122 float focal=271.76;
123 float z,dz;
124 AltitudeValues(z, dz);
125 float scale=z/focal;
126 opticalFlowRealSpeed->SetValue(0,0,opticalFlowSpeed->Output(0,0)*scale);
127 opticalFlowRealSpeed->SetValue(1,0,opticalFlowSpeed->Output(1,0)*scale);
128 opticalFlowRealAcceleration->SetValue(0,0,opticalFlowAcceleration->Output(0,0)*scale);
129 opticalFlowRealAcceleration->SetValue(1,0,opticalFlowAcceleration->Output(1,0)*scale);
130 break;
131 }
132}
133
134void DemoOpticalFlow::StartOpticalFlow(void) {
135 if (SetOrientationMode(OrientationMode_t::Custom)) {
136 Thread::Info("(Re)entering optical flow mode\n");
137 u_x->Reset();
138 u_y->Reset();
139 } else {
140 Thread::Warn("Could not enter optical flow mode\n");
141 }
142}
143
144void DemoOpticalFlow::ExtraCheckPushButton(void) {
145 if(startOpticalflow->Clicked()) {
146 StartOpticalFlow();
147 }
148}
149
150void DemoOpticalFlow::ExtraCheckJoystick(void) {
151 static bool wasOpticalFlowModeButtonPressed=false;
152 // controller button R1 enters optical flow mode
153 if(GetTargetController()->IsButtonPressed(9)) { // R1
154 if (!wasOpticalFlowModeButtonPressed) {
155 wasOpticalFlowModeButtonPressed=true;
156 StartOpticalFlow();
157 }
158 } else {
159 wasOpticalFlowModeButtonPressed=false;
160 }
161}
162
163const AhrsData *DemoOpticalFlow::GetReferenceOrientation(void) {
164 Euler refAngles=GetDefaultReferenceOrientation()->GetQuaternion().ToEuler();//to keep default yaw reference
165
166 // /!\ 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
167 Vector2Df error, errorVariation; // in Uav coordinate system
168
169 error.x=opticalFlowRealSpeed->Value(0,0)-opticalFlowReference->Value(0,0);
170 error.y=opticalFlowRealSpeed->Value(1,0)-opticalFlowReference->Value(1,0);
171 errorVariation.x=opticalFlowRealAcceleration->Value(0,0);
172 errorVariation.y=opticalFlowRealAcceleration->Value(1,0);
173//Printf("Altitude=%f, Error=(%f,%f)\n",z,error.x,error.y);
174
175 u_x->SetValues(error.x, errorVariation.x);
176 u_x->Update(GetTime());
177 refAngles.pitch=u_x->Output();
178
179 u_y->SetValues(error.y, errorVariation.y);
180 u_y->Update(GetTime());
181 refAngles.roll=-u_y->Output();
182
183 customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3Df(0,0,0));
184
185 return customReferenceOrientation;
186}
187
188void DemoOpticalFlow::ExtraSecurityCheck(void) {
189 if(GetUav()->GetType()=="hds_x8") {
190 if(((V4LCamera*)GetUav()->GetVerticalCamera())->HasProblems() && !flagCameraLost) {
191 flagCameraLost = true;
192 Thread::Err("Camera lost\n");
193 SafeStop();
194 Land();
195 }
196 }
197}
198
199DemoOpticalFlow::~DemoOpticalFlow() {
200}
Note: See TracBrowser for help on using the repository browser.