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
}
示例#2
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 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
}