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

//  created:    2011/05/01

//  filename:   DemoOpticalFlow.cpp

//

//  author:     Guillaume Sanahuja

//              Copyright Heudiasyc UMR UTC/CNRS 7253

//

//  version:    $Id: $

//

//  purpose:    demo optical flow

//

//

/*********************************************************************/



#include "DemoOpticalFlow.h"

#include <Uav.h>

#include <Camera.h>

#include <CvtColor.h>

#include <OpticalFlow.h>

#include <OpticalFlowCompensated.h>

#include <OpticalFlowSpeed.h>

#include <LowPassFilter.h>

#include <EulerDerivative.h>

#include <cvmatrix.h>

#include <GridLayout.h>

#include <DataPlot1D.h>

#include <Tab.h>

#include <TabWidget.h>

#include <GroupBox.h>

#include <DoubleSpinBox.h>

#include <PushButton.h>

#include <FrameworkManager.h>

#include <MetaDualShock3.h>

#include <Vector2D.h>

#include <AhrsData.h>

#include <Ahrs.h>

#include <Pid.h>



#include <stdio.h>



using namespace std;

using namespace flair::core;

using namespace flair::gui;

using namespace flair::filter;

using namespace flair::meta;

using namespace flair::sensor;



DemoOpticalFlow::DemoOpticalFlow(TargetController *controller): UavStateMachine(controller) {

  Uav* uav=GetUav();

        if (uav->GetVerticalCamera() == NULL) {

                                Err("no vertical camera found\n");

        exit(1);

    }

        

        startOpticalflow=new PushButton(GetButtonsLayout()->NewRow(),"start optical flow");



        greyCameraImage=new CvtColor(uav->GetVerticalCamera(),"gray",CvtColor::Conversion_t::ToGray);



        uav->GetVerticalCamera()->UseDefaultPlot(greyCameraImage->Output()); // Le defaultPlot de la caméra peut afficher n'importe quoi?



        //optical flow stack

  //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives

  opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");

  opticalFlowCompensated=new OpticalFlowCompensated(opticalFlow,uav->GetAhrs(),"flux optique compensé");

  opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlowCompensated,"vitesse du flux optique");

  //opticalFlowSpeed=vitesse de déplacement en pixels par seconde (moyenne sur tous les points et division par le delta T)

  cvmatrix* twoByOneOFS=new cvmatrix((const Thread*)this,2,1,floatType);

  cvmatrix* twoByOneOFAR=new cvmatrix((const Thread*)this,2,1,floatType);

  cvmatrix* twoByOneOFA=new cvmatrix((const Thread*)this,2,1,floatType);

  opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOneOFS);

  opticalFlowAccelerationRaw=new EulerDerivative(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"derivative",twoByOneOFAR);

  opticalFlowAcceleration=new LowPassFilter(opticalFlowAccelerationRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Acceleration lowPass",twoByOneOFA);

  

  getFrameworkManager()->AddDeviceToLog(opticalFlowSpeedRaw);



  Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");

  DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x speed (px/s)",-250,250);

  DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y speed (px/s)",-250,250);

  DataPlot1D* xFirstPointPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x movement first point",-25,25);

  DataPlot1D* yFirstPointPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y movement first point",-25,25);

//  DataPlot1D* xAccelerationPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_acceleration",-250,250);

//  DataPlot1D* yAccelerationPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_acceleration",-250,250);



  xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0));

  xVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(0,0),DataPlot::Blue);

  yVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(1,0));

  yVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(1,0),DataPlot::Blue);

  xFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(0,0));

  xFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(1,0),DataPlot::Blue);

  xFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(2,0),DataPlot::Green);

//  xAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(0,0));

//  xAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(0,0),DataPlot::Blue);

//  yAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(1,0));

//  yAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(1,0),DataPlot::Blue);



  u_x=new Pid(setupLawTab->At(1,0),"u_x");

  u_x->UseDefaultPlot(graphLawTab->NewRow());

  u_y=new Pid(setupLawTab->At(1,1),"u_y");

  u_y->UseDefaultPlot(graphLawTab->LastRowLastCol());



  opticalFlowGroupBox=new GroupBox(GetJoystick()->GetTab()->NewRow(),"consignes fo");

  maxXSpeed=new DoubleSpinBox(opticalFlowGroupBox->NewRow(),"debattement x"," m/s",-5,5,0.1,1);

  maxYSpeed=new DoubleSpinBox(opticalFlowGroupBox->LastRowLastCol(),"debattement y"," m/s",-5,5,0.1,1);

  

  Tab* opticalFlowRealTab=new Tab(getFrameworkManager()->GetTabWidget(),"real speed");

  opticalFlowRealSpeed=new cvmatrix((const Thread*)this,2,1,floatType);

  opticalFlowReference=new cvmatrix((const Thread*)this,2,1,floatType);

  opticalFlowRealAcceleration=new cvmatrix((const Thread*)this,2,1,floatType);

  DataPlot1D* xRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->NewRow(),"x speed (m/s)",-2,2);

  DataPlot1D* yRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y speed (m/s)",-2,2);

  DataPlot1D* xRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->NewRow(),"x acceleration (m/s2)",-2,2);

  DataPlot1D* yRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y acceleration (m/s2)",-2,2);

  xRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(0));

  xRealVelocityPlot->AddCurve(opticalFlowReference->Element(0),DataPlot::Blue,"consigne");

  yRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(1));

  yRealVelocityPlot->AddCurve(opticalFlowReference->Element(1),DataPlot::Blue,"consigne");

  xRealAccelerationPlot->AddCurve(opticalFlowRealAcceleration->Element(0));

  yRealAccelerationPlot->AddCurve(opticalFlowRealAcceleration->Element(1));



  customReferenceOrientation= new AhrsData(this,"reference");

  uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);

  AddDataToControlLawLog(customReferenceOrientation);

}



void DemoOpticalFlow::SignalEvent(Event_t event) {

    switch(event) {

    case Event_t::EnteringControlLoop:

        opticalFlowReference->SetValue(0,0,GetJoystick()->GetAxisValue(1)*maxXSpeed->Value());//joy axis 0 maps to x displacement

        opticalFlowReference->SetValue(1,0,GetJoystick()->GetAxisValue(0)*maxYSpeed->Value());//joy axis 1 maps to y displacement

        float focal=271.76;

        float z,dz;

        AltitudeValues(z, dz);

        float scale=z/focal;

        opticalFlowRealSpeed->SetValue(0,0,opticalFlowSpeed->Output(0,0)*scale);

        opticalFlowRealSpeed->SetValue(1,0,opticalFlowSpeed->Output(1,0)*scale);

        opticalFlowRealAcceleration->SetValue(0,0,opticalFlowAcceleration->Output(0,0)*scale);

        opticalFlowRealAcceleration->SetValue(1,0,opticalFlowAcceleration->Output(1,0)*scale);

        break;

    }

}



void DemoOpticalFlow::StartOpticalFlow(void) {

        if (SetOrientationMode(OrientationMode_t::Custom)) {

                        Thread::Info("(Re)entering optical flow mode\n");

                        u_x->Reset();

                        u_y->Reset();

        } else {

                        Thread::Warn("Could not enter optical flow mode\n");

        }

}



void DemoOpticalFlow::ExtraCheckPushButton(void) {

        if(startOpticalflow->Clicked()) {

            StartOpticalFlow();

        }

}



void DemoOpticalFlow::ExtraCheckJoystick(void) {

    static bool wasOpticalFlowModeButtonPressed=false;

    // controller button R1 enters optical flow mode

    if(GetJoystick()->IsButtonPressed(9)) { // R1

        if (!wasOpticalFlowModeButtonPressed) {

            wasOpticalFlowModeButtonPressed=true;

            StartOpticalFlow();

        }

    } else {

        wasOpticalFlowModeButtonPressed=false;

    }

}



const AhrsData *DemoOpticalFlow::GetReferenceOrientation(void) {

    Euler refAngles=GetDefaultReferenceOrientation()->GetQuaternion().ToEuler();//to keep default yaw reference



    // /!\ 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

    Vector2D error, errorVariation; // in Uav coordinate system



    error.x=opticalFlowRealSpeed->Value(0,0)-opticalFlowReference->Value(0,0);

    error.y=opticalFlowRealSpeed->Value(1,0)-opticalFlowReference->Value(1,0);

    errorVariation.x=opticalFlowRealAcceleration->Value(0,0);

    errorVariation.y=opticalFlowRealAcceleration->Value(1,0);

//Printf("Altitude=%f, Error=(%f,%f)\n",z,error.x,error.y);



    u_x->SetValues(error.x, errorVariation.x);

    u_x->Update(GetTime());

    refAngles.pitch=u_x->Output();



    u_y->SetValues(error.y, errorVariation.y);

    u_y->Update(GetTime());

    refAngles.roll=-u_y->Output();



    customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3D(0,0,0));



    return customReferenceOrientation;

}



DemoOpticalFlow::~DemoOpticalFlow() {

}