Exemple #1
0
// ------------------------------------------------------------------------------------------------------------
// Se hace un paso del EKF con prediccion, matching, ransac y update.
// ------------------------------------------------------------------------------------------------------------
void EKF::step(const cv::Mat &image)
{
    _ekfSteps++;

    if (_logFile.is_open())
    {
        _logFile << std::endl << std::endl;
        _logFile << "~~~~~~~~~~~~ STEP " << _ekfSteps << " ~~~~~~~~~~~~" << std::endl;
    }

    //-------------------------------------------------------------------------------------------------------------
    // Prediccion

    Timer timer;

    if (!_strOutputPath.empty())
    {
        std::stringstream frameName;
        frameName << "Frame " << _ekfSteps;

        _outputFileStorage << frameName.str() << "{";

        cvWriteComment(_outputFileStorage.fs, "", 0);
        cvWriteComment(_outputFileStorage.fs, "Running time (microseconds)", 0);
        cvWriteComment(_outputFileStorage.fs, "", 0);
        timer.start();
    }

    VectorImageFeaturePrediction predictedDistortedFeatures;
    VectorMatd predictedFeatureJacobians;

	SYSTEMTIME sys; 
	GetLocalTime( &sys ); 
	int MileTs = sys.wSecond;int MileT = sys.wMilliseconds;

    stateAndCovariancePrediction(state, stateCovarianceMatrix);

	GetLocalTime( &sys ); 
	int MileT2s = sys.wSecond;int MileT2 = sys.wMilliseconds;
	int  DetaT = MileT2  - MileT;
	int DetaTs = MileT2s -MileTs;
	//std::cout<< std::endl;std::cout<< "The CovariancePrediction time is :"<< DetaTs<<"S......................."<< std::endl;
	//std::cout<< "CovariancePrediction time is :"<< DetaT<<"mS......................."<< std::endl;    std::cout<< std::endl;

    // se le pasa mapFeatureIndexes vacio para que sepa que la prediccion es para todos los features en el mapa
    std::vector<int> mapFeatureIndexes;
    VectorMapFeature unseenFeatures;

    predictCameraMeasurements( state,
                               stateCovarianceMatrix,
                               state.mapFeatures,
                               mapFeatureIndexes,
                               predictedDistortedFeatures,
                               predictedFeatureJacobians,
                               unseenFeatures );

	GetLocalTime( &sys ); 
	MileT2s = sys.wSecond;MileT2 = sys.wMilliseconds;
	DetaT = MileT2  - MileT;
	DetaTs = MileT2s -MileTs;
	//std::cout<< std::endl;std::cout<< "The predictCamera time is :"<< DetaTs<<"S......................."<< std::endl;
	//std::cout<< "predictCamera time is :"<< DetaT<<"mS......................."<< std::endl;    std::cout<< std::endl;
	
    if (!_strOutputPath.empty())
    {
        double predictionTime = timer.getElapsedTimeInMicroSec();
        timer.stop();

        _outputFileStorage << "Prediction" << predictionTime;
    }

    if (!_strOutputPath.empty())
    {
        cv::Mat predictionImage;
        drawPrediction(image, predictedDistortedFeatures, state.mapFeatures, predictionImage);
		//Test!
		cv::imshow("Predicciones", predictionImage);
		cv::waitKey(2);

        //std::stringstream imageFileName;
        //imageFileName << _strOutputPath << std::setfill('0') << std::setw(5) << _ekfSteps << ".png";
        //cv::imwrite(imageFileName.str().c_str(), predictionImage);

#ifndef ANDROID
       // _outputVideoWriter.write(predictionImage);
#endif

#ifndef DEBUG_SHOW_IMAGES
    }
#else
        // Mostramos la imagen
        cv::namedWindow("Predicciones");
        cv::imshow("Predicciones", predictionImage);
        cv::waitKey(2);
    }
Exemple #2
0
std::list<Garbage*> 
GarbageRecognition::getGarbageList(IplImage * src)
{
		//~ IplImage * model = cvLoadImage("./colilla-sinBlanco.png",1);
		IplImage * copy;
		//windowing
		if(this->focused){
			if(window->currentGarbage->state!=DEAD){
				if(src_window!=NULL){
					if(window->last_last_window!=NULL)
						//cvReleaseImageHeader(&(window->last_last_window));
				
					//logic to release sub-windowing memory
					if(window->release_window){
						window->last_last_window=window->last_window;
						window->last_window=src_window;
					}
				
				}
				
				src_window=this->window->getWindow(src);
				if(src_window==NULL){
					this->focused=false;
					delete window;
					src_window=src;
				}
				window->release_window=true;
			}
			else{
				this->focused=false;
				delete window;
				src_window=src;
			}
		}else{
			src_window=src;
		}
		
		//get garbages from vision system
		garbages = this->garbageList(src_window,NULL);
		
		if(this->focused){
			garbages=this->window->correctGarbages(garbages);
			#ifdef BENCHMARK_H
				focusedFrames++;
			#endif
		}
		
		//Feed retrieved garbages to prediction system
		if(this->doPrediction)
			garbagePrediction= this->prediction->getPrediction(garbages);
		else
			garbagePrediction=garbages;
		
		//start  windowing
		if(this->doWindowing && (this->frameNumber % NUMBER_OF_FRAMES_TO_FOCUS) && this->focused==false ){
			
			GarbageHistoric * focusedGarbage=prediction->focusGarbage();
			if(focusedGarbage!=NULL){
				this->window=new Windowing(focusedGarbage,cvGetSize(src));
				this->focused=true;
				window->release_window=false;
			}
		}
		
		drawPrediction(src,garbagePrediction);
		
		//~ cvReleaseImage(&model);
		this->frameNumber++;
		
	//return garbages;
	return garbagePrediction;
}