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

Last change on this file since 142 was 142, checked in by Bayard Gildas, 7 years ago

With EulerDerivative acceleration estimation (noisy...)
File size: 7.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 <OpticalFlowSpeed.h>
20#include <LowPassFilter.h>
21#include <EulerDerivative.h>
22#include <cvmatrix.h>
23#include <GridLayout.h>
24#include <DataPlot1D.h>
25#include <Tab.h>
26#include <TabWidget.h>
27#include <GroupBox.h>
28#include <DoubleSpinBox.h>
29#include <PushButton.h>
30#include <FrameworkManager.h>
31#include <MetaDualShock3.h>
32#include <Vector2D.h>
33#include <AhrsData.h>
34#include <Ahrs.h>
35#include <Pid.h>
36
37#include <stdio.h>
38
39using namespace std;
40using namespace flair::core;
41using namespace flair::gui;
42using namespace flair::filter;
43using namespace flair::meta;
44using namespace flair::sensor;
45
46DemoOpticalFlow::DemoOpticalFlow(TargetController *controller): UavStateMachine(controller) {
47 Uav* uav=GetUav();
48 if (uav->GetVerticalCamera() == NULL) {
49 Err("no vertical camera found\n");
50 exit(1);
51 }
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 opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlow,"vitesse du flux optique");
62 cvmatrix* twoByOneSpeed=new cvmatrix((const Thread*)this,2,1,floatType);
63 opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOneSpeed);
64 cvmatrix* twoByOneAccelerationRaw=new cvmatrix((const Thread*)this,2,1,floatType);
65 opticalFlowAccelerationRaw=new EulerDerivative(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"derivative",twoByOneAccelerationRaw);
66 cvmatrix* twoByOneAcceleration=new cvmatrix((const Thread*)this,2,1,floatType);
67 opticalFlowAcceleration=new LowPassFilter(opticalFlowAccelerationRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Acceleration lowPass",twoByOneAcceleration);
68
69 getFrameworkManager()->AddDeviceToLog(opticalFlowSpeedRaw);
70
71 Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
72 DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_velocity",-5,5);
73 DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_velocity",-5,5);
74 DataPlot1D* xAccelerationPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_acceleration",-5,5);
75 DataPlot1D* yAccelerationPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_acceleration",-5,5);
76
77 xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0));
78 xVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(0,0),DataPlot::Blue);
79 yVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(1,0));
80 yVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(1,0),DataPlot::Blue);
81 xAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(0,0));
82 xAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(0,0),DataPlot::Blue);
83 yAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(1,0));
84 yAccelerationPlot->AddCurve(opticalFlowAcceleration->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 }
127}
128
129void DemoOpticalFlow::ExtraCheckJoystick(void) {
130 static bool wasOpticalFlowModeButtonPressed=false;
131 // controller button R1 enters optical flow mode
132 if(GetJoystick()->IsButtonPressed(9)) { // R1
133 if (!wasOpticalFlowModeButtonPressed) {
134 wasOpticalFlowModeButtonPressed=true;
135 StartOpticalFlow();
136 }
137 } else {
138 wasOpticalFlowModeButtonPressed=false;
139 }
140}
141
142const AhrsData *DemoOpticalFlow::GetReferenceOrientation(void) {
143 Euler refAngles=GetDefaultReferenceOrientation()->GetQuaternion().ToEuler();//to keep default yaw reference
144
145 // /!\ 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
146 Vector2D error, errorVariation; // in Uav coordinate system
147
148 //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
149 //opticalFlowSpeed=vitesse de déplacement en pixel par seconde (moyenne sur tous les points et division par le delta T)
150 error.x=opticalFlowSpeed->Output(0,0)-opticalFlowReference->Value(0,0);
151 error.y=opticalFlowSpeed->Output(1,0)-opticalFlowReference->Value(1,0);
152 errorVariation.x=opticalFlowAcceleration->Output(0,0);
153 errorVariation.y=opticalFlowAcceleration->Output(1,0);
154
155 u_x->SetValues(error.x, errorVariation.x);
156 u_x->Update(GetTime());
157 refAngles.pitch=u_x->Output();
158
159 u_y->SetValues(error.y, errorVariation.y);
160 u_y->Update(GetTime());
161 refAngles.roll=-u_y->Output();
162
163 customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3D(0,0,0));
164
165 return customReferenceOrientation;
166}
167
168DemoOpticalFlow::~DemoOpticalFlow() {
169}
Note: See TracBrowser for help on using the repository browser.