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 |
|
---|
27 | using std::string;
|
---|
28 | using std::swap;
|
---|
29 | using namespace flair::core;
|
---|
30 | using namespace flair::gui;
|
---|
31 |
|
---|
32 | namespace flair
|
---|
33 | {
|
---|
34 | namespace filter
|
---|
35 | {
|
---|
36 |
|
---|
37 | OpticalFlow::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 |
|
---|
67 | OpticalFlow::~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 |
|
---|
77 | void 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
|
---|