[122] | 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"
|
---|
[338] | 16 | #include "VisionFilter.h"
|
---|
| 17 | #include <Image.h>
|
---|
[122] | 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 |
|
---|
| 26 | using std::string;
|
---|
| 27 | using std::swap;
|
---|
| 28 | using namespace flair::core;
|
---|
| 29 | using namespace flair::gui;
|
---|
| 30 |
|
---|
[338] | 31 | class OpticalFlow_impl {
|
---|
| 32 | public:
|
---|
| 33 | OpticalFlow_impl(flair::filter::OpticalFlow *self,const LayoutPosition* position,string name) {
|
---|
| 34 | this->self=self;
|
---|
| 35 | is_init=false;
|
---|
[122] | 36 |
|
---|
[338] | 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));
|
---|
[122] | 49 |
|
---|
[338] | 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 | }
|
---|
[122] | 77 |
|
---|
[338] | 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);*/
|
---|
[122] | 88 | }
|
---|
[338] | 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;
|
---|
[122] | 107 |
|
---|
[338] | 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;
|
---|
[122] | 112 |
|
---|
[338] | 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] | 122 |
|
---|
[338] | 123 | output->Resize(max_features->Value());
|
---|
| 124 | }
|
---|
[122] | 125 |
|
---|
[338] | 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 | }
|
---|
[122] | 135 |
|
---|
[338] | 136 | data->GetMutex();
|
---|
| 137 | data->Prev(1)->GetMutex();
|
---|
[122] | 138 |
|
---|
[338] | 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) ;
|
---|
[122] | 146 |
|
---|
[338] | 147 | data->Prev(1)->ReleaseMutex();
|
---|
| 148 | data->ReleaseMutex();
|
---|
[122] | 149 |
|
---|
[338] | 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;
|
---|
[122] | 159 |
|
---|
[338] | 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 | }
|
---|
[122] | 167 |
|
---|
[338] | 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();
|
---|
[330] | 175 |
|
---|
[338] | 176 | output->SetPointsA(pointsA);
|
---|
| 177 | output->SetFoundFeature(found_feature);
|
---|
| 178 | output->SetFeatureError(feature_error);
|
---|
| 179 | output->SetNbFeatures(count);
|
---|
[122] | 180 |
|
---|
[338] | 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;
|
---|
[122] | 189 |
|
---|
[338] | 190 | private:
|
---|
| 191 | flair::filter::OpticalFlow *self;
|
---|
| 192 | SpinBox *max_features;
|
---|
| 193 | OneAxisRotation* rotation;
|
---|
[122] | 194 |
|
---|
[338] | 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;
|
---|
[330] | 202 |
|
---|
[338] | 203 | bool is_init;
|
---|
| 204 | };
|
---|
[122] | 205 |
|
---|
| 206 |
|
---|
[338] | 207 | namespace flair
|
---|
| 208 | {
|
---|
| 209 | namespace filter
|
---|
| 210 | {
|
---|
[122] | 211 |
|
---|
[338] | 212 | OpticalFlow::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 | }
|
---|
[122] | 216 |
|
---|
[338] | 217 | OpticalFlow::~OpticalFlow(void) {
|
---|
| 218 | delete pimpl_;
|
---|
[122] | 219 | }
|
---|
| 220 |
|
---|
[338] | 221 | void OpticalFlow::UpdateFrom(const io_data *data) {
|
---|
| 222 | pimpl_->UpdateFrom(data);
|
---|
| 223 | ProcessUpdate(pimpl_->output);
|
---|
| 224 | }
|
---|
| 225 |
|
---|
| 226 | OpticalFlowData* OpticalFlow::Output(void) {
|
---|
| 227 | return pimpl_->output;
|
---|
| 228 |
|
---|
| 229 | }
|
---|
| 230 |
|
---|
| 231 | DataType const &OpticalFlow::GetOutputDataType() const {
|
---|
| 232 | if(pimpl_->output!=NULL) {
|
---|
| 233 | return pimpl_->output->GetDataType();
|
---|
| 234 | } else {
|
---|
| 235 | return dummyType;
|
---|
| 236 | }
|
---|
| 237 | }
|
---|
| 238 |
|
---|
[122] | 239 | } // end namespace filter
|
---|
| 240 | } // end namespace flair
|
---|