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
}
示例#3
0
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;
}