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

Last change on this file since 318 was 318, checked in by Sanahuja Guillaume, 2 years ago
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  MatrixDescriptor* desc=new MatrixDescriptor(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.