[122] | 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>
|
---|
[142] | 21 | #include <EulerDerivative.h>
|
---|
[122] | 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>
|
---|
[124] | 29 | #include <PushButton.h>
|
---|
[122] | 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 |
|
---|
| 39 | using namespace std;
|
---|
| 40 | using namespace flair::core;
|
---|
| 41 | using namespace flair::gui;
|
---|
| 42 | using namespace flair::filter;
|
---|
| 43 | using namespace flair::meta;
|
---|
| 44 | using namespace flair::sensor;
|
---|
| 45 |
|
---|
| 46 | DemoOpticalFlow::DemoOpticalFlow(TargetController *controller): UavStateMachine(controller) {
|
---|
| 47 | Uav* uav=GetUav();
|
---|
| 48 | if (uav->GetVerticalCamera() == NULL) {
|
---|
[131] | 49 | Err("no vertical camera found\n");
|
---|
[122] | 50 | exit(1);
|
---|
| 51 | }
|
---|
[124] | 52 |
|
---|
| 53 | startOpticalflow=new PushButton(GetButtonsLayout()->NewRow(),"start optical flow");
|
---|
[122] | 54 |
|
---|
[124] | 55 | greyCameraImage=new CvtColor(uav->GetVerticalCamera(),"gray",CvtColor::Conversion_t::ToGray);
|
---|
[122] | 56 |
|
---|
[124] | 57 | uav->GetVerticalCamera()->UseDefaultPlot(greyCameraImage->Output()); // Le defaultPlot de la caméra peut afficher n'importe quoi?
|
---|
[122] | 58 |
|
---|
[124] | 59 | //optical flow stack
|
---|
[143] | 60 | //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
|
---|
[142] | 61 | opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
|
---|
| 62 | opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlow,"vitesse du flux optique");
|
---|
[143] | 63 | //opticalFlowSpeed=vitesse de déplacement en pixel par seconde (moyenne sur tous les points et division par le delta T)
|
---|
[142] | 64 | cvmatrix* twoByOneSpeed=new cvmatrix((const Thread*)this,2,1,floatType);
|
---|
| 65 | opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOneSpeed);
|
---|
| 66 | cvmatrix* twoByOneAccelerationRaw=new cvmatrix((const Thread*)this,2,1,floatType);
|
---|
| 67 | opticalFlowAccelerationRaw=new EulerDerivative(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"derivative",twoByOneAccelerationRaw);
|
---|
| 68 | cvmatrix* twoByOneAcceleration=new cvmatrix((const Thread*)this,2,1,floatType);
|
---|
| 69 | opticalFlowAcceleration=new LowPassFilter(opticalFlowAccelerationRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Acceleration lowPass",twoByOneAcceleration);
|
---|
[122] | 70 |
|
---|
[142] | 71 | getFrameworkManager()->AddDeviceToLog(opticalFlowSpeedRaw);
|
---|
[124] | 72 |
|
---|
| 73 | Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
|
---|
[143] | 74 | DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x speed (px/s)",-250,250);
|
---|
| 75 | DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y speed (px/s)",-250,250);
|
---|
| 76 | DataPlot1D* xAccelerationPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_acceleration",-250,250);
|
---|
| 77 | DataPlot1D* yAccelerationPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_acceleration",-250,250);
|
---|
[122] | 78 |
|
---|
[142] | 79 | xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0));
|
---|
| 80 | xVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(0,0),DataPlot::Blue);
|
---|
| 81 | yVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(1,0));
|
---|
| 82 | yVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(1,0),DataPlot::Blue);
|
---|
| 83 | xAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(0,0));
|
---|
| 84 | xAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(0,0),DataPlot::Blue);
|
---|
| 85 | yAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(1,0));
|
---|
| 86 | yAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(1,0),DataPlot::Blue);
|
---|
[122] | 87 |
|
---|
[124] | 88 | u_x=new Pid(setupLawTab->At(1,0),"u_x");
|
---|
| 89 | u_x->UseDefaultPlot(graphLawTab->NewRow());
|
---|
| 90 | u_y=new Pid(setupLawTab->At(1,1),"u_y");
|
---|
| 91 | u_y->UseDefaultPlot(graphLawTab->LastRowLastCol());
|
---|
[122] | 92 |
|
---|
[124] | 93 | opticalFlowGroupBox=new GroupBox(GetJoystick()->GetTab()->NewRow(),"consignes fo");
|
---|
| 94 | maxXSpeed=new DoubleSpinBox(opticalFlowGroupBox->NewRow(),"debattement x"," m/s",-5,5,0.1,1);
|
---|
| 95 | maxYSpeed=new DoubleSpinBox(opticalFlowGroupBox->LastRowLastCol(),"debattement y"," m/s",-5,5,0.1,1);
|
---|
[122] | 96 |
|
---|
[124] | 97 | opticalFlowReference=new cvmatrix((const Thread*)this,2,1,floatType);
|
---|
| 98 | xVelocityPlot->AddCurve(opticalFlowReference->Element(0,0),DataPlot::Green,"consigne");
|
---|
| 99 | yVelocityPlot->AddCurve(opticalFlowReference->Element(1,0),DataPlot::Green,"consigne");
|
---|
[122] | 100 |
|
---|
[124] | 101 | customReferenceOrientation= new AhrsData(this,"reference");
|
---|
| 102 | uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
|
---|
| 103 | AddDataToControlLawLog(customReferenceOrientation);
|
---|
[122] | 104 | }
|
---|
| 105 |
|
---|
| 106 | void DemoOpticalFlow::SignalEvent(Event_t event) {
|
---|
| 107 | switch(event) {
|
---|
| 108 | case Event_t::EnteringControlLoop:
|
---|
| 109 | opticalFlowReference->SetValue(0,0,GetJoystick()->GetAxisValue(1)*maxXSpeed->Value());//joy axis 0 maps to x displacement
|
---|
| 110 | opticalFlowReference->SetValue(1,0,GetJoystick()->GetAxisValue(0)*maxYSpeed->Value());//joy axis 1 maps to y displacement
|
---|
| 111 | break;
|
---|
| 112 | }
|
---|
| 113 | }
|
---|
| 114 |
|
---|
[124] | 115 | void DemoOpticalFlow::StartOpticalFlow(void) {
|
---|
| 116 | if (SetOrientationMode(OrientationMode_t::Custom)) {
|
---|
| 117 | Thread::Info("(Re)entering optical flow mode\n");
|
---|
| 118 | u_x->Reset();
|
---|
| 119 | u_y->Reset();
|
---|
| 120 | } else {
|
---|
| 121 | Thread::Warn("Could not enter optical flow mode\n");
|
---|
| 122 | }
|
---|
| 123 | }
|
---|
| 124 |
|
---|
| 125 | void DemoOpticalFlow::ExtraCheckPushButton(void) {
|
---|
| 126 | if(startOpticalflow->Clicked()) {
|
---|
[142] | 127 | StartOpticalFlow();
|
---|
[124] | 128 | }
|
---|
| 129 | }
|
---|
| 130 |
|
---|
[122] | 131 | void 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;
|
---|
[124] | 137 | StartOpticalFlow();
|
---|
[122] | 138 | }
|
---|
| 139 | } else {
|
---|
| 140 | wasOpticalFlowModeButtonPressed=false;
|
---|
| 141 | }
|
---|
| 142 | }
|
---|
| 143 |
|
---|
| 144 | const AhrsData *DemoOpticalFlow::GetReferenceOrientation(void) {
|
---|
| 145 | Euler refAngles=GetDefaultReferenceOrientation()->GetQuaternion().ToEuler();//to keep default yaw reference
|
---|
| 146 |
|
---|
[143] | 147 | // /!\ 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
|
---|
[122] | 148 | Vector2D error, errorVariation; // in Uav coordinate system
|
---|
[143] | 149 | float focal=271.76;
|
---|
| 150 | float z,dz;
|
---|
| 151 | AltitudeValues(z, dz);
|
---|
| 152 | float scale=z/focal;
|
---|
| 153 | error.x=opticalFlowSpeed->Output(0,0)*scale-opticalFlowReference->Value(0,0);
|
---|
| 154 | error.y=opticalFlowSpeed->Output(1,0)*scale-opticalFlowReference->Value(1,0);
|
---|
| 155 | errorVariation.x=opticalFlowAcceleration->Output(0,0)*scale;
|
---|
| 156 | errorVariation.y=opticalFlowAcceleration->Output(1,0)*scale;
|
---|
| 157 | //Printf("Altitude=%f, Error=(%f,%f)\n",z,error.x,error.y);
|
---|
[122] | 158 |
|
---|
| 159 | u_x->SetValues(error.x, errorVariation.x);
|
---|
| 160 | u_x->Update(GetTime());
|
---|
| 161 | refAngles.pitch=u_x->Output();
|
---|
| 162 |
|
---|
| 163 | u_y->SetValues(error.y, errorVariation.y);
|
---|
| 164 | u_y->Update(GetTime());
|
---|
| 165 | refAngles.roll=-u_y->Output();
|
---|
| 166 |
|
---|
| 167 | customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3D(0,0,0));
|
---|
| 168 |
|
---|
| 169 | return customReferenceOrientation;
|
---|
| 170 | }
|
---|
| 171 |
|
---|
| 172 | DemoOpticalFlow::~DemoOpticalFlow() {
|
---|
| 173 | }
|
---|