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

Last change on this file since 155 was 155, checked in by Bayard Gildas, 5 years ago

Compensation rotations

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