source: flair-src/trunk/lib/FlairVisionFilter/src/OpticalFlowCompensated.cpp@ 419

Last change on this file since 419 was 338, checked in by Sanahuja Guillaume, 3 years ago

remove opencv dep

File size: 4.8 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5// created: 2014/01/14
6// filename: OpticalFlowCompensated.cpp
7//
8// author: Gildas Bayard
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: Compensate optical flow data for rotations
14//
15//
16/*********************************************************************/
17#include "OpticalFlowCompensated.h"
18#include <io_data.h>
19#include <Ahrs.h>
20#include <AhrsData.h>
21#include <OpticalFlow.h>
22#include <OpticalFlowData.h>
23#include <Euler.h>
24#include <Matrix.h>
25#include <Layout.h>
26#include <GroupBox.h>
27#include <SpinBox.h>
28#include <DoubleSpinBox.h>
29
30using std::string;
31using namespace flair::core;
32using namespace flair::gui;
33
34class OpticalFlowCompensated_impl {
35public:
36 OpticalFlowCompensated_impl(flair::filter::OpticalFlowCompensated *self,const flair::filter::Ahrs *ahrs, const LayoutPosition* position, string name): ahrs(ahrs), output(NULL) {
37 this->self=self;
38 previousStepsAngularRates=new Vector3Df*[10];
39 for (int i=0; i<10; i++) previousStepsAngularRates[i]=NULL;
40 previousStepsAngularRatesIndex=0;
41
42 GroupBox* reglages_groupbox=new GroupBox(position,name);
43 //TODO: the gyroDelay is set to compensate for the time difference between image snapshot et gyro measurements
44 //it is equal to the time between 2 images (because optical flow always lags 1 frame) + the time to compute optical flow
45 //here it is approximated by 2 video frames
46 gyroDelay=new SpinBox(reglages_groupbox->NewRow(),"gyro delay (in video frames):",0,10,1,2);
47 gyroGain=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"gyro gain:",0.,500.,10.,2,300.);
48
49
50 }
51
52 ~OpticalFlowCompensated_impl() {
53 delete output;
54 }
55
56 void UpdateFrom(const io_data *data){
57 flair::filter::OpticalFlowData *input=(flair::filter::OpticalFlowData *)data;
58 if (!output) { //first pass
59 output=new flair::filter::OpticalFlowData(self,input->MaxFeatures(),input->ObjectName()+"_filtered");
60 previousTime=input->DataTime();
61 return;
62 }
63 // float kX=320/Euler::ToRadian(70); //TEST: only for default simuCameraGL. fov=70° and image width=320 => k=320*180/(70*pi)
64 // float kY=240/Euler::ToRadian(70); //TEST: only for default simuCameraGL. fov=70° and image height=240
65 float kX=gyroGain->Value();
66 float kY=gyroGain->Value();
67 float deltaT=(input->DataTime()-previousTime)/(1000*1000*1000.);
68 previousTime=input->DataTime();
69
70 int delayedIndex=previousStepsAngularRatesIndex-gyroDelay->Value(); // Ahhh décalage, esprit canal...
71 if (delayedIndex<0) delayedIndex+=10;
72
73 if (!previousStepsAngularRates[previousStepsAngularRatesIndex]) {
74 previousStepsAngularRates[previousStepsAngularRatesIndex]=new Vector3Df();
75 }
76 *previousStepsAngularRates[previousStepsAngularRatesIndex++]=ahrs->GetDatas()->GetAngularRates();
77
78 if (!previousStepsAngularRates[delayedIndex]) return;
79 float rotationFlowX=previousStepsAngularRates[delayedIndex]->y*deltaT*kY;
80 float rotationFlowY=-previousStepsAngularRates[delayedIndex]->x*deltaT*kX;
81 if (previousStepsAngularRatesIndex==10) previousStepsAngularRatesIndex=0;
82 input->GetMutex();
83 output->GetMutex();
84
85 for (int i=0; i<input->NbFeatures(); i++) {
86 output->PointsA()[i].x=input->PointsA()[i].x;
87 output->PointsA()[i].y=input->PointsA()[i].y;
88 output->PointsB()[i].x=input->PointsB()[i].x-rotationFlowX;
89 output->PointsB()[i].y=input->PointsB()[i].y-rotationFlowY;
90 }
91 output->SetNbFeatures(input->NbFeatures());
92 output->SetFoundFeature(input->FoundFeature());
93 output->SetFeatureError(input->FeatureError());
94
95 output->ReleaseMutex();
96 input->ReleaseMutex();
97
98 output->SetDataTime(input->DataTime());
99
100 };
101
102 flair::filter::OpticalFlowData *output;
103
104private:
105 flair::filter::OpticalFlowCompensated *self;
106 Time previousTime;
107 const flair::filter::Ahrs *ahrs;
108 Vector3Df **previousStepsAngularRates;
109 unsigned int previousStepsAngularRatesIndex;
110 SpinBox *gyroDelay;
111 DoubleSpinBox *gyroGain;
112};
113
114
115namespace flair {
116namespace filter {
117
118OpticalFlowCompensated::OpticalFlowCompensated(const OpticalFlow *parent, const Ahrs *ahrs, const LayoutPosition* position, string name) : IODevice(parent, name) {
119 pimpl_=new OpticalFlowCompensated_impl(this,ahrs,position,name);
120
121}
122
123OpticalFlowCompensated::~OpticalFlowCompensated() {
124 delete pimpl_;
125}
126
127void OpticalFlowCompensated::UpdateFrom(const io_data *data) {
128 pimpl_->UpdateFrom(data);
129 ProcessUpdate(pimpl_->output);
130}
131
132
133} // end namespace filter
134} // end namespace flair
Note: See TracBrowser for help on using the repository browser.