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

Last change on this file was 338, checked in by Sanahuja Guillaume, 2 years ago

remove opencv dep

File size: 7.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 "VisionFilter.h"
17#include <Image.h>
18#include <Layout.h>
19#include <GroupBox.h>
20#include <SpinBox.h>
21#include <OneAxisRotation.h>
22#include <typeinfo>
23
24#define LEVEL_PYR 2
25
26using std::string;
27using std::swap;
28using namespace flair::core;
29using namespace flair::gui;
30
31class OpticalFlow_impl {
32public:
33    OpticalFlow_impl(flair::filter::OpticalFlow *self,const LayoutPosition* position,string name) {
34        this->self=self;
35        is_init=false;
36
37        GroupBox* reglages_groupbox=new GroupBox(position,name);
38        rotation=new OneAxisRotation(reglages_groupbox->NewRow(),"post rotation",OneAxisRotation::PostRotation);
39        max_features=new SpinBox(reglages_groupbox->NewRow(),"max features:",1,65535,1,1);
40       
41        try{
42            Image::Type const &imageType=dynamic_cast<Image::Type const &>(((IODevice*)(self->Parent()))->GetOutputDataType());
43            pyr=new Image(self,imageType.GetWidth(),imageType.GetHeight(),Image::Type::Format::Gray);
44            pyr_old=new Image(self,imageType.GetWidth(),imageType.GetHeight(),Image::Type::Format::Gray);
45            output=new flair::filter::OpticalFlowData(self->Parent(),max_features->Value());
46/*
47            pointsA=(CvPoint*)AllocFunction(max_features->Value()*sizeof(CvPoint));
48            pointsB=(CvPoint*)AllocFunction(max_features->Value()*sizeof(CvPoint));
49
50            found_feature=(char*)AllocFunction(max_features->Value()*sizeof(char));
51            feature_error=(unsigned int*)AllocFunction(max_features->Value()*sizeof(unsigned int));
52           
53            iplgimg = (IplImage*)AllocFunction(sizeof(IplImage));
54            iplgimg_old = (IplImage*)AllocFunction(sizeof(IplImage));
55            iplpyr = (IplImage*)AllocFunction(sizeof(IplImage));
56            iplpyr_old = (IplImage*)AllocFunction(sizeof(IplImage));
57           
58            iplpyr->width=imageType.GetWidth();
59            iplpyr->height=imageType.GetHeight();
60            iplpyr->imageSize=imageType.GetSize();
61            iplpyr_old->width=imageType.GetWidth();
62            iplpyr_old->height=imageType.GetHeight();
63            iplpyr_old->imageSize=imageType.GetSize();
64           
65            iplgimg->roi=NULL;
66            iplgimg_old->roi=NULL;
67            iplpyr->roi=NULL;
68            iplpyr_old->roi=NULL;
69*/ 
70      } catch(std::bad_cast& bc) {
71            self->Err("io type mismatch\n");
72            pyr=NULL;
73            pyr_old=NULL;
74        }
75     
76    }
77
78    ~OpticalFlow_impl() {
79        /*
80        FreeFunction((char*)pointsA);
81        FreeFunction((char*)pointsB);
82        FreeFunction((char*)found_feature);
83        FreeFunction((char*)feature_error);
84        FreeFunction((char*)iplgimg);
85        FreeFunction((char*)iplgimg_old);
86        FreeFunction((char*)iplpyr);
87        FreeFunction((char*)iplpyr_old);*/
88    }
89   
90    void UpdateFrom(const io_data *data){
91        //Time tStart=GetTime();
92        Image *img=(Image*)data;
93        Image *img_old=((Image*)data->Prev(1));
94/*
95        iplgimg->width=img->GetDataType().GetWidth();
96        iplgimg->height=img->GetDataType().GetHeight();
97        iplgimg->imageData=img->buffer;
98        iplgimg->imageSize=img->GetDataType().GetSize();
99       
100        iplgimg_old->width=img_old->GetDataType().GetWidth();
101        iplgimg_old->height=img_old->GetDataType().GetHeight();
102        iplgimg_old->imageData=img_old->buffer;
103        iplgimg_old->imageSize=img_old->GetDataType().GetSize();
104       
105        iplpyr->imageData=pyr->buffer;
106        iplpyr_old->imageData=pyr_old->buffer;
107
108        unsigned int count;
109        CvSize window = {3,3};
110        CvTermCriteria termination_criteria ={CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3 };
111        unsigned int i;
112
113        if(max_features->ValueChanged()==true) {
114            FreeFunction((char*)pointsA);
115            FreeFunction((char*)pointsB);
116            FreeFunction((char*)found_feature);
117            FreeFunction((char*)feature_error);
118            pointsA=(CvPoint*)AllocFunction(max_features->Value()*sizeof(CvPoint));
119            pointsB=(CvPoint*)AllocFunction(max_features->Value()*sizeof(CvPoint));
120            found_feature=(char*)AllocFunction(max_features->Value()*sizeof(char));
121            feature_error=(unsigned int*)AllocFunction(max_features->Value()*sizeof(unsigned int));
122
123            output->Resize(max_features->Value());
124        }
125
126        if(is_init==false) {
127            data->GetMutex();
128            //init image old
129            dspPyrDown(iplgimg,iplpyr_old,LEVEL_PYR);
130            data->ReleaseMutex();
131            is_init=true;
132            printf("ajouter mise a 0 des points\n");
133            return;
134        }
135
136        data->GetMutex();
137        data->Prev(1)->GetMutex();
138
139        //select good features
140        count=max_features->Value();
141        dspGoodFeaturesToTrack(iplgimg_old,pointsA,&count,0.08,5);
142        //pyramide
143        dspPyrDown(iplgimg,iplpyr,LEVEL_PYR);
144        //lk
145        dspCalcOpticalFlowPyrLK(iplgimg_old,iplgimg,iplpyr_old,iplpyr,pointsA,pointsB,count,window,LEVEL_PYR,found_feature,feature_error,termination_criteria,0) ;
146
147        data->Prev(1)->ReleaseMutex();
148        data->ReleaseMutex();
149
150        //apply rotation
151        for(i=0;i<count;i++) {
152            Vector3Df tmp;
153            tmp.x=pointsA[i].x;
154            tmp.y=pointsA[i].y;
155            tmp.z=0;
156            rotation->ComputeRotation(tmp);
157            pointsA[i].x=tmp.x;
158            pointsA[i].y=tmp.y;
159
160            tmp.x=pointsB[i].x;
161            tmp.y=pointsB[i].y;
162            tmp.z=0;
163            rotation->ComputeRotation(tmp);
164            pointsB[i].x=tmp.x;
165            pointsB[i].y=tmp.y;
166        }
167
168        output->GetMutex();
169        CvPoint2D32f* pointsBf= output->PointsB();
170        for(i=0;i<count;i++) {
171            pointsBf[i].x=pointsA[i].x+((float)pointsB[i].x)/256;
172            pointsBf[i].y=pointsA[i].y+((float)pointsB[i].y)/256;
173        }
174        output->ReleaseMutex();
175
176        output->SetPointsA(pointsA);
177        output->SetFoundFeature(found_feature);
178        output->SetFeatureError(feature_error);
179        output->SetNbFeatures(count);
180
181        //rotation
182        swap(pyr,pyr_old);
183*/
184        output->SetDataTime(data->DataTime());
185        //Printf("Optical flow computation time=%f\n",(GetTime()-tStart)/(1000.*1000));
186    };
187   
188    flair::filter::OpticalFlowData *output;
189
190private:
191    flair::filter::OpticalFlow *self;
192    SpinBox *max_features;
193    OneAxisRotation* rotation;
194
195    //CvPoint* pointsA;
196    //CvPoint* pointsB;
197    char *found_feature;
198    unsigned int *feature_error;
199    Image *pyr,*pyr_old;
200    //IplImage *iplgimg,*iplgimg_old;
201    //IplImage *iplpyr,*iplpyr_old;
202
203    bool is_init;
204};
205
206
207namespace flair
208{
209namespace filter
210{
211
212OpticalFlow::OpticalFlow(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name) {
213    Printf("optical flow: voir pour faire du multiple output\n");//pour pts A et B, found et error
214    pimpl_=new OpticalFlow_impl(this,position,name);
215}
216
217OpticalFlow::~OpticalFlow(void) {
218    delete pimpl_;
219}
220
221void OpticalFlow::UpdateFrom(const io_data *data) {
222    pimpl_->UpdateFrom(data);
223    ProcessUpdate(pimpl_->output);
224}
225
226OpticalFlowData* OpticalFlow::Output(void) {
227    return pimpl_->output;
228
229}
230
231DataType const &OpticalFlow::GetOutputDataType() const {
232    if(pimpl_->output!=NULL) {
233        return pimpl_->output->GetDataType();
234    } else {
235        return dummyType;
236    }
237}
238
239} // end namespace filter
240} // end namespace flair
Note: See TracBrowser for help on using the repository browser.