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