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

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

modifs pour template vectors

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        return;
51    }
52
53    is_init=false;
54
55    GroupBox* reglages_groupbox=new GroupBox(position,name);
56    rotation=new OneAxisRotation(reglages_groupbox->NewRow(),"post rotation");
57    max_features=new SpinBox(reglages_groupbox->NewRow(),"max features:",1,65535,1,1);
58
59    output=new OpticalFlowData(parent,max_features->Value());
60
61    pointsA=(CvPoint *)cvAlloc(max_features->Value()*sizeof(CvPoint));
62    pointsB=(CvPoint *)cvAlloc(max_features->Value()*sizeof(CvPoint));
63
64    found_feature=(char *)cvAlloc(max_features->Value()*sizeof(char));
65    feature_error=(unsigned int *)cvAlloc(max_features->Value()*sizeof(unsigned int));
66   
67    SetIsReady(true);
68}
69
70OpticalFlow::~OpticalFlow(void)
71{
72    cvReleaseImage(&pyr);
73    cvReleaseImage(&pyr_old);
74    cvFree(&pointsA);
75    cvFree(&pointsB);
76    cvFree(&found_feature);
77    cvFree(&feature_error);
78}
79
80void OpticalFlow::UpdateFrom(const io_data *data)
81{
82    IplImage *gimg=((cvimage*)data)->img;
83    IplImage *gimg_old=((cvimage*)data->Prev(1))->img;
84
85    unsigned int count;
86    CvSize window = cvSize(3,3);
87    CvTermCriteria termination_criteria = cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3 );
88    unsigned int i;
89
90    if(max_features->ValueChanged()==true) {
91        cvFree(&pointsA);
92        cvFree(&pointsB);
93        cvFree(&found_feature);
94        cvFree(&feature_error);
95        pointsA=(CvPoint *)cvAlloc(max_features->Value()*sizeof(CvPoint));
96        pointsB=(CvPoint *)cvAlloc(max_features->Value()*sizeof(CvPoint));
97        found_feature=(char *)cvAlloc(max_features->Value()*sizeof(char));
98        feature_error=(unsigned int *)cvAlloc(max_features->Value()*sizeof(unsigned int));
99
100        output->Resize(max_features->Value());
101    }
102/*
103    if(is_init==false)
104    {
105        data->GetMutex();
106        //init image old
107        dspPyrDown(gimg,pyr_old,LEVEL_PYR);
108        data->ReleaseMutex();
109        is_init=true;
110        printf("ajouter mise a 0 des points\n");
111        return;
112    }
113
114    data->GetMutex();
115    data->Prev(1)->GetMutex();
116
117    //select good features
118    count=max_features->Value();
119    dspGoodFeaturesToTrack(gimg_old,pointsA,&count,0.08,5);
120    //pyramide
121    dspPyrDown(gimg,pyr,LEVEL_PYR);
122    //lk
123    dspCalcOpticalFlowPyrLK(gimg_old,gimg,pyr_old,pyr,pointsA,pointsB,count,window,LEVEL_PYR,found_feature,feature_error,termination_criteria,0) ;
124
125    data->Prev(1)->ReleaseMutex();
126    data->ReleaseMutex();
127*/
128    //apply rotation
129    for(i=0;i<count;i++) {
130        Vector3Df tmp;
131        tmp.x=pointsA[i].x;
132        tmp.y=pointsA[i].y;
133        tmp.z=0;
134        rotation->ComputeRotation(tmp);
135        pointsA[i].x=tmp.x;
136        pointsA[i].y=tmp.y;
137
138        tmp.x=pointsB[i].x;
139        tmp.y=pointsB[i].y;
140        tmp.z=0;
141        rotation->ComputeRotation(tmp);
142        pointsB[i].x=tmp.x;
143        pointsB[i].y=tmp.y;
144    }
145
146    output->GetMutex();
147    CvPoint2D32f* pointsBf= output->PointsB();
148    for(i=0;i<count;i++)
149    {
150        pointsBf[i].x=pointsA[i].x+((float)pointsB[i].x)/256;
151        pointsBf[i].y=pointsA[i].y+((float)pointsB[i].y)/256;
152    }
153    output->ReleaseMutex();
154
155    output->SetPointsA(pointsA);
156    output->SetFoundFeature(found_feature);
157    output->SetFeatureError(feature_error);
158    output->SetNbFeatures(count);
159
160    //rotation
161    swap(pyr,pyr_old);
162
163    output->SetDataTime(data->DataTime());
164    ProcessUpdate(output);
165}
166
167} // end namespace filter
168} // end namespace flair
Note: See TracBrowser for help on using the repository browser.