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 |
|
---|
41 | using namespace std;
|
---|
42 | using namespace flair::core;
|
---|
43 | using namespace flair::gui;
|
---|
44 | using namespace flair::filter;
|
---|
45 | using namespace flair::meta;
|
---|
46 | using namespace flair::sensor;
|
---|
47 |
|
---|
48 | DemoOpticalFlow::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 |
|
---|
116 | void 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 |
|
---|
134 | void 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 |
|
---|
144 | void DemoOpticalFlow::ExtraCheckPushButton(void) {
|
---|
145 | if(startOpticalflow->Clicked()) {
|
---|
146 | StartOpticalFlow();
|
---|
147 | }
|
---|
148 | }
|
---|
149 |
|
---|
150 | void 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 |
|
---|
163 | const 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 |
|
---|
188 | void 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 |
|
---|
199 | DemoOpticalFlow::~DemoOpticalFlow() {
|
---|
200 | }
|
---|