Changeset 325 in flair-src
- Timestamp:
- Aug 28, 2019, 4:12:11 PM (5 years ago)
- Location:
- branches/sanscv
- Files:
-
- 18 edited
Legend:
- Unmodified
- Added
- Removed
-
branches/sanscv/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp
r324 r325 77 77 DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x speed (px/s)",-250,250); 78 78 DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y speed (px/s)",-250,250); 79 DataPlot1D* xFirstPointPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x movement first point",-25,25); 80 DataPlot1D* yFirstPointPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y movement first point",-25,25); 81 // DataPlot1D* xAccelerationPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_acceleration",-250,250); 82 // DataPlot1D* yAccelerationPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_acceleration",-250,250); 83 79 84 80 xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0)); 85 81 xVelocityPlot->AddCurve(opticalFlowSpeed->GetMatrix()->Element(0,0),DataPlot::Blue); 86 82 yVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(1,0)); 87 83 yVelocityPlot->AddCurve(opticalFlowSpeed->GetMatrix()->Element(1,0),DataPlot::Blue); 88 xFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(0,0)); 89 xFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(1,0),DataPlot::Blue); 90 xFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(2,0),DataPlot::Green); 91 yFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(0,1)); 92 yFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(1,1),DataPlot::Blue); 93 yFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(2,1),DataPlot::Green); 94 // xAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(0,0)); 95 // xAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(0,0),DataPlot::Blue); 96 // yAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(1,0)); 97 // yAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(1,0),DataPlot::Blue); 98 84 99 85 u_x=new Pid(setupLawTab->At(1,0),"u_x"); 100 86 u_x->UseDefaultPlot(graphLawTab->NewRow()); -
branches/sanscv/demos/OpticalFlow/uav/src/DemoOpticalFlow.h
r324 r325 65 65 flair::filter::OpticalFlowCompensated *opticalFlowCompensated; 66 66 flair::filter::OpticalFlowSpeed *opticalFlowSpeedRaw; 67 flair::filter::EulerDerivative *opticalFlowAccelerationRaw;67 //flair::filter::EulerDerivative *opticalFlowAccelerationRaw; 68 68 flair::gui::PushButton *startOpticalflow,*stopOpticalflow; 69 69 void StartOpticalFlow(void); -
branches/sanscv/lib/FlairVisionFilter/src/CvtColor.cpp
r324 r325 13 13 14 14 #include "CvtColor.h" 15 #include "VisionFilter.h" 15 16 #include <Image.h> 17 //#include <dspcv_gpp.h> 16 18 #include <typeinfo> 17 19 … … 19 21 using namespace flair::core; 20 22 23 class CvtColor_impl { 24 public: 25 CvtColor_impl(flair::filter::CvtColor *self,flair::filter::CvtColor::Conversion_t conversion):output(0) { 26 this->conversion=conversion; 27 this->self=self; 28 29 switch(conversion) { 30 case flair::filter::CvtColor::Conversion_t::ToGray: 31 try{ 32 Image::Type const &imageType=dynamic_cast<Image::Type const &>(((IODevice*)(self->Parent()))->GetOutputDataType()); 33 output=new Image(self,imageType.GetWidth(),imageType.GetHeight(),Image::Type::Format::Gray,"conversion",true,2); 34 //inIplImage = (IplImage*)AllocFunction(sizeof(IplImage)); 35 //outIplImage = (IplImage*)AllocFunction(sizeof(IplImage)); 36 } catch(std::bad_cast& bc) { 37 self->Err("io type mismatch\n"); 38 } 39 break; 40 default: 41 self->Err("conversion not supported\n"); 42 } 43 } 44 45 ~CvtColor_impl() { 46 //FreeFunction((char*)(inIplImage)); 47 //FreeFunction((char*)(outIplImage)); 48 } 49 50 void UpdateFrom(const io_data *data){ 51 Image *img=(Image*)data; 52 53 if(output!=NULL) { 54 data->GetMutex();/* 55 inIplImage->width=img->GetDataType().GetWidth(); 56 inIplImage->height=img->GetDataType().GetHeight(); 57 inIplImage->imageData=img->buffer; 58 */ 59 output->GetMutex();/* 60 outIplImage->width=output->GetDataType().GetWidth(); 61 outIplImage->height=output->GetDataType().GetHeight(); 62 outIplImage->imageData=output->buffer; 63 */ 64 switch(conversion) { 65 case flair::filter::CvtColor::Conversion_t::ToGray: 66 switch(((Image*)data)->GetDataType().GetFormat()) { 67 case Image::Type::Format::YUYV: 68 //dspCvtColor(inIplImage,outIplImage,DSP_YUYV2GRAY); 69 break; 70 case Image::Type::Format::UYVY: 71 //dspCvtColor(inIplImage,outIplImage,DSP_UYVY2GRAY); 72 break; 73 case Image::Type::Format::BGR: 74 //dspCvtColor(inIplImage,outIplImage,DSP_BGR2GRAY); 75 break; 76 default: 77 self->Err("input format not supported\n"); 78 } 79 break; 80 default: 81 self->Err("conversion not supported\n"); 82 } 83 84 output->ReleaseMutex(); 85 data->ReleaseMutex(); 86 87 output->SetDataTime(data->DataTime()); 88 } 89 }; 90 91 Image *output; 92 93 private: 94 flair::filter::CvtColor::Conversion_t conversion; 95 //IplImage *inIplImage,*outIplImage; 96 flair::filter::CvtColor *self; 97 }; 98 99 21 100 namespace flair { namespace filter { 22 101 23 CvtColor::CvtColor(const core::IODevice* parent,std::string name,Conversion_t conversion) : IODevice(parent,name),output(0) { 24 this->conversion=conversion; 25 26 switch(conversion) { 27 case Conversion_t::ToGray: 28 try{ 29 Image::Type const &imageType=dynamic_cast<Image::Type const &>(parent->GetOutputDataType()); 30 output=new Image(this,imageType.GetWidth(),imageType.GetHeight(),Image::Type::Format::Gray,"conversion",true,2); 31 32 } catch(std::bad_cast& bc) { 33 Err("io type mismatch\n"); 34 } 35 break; 36 default: 37 Err("conversion not supported\n"); 38 } 39 40 SetIsReady(true); 102 CvtColor::CvtColor(const core::IODevice* parent,std::string name,Conversion_t conversion) : IODevice(parent,name) { 103 pimpl_=new CvtColor_impl(this,conversion); 41 104 } 42 105 43 CvtColor::~CvtColor(void) {} 106 CvtColor::~CvtColor(void) { 107 delete pimpl_; 108 } 44 109 45 110 Image* CvtColor::Output(void) { 46 return output;111 return pimpl_->output; 47 112 48 113 } 49 114 50 115 DataType const &CvtColor::GetOutputDataType() const { 51 if( output!=NULL) {52 return output->GetDataType();116 if(pimpl_->output!=NULL) { 117 return pimpl_->output->GetDataType(); 53 118 } else { 54 119 return dummyType; … … 57 122 58 123 void CvtColor::UpdateFrom(const io_data *data) { 59 Image *img=(Image*)data; 60 61 data->GetMutex(); 62 output->GetMutex(); 63 64 switch(conversion) { 65 case Conversion_t::ToGray: 66 switch(img->GetDataType().GetFormat()) { 67 case Image::Type::Format::YUYV: 68 //dspCvtColor(img,output->img,DSP_YUYV2GRAY); 69 break; 70 case Image::Type::Format::UYVY: 71 //dspCvtColor(img,output->img,DSP_UYVY2GRAY); 72 break; 73 case Image::Type::Format::BGR: 74 //dspCvtColor(img,output->img,DSP_BGR2GRAY); 75 break; 76 default: 77 Err("input format not supported\n"); 78 } 79 break; 80 default: 81 Err("conversion not supported\n"); 82 } 83 84 output->ReleaseMutex(); 85 data->ReleaseMutex(); 86 87 output->SetDataTime(data->DataTime()); 88 ProcessUpdate(output); 124 pimpl_->UpdateFrom(data); 125 if(pimpl_->output!=NULL) ProcessUpdate(pimpl_->output); 126 89 127 } 90 128 -
branches/sanscv/lib/FlairVisionFilter/src/CvtColor.h
r324 r325 13 13 #include <IODevice.h> 14 14 15 namespace flair 16 { 17 namespace core 18 { 15 namespace flair { 16 namespace core { 19 17 class Image; 20 18 } 21 19 } 22 20 23 namespace flair 24 { 21 class CvtColor_impl; 22 23 namespace flair { 25 24 namespace filter 26 25 { … … 82 81 void UpdateFrom(const core::io_data *data); 83 82 84 core::Image *output; 85 Conversion_t conversion; 83 class CvtColor_impl *pimpl_; 86 84 }; 87 85 } // end namespace filter -
branches/sanscv/lib/FlairVisionFilter/src/HoughLines.cpp
r324 r325 13 13 14 14 #include "HoughLines.h" 15 #include "VisionFilter.h" 15 16 #include <Image.h> 17 #include <OneAxisRotation.h> 16 18 #include <Matrix.h> 17 19 #include <Layout.h> … … 19 21 #include <SpinBox.h> 20 22 #include <DoubleSpinBox.h> 23 //#include <dspcv_gpp.h> 21 24 #include <typeinfo> 25 #include <math.h> 22 26 23 27 #define MAX_LINES 100 … … 27 31 using namespace flair::gui; 28 32 33 class HoughLines_impl { 34 public: 35 HoughLines_impl(flair::filter::HoughLines *self,const LayoutPosition* position,string name,const Vector2Df *inPtRefGlobal,float inThetaRefGlobal) { 36 this->self=self; 37 GroupBox* reglages_groupbox=new GroupBox(position,name); 38 rotation=new OneAxisRotation(reglages_groupbox->NewRow(),"pre rotation",OneAxisRotation::PostRotation); 39 fullRhoStep=new SpinBox(reglages_groupbox->NewRow(),"full rho step:","pixels",0,255,1,1); 40 fullThetaStep=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"full theta step:","degrees",0,90,1,1); 41 trackingRhoStep=new SpinBox(reglages_groupbox->NewRow(),"tracking rho step:","pixels",0,255,1,1); 42 trackingThetaStep=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"tracking theta step:","degrees",0,90,1,1); 43 trackingDeltaTheta=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"tracking delta theta:","degrees",0,90,1,1); 44 nbPoints=new SpinBox(reglages_groupbox->NewRow(),"nb points:",0,10000,10,100); 45 46 isTracking=false; 47 //linesStorage = (CvMat*)AllocFunction(sizeof(CvMat)); 48 //linesStorage->data.fl = (float*)AllocFunction(MAX_LINES*2*sizeof(float));//was CV_32FC2, 2 channels 49 //gimg = (IplImage*)AllocFunction(sizeof(IplImage)); 50 51 //init output matrix of same size as init 52 MatrixDescriptor* desc=new MatrixDescriptor(4,1); 53 desc->SetElementName(0,0,"distance"); 54 desc->SetElementName(1,0,"orientation rad"); 55 desc->SetElementName(2,0,"orientation deg"); 56 desc->SetElementName(3,0,"line_detected"); 57 output=new Matrix(self,desc,floatType,name); 58 delete desc; 59 60 try{ 61 Image::Type const &imageType=dynamic_cast<Image::Type const &>(((IODevice*)(self->Parent()))->GetOutputDataType()); 62 if(imageType.GetFormat()!=Image::Type::Format::Gray) { 63 self->Err("input image is not gray\n"); 64 } 65 } catch(std::bad_cast& bc) { 66 self->Err("io type mismatch\n"); 67 } 68 69 thetaRefGlobal=inThetaRefGlobal; 70 if (inPtRefGlobal==NULL) { 71 ptRefGlobal=NULL; 72 } else { //rotation from global coordinates to hough space 73 ptRefGlobal =new Vector2Df(inPtRefGlobal->x,inPtRefGlobal->y); 74 } 75 76 } 77 78 ~HoughLines_impl() { 79 //FreeFunction((char*)(linesStorage->data.fl)); 80 //FreeFunction((char*)linesStorage); 81 //FreeFunction((char*)gimg); 82 if(ptRefGlobal!=NULL) delete ptRefGlobal; 83 } 84 85 void UpdateFrom(const io_data *data){ 86 Image *img=(Image*)data; 87 /* 88 gimg->width=img->GetDataType().GetWidth(); 89 gimg->height=img->GetDataType().GetHeight(); 90 gimg->imageData=img->buffer; 91 92 size_t nbLines; 93 Vector2Df ptRef; 94 float thetaRef; 95 96 if (ptRefGlobal==NULL) { 97 ptRef.x=img->GetDataType().GetWidth()/2; 98 ptRef.y=img->GetDataType().GetHeight()/2; 99 } else { //rotation from global coordinates to hough space 100 Vector3Df ptRef3D(ptRefGlobal->x,ptRefGlobal->y,0); 101 rotation->ComputeRotation(ptRef3D); 102 ptRef.x=ptRef3D.x; 103 ptRef.y=ptRef3D.y; 104 } 105 106 //orientation in global space is rotated by pi/2 compared to orientation in hough space 107 //eg: vertical line has a 0 orientation in global space (north), but a pi/2 (or -pi/2) orientation in hough space (theta) 108 thetaRef=thetaRefGlobal+CV_PI/2+rotation->GetAngle(); 109 if (thetaRef>CV_PI) thetaRef-=CV_PI; 110 if (thetaRef<0) thetaRef+=CV_PI; 111 112 data->GetMutex(); 113 if(!isTracking) { 114 nbLines=dspHoughLines2(gimg,linesStorage,CV_HOUGH_STANDARD, 115 fullRhoStep->Value(),fullThetaStep->Value()*CV_PI/180, 116 nbPoints->Value()); 117 } else { 118 nbLines=dspHoughLinesTracking(gimg,linesStorage,CV_HOUGH_STANDARD, 119 trackingRhoStep->Value(), 120 theta,trackingDeltaTheta->Value()*CV_PI/180, 121 trackingThetaStep->Value()*CV_PI/180, 122 nbPoints->Value()); 123 // nbLines=dspHoughLines2_test(gimg,linesStorage,CV_HOUGH_STANDARD,trackingRhoStep->Value(),thetaPrev-trackingDeltaTheta->Value()*CV_PI/180,thetaPrev+trackingDeltaTheta->Value()*CV_PI/180,trackingThetaStep->Value()*CV_PI/180,nbPoints->Value()); 124 } 125 data->ReleaseMutex(); 126 127 //saturation sur le nb max de ligne, au cas ou le DSP n'aurait pas la meme valeur 128 if(nbLines>MAX_LINES) { 129 self->Warn("erreur nb lines %u>%u\n",nbLines,MAX_LINES); 130 nbLines=MAX_LINES; 131 } 132 float rho; 133 bool noLine=!SelectBestLine(linesStorage,nbLines,rho,theta); 134 135 if (noLine) { 136 isTracking=false; 137 } else { 138 isTracking=true; 139 // float thetaRef=0; 140 141 //line equation is ax+by+c=0 with a=cos(theta), b=sin(theta) and c=-rho 142 //distance from point xRef,yRef to the line is (a.xRef+b.yRef+c)/sqrt(a*a+b*b) 143 distance=-(cosf(theta)*ptRef.x+sinf(theta)*ptRef.y-rho); 144 145 orientation=theta-thetaRef; 146 if (orientation<-CV_PI/2) { 147 orientation+=CV_PI; 148 distance=-distance; 149 } 150 if (orientation>CV_PI/2) { 151 orientation-=CV_PI; 152 distance=-distance; 153 } 154 155 //printf("=> pour theta=%f et rho=%f, distance au point(%f,%f)=%f\n",theta,rho,xRef,yRef,distance); 156 } 157 158 output->GetMutex(); 159 output->SetValueNoMutex(0,0,distance); 160 output->SetValueNoMutex(1,0,orientation); 161 output->SetValueNoMutex(2,0,orientation*180/CV_PI); 162 if(noLine) { 163 output->SetValueNoMutex(3,0,0); 164 } else { 165 output->SetValueNoMutex(3,0,1); 166 } 167 output->ReleaseMutex(); 168 */ 169 output->SetDataTime(data->DataTime()); 170 }; 171 172 Matrix *output; 173 174 private:/* 175 //select best line. Returns false if no line found 176 bool SelectBestLine(CvMat* linesStorage, size_t nbLines, float &rho, float &theta) { 177 if(nbLines==0) { 178 return false; 179 } 180 //one line is found 181 if (nbLines==1) { 182 rho=linesStorage->data.fl[0]; 183 theta=linesStorage->data.fl[1]; 184 //printf("rho=%f,theta=%f (one line)\n",rho,theta); 185 return true; 186 } 187 //lines are ordered by quality, the first one will be our reference 188 float thetaRef=linesStorage->data.fl[1]; 189 float thetaRefErrorSum=0; 190 float rhoSum=linesStorage->data.fl[0]; 191 //printf("rho=%f,theta=%f (first of multilines)\n",rhoSum,thetaRef); 192 for(int i=1;i<nbLines;i++) { 193 //printf("rho=%f,theta=%f (multilines)\n",linesStorage->data.fl[2*i],linesStorage->data.fl[2*i+1]); 194 float thetaDiff=linesStorage->data.fl[2*i+1]-thetaRef; 195 float rhoLine=linesStorage->data.fl[2*i]; 196 if (thetaDiff>CV_PI/2) { 197 thetaDiff-=CV_PI; 198 rhoLine=-rhoLine; 199 } else if (thetaDiff<-CV_PI/2) { 200 thetaDiff+=CV_PI; 201 rhoLine=-rhoLine; 202 } 203 thetaRefErrorSum += thetaDiff; 204 rhoSum+=rhoLine; 205 } 206 rho=rhoSum/nbLines; 207 theta=thetaRef+thetaRefErrorSum/nbLines; 208 if (theta<0) { 209 theta+=CV_PI; 210 rho=-rho; 211 } 212 if (theta>CV_PI) { 213 theta-=CV_PI; 214 rho=-rho; 215 } 216 return true; 217 } 218 */ 219 flair::filter::HoughLines *self; 220 OneAxisRotation* rotation; 221 SpinBox *fullRhoStep,*trackingRhoStep,*nbPoints; 222 DoubleSpinBox *fullThetaStep,*trackingThetaStep,*trackingDeltaTheta; 223 bool isTracking; 224 float theta; 225 float distance,orientation; 226 Vector2Df* ptRefGlobal; 227 float thetaRefGlobal; 228 //CvMat* linesStorage; 229 //IplImage *gimg; 230 }; 231 29 232 namespace flair { namespace filter { 30 233 31 HoughLines::HoughLines(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name) { 32 GroupBox* reglages_groupbox=new GroupBox(position,name); 33 fullRhoStep=new SpinBox(reglages_groupbox->NewRow(),"full rho step:","pixels",0,255,1,1); 34 fullThetaStep=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"full theta step:","degrees",0,90,1,1); 35 trackingRhoStep=new SpinBox(reglages_groupbox->NewRow(),"tracking rho step:","pixels",0,255,1,1); 36 trackingThetaStep=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"tracking theta step:","degrees",0,90,1,1); 37 trackingDeltaTheta=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"tracking delta theta:","degrees",0,90,1,1); 38 nbPoints=new SpinBox(reglages_groupbox->NewRow(),"nb points:",0,10000,10,100); 39 40 isTracking=false; 41 lostLine=false; 42 initLine=false; 43 linesStorage = (CvMat*)malloc(sizeof(CvMat)); 44 linesStorage->data.fl = (float*)malloc(MAX_LINES*2*sizeof(float));//was CV_32FC2, 2 channels 45 46 47 //init output matrix of same size as init 48 MatrixDescriptor* desc=new MatrixDescriptor(4,1); 49 desc->SetElementName(0,0,"distance"); 50 desc->SetElementName(1,0,"orientation rad"); 51 desc->SetElementName(2,0,"orientation deg"); 52 desc->SetElementName(3,0,"line_detected"); 53 output=new Matrix(this,desc,floatType,name); 54 delete desc; 55 56 try{ 57 Image::Type const &imageType=dynamic_cast<Image::Type const &>(parent->GetOutputDataType()); 58 if(imageType.GetFormat()!=Image::Type::Format::Gray) { 59 Err("input image is not gray\n"); 60 return; 61 } 62 } catch(std::bad_cast& bc) { 63 Err("io type mismatch\n"); 64 return; 65 } 66 67 SetIsReady(true); 234 HoughLines::HoughLines(const IODevice* parent,const LayoutPosition* position,string name,const Vector2Df *inPtRefGlobal,float inThetaRefGlobal) : IODevice(parent,name) { 235 pimpl_=new HoughLines_impl(this,position,name,inPtRefGlobal,inThetaRefGlobal); 236 68 237 } 69 238 70 239 HoughLines::~HoughLines(void) { 71 free((char*)(linesStorage->data.fl)); 72 free((char*)linesStorage); 240 delete pimpl_; 73 241 } 74 242 75 243 void HoughLines::UpdateFrom(const io_data *data) { 76 244 pimpl_->UpdateFrom(data); 245 ProcessUpdate(pimpl_->output); 77 246 } 78 247 79 248 bool HoughLines::isLineDetected() const { 80 if( output->Value(3,0)==1) {249 if(pimpl_->output->Value(3,0)==1) { 81 250 return true; 82 251 } else { … … 86 255 87 256 float HoughLines::GetOrientation(void) const { 88 return output->Value(1,0);257 return pimpl_->output->Value(1,0); 89 258 } 90 259 91 260 float HoughLines::GetDistance(void) const { 92 return output->Value(0,0);261 return pimpl_->output->Value(0,0); 93 262 } 94 263 95 264 Matrix *HoughLines::Output(void) const { 96 return output;265 return pimpl_->output; 97 266 } 98 267 -
branches/sanscv/lib/FlairVisionFilter/src/HoughLines.h
r324 r325 12 12 13 13 #include <IODevice.h> 14 #include < cvtypes.h>14 #include <Vector2D.h> 15 15 16 16 namespace flair { 17 17 namespace core { 18 class Image;19 18 class Matrix; 20 19 } 21 20 namespace gui { 22 21 class LayoutPosition; 23 class SpinBox;24 class DoubleSpinBox;25 22 } 26 23 } 24 25 class HoughLines_impl; 27 26 28 27 namespace flair { namespace filter { … … 44 43 * \param position position 45 44 * \param name name 45 * \param ptRefGlobal reference point used to compute distance to the line 46 * \param thetaRefGlobal reference angle used to compute orientation in range [-pi/2,pi/2]. Default value is 0 which defines a vertical line 46 47 */ 47 HoughLines(const core::IODevice* parent,const gui::LayoutPosition* position,std::string name );48 HoughLines(const core::IODevice* parent,const gui::LayoutPosition* position,std::string name,const core::Vector2Df *ptRefGlobal=NULL,float thetaRefGlobal=0); 48 49 49 50 /*! … … 60 61 private: 61 62 void UpdateFrom(const core::io_data *data); 62 gui::SpinBox *fullRhoStep,*trackingRhoStep,*nbPoints; 63 gui::DoubleSpinBox *fullThetaStep,*trackingThetaStep,*trackingDeltaTheta; 64 bool isTracking,lostLine,initLine,noLine; 65 int nbLines; 66 float yawMean,prevYawMean; 67 float prevRhoMean,rhoMean; 68 float prevThetaMean,thetaMean; 69 float distance,orientation; 70 CvMat* linesStorage; 71 core::Matrix *output; 63 class HoughLines_impl *pimpl_; 64 65 72 66 }; 73 67 } // end namespace filter -
branches/sanscv/lib/FlairVisionFilter/src/ImgThreshold.cpp
r324 r325 13 13 14 14 #include "ImgThreshold.h" 15 #include "VisionFilter.h" 15 16 #include <Image.h> 16 17 #include <Layout.h> 17 18 #include <GroupBox.h> 18 19 #include <SpinBox.h> 20 //#include <dspcv_gpp.h> 19 21 #include <typeinfo> 20 22 … … 23 25 using namespace flair::gui; 24 26 27 class ImgThreshold_impl { 28 public: 29 ImgThreshold_impl(flair::filter::ImgThreshold *self,const LayoutPosition* position,string name):output(0) { 30 this->self=self; 31 32 GroupBox* reglages_groupbox=new GroupBox(position,name); 33 threshold=new SpinBox(reglages_groupbox->NewRow(),"threshold:",0,255,1,127); 34 35 Printf("todo: pouvoir reutiliser la meme image en sortie\n"); 36 try{ 37 Image::Type const &imageType=dynamic_cast<Image::Type const &>(((IODevice*)(self->Parent()))->GetOutputDataType()); 38 if(imageType.GetFormat()==Image::Type::Format::Gray) { 39 output=new Image(self,imageType.GetWidth(),imageType.GetHeight(),imageType.GetFormat(),"threshold"); 40 //inIplImage = (IplImage*)AllocFunction(sizeof(IplImage)); 41 //outIplImage = (IplImage*)AllocFunction(sizeof(IplImage)); 42 } else { 43 self->Err("input image is not gray\n"); 44 } 45 } catch(std::bad_cast& bc) { 46 self->Err("io type mismatch\n"); 47 } 48 49 50 } 51 52 ~ImgThreshold_impl() { 53 //FreeFunction((char*)(inIplImage)); 54 //FreeFunction((char*)(outIplImage)); 55 } 56 57 void UpdateFrom(const io_data *data){ 58 Image *img=(Image*)data; 59 /* 60 data->GetMutex(); 61 inIplImage->width=img->GetDataType().GetWidth(); 62 inIplImage->height=img->GetDataType().GetHeight(); 63 inIplImage->imageData=img->buffer; 64 65 output->GetMutex(); 66 outIplImage->width=output->GetDataType().GetWidth(); 67 outIplImage->height=output->GetDataType().GetHeight(); 68 outIplImage->imageData=output->buffer; 69 70 dspThreshold(inIplImage,outIplImage, threshold->Value(), 255, CV_THRESH_BINARY); 71 output->ReleaseMutex(); 72 data->ReleaseMutex(); 73 */ 74 output->SetDataTime(data->DataTime()); 75 }; 76 77 Image *output; 78 79 private: 80 flair::filter::ImgThreshold *self; 81 SpinBox *threshold; 82 //IplImage *inIplImage,*outIplImage; 83 }; 84 25 85 namespace flair { namespace filter { 26 86 27 ImgThreshold::ImgThreshold(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name),output(0) { 28 GroupBox* reglages_groupbox=new GroupBox(position,name); 29 threshold=new SpinBox(reglages_groupbox->NewRow(),"threshold:",0,255,1,127); 30 31 Printf("todo: pouvoir reutiliser la meme image en sortie\n"); 32 try{ 33 Image::Type const &imageType=dynamic_cast<Image::Type const &>(parent->GetOutputDataType()); 34 if(imageType.GetFormat()==Image::Type::Format::Gray) { 35 output=new Image(this,imageType.GetWidth(),imageType.GetHeight(),imageType.GetFormat(),"threshold"); 36 } else { 37 Err("input image is not gray\n"); 38 return; 39 } 40 } catch(std::bad_cast& bc) { 41 Err("io type mismatch\n"); 42 return; 43 } 44 SetIsReady(true); 87 ImgThreshold::ImgThreshold(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name) { 88 pimpl_=new ImgThreshold_impl(this,position,name); 45 89 } 46 90 47 91 ImgThreshold::~ImgThreshold(void) { 92 delete pimpl_; 48 93 } 49 94 50 95 Image* ImgThreshold::Output(void) { 51 return output;96 return pimpl_->output; 52 97 } 53 98 54 99 void ImgThreshold::UpdateFrom(const io_data *data) { 55 /* Image *cvImage=(Image*)data; 56 IplImage *gimg=cvImage->img; 57 58 data->GetMutex(); 59 output->GetMutex(); 60 dspThreshold(gimg, output->img, threshold->Value(), 255, CV_THRESH_BINARY); 61 output->ReleaseMutex(); 62 data->ReleaseMutex(); 63 */ 64 output->SetDataTime(data->DataTime()); 65 ProcessUpdate(output); 100 pimpl_->UpdateFrom(data); 101 ProcessUpdate(pimpl_->output); 66 102 } 67 103 68 104 DataType const &ImgThreshold::GetOutputDataType() const { 69 if( output!=NULL) {70 return output->GetDataType();105 if(pimpl_->output!=NULL) { 106 return pimpl_->output->GetDataType(); 71 107 } else { 72 108 return dummyType; -
branches/sanscv/lib/FlairVisionFilter/src/ImgThreshold.h
r324 r325 19 19 namespace gui { 20 20 class LayoutPosition; 21 class SpinBox;22 21 } 23 22 } 23 24 class ImgThreshold_impl; 24 25 25 26 namespace flair { namespace filter { … … 63 64 private: 64 65 void UpdateFrom(const core::io_data *data); 65 core::Image *output; 66 gui::SpinBox *threshold; 66 class ImgThreshold_impl *pimpl_; 67 67 }; 68 68 } // end namespace filter -
branches/sanscv/lib/FlairVisionFilter/src/OpticalFlow.cpp
r324 r325 14 14 #include "OpticalFlow.h" 15 15 #include "OpticalFlowData.h" 16 #include "VisionFilter.h" 16 17 #include <Image.h> 17 18 #include <Layout.h> 18 19 #include <GroupBox.h> 19 20 #include <SpinBox.h> 20 //#include <algorithm> 21 //#include <dspcv_gpp.h> 22 #include <algorithm> 21 23 #include <OneAxisRotation.h> 22 24 #include <Vector3D.h> … … 30 32 using namespace flair::gui; 31 33 34 class OpticalFlow_impl { 35 public: 36 OpticalFlow_impl(flair::filter::OpticalFlow *self,const LayoutPosition* position,string name) { 37 this->self=self; 38 is_init=false; 39 40 GroupBox* reglages_groupbox=new GroupBox(position,name); 41 rotation=new OneAxisRotation(reglages_groupbox->NewRow(),"post rotation",OneAxisRotation::PostRotation); 42 max_features=new SpinBox(reglages_groupbox->NewRow(),"max features:",1,65535,1,1); 43 44 try{ 45 Image::Type const &imageType=dynamic_cast<Image::Type const &>(((IODevice*)(self->Parent()))->GetOutputDataType()); 46 pyr=new Image(self,imageType.GetWidth(),imageType.GetHeight(),Image::Type::Format::Gray); 47 pyr_old=new Image(self,imageType.GetWidth(),imageType.GetHeight(),Image::Type::Format::Gray); 48 output=new flair::filter::OpticalFlowData(self->Parent(),max_features->Value()); 49 /* 50 pointsA=(CvPoint*)AllocFunction(max_features->Value()*sizeof(CvPoint)); 51 pointsB=(CvPoint*)AllocFunction(max_features->Value()*sizeof(CvPoint)); 52 53 found_feature=(char*)AllocFunction(max_features->Value()*sizeof(char)); 54 feature_error=(unsigned int*)AllocFunction(max_features->Value()*sizeof(unsigned int)); 55 56 iplgimg = (IplImage*)AllocFunction(sizeof(IplImage)); 57 iplgimg_old = (IplImage*)AllocFunction(sizeof(IplImage)); 58 iplpyr = (IplImage*)AllocFunction(sizeof(IplImage)); 59 iplpyr_old = (IplImage*)AllocFunction(sizeof(IplImage)); 60 61 iplpyr->width=imageType.GetWidth(); 62 iplpyr->height=imageType.GetHeight(); 63 iplpyr_old->width=imageType.GetWidth(); 64 iplpyr_old->height=imageType.GetHeight(); 65 */ 66 } catch(std::bad_cast& bc) { 67 self->Err("io type mismatch\n"); 68 pyr=NULL; 69 pyr_old=NULL; 70 } 71 72 } 73 74 ~OpticalFlow_impl() { 75 /* 76 FreeFunction((char*)pointsA); 77 FreeFunction((char*)pointsB); 78 FreeFunction((char*)found_feature); 79 FreeFunction((char*)feature_error); 80 FreeFunction((char*)iplgimg); 81 FreeFunction((char*)iplgimg_old); 82 FreeFunction((char*)iplpyr); 83 FreeFunction((char*)iplpyr_old);*/ 84 } 85 86 void UpdateFrom(const io_data *data){ 87 //Time tStart=GetTime(); 88 Image *img=(Image*)data; 89 Image *img_old=((Image*)data->Prev(1)); 90 /* 91 iplgimg->width=img->GetDataType().GetWidth(); 92 iplgimg->height=img->GetDataType().GetHeight(); 93 iplgimg->imageData=img->buffer; 94 95 iplgimg_old->width=img_old->GetDataType().GetWidth(); 96 iplgimg_old->height=img_old->GetDataType().GetHeight(); 97 iplgimg_old->imageData=img_old->buffer; 98 99 iplpyr->imageData=pyr->buffer; 100 iplpyr_old->imageData=pyr_old->buffer; 101 102 unsigned int count; 103 CvSize window = {3,3}; 104 CvTermCriteria termination_criteria ={CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3 }; 105 unsigned int i; 106 107 if(max_features->ValueChanged()==true) { 108 FreeFunction((char*)pointsA); 109 FreeFunction((char*)pointsB); 110 FreeFunction((char*)found_feature); 111 FreeFunction((char*)feature_error); 112 pointsA=(CvPoint*)AllocFunction(max_features->Value()*sizeof(CvPoint)); 113 pointsB=(CvPoint*)AllocFunction(max_features->Value()*sizeof(CvPoint)); 114 found_feature=(char*)AllocFunction(max_features->Value()*sizeof(char)); 115 feature_error=(unsigned int*)AllocFunction(max_features->Value()*sizeof(unsigned int)); 116 117 output->Resize(max_features->Value()); 118 } 119 120 if(is_init==false) { 121 data->GetMutex(); 122 //init image old 123 dspPyrDown(iplgimg,iplpyr_old,LEVEL_PYR); 124 data->ReleaseMutex(); 125 is_init=true; 126 printf("ajouter mise a 0 des points\n"); 127 return; 128 } 129 130 data->GetMutex(); 131 data->Prev(1)->GetMutex(); 132 133 //select good features 134 count=max_features->Value(); 135 dspGoodFeaturesToTrack(iplgimg_old,pointsA,&count,0.08,5); 136 //pyramide 137 dspPyrDown(iplgimg,iplpyr,LEVEL_PYR); 138 //lk 139 dspCalcOpticalFlowPyrLK(iplgimg_old,iplgimg,iplpyr_old,iplpyr,pointsA,pointsB,count,window,LEVEL_PYR,found_feature,feature_error,termination_criteria,0) ; 140 141 data->Prev(1)->ReleaseMutex(); 142 data->ReleaseMutex(); 143 144 //apply rotation 145 for(i=0;i<count;i++) { 146 Vector3Df tmp; 147 tmp.x=pointsA[i].x; 148 tmp.y=pointsA[i].y; 149 tmp.z=0; 150 rotation->ComputeRotation(tmp); 151 pointsA[i].x=tmp.x; 152 pointsA[i].y=tmp.y; 153 154 tmp.x=pointsB[i].x; 155 tmp.y=pointsB[i].y; 156 tmp.z=0; 157 rotation->ComputeRotation(tmp); 158 pointsB[i].x=tmp.x; 159 pointsB[i].y=tmp.y; 160 } 161 162 output->GetMutex(); 163 CvPoint2D32f* pointsBf= output->PointsB(); 164 for(i=0;i<count;i++) { 165 pointsBf[i].x=pointsA[i].x+((float)pointsB[i].x)/256; 166 pointsBf[i].y=pointsA[i].y+((float)pointsB[i].y)/256; 167 } 168 output->ReleaseMutex(); 169 170 output->SetPointsA(pointsA); 171 output->SetFoundFeature(found_feature); 172 output->SetFeatureError(feature_error); 173 output->SetNbFeatures(count); 174 175 //rotation 176 swap(pyr,pyr_old); 177 */ 178 output->SetDataTime(data->DataTime()); 179 //Printf("Optical flow computation time=%f\n",(GetTime()-tStart)/(1000.*1000)); 180 }; 181 182 flair::filter::OpticalFlowData *output; 183 184 private: 185 flair::filter::OpticalFlow *self; 186 SpinBox *max_features; 187 OneAxisRotation* rotation; 188 189 //CvPoint* pointsA; 190 //CvPoint* pointsB; 191 char *found_feature; 192 unsigned int *feature_error; 193 Image *pyr,*pyr_old; 194 //IplImage *iplgimg,*iplgimg_old; 195 //IplImage *iplpyr,*iplpyr_old; 196 197 bool is_init; 198 }; 199 200 32 201 namespace flair 33 202 { … … 35 204 { 36 205 37 OpticalFlow::OpticalFlow(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name) 38 { 206 OpticalFlow::OpticalFlow(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name) { 39 207 Printf("optical flow: voir pour faire du multiple output\n");//pour pts A et B, found et error 40 41 try{ 42 Image::Type const &imageType=dynamic_cast<Image::Type const &>(parent->GetOutputDataType()); 43 pyr=new Image(this,imageType.GetWidth(),imageType.GetHeight(),Image::Type::Format::Gray); 44 pyr_old=new Image(this,imageType.GetWidth(),imageType.GetHeight(),Image::Type::Format::Gray); 45 } catch(std::bad_cast& bc) { 46 Err("io type mismatch\n"); 47 pyr=NULL; 48 pyr_old=NULL; 49 return; 208 pimpl_=new OpticalFlow_impl(this,position,name); 209 } 210 211 OpticalFlow::~OpticalFlow(void) { 212 delete pimpl_; 213 } 214 215 void OpticalFlow::UpdateFrom(const io_data *data) { 216 pimpl_->UpdateFrom(data); 217 ProcessUpdate(pimpl_->output); 218 } 219 220 OpticalFlowData* OpticalFlow::Output(void) { 221 return pimpl_->output; 222 223 } 224 225 DataType const &OpticalFlow::GetOutputDataType() const { 226 if(pimpl_->output!=NULL) { 227 return pimpl_->output->GetDataType(); 228 } else { 229 return dummyType; 50 230 } 51 52 is_init=false;53 54 GroupBox* reglages_groupbox=new GroupBox(position,name);55 rotation=new OneAxisRotation(reglages_groupbox->NewRow(),"post rotation",OneAxisRotation::PostRotation);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*)malloc(max_features->Value()*sizeof(CvPoint));61 pointsB=(CvPoint*)malloc(max_features->Value()*sizeof(CvPoint));62 63 found_feature=(char*)malloc(max_features->Value()*sizeof(char));64 feature_error=(unsigned int*)malloc(max_features->Value()*sizeof(unsigned int));65 66 SetIsReady(true);67 }68 69 OpticalFlow::~OpticalFlow(void) {70 free((char*)pointsA);71 free((char*)pointsB);72 free((char*)found_feature);73 free((char*)feature_error);74 }75 76 void OpticalFlow::UpdateFrom(const io_data *data) {/*77 IplImage *gimg=((Image*)data)->img;78 IplImage *gimg_old=((Image*)data->Prev(1))->img;79 80 unsigned int count;81 CvSize window = cvSize(3,3);82 CvTermCriteria termination_criteria = cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3 );83 unsigned int i;84 85 if(max_features->ValueChanged()==true) {86 cvFree(&pointsA);87 cvFree(&pointsB);88 cvFree(&found_feature);89 cvFree(&feature_error);90 pointsA=(CvPoint *)cvAlloc(max_features->Value()*sizeof(CvPoint));91 pointsB=(CvPoint *)cvAlloc(max_features->Value()*sizeof(CvPoint));92 found_feature=(char *)cvAlloc(max_features->Value()*sizeof(char));93 feature_error=(unsigned int *)cvAlloc(max_features->Value()*sizeof(unsigned int));94 95 output->Resize(max_features->Value());96 }97 98 if(is_init==false)99 {100 data->GetMutex();101 //init image old102 dspPyrDown(gimg,pyr_old,LEVEL_PYR);103 data->ReleaseMutex();104 is_init=true;105 printf("ajouter mise a 0 des points\n");106 return;107 }108 109 data->GetMutex();110 data->Prev(1)->GetMutex();111 112 //select good features113 count=max_features->Value();114 dspGoodFeaturesToTrack(gimg_old,pointsA,&count,0.08,5);115 //pyramide116 dspPyrDown(gimg,pyr,LEVEL_PYR);117 //lk118 dspCalcOpticalFlowPyrLK(gimg_old,gimg,pyr_old,pyr,pointsA,pointsB,count,window,LEVEL_PYR,found_feature,feature_error,termination_criteria,0) ;119 120 data->Prev(1)->ReleaseMutex();121 data->ReleaseMutex();122 123 //apply rotation124 for(i=0;i<count;i++) {125 Vector3Df tmp;126 tmp.x=pointsA[i].x;127 tmp.y=pointsA[i].y;128 tmp.z=0;129 rotation->ComputeRotation(tmp);130 pointsA[i].x=tmp.x;131 pointsA[i].y=tmp.y;132 133 tmp.x=pointsB[i].x;134 tmp.y=pointsB[i].y;135 tmp.z=0;136 rotation->ComputeRotation(tmp);137 pointsB[i].x=tmp.x;138 pointsB[i].y=tmp.y;139 }140 141 output->GetMutex();142 CvPoint2D32f* pointsBf= output->PointsB();143 for(i=0;i<count;i++)144 {145 pointsBf[i].x=pointsA[i].x+((float)pointsB[i].x)/256;146 pointsBf[i].y=pointsA[i].y+((float)pointsB[i].y)/256;147 }148 output->ReleaseMutex();149 150 output->SetPointsA(pointsA);151 output->SetFoundFeature(found_feature);152 output->SetFeatureError(feature_error);153 output->SetNbFeatures(count);154 155 //rotation156 swap(pyr,pyr_old);157 */158 output->SetDataTime(data->DataTime());159 ProcessUpdate(output);160 231 } 161 232 -
branches/sanscv/lib/FlairVisionFilter/src/OpticalFlow.h
r324 r325 12 12 13 13 #include <IODevice.h> 14 #include <cvtypes.h>15 14 16 15 namespace flair { 17 namespace core {18 class Image;19 class OneAxisRotation;20 }21 16 namespace gui { 22 17 class LayoutPosition; 23 class SpinBox;24 18 } 25 19 namespace filter { … … 27 21 } 28 22 } 23 24 class OpticalFlow_impl; 29 25 30 26 namespace flair { … … 57 53 */ 58 54 ~OpticalFlow(); 55 56 filter::OpticalFlowData* Output(void); 57 58 core::DataType const &GetOutputDataType() const; 59 59 60 60 private: 61 61 void UpdateFrom(const core::io_data *data); 62 OpticalFlowData *output; 63 gui::SpinBox *max_features; 64 core::OneAxisRotation* rotation; 65 66 CvPoint* pointsA; 67 CvPoint* pointsB; 68 char *found_feature; 69 unsigned int *feature_error; 70 core::Image *pyr,*pyr_old; 71 72 bool is_init; 62 63 class OpticalFlow_impl *pimpl_; 64 73 65 }; 74 66 } // end namespace filter -
branches/sanscv/lib/FlairVisionFilter/src/OpticalFlowCompensated.cpp
r324 r325 32 32 using namespace flair::gui; 33 33 34 class OpticalFlowCompensated_impl { 35 public: 36 OpticalFlowCompensated_impl(flair::filter::OpticalFlowCompensated *self,const flair::filter::Ahrs *ahrs, const LayoutPosition* position, string name): ahrs(ahrs), output(NULL) { 37 this->self=self; 38 previousStepsAngularRates=new Vector3Df*[10]; 39 for (int i=0; i<10; i++) previousStepsAngularRates[i]=NULL; 40 previousStepsAngularRatesIndex=0; 41 42 GroupBox* reglages_groupbox=new GroupBox(position,name); 43 //TODO: the gyroDelay is set to compensate for the time difference between image snapshot et gyro measurements 44 //it is equal to the time between 2 images (because optical flow always lags 1 frame) + the time to compute optical flow 45 //here it is approximated by 2 video frames 46 gyroDelay=new SpinBox(reglages_groupbox->NewRow(),"gyro delay (in video frames):",0,10,1,2); 47 gyroGain=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"gyro gain:",0.,500.,10.,2,300.); 48 49 50 } 51 52 ~OpticalFlowCompensated_impl() { 53 delete output; 54 } 55 56 void UpdateFrom(const io_data *data){ 57 flair::filter::OpticalFlowData *input=(flair::filter::OpticalFlowData *)data; 58 if (!output) { //first pass 59 output=new flair::filter::OpticalFlowData(self,input->MaxFeatures(),input->ObjectName()+"_filtered"); 60 previousTime=input->DataTime(); 61 return; 62 } 63 // float kX=320/Euler::ToRadian(70); //TEST: only for default simuCameraGL. fov=70° and image width=320 => k=320*180/(70*pi) 64 // float kY=240/Euler::ToRadian(70); //TEST: only for default simuCameraGL. fov=70° and image height=240 65 float kX=gyroGain->Value(); 66 float kY=gyroGain->Value(); 67 float deltaT=(input->DataTime()-previousTime)/(1000*1000*1000.); 68 previousTime=input->DataTime(); 69 70 int delayedIndex=previousStepsAngularRatesIndex-gyroDelay->Value(); // Ahhh décalage, esprit canal... 71 if (delayedIndex<0) delayedIndex+=10; 72 73 if (!previousStepsAngularRates[previousStepsAngularRatesIndex]) { 74 previousStepsAngularRates[previousStepsAngularRatesIndex]=new Vector3Df(); 75 } 76 *previousStepsAngularRates[previousStepsAngularRatesIndex++]=ahrs->GetDatas()->GetAngularRates(); 77 78 if (!previousStepsAngularRates[delayedIndex]) return; 79 float rotationFlowX=previousStepsAngularRates[delayedIndex]->y*deltaT*kY; 80 float rotationFlowY=-previousStepsAngularRates[delayedIndex]->x*deltaT*kX; 81 if (previousStepsAngularRatesIndex==10) previousStepsAngularRatesIndex=0; 82 input->GetMutex(); 83 output->GetMutex(); 84 85 for (int i=0; i<input->NbFeatures(); i++) { 86 output->PointsA()[i].x=input->PointsA()[i].x; 87 output->PointsA()[i].y=input->PointsA()[i].y; 88 output->PointsB()[i].x=input->PointsB()[i].x-rotationFlowX; 89 output->PointsB()[i].y=input->PointsB()[i].y-rotationFlowY; 90 } 91 output->SetNbFeatures(input->NbFeatures()); 92 output->SetFoundFeature(input->FoundFeature()); 93 output->SetFeatureError(input->FeatureError()); 94 95 output->ReleaseMutex(); 96 input->ReleaseMutex(); 97 98 output->SetDataTime(input->DataTime()); 99 100 }; 101 102 flair::filter::OpticalFlowData *output; 103 104 private: 105 flair::filter::OpticalFlowCompensated *self; 106 Time previousTime; 107 const flair::filter::Ahrs *ahrs; 108 Vector3Df **previousStepsAngularRates; 109 unsigned int previousStepsAngularRatesIndex; 110 SpinBox *gyroDelay; 111 DoubleSpinBox *gyroGain; 112 }; 113 114 34 115 namespace flair { 35 116 namespace filter { 36 117 37 OpticalFlowCompensated::OpticalFlowCompensated(const OpticalFlow *parent, const Ahrs *ahrs, const LayoutPosition* position, string name) : IODevice(parent, name), ahrs(ahrs), output(NULL) { 38 MatrixDescriptor* desc=new MatrixDescriptor(3,2); 39 desc->SetElementName(0,0,"raw displacement X"); 40 desc->SetElementName(0,1,"raw displacement Y"); 41 desc->SetElementName(1,0,"compensation X"); 42 desc->SetElementName(1,1,"compensation Y"); 43 desc->SetElementName(2,0,"displacement with compensation X"); 44 desc->SetElementName(2,0,"displacement with compensation Y"); 45 firstPointDisplacement=new Matrix(this,desc,floatType,name); 46 delete desc; 47 previousStepsAngularRates=new Vector3Df*[10]; 48 for (int i=0; i<10; i++) previousStepsAngularRates[i]=NULL; 49 previousStepsAngularRatesIndex=0; 50 51 GroupBox* reglages_groupbox=new GroupBox(position,name); 52 //TODO: the gyroDelay is set to compensate for the time difference between image snapshot et gyro measurements 53 //it is equal to the time between 2 images (because optical flow always lags 1 frame) + the time to compute optical flow 54 //here it is approximated by 2 video frames 55 gyroDelay=new SpinBox(reglages_groupbox->NewRow(),"gyro delay (in video frames):",0,10,1,2); 56 gyroGain=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"gyro gain:",0.,500.,10.,2,300.); 118 OpticalFlowCompensated::OpticalFlowCompensated(const OpticalFlow *parent, const Ahrs *ahrs, const LayoutPosition* position, string name) : IODevice(parent, name) { 119 pimpl_=new OpticalFlowCompensated_impl(this,ahrs,position,name); 57 120 58 121 } 59 122 60 123 OpticalFlowCompensated::~OpticalFlowCompensated() { 61 delete output;124 delete pimpl_; 62 125 } 63 126 64 127 void OpticalFlowCompensated::UpdateFrom(const io_data *data) { 65 OpticalFlowData *input=(OpticalFlowData *)data; 66 if (!output) { //first pass 67 output=new OpticalFlowData(this,input->MaxFeatures(),input->ObjectName()+"_filtered"); 68 previousTime=input->DataTime(); 69 return; 70 } 71 // float kX=320/Euler::ToRadian(70); //TEST: only for default simuCameraGL. fov=70° and image width=320 => k=320*180/(70*pi) 72 // float kY=240/Euler::ToRadian(70); //TEST: only for default simuCameraGL. fov=70° and image height=240 73 float kX=gyroGain->Value(); 74 float kY=gyroGain->Value(); 75 float deltaT=(input->DataTime()-previousTime)/(1000*1000*1000.); 76 previousTime=input->DataTime(); 77 78 int delayedIndex=previousStepsAngularRatesIndex-gyroDelay->Value(); // Ahhh décalage, esprit canal... 79 if (delayedIndex<0) delayedIndex+=10; 80 81 if (!previousStepsAngularRates[previousStepsAngularRatesIndex]) { 82 previousStepsAngularRates[previousStepsAngularRatesIndex]=new Vector3Df(); 83 } 84 *previousStepsAngularRates[previousStepsAngularRatesIndex++]=ahrs->GetDatas()->GetAngularRates(); 85 86 if (!previousStepsAngularRates[delayedIndex]) return; 87 float rotationFlowX=previousStepsAngularRates[delayedIndex]->y*deltaT*kY; 88 float rotationFlowY=-previousStepsAngularRates[delayedIndex]->x*deltaT*kX; 89 if (previousStepsAngularRatesIndex==10) previousStepsAngularRatesIndex=0; 90 input->GetMutex(); 91 output->GetMutex(); 92 93 for (int i=0; i<input->NbFeatures(); i++) { 94 if (!i) { 95 firstPointDisplacement->SetValue(0,0,input->PointsB()[i].x-input->PointsA()[i].x); 96 firstPointDisplacement->SetValue(0,1,input->PointsB()[i].y-input->PointsA()[i].y); 97 firstPointDisplacement->SetValue(1,0,-rotationFlowX); 98 firstPointDisplacement->SetValue(1,1,-rotationFlowY); 99 firstPointDisplacement->SetValue(2,0,input->PointsB()[i].x-input->PointsA()[i].x-rotationFlowX); 100 firstPointDisplacement->SetValue(2,1,input->PointsB()[i].y-input->PointsA()[i].y-rotationFlowY); 101 } 102 output->PointsA()[i].x=input->PointsA()[i].x; 103 output->PointsA()[i].y=input->PointsA()[i].y; 104 output->PointsB()[i].x=input->PointsB()[i].x-rotationFlowX; 105 output->PointsB()[i].y=input->PointsB()[i].y-rotationFlowY; 106 } 107 output->SetNbFeatures(input->NbFeatures()); 108 output->SetFoundFeature(input->FoundFeature()); 109 output->SetFeatureError(input->FeatureError()); 110 111 output->ReleaseMutex(); 112 input->ReleaseMutex(); 113 114 output->SetDataTime(input->DataTime()); 115 ProcessUpdate(output); 128 pimpl_->UpdateFrom(data); 129 ProcessUpdate(pimpl_->output); 116 130 } 117 131 118 Matrix *OpticalFlowCompensated::GetFirstPointDisplacement() const {119 return firstPointDisplacement;120 }121 132 122 133 } // end namespace filter -
branches/sanscv/lib/FlairVisionFilter/src/OpticalFlowCompensated.h
r324 r325 16 16 #include <IODevice.h> 17 17 #include <Object.h> 18 #include <Vector3D.h>19 18 20 19 namespace flair { 21 namespace core {22 class io_data;23 class Matrix;24 }25 20 namespace gui { 26 21 class LayoutPosition; 27 class SpinBox;28 class DoubleSpinBox;29 22 } 30 23 namespace filter { 31 24 class Ahrs; 32 25 class OpticalFlow; 33 class OpticalFlowData;34 26 } 35 27 } 28 29 class OpticalFlowCompensated_impl; 36 30 37 31 namespace flair { namespace filter { … … 61 55 ~OpticalFlowCompensated(); 62 56 63 void UpdateFrom(const core::io_data *data);64 65 core::Matrix *GetFirstPointDisplacement() const;66 57 private: 67 OpticalFlowData *output; 68 core::Time previousTime; 69 const Ahrs *ahrs; 70 core::Matrix *firstPointDisplacement; 71 core::Vector3Df **previousStepsAngularRates; 72 unsigned int previousStepsAngularRatesIndex; 73 gui::SpinBox *gyroDelay; 74 gui::DoubleSpinBox *gyroGain; 58 void UpdateFrom(const core::io_data *data); 59 OpticalFlowCompensated_impl *pimpl_; 60 75 61 }; 76 62 -
branches/sanscv/lib/FlairVisionFilter/src/OpticalFlowSpeed.cpp
r324 r325 26 26 using namespace flair::gui; 27 27 28 class OpticalFlowSpeed_impl { 29 public: 30 OpticalFlowSpeed_impl(flair::filter::OpticalFlowSpeed *self,const LayoutPosition* position,string name):output(0) { 31 this->self=self; 32 MatrixDescriptor* desc=new MatrixDescriptor(2,1); 33 desc->SetElementName(0,0,"vx"); 34 desc->SetElementName(1,0,"vy"); 35 output=new Matrix(self,desc,floatType,name); 36 delete desc; 37 38 self->AddDataToLog(output); 39 40 GroupBox* reglages_groupbox=new GroupBox(position,name); 41 quality=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"optical flow quality:",0.,100.,1.,1,5.); 42 weightedAverage=new CheckBox(reglages_groupbox->LastRowLastCol(),"Weighted average", true); 43 timeMultiplication=new CheckBox(reglages_groupbox->LastRowLastCol(),"Time multiplication", true); 44 45 46 } 47 48 ~OpticalFlowSpeed_impl() { 49 50 } 51 52 void UpdateFrom(const io_data *data){ 53 flair::filter::OpticalFlowData *input=(flair::filter::OpticalFlowData*)data; 54 float deplx,deply; 55 float nb_depl=0; 56 57 deplx=0; 58 deply=0; 59 60 //error is 0 if perfect match and 7x7x255x255 at worst 61 float qualityThreshold=quality->Value()/100.*7*7*255*255; 62 input->GetMutex(); 63 int nbUsedPoints=0; 64 for(int i=0;i<input->NbFeatures();i++) { 65 //if point is found in both images and quality is sufficient 66 if((input->FoundFeature()[i]!=0)&&(input->FeatureError()[i]<qualityThreshold)) { 67 nbUsedPoints++; 68 float qualityFactor=1.0; 69 if (weightedAverage->Value()) { 70 //displacement is weigthed by quality 71 qualityFactor/=(1+input->FeatureError()[i]); 72 } 73 deplx+=(input->PointsB()[i].x-input->PointsA()[i].x)*qualityFactor; 74 deply+=(input->PointsB()[i].y-input->PointsA()[i].y)*qualityFactor; 75 nb_depl+=qualityFactor; 76 } 77 } 78 input->ReleaseMutex(); 79 float deltaT; 80 if (timeMultiplication->Value()) deltaT=(float)(data->DataTime()-output->DataTime())/(1000.*1000.*1000.); 81 else deltaT=1.; 82 output->SetDataTime(data->DataTime()); 83 84 if(nb_depl!=0) { 85 //Printf("Nombre de points=%d/%d (nb_depl=%f,pondération=%d), deltaT=%f\n",nbUsedPoints,input->NbFeatures(),nb_depl,weightedAverage->Value(),deltaT); 86 output->SetValue(0,0,deplx/(nb_depl*deltaT)); 87 output->SetValue(1,0,deply/(nb_depl*deltaT)); 88 } 89 }; 90 91 Matrix *output; 92 93 private: 94 flair::filter::OpticalFlowSpeed *self; 95 DoubleSpinBox *quality; 96 CheckBox *weightedAverage; 97 CheckBox *timeMultiplication; 98 }; 99 28 100 namespace flair { namespace filter { 29 101 30 102 OpticalFlowSpeed::OpticalFlowSpeed(const IODevice* parent, const LayoutPosition* position,string name) : IODevice(parent,name) { 31 MatrixDescriptor* desc=new MatrixDescriptor(2,1); 32 desc->SetElementName(0,0,"vx"); 33 desc->SetElementName(1,0,"vy"); 34 output=new Matrix(this,desc,floatType,name); 35 delete desc; 36 37 AddDataToLog(output); 38 39 GroupBox* reglages_groupbox=new GroupBox(position,name); 40 quality=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"optical flow quality:",0.,100.,1.,1,5.); 41 weightedAverage=new CheckBox(reglages_groupbox->LastRowLastCol(),"Weighted average", true); 42 timeMultiplication=new CheckBox(reglages_groupbox->LastRowLastCol(),"Time multiplication", true); 103 pimpl_=new OpticalFlowSpeed_impl(this,position,name); 43 104 } 44 105 45 OpticalFlowSpeed::~OpticalFlowSpeed(void) { } 106 OpticalFlowSpeed::~OpticalFlowSpeed(void) { 107 delete pimpl_; 108 } 46 109 47 110 void OpticalFlowSpeed::UpdateFrom(const io_data *data) { 48 OpticalFlowData *input=(OpticalFlowData*)data; 49 float deplx,deply; 50 float nb_depl=0; 51 52 deplx=0; 53 deply=0; 54 55 //error is 0 if perfect match and 7x7x255x255 at worst 56 float qualityThreshold=quality->Value()/100.*7*7*255*255; 57 input->GetMutex(); 58 int nbUsedPoints=0; 59 for(int i=0;i<input->NbFeatures();i++) { 60 //if point is found in both images and quality is sufficient 61 if((input->FoundFeature()[i]!=0)&&(input->FeatureError()[i]<qualityThreshold)) { 62 nbUsedPoints++; 63 float qualityFactor=1.0; 64 if (weightedAverage->Value()) { 65 //displacement is weigthed by quality 66 qualityFactor/=(1+input->FeatureError()[i]); 67 } 68 deplx+=(input->PointsB()[i].x-input->PointsA()[i].x)*qualityFactor; 69 deply+=(input->PointsB()[i].y-input->PointsA()[i].y)*qualityFactor; 70 nb_depl+=qualityFactor; 71 } 72 } 73 input->ReleaseMutex(); 74 float deltaT; 75 if (timeMultiplication->Value()) deltaT=(float)(data->DataTime()-output->DataTime())/(1000.*1000.*1000.); 76 else deltaT=1.; 77 output->SetDataTime(data->DataTime()); 78 79 if(nb_depl!=0) { 80 //Printf("Nombre de points=%d/%d (nb_depl=%f,pondération=%d), deltaT=%f\n",nbUsedPoints,input->NbFeatures(),nb_depl,weightedAverage->Value(),deltaT); 81 output->SetValue(0,0,deplx/(nb_depl*deltaT)); 82 output->SetValue(1,0,deply/(nb_depl*deltaT)); 83 } 84 // output->SetDataTime(data->DataTime()); 85 ProcessUpdate(output); 111 pimpl_->UpdateFrom(data); 112 ProcessUpdate(pimpl_->output); 86 113 } 87 114 88 115 float OpticalFlowSpeed::Vx(void) const { 89 return output->Value(0,0);116 return pimpl_->output->Value(0,0); 90 117 } 91 118 92 119 float OpticalFlowSpeed::Vy(void) const { 93 return output->Value(1,0);120 return pimpl_->output->Value(1,0); 94 121 } 95 122 96 123 core::Matrix *OpticalFlowSpeed::Output() const { 97 return output;124 return pimpl_->output; 98 125 } 99 126 } // end namespace filter -
branches/sanscv/lib/FlairVisionFilter/src/OpticalFlowSpeed.h
r324 r325 21 21 namespace gui { 22 22 class LayoutPosition; 23 class SpinBox;24 class DoubleSpinBox;25 class CheckBox;26 23 } 27 24 } 25 26 class OpticalFlowSpeed_impl; 28 27 29 28 namespace flair … … 88 87 */ 89 88 void UpdateFrom(const core::io_data *data); 90 91 core::Matrix *output; 92 gui::DoubleSpinBox *quality; 93 gui::CheckBox *weightedAverage; 94 gui::CheckBox *timeMultiplication; 89 90 OpticalFlowSpeed_impl *pimpl_; 95 91 }; 96 92 -
branches/sanscv/lib/FlairVisionFilter/src/Sobel.cpp
r324 r325 13 13 14 14 #include "Sobel.h" 15 #include "VisionFilter.h" 15 16 #include <Image.h> 16 17 #include <Layout.h> 17 18 #include <GroupBox.h> 18 19 #include <SpinBox.h> 20 //#include <dspcv_gpp.h> 19 21 #include <typeinfo> 20 22 … … 23 25 using namespace flair::gui; 24 26 27 class Sobel_impl { 28 public: 29 Sobel_impl(flair::filter::Sobel *self,const LayoutPosition* position,string name):output(0) { 30 this->self=self; 31 32 GroupBox* reglages_groupbox=new GroupBox(position,name); 33 dx=new SpinBox(reglages_groupbox->NewRow(),"dx:",0,1,1,1); 34 dy=new SpinBox(reglages_groupbox->NewRow(),"dy:",0,1,1,1); 35 36 Printf("TODO: IODevice doit faire un check de GetInputDataType et GetOutputDataType\n"); 37 //Image devrait accepter un type dans son constructeur pour construire un type identique 38 try{ 39 Image::Type const &imageType=dynamic_cast<Image::Type const &>(((IODevice*)(self->Parent()))->GetOutputDataType()); 40 if(imageType.GetFormat()==Image::Type::Format::Gray) { 41 output=new Image(self,imageType.GetWidth(),imageType.GetHeight(),imageType.GetFormat(),"sobel"); 42 //inIplImage = (IplImage*)AllocFunction(sizeof(IplImage)); 43 //outIplImage = (IplImage*)AllocFunction(sizeof(IplImage)); 44 } else { 45 self->Err("input image is not gray\n"); 46 } 47 48 } catch(std::bad_cast& bc) { 49 self->Err("io type mismatch\n"); 50 } 51 } 52 53 ~Sobel_impl() { 54 //FreeFunction((char*)(inIplImage)); 55 //FreeFunction((char*)(outIplImage)); 56 } 57 58 void UpdateFrom(const io_data *data){ 59 Image *image=(Image*)data; 60 /* 61 data->GetMutex(); 62 inIplImage->width=image->GetDataType().GetWidth(); 63 inIplImage->height=image->GetDataType().GetHeight(); 64 inIplImage->imageData=image->buffer; 65 66 output->GetMutex(); 67 outIplImage->width=output->GetDataType().GetWidth(); 68 outIplImage->height=output->GetDataType().GetHeight(); 69 outIplImage->imageData=output->buffer; 70 71 dspSobel(inIplImage,outIplImage,dx->Value(),dy->Value()); 72 output->ReleaseMutex(); 73 data->ReleaseMutex(); 74 */ 75 output->SetDataTime(data->DataTime()); 76 77 }; 78 79 Image *output; 80 81 private: 82 flair::filter::Sobel *self; 83 SpinBox *dx; 84 SpinBox *dy; 85 //IplImage *inIplImage,*outIplImage; 86 }; 87 25 88 namespace flair { namespace filter { 26 89 27 Sobel::Sobel(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name),output(0) { 28 GroupBox* reglages_groupbox=new GroupBox(position,name); 29 dx=new SpinBox(reglages_groupbox->NewRow(),"dx:",0,1,1,1); 30 dy=new SpinBox(reglages_groupbox->NewRow(),"dy:",0,1,1,1); 31 32 Printf("TODO: IODevice doit faire un check de GetInputDataType et GetOutputDataType\n"); 33 //Image devrait accepter un type dans son constructeur pour construire un type identique 34 try{ 35 Image::Type const &imageType=dynamic_cast<Image::Type const &>(parent->GetOutputDataType()); 36 if(imageType.GetFormat()==Image::Type::Format::Gray) { 37 output=new Image(this,imageType.GetWidth(),imageType.GetHeight(),imageType.GetFormat(),"sobel"); 38 } else { 39 Err("input image is not gray\n"); 40 return; 41 } 42 43 } catch(std::bad_cast& bc) { 44 Err("io type mismatch\n"); 45 return; 46 } 47 SetIsReady(true); 90 Sobel::Sobel(const IODevice* parent,const LayoutPosition* position,string name) : IODevice(parent,name) { 91 pimpl_=new Sobel_impl(this,position,name); 48 92 } 49 93 50 94 Sobel::~Sobel(void) { 95 delete pimpl_; 51 96 } 52 97 53 98 Image* Sobel::Output(void) { 54 return output;99 return pimpl_->output; 55 100 } 56 101 57 void Sobel::UpdateFrom(const io_data *data) {/* 58 Image *cvImage=(Image*)data; 59 IplImage *gimg=cvImage->img; 60 61 data->GetMutex(); 62 output->GetMutex(); 63 dspSobel(gimg,output->img,dx->Value(),dy->Value()); 64 output->ReleaseMutex(); 65 data->ReleaseMutex(); 66 */ 67 output->SetDataTime(data->DataTime()); 68 ProcessUpdate(output); 102 void Sobel::UpdateFrom(const io_data *data) { 103 pimpl_->UpdateFrom(data); 104 ProcessUpdate(pimpl_->output); 69 105 } 70 106 71 107 DataType const &Sobel::GetOutputDataType() const { 72 if( output!=NULL) {73 return output->GetDataType();108 if(pimpl_->output!=NULL) { 109 return pimpl_->output->GetDataType(); 74 110 } else { 75 111 return dummyType; -
branches/sanscv/lib/FlairVisionFilter/src/Sobel.h
r324 r325 12 12 13 13 #include <IODevice.h> 14 #include <cvtypes.h> 14 15 15 16 16 namespace flair { … … 20 20 namespace gui { 21 21 class LayoutPosition; 22 class SpinBox;23 22 } 24 23 } 24 25 class Sobel_impl; 25 26 26 27 namespace flair { namespace filter { … … 64 65 private: 65 66 void UpdateFrom(const core::io_data *data); 66 core::Image *output; 67 gui::SpinBox *dx; 68 gui::SpinBox *dy; 67 Sobel_impl *pimpl_; 69 68 }; 70 69 } // end namespace filter -
branches/sanscv/lib/FlairVisionFilter/src/VisionFilter.cpp
r324 r325 8 8 } 9 9 10 void saveToJpeg( IplImage* src_img,std::string filename,PictureFormat_t input_format,PictureFormat_t output_format,unsigned char compression_level) {10 void saveToJpeg(flair::core::Image* src_img,std::string filename,PictureFormat_t input_format,PictureFormat_t output_format,unsigned char compression_level) { 11 11 printf("saveToJpeg todo\n"); 12 12 //if(!cvSaveImage(filename.c_str(),src_img)) printf("Could not save.\n"); -
branches/sanscv/lib/FlairVisionFilter/src/VisionFilter.h
r324 r325 1 1 #include <string> 2 #include <cvtypes.h> 2 3 namespace flair { 4 namespace core { 5 class Image; 6 } 7 } 8 3 9 4 10 /*! … … 19 25 } ; 20 26 21 void saveToJpeg( IplImage* src_img,std::string filename,PictureFormat_t input_format,PictureFormat_t output_format,unsigned char compression_level=95);27 void saveToJpeg(flair::core::Image* src_img,std::string filename,PictureFormat_t input_format,PictureFormat_t output_format,unsigned char compression_level=95);
Note:
See TracChangeset
for help on using the changeset viewer.