| 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 | }
|
---|