void addFeaturesToStateAndCovariance( const VectorImageFeatureMeasurement &imageFeatureMeasurement, State &state, Matd &stateCovarianceMatrix ) { size_t imageFeatureMeasurementSize = imageFeatureMeasurement.size(); for (uint i = 0; i < imageFeatureMeasurementSize; ++i) { addFeatureToStateAndCovariance(imageFeatureMeasurement[i], state, stateCovarianceMatrix); } #ifdef DEBUG std::cout << "Se agregaron " << imageFeatureMeasurementSize << " nuevos features al mapa." << std::endl; #endif }
void detectNewImageFeatures( const cv::Mat image, const VectorImageFeaturePrediction &featuresPrediction, uint newImageFeaturesMaxSize, VectorImageFeatureMeasurement &newImageFeatures ) { newImageFeatures.clear(); // Calculamos el tamaño de las zonas de la imágen según los parámetros configurados ConfigurationManager& configManager = ConfigurationManager::getInstance(); //int zonesInARow = exp2f(configManager.ekfParams->detectNewFeaturesImageAreasDivideTimes); int zonesInARow = 1; int zoneWidth = image.cols / zonesInARow; int zoneHeight = image.rows / zonesInARow; // Construimos la mascara para buscar features solamente en zonas poco densas cv::Mat imageMask( cv::Mat::ones(image.rows, image.cols, CV_8UC1) * 255 ); buildImageMask( featuresPrediction, imageMask ); // Detectamos features std::vector<cv::KeyPoint> imageKeypoints; configManager.featureDetector->detect(image, imageKeypoints, imageMask); // Extraemos descriptores cv::Mat descriptors; configManager.descriptorExtractor->compute(image, imageKeypoints, descriptors); // Caso particular: la cantidad de features encontrados no supera los pedidos size_t imageKeypointsSize = imageKeypoints.size(); #ifdef DEBUG std::cout << "Cantidad de features detectados al agregar nuevos: " << imageKeypointsSize << std::endl; #endif if (imageKeypointsSize <= newImageFeaturesMaxSize) { double imagePos[2]; for (int i = 0; i < imageKeypointsSize; ++i) { cv::KeyPoint &currKeypoint = imageKeypoints[i]; imagePos[0] = currKeypoint.pt.x; imagePos[1] = currKeypoint.pt.y; newImageFeatures.push_back( new ImageFeatureMeasurement( imagePos, descriptors.row(i) ) ); } } else { // Buscamos nuevos features intentando que esten // lo mejor distribuidos posible en la imagen searchFeaturesByZone( featuresPrediction, imageKeypoints, descriptors, zonesInARow, zoneWidth, zoneHeight, imageMask, newImageFeaturesMaxSize, newImageFeatures ); } #ifdef DEBUG_SHOW_NEW_FEATURES cv::Mat imageCopy; image.copyTo(imageCopy); for (int i = 1; i < zonesInARow; ++i) { cv::line(imageCopy, cv::Point(i * zoneWidth, 0), cv::Point(i * zoneWidth, imageCopy.rows), cv::Scalar(0, 255, 0)); cv::line(imageCopy, cv::Point(0, i * zoneHeight), cv::Point(imageCopy.cols, i * zoneHeight), cv::Scalar(0, 255, 0)); } int featuresPredictionSize = featuresPrediction.size(); for (int i = 0; i < featuresPredictionSize; ++i) { ImageFeaturePrediction *currFeaturePrediction = featuresPrediction[i]; drawUncertaintyEllipse2D( imageCopy, cv::Point2f(currFeaturePrediction->imagePos[0], currFeaturePrediction->imagePos[1]), currFeaturePrediction->covarianceMatrix, 2 * (image.cols + image.rows), cv::Scalar(0, 255, 0), false ); } cv::namedWindow("Busqueda de nuevos features: mascara"); cv::imshow("Busqueda de nuevos features: mascara", imageMask); cv::waitKey(0); for (int i = 0; i < newImageFeatures.size(); ++i) { drawPoint(imageCopy, newImageFeatures[i]->imagePos, cv::Scalar(0, 255, 255)); } cv::namedWindow("Busqueda de nuevos features: imagen con nuevos features"); cv::imshow("Busqueda de nuevos features: imagen con nuevos features", imageCopy); cv::waitKey(0); // Se borran todas las ventanas creadas cv::destroyWindow("Busqueda de nuevos features: mascara"); cv::destroyWindow("Busqueda de nuevos features: imagen con nuevos features"); #endif }
void EKF::init(const cv::Mat &image) { if (_logFile.is_open()) { time_t seed = time(NULL); srand(static_cast<uint>(seed)); _logFile << "Random Seed: " << seed << std::endl << std::endl; _logFile << "~~~~~~~~~~~~ STEP " << _ekfSteps << " ~~~~~~~~~~~~" << std::endl; } ExtendedKalmanFilterParameters *ekfParams = ConfigurationManager::getInstance().ekfParams; initState(state); state.mapFeatures.reserve(ekfParams->reserveFeaturesDepth); state.mapFeaturesDepth.reserve(ekfParams->reserveFeaturesDepth); state.mapFeaturesInvDepth.reserve(ekfParams->reserveFeaturesInvDepth); initCovariance(stateCovarianceMatrix); // Detectar features en la imagen VectorFeatureMatch noMatches; // Al principio no tiene nada ya que no hay matches VectorImageFeaturePrediction noPredictions; VectorImageFeatureMeasurement newFeatureMeasurements; detectNewImageFeatures(image, noPredictions, ekfParams->minMatchesPerImage, newFeatureMeasurements); #ifdef DEBUG_SHOW_IMAGES cv::Mat imageWithKeypoints; image.copyTo(imageWithKeypoints); for (uint i = 0; i < newFeatureMeasurements.size(); ++i) { drawPoint(imageWithKeypoints, newFeatureMeasurements[i]->imagePos, cv::Scalar(0, 0, 255)); } std::cout << std::endl; std::string windowName = "Features detectados en la primer imagen ("; std::stringstream convert; convert << newFeatureMeasurements.size(); windowName += convert.str(); windowName += ")"; cv::namedWindow(windowName); cv::imshow(windowName, imageWithKeypoints); cv::waitKey(0); cv::destroyWindow(windowName); #endif // Agregar los features nuevos al estado addFeaturesToStateAndCovariance(newFeatureMeasurements, state, stateCovarianceMatrix); size_t newFeatureMeasurementsSize = newFeatureMeasurements.size(); for (uint i = 0; i < newFeatureMeasurementsSize; ++i) { delete newFeatureMeasurements[i]; } if (_logFile.is_open()) { state.showDetailed(_logFile); } }
void searchFeaturesByZone( const VectorImageFeaturePrediction &featuresPrediction, std::vector<cv::KeyPoint> &imageKeypoints, cv::Mat &descriptors, int zonesInARow, int zoneWidth, int zoneHeight, cv::Mat& imageMask, int imageFeaturesMaxSize, VectorImageFeatureMeasurement &newImageFeatures ) { int zonesCount = zonesInARow * zonesInARow; // Inicializamos el ZoneInfo para cada zona. ZoneInfo **zonesInfo = new ZoneInfo *[zonesCount]; for (int i = 0; i < zonesCount; ++i) { ZoneInfo *currZoneInfo = new ZoneInfo; currZoneInfo->candidateImageFeatureMeasurementLeft = 0; currZoneInfo->predictionsPlusFeaturesCount = 0; currZoneInfo->zoneId = i; zonesInfo[i] = currZoneInfo; } groupImageFeaturesAndPredictionsByZone( featuresPrediction, imageKeypoints, descriptors, zoneWidth, zoneHeight, imageMask.cols, imageMask.rows, zonesCount, zonesInfo ); // Ordenamos el arreglo de ZoneInfo por su cantidad de predicciones tal que su media cae dentro de la zona qsort(zonesInfo, zonesCount, sizeof(ZoneInfo *), &sortCompare); // Convertimos el arreglo de ZoneInfo a lista para lograr mayor performance en la eliminacion std::list< ZoneInfo * > zoneInfoList; for (int i = 0; i < zonesCount; ++i) { zoneInfoList.push_back( zonesInfo[i] ); } // Agregamos los features al resultado newImageFeatures.reserve(imageFeaturesMaxSize); double ellipseSize = ConfigurationManager::getInstance().ekfParams->detectNewFeaturesImageMaskEllipseSize; Matd newFeatMeasToAddEllipse( (Matd(2, 2) << ellipseSize, 0.0L, 0.0L, ellipseSize) ); int zonesLeft = zonesCount; while(zonesLeft > 0 && imageFeaturesMaxSize > 0) { ZoneInfo *currZoneInfo = zoneInfoList.front(); int &currZoneFeaturesLeft = currZoneInfo->candidateImageFeatureMeasurementLeft; // Si no hay mas features en la zona, la quitamos de la lista if (currZoneFeaturesLeft == 0) { zoneInfoList.pop_front(); #ifdef DEBUG_SHOW_NEW_FEATURES std::cout << "Se elimino la zona " << currZoneInfo->zoneId << " por no tener más features candidatos restantes." << std::endl; std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl << std::endl; #endif zonesLeft--; } else { // Tomamos un feature de forma aleatoria int randKeypointIdx = rand() % currZoneFeaturesLeft; ImageFeatureMeasurement *imFeatMeasToAdd = currZoneInfo->candidateImageFeatureMeasurement[randKeypointIdx]; int imFeatMeasToAddX = static_cast<int>(imFeatMeasToAdd->imagePos[0]); int imFeatMeasToAddY = static_cast<int>(imFeatMeasToAdd->imagePos[1]); if ( imageMask.at<uchar>(imFeatMeasToAddY, imFeatMeasToAddX) ) { // Agregamos el feature al resultado y utilizamos el constructor por copia // Aca si que copiamos el descriptor, para poder devolverlo newImageFeatures.push_back( new ImageFeatureMeasurement(*imFeatMeasToAdd) ); currZoneInfo->predictionsPlusFeaturesCount++; #ifdef DEBUG_SHOW_NEW_FEATURES std::cout << "Agregamos feature en la zona " << currZoneInfo->zoneId << std::endl; std::cout << "Hay ahora " << currZoneInfo->predictionsPlusFeaturesCount << " predicciones y features" << std::endl; std::cout << "Quedan " << currZoneInfo->candidateImageFeatureMeasurementLeft << " feature candidatos por agregar" << std::endl; std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl << std::endl; #endif // Reordeno la lista std::list< ZoneInfo * >::iterator itCurr = zoneInfoList.begin(); std::list< ZoneInfo * >::iterator itNext = zoneInfoList.begin(); std::list< ZoneInfo * >::iterator itEnd = zoneInfoList.end(); itNext++; // Seguimos recorriendo while(itNext != itEnd) { ZoneInfo *nextZoneInfo = static_cast<ZoneInfo *>(*itNext); // Si la cantidad de features y predicciones de la zona actual // supera o iguala a la cantidad de features y predicciones de la siguiente zona en la lista, // cambio el orden de la zona actual por la siguiente, para asegurar el orden total // de las zonas con respecto a la cantidad de features if (currZoneInfo->predictionsPlusFeaturesCount >= nextZoneInfo->predictionsPlusFeaturesCount) { (*itNext) = currZoneInfo; (*itCurr) = nextZoneInfo; } else { break; } itCurr++; itNext++; } // Actualizamos la mascara drawUncertaintyEllipse2D( imageMask, cv::Point2d(imFeatMeasToAdd->imagePos[0], imFeatMeasToAdd->imagePos[1]), newFeatMeasToAddEllipse, 2 * (imageMask.cols + imageMask.rows), cv::Scalar(0, 0, 0), true ); // Actualizamos la cantidad de features buscada imageFeaturesMaxSize--; } // Nos aseguramos que el mismo feature no sea reelecto currZoneFeaturesLeft--; currZoneInfo->candidateImageFeatureMeasurement[randKeypointIdx] = currZoneInfo->candidateImageFeatureMeasurement[currZoneFeaturesLeft]; delete imFeatMeasToAdd; } } // Liberamos toda la memoria auxiliar utilizada for (int i = 0; i < zonesCount; ++i) { ZoneInfo *currZoneInfo = zonesInfo[i]; for (int j = 0; j < currZoneInfo->candidateImageFeatureMeasurementLeft; ++j) { delete currZoneInfo->candidateImageFeatureMeasurement[j]; } delete currZoneInfo; } delete [] zonesInfo; }