source: flair-src/trunk/lib/FlairVisionFilter/src/OpticalFlow.cpp@ 122

Last change on this file since 122 was 122, checked in by Sanahuja Guillaume, 5 years ago

modifs uav vrpn i686

File size: 4.6 KB
Line 
1// created: 2012/04/12
2// filename: OpticalFlow.cpp
3//
4// author: Guillaume Sanahuja
5// Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7// version: $Id: $
8//
9// purpose: calcul flux optique lk
10//
11//
12/*********************************************************************/
13
14#include "OpticalFlow.h"
15#include "OpticalFlowData.h"
16#include <cvimage.h>
17#include <Layout.h>
18#include <GroupBox.h>
19#include <SpinBox.h>
20//#include <algorithm>
21#include <OneAxisRotation.h>
22#include <Vector3D.h>
23#include <typeinfo>
24
25#define LEVEL_PYR 2
26
27using std::string;
28using std::swap;
29using namespace flair::core;
30using namespace flair::gui;
31
32namespace flair
33{
34namespace filter
35{
36
37OpticalFlow::OpticalFlow(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name)
38{
39 Printf("optical flow: voir pour faire du multiple output\n");//pour pts A et B, found et error
40
41 try{
42 cvimage::Type const &imageType=dynamic_cast<cvimage::Type const &>(parent->GetOutputDataType());
43 pyr=cvCreateImage(cvSize(imageType.GetWidth(),imageType.GetHeight()),IPL_DEPTH_8U,1);
44 pyr_old=cvCreateImage(cvSize(imageType.GetWidth(),imageType.GetHeight()),IPL_DEPTH_8U,1);
45
46 } catch(std::bad_cast& bc) {
47 Err("io type mismatch\n");
48 pyr=NULL;
49 pyr_old=NULL;
50 }
51
52 is_init=false;
53
54 GroupBox* reglages_groupbox=new GroupBox(position,name);
55 rotation=new OneAxisRotation(reglages_groupbox->NewRow(),"post rotation");
56 max_features=new SpinBox(reglages_groupbox->NewRow(),"max features:",1,65535,1,1);
57
58 output=new OpticalFlowData(parent,max_features->Value());
59
60 pointsA=(CvPoint *)cvAlloc(max_features->Value()*sizeof(CvPoint));
61 pointsB=(CvPoint *)cvAlloc(max_features->Value()*sizeof(CvPoint));
62
63 found_feature=(char *)cvAlloc(max_features->Value()*sizeof(char));
64 feature_error=(unsigned int *)cvAlloc(max_features->Value()*sizeof(unsigned int));
65}
66
67OpticalFlow::~OpticalFlow(void)
68{
69 cvReleaseImage(&pyr);
70 cvReleaseImage(&pyr_old);
71 cvFree(&pointsA);
72 cvFree(&pointsB);
73 cvFree(&found_feature);
74 cvFree(&feature_error);
75}
76
77void OpticalFlow::UpdateFrom(const io_data *data)
78{
79 IplImage *gimg=((cvimage*)data)->img;
80 IplImage *gimg_old=((cvimage*)data->Prev(1))->img;
81
82 unsigned int count;
83 CvSize window = cvSize(3,3);
84 CvTermCriteria termination_criteria = cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3 );
85 unsigned int i;
86
87 if(max_features->ValueChanged()==true)
88 {
89 cvFree(&pointsA);
90 cvFree(&pointsB);
91 cvFree(&found_feature);
92 cvFree(&feature_error);
93 pointsA=(CvPoint *)cvAlloc(max_features->Value()*sizeof(CvPoint));
94 pointsB=(CvPoint *)cvAlloc(max_features->Value()*sizeof(CvPoint));
95 found_feature=(char *)cvAlloc(max_features->Value()*sizeof(char));
96 feature_error=(unsigned int *)cvAlloc(max_features->Value()*sizeof(unsigned int));
97
98 output->Resize(max_features->Value());
99 }
100/*
101 if(is_init==false)
102 {
103 data->GetMutex();
104 //init image old
105 dspPyrDown(gimg,pyr_old,LEVEL_PYR);
106 data->ReleaseMutex();
107 is_init=true;
108 printf("ajouter mise a 0 des points\n");
109 return;
110 }
111
112 data->GetMutex();
113 data->Prev(1)->GetMutex();
114
115 //select good features
116 count=max_features->Value();
117 dspGoodFeaturesToTrack(gimg_old,pointsA,&count,0.08,5);
118 //pyramide
119 dspPyrDown(gimg,pyr,LEVEL_PYR);
120 //lk
121 dspCalcOpticalFlowPyrLK(gimg_old,gimg,pyr_old,pyr,pointsA,pointsB,count,window,LEVEL_PYR,found_feature,feature_error,termination_criteria,0) ;
122
123 data->Prev(1)->ReleaseMutex();
124 data->ReleaseMutex();
125*/
126 //apply rotation
127 for(i=0;i<count;i++)
128 {
129 Vector3D tmp;
130 tmp.x=pointsA[i].x;
131 tmp.y=pointsA[i].y;
132 tmp.z=0;
133 rotation->ComputeRotation(tmp);
134 pointsA[i].x=tmp.x;
135 pointsA[i].y=tmp.y;
136
137 tmp.x=pointsB[i].x;
138 tmp.y=pointsB[i].y;
139 tmp.z=0;
140 rotation->ComputeRotation(tmp);
141 pointsB[i].x=tmp.x;
142 pointsB[i].y=tmp.y;
143 }
144
145 output->GetMutex();
146 CvPoint2D32f* pointsBf= output->PointsB();
147 for(i=0;i<count;i++)
148 {
149 pointsBf[i].x=pointsA[i].x+((float)pointsB[i].x)/256;
150 pointsBf[i].y=pointsA[i].y+((float)pointsB[i].y)/256;
151 }
152 output->ReleaseMutex();
153
154 output->SetPointsA(pointsA);
155 output->SetFoundFeature(found_feature);
156 output->SetFeatureError(feature_error);
157 output->SetNbFeatures(count);
158
159 //rotation
160 swap(pyr,pyr_old);
161
162 output->SetDataTime(data->DataTime());
163 ProcessUpdate(output);
164}
165
166} // end namespace filter
167} // end namespace flair
Note: See TracBrowser for help on using the repository browser.