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

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

modifs jpeg

File size: 6.6 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 <cvmatrix.h>
22#include <GridLayout.h>
23#include <DataPlot1D.h>
24#include <Tab.h>
25#include <TabWidget.h>
26#include <GroupBox.h>
27#include <DoubleSpinBox.h>
28#include <PushButton.h>
29#include <FrameworkManager.h>
30#include <MetaDualShock3.h>
31#include <Vector2D.h>
32#include <AhrsData.h>
33#include <Ahrs.h>
34#include <Pid.h>
35
36#include <stdio.h>
37
38using namespace std;
39using namespace flair::core;
40using namespace flair::gui;
41using namespace flair::filter;
42using namespace flair::meta;
43using namespace flair::sensor;
44
45DemoOpticalFlow::DemoOpticalFlow(TargetController *controller): UavStateMachine(controller) {
46 Uav* uav=GetUav();
47 if (uav->GetVerticalCamera() == NULL) {
48 exit(1);
49 }
50uav->GetVerticalCamera()->SetLogFormat(Camera::LogFormat::JPG);
51getFrameworkManager()->AddDeviceToLog(uav->GetVerticalCamera());
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=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
61 opticalFlowSpeed=new OpticalFlowSpeed(opticalFlow,"vitesse du flux optique");
62
63 cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,1);
64 for(int i=0;i<2;i++) {
65 desc->SetElementName(i,0,opticalFlowSpeed->Output()->Name(i,0));
66 }
67 cvmatrix* prev_value=new cvmatrix((const Thread*)this,desc,floatType,uav->ObjectName()); // diamond inheritance
68 for(int i=0;i<2;i++) {
69 prev_value->SetValue(i,0,0);
70 }
71
72 opticalFlowSpeedFiltered=new LowPassFilter(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"passe bas",prev_value);
73// delete prev_value?
74
75 getFrameworkManager()->AddDeviceToLog(opticalFlowSpeed);
76
77 Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
78 DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_velocity",-5,5);
79 DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_velocity",-5,5);
80
81 xVelocityPlot->AddCurve(opticalFlowSpeed->Output()->Element(0,0));
82 xVelocityPlot->AddCurve(opticalFlowSpeedFiltered->Matrix()->Element(0,0),DataPlot::Blue);
83 yVelocityPlot->AddCurve(opticalFlowSpeed->Output()->Element(1,0));
84 yVelocityPlot->AddCurve(opticalFlowSpeedFiltered->Matrix()->Element(1,0),DataPlot::Blue);
85
86 u_x=new Pid(setupLawTab->At(1,0),"u_x");
87 u_x->UseDefaultPlot(graphLawTab->NewRow());
88 u_y=new Pid(setupLawTab->At(1,1),"u_y");
89 u_y->UseDefaultPlot(graphLawTab->LastRowLastCol());
90
91 opticalFlowGroupBox=new GroupBox(GetJoystick()->GetTab()->NewRow(),"consignes fo");
92 maxXSpeed=new DoubleSpinBox(opticalFlowGroupBox->NewRow(),"debattement x"," m/s",-5,5,0.1,1);
93 maxYSpeed=new DoubleSpinBox(opticalFlowGroupBox->LastRowLastCol(),"debattement y"," m/s",-5,5,0.1,1);
94
95 opticalFlowReference=new cvmatrix((const Thread*)this,2,1,floatType);
96 xVelocityPlot->AddCurve(opticalFlowReference->Element(0,0),DataPlot::Green,"consigne");
97 yVelocityPlot->AddCurve(opticalFlowReference->Element(1,0),DataPlot::Green,"consigne");
98
99 customReferenceOrientation= new AhrsData(this,"reference");
100 uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
101 AddDataToControlLawLog(customReferenceOrientation);
102}
103
104void DemoOpticalFlow::SignalEvent(Event_t event) {
105 switch(event) {
106 case Event_t::EnteringControlLoop:
107 opticalFlowReference->SetValue(0,0,GetJoystick()->GetAxisValue(1)*maxXSpeed->Value());//joy axis 0 maps to x displacement
108 opticalFlowReference->SetValue(1,0,GetJoystick()->GetAxisValue(0)*maxYSpeed->Value());//joy axis 1 maps to y displacement
109 break;
110 }
111}
112
113void DemoOpticalFlow::StartOpticalFlow(void) {
114 if (SetOrientationMode(OrientationMode_t::Custom)) {
115 Thread::Info("(Re)entering optical flow mode\n");
116 u_x->Reset();
117 u_y->Reset();
118 } else {
119 Thread::Warn("Could not enter optical flow mode\n");
120 }
121}
122
123void DemoOpticalFlow::ExtraCheckPushButton(void) {
124 if(startOpticalflow->Clicked()) {
125 //StartOpticalFlow();
126 GetUav()->GetVerticalCamera()->SavePictureToFile("./toto.jpg");
127
128 }
129}
130
131void DemoOpticalFlow::ExtraCheckJoystick(void) {
132 static bool wasOpticalFlowModeButtonPressed=false;
133 // controller button R1 enters optical flow mode
134 if(GetJoystick()->IsButtonPressed(9)) { // R1
135 if (!wasOpticalFlowModeButtonPressed) {
136 wasOpticalFlowModeButtonPressed=true;
137 StartOpticalFlow();
138 }
139 } else {
140 wasOpticalFlowModeButtonPressed=false;
141 }
142}
143
144const AhrsData *DemoOpticalFlow::GetReferenceOrientation(void) {
145 Euler refAngles=GetDefaultReferenceOrientation()->GetQuaternion().ToEuler();//to keep default yaw reference
146
147 // /!\ 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
148 Vector2D error, errorVariation; // in Uav coordinate system
149
150 //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
151 //opticalFlowSpeed=vitesse de déplacement en pixel par seconde (moyenne sur tous les points et division par le delta T)
152 error.x=opticalFlowSpeedFiltered->Output(0,0)-opticalFlowReference->Value(0,0);
153 error.y=opticalFlowSpeedFiltered->Output(1,0)-opticalFlowReference->Value(1,0);
154
155 //la dérivée est à la fréquence de la loi de commande ("rapide") alors que le flux optique est à la fréquence de la caméra
156 // fréquemment la dérivée car le signal n'a pas bougé -> dérivée super crade
157 //gsanahuj: brancher un eulerderivative derriere le opticalFlowSpeedFiltered pour avoir la derivee
158 //opticalFlowSpeed doit etre renomme car finalement ce n'est pas une vitesse mais un deplacement
159 errorVariation.x=0;
160 errorVariation.y=0;
161
162 u_x->SetValues(error.x, errorVariation.x);
163 u_x->Update(GetTime());
164 refAngles.pitch=u_x->Output();
165
166 u_y->SetValues(error.y, errorVariation.y);
167 u_y->Update(GetTime());
168 refAngles.roll=-u_y->Output();
169
170 customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3D(0,0,0));
171
172 return customReferenceOrientation;
173}
174
175DemoOpticalFlow::~DemoOpticalFlow() {
176}
Note: See TracBrowser for help on using the repository browser.