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

Last change on this file since 274 was 274, checked in by Sanahuja Guillaume, 4 years ago

opticalflow modif

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
34namespace flair {
35namespace filter {
36
37OpticalFlowCompensated::OpticalFlowCompensated(const OpticalFlow *parent, const Ahrs *ahrs, const LayoutPosition* position, string name) : IODevice(parent, name), ahrs(ahrs), output(NULL) {
38 cvmatrix_descriptor* desc=new cvmatrix_descriptor(3,2);
39 desc->SetElementName(0,0,"raw displacement X");
40 desc->SetElementName(0,1,"raw displacement Y");
41 desc->SetElementName(1,0,"compensation X");
42 desc->SetElementName(1,1,"compensation Y");
43 desc->SetElementName(2,0,"displacement with compensation X");
44 desc->SetElementName(2,0,"displacement with compensation Y");
45 firstPointDisplacement=new Matrix(this,desc,floatType,name);
46 delete desc;
47 previousStepsAngularRates=new Vector3Df*[10];
48 for (int i=0; i<10; i++) previousStepsAngularRates[i]=NULL;
49 previousStepsAngularRatesIndex=0;
50
51 GroupBox* reglages_groupbox=new GroupBox(position,name);
52 //TODO: the gyroDelay is set to compensate for the time difference between image snapshot et gyro measurements
53//it is equal to the time between 2 images (because optical flow always lags 1 frame) + the time to compute optical flow
54//here it is approximated by 2 video frames
55 gyroDelay=new SpinBox(reglages_groupbox->NewRow(),"gyro delay (in video frames):",0,10,1,2);
56 gyroGain=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"gyro gain:",0.,500.,10.,2,300.);
57
58}
59
60OpticalFlowCompensated::~OpticalFlowCompensated() {
61 delete output;
62}
63
64void OpticalFlowCompensated::UpdateFrom(const io_data *data) {
65 OpticalFlowData *input=(OpticalFlowData *)data;
66 if (!output) { //first pass
67 output=new OpticalFlowData(this,input->MaxFeatures(),input->ObjectName()+"_filtered");
68 previousTime=input->DataTime();
69 return;
70 }
71// float kX=320/Euler::ToRadian(70); //TEST: only for default simuCameraGL. fov=70° and image width=320 => k=320*180/(70*pi)
72// float kY=240/Euler::ToRadian(70); //TEST: only for default simuCameraGL. fov=70° and image height=240
73 float kX=gyroGain->Value();
74 float kY=gyroGain->Value();
75 float deltaT=(input->DataTime()-previousTime)/(1000*1000*1000.);
76 previousTime=input->DataTime();
77
78 int delayedIndex=previousStepsAngularRatesIndex-gyroDelay->Value(); // Ahhh décalage, esprit canal...
79 if (delayedIndex<0) delayedIndex+=10;
80
81 if (!previousStepsAngularRates[previousStepsAngularRatesIndex]) {
82 previousStepsAngularRates[previousStepsAngularRatesIndex]=new Vector3Df();
83 }
84 *previousStepsAngularRates[previousStepsAngularRatesIndex++]=ahrs->GetDatas()->GetAngularRates();
85
86 if (!previousStepsAngularRates[delayedIndex]) return;
87 float rotationFlowX=previousStepsAngularRates[delayedIndex]->y*deltaT*kY;
88 float rotationFlowY=-previousStepsAngularRates[delayedIndex]->x*deltaT*kX;
89 if (previousStepsAngularRatesIndex==10) previousStepsAngularRatesIndex=0;
90 input->GetMutex();
91 output->GetMutex();
92
93 for (int i=0; i<input->NbFeatures(); i++) {
94 if (!i) {
95 firstPointDisplacement->SetValue(0,0,input->PointsB()[i].x-input->PointsA()[i].x);
96 firstPointDisplacement->SetValue(0,1,input->PointsB()[i].y-input->PointsA()[i].y);
97 firstPointDisplacement->SetValue(1,0,-rotationFlowX);
98 firstPointDisplacement->SetValue(1,1,-rotationFlowY);
99 firstPointDisplacement->SetValue(2,0,input->PointsB()[i].x-input->PointsA()[i].x-rotationFlowX);
100 firstPointDisplacement->SetValue(2,1,input->PointsB()[i].y-input->PointsA()[i].y-rotationFlowY);
101 }
102 output->PointsA()[i].x=input->PointsA()[i].x;
103 output->PointsA()[i].y=input->PointsA()[i].y;
104 output->PointsB()[i].x=input->PointsB()[i].x-rotationFlowX;
105 output->PointsB()[i].y=input->PointsB()[i].y-rotationFlowY;
106 }
107 output->SetNbFeatures(input->NbFeatures());
108 output->SetFoundFeature(input->FoundFeature());
109 output->SetFeatureError(input->FeatureError());
110
111 output->ReleaseMutex();
112 input->ReleaseMutex();
113
114 output->SetDataTime(input->DataTime());
115 ProcessUpdate(output);
116}
117
118 Matrix *OpticalFlowCompensated::GetFirstPointDisplacement() const {
119 return firstPointDisplacement;
120 }
121
122} // end namespace filter
123} // end namespace flair
Note: See TracBrowser for help on using the repository browser.