[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"
|
---|
| 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;
|
---|
[157] | 50 | return;
|
---|
[122] | 51 | }
|
---|
| 52 |
|
---|
| 53 | is_init=false;
|
---|
| 54 |
|
---|
| 55 | GroupBox* reglages_groupbox=new GroupBox(position,name);
|
---|
[187] | 56 | rotation=new OneAxisRotation(reglages_groupbox->NewRow(),"post rotation",OneAxisRotation::PostRotation);
|
---|
[144] | 57 | max_features=new SpinBox(reglages_groupbox->NewRow(),"max features:",1,65535,1,1);
|
---|
[122] | 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));
|
---|
[157] | 66 |
|
---|
| 67 | SetIsReady(true);
|
---|
[122] | 68 | }
|
---|
| 69 |
|
---|
| 70 | OpticalFlow::~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 |
|
---|
| 80 | void 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 |
|
---|
[144] | 90 | if(max_features->ValueChanged()==true) {
|
---|
[122] | 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
|
---|
[144] | 129 | for(i=0;i<count;i++) {
|
---|
[167] | 130 | Vector3Df tmp;
|
---|
[122] | 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
|
---|