Changeset 338 in flair-src for trunk/lib/FlairVisionFilter/src/OpticalFlowCompensated.cpp
- Timestamp:
- Oct 17, 2019, 2:49:35 PM (5 years ago)
- Location:
- trunk
- Files:
-
- 2 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk
-
Property svn:mergeinfo
set to
/branches/sanscv merged eligible
-
Property svn:mergeinfo
set to
-
trunk/lib/FlairVisionFilter/src/OpticalFlowCompensated.cpp
r318 r338 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
Note:
See TracChangeset
for help on using the changeset viewer.