// ------------------------------------------------------------------------------------------------------------
// Agrupa un conjunto de features y predicciones por zona, y devuelve el zoneInfo para cada zona.
void groupImageFeaturesAndPredictionsByZone( const VectorImageFeaturePrediction &featuresPrediction,
                                             std::vector<cv::KeyPoint> &imageKeypoints,
                                             cv::Mat &keypointsDescriptors,
                                             int zoneWidth, int zoneHeight,
                                             int totalWidth, int totalHeight,
                                             int zonesCount, ZoneInfo **zonesInfo )
{
    size_t imageKeypointsSize = imageKeypoints.size();
    for (uint i = 0; i < imageKeypointsSize; ++i)
    {
        cv::KeyPoint &currKeypoint = imageKeypoints[i];

        double imagePos[2] = {currKeypoint.pt.x, currKeypoint.pt.y};
        ImageFeatureMeasurement *imageFeature = new ImageFeatureMeasurement(imagePos, keypointsDescriptors.row(i));

        int currFeatureZoneId = getPointZone(imageFeature->imagePos, zoneWidth, zoneHeight, totalWidth, totalHeight);

        ZoneInfo *currZoneInfo = zonesInfo[currFeatureZoneId];
        currZoneInfo->candidateImageFeatureMeasurement.push_back(imageFeature);
        currZoneInfo->candidateImageFeatureMeasurementLeft++;
    }

    size_t featuresPredictionSize = featuresPrediction.size();
    for (uint i = 0; i < featuresPredictionSize; ++i)
    {
        int currPredZoneId = getPointZone(featuresPrediction[i]->imagePos, zoneWidth, zoneHeight, totalWidth, totalHeight);

        ZoneInfo *currZoneInfo = zonesInfo[currPredZoneId];
        currZoneInfo->featuresPrediction.push_back(featuresPrediction[i]);
        currZoneInfo->predictionsPlusFeaturesCount++;
    }
}
void buildImageMask( const VectorImageFeaturePrediction &inlierPredictions,
                     cv::Mat &imageMask )
{
    // Filtramos las zonas alrededor de cada prediccion
    size_t inlierPredictionsSize = inlierPredictions.size();

    cv::Scalar colorBlack(0, 0, 0);

    // Para cada prediccion, se dibuja dentro de la mascara su elipse de incertidumbre
    // De esta forma se previene el hecho de agregar nuevos features dentro de
    // elipses de busqueda de features anteriores
    for (int i = 0; i < inlierPredictionsSize; ++i)
    {
        ImageFeaturePrediction *inlierPrediction = inlierPredictions[i];

        drawUncertaintyEllipse2D( imageMask,
                                  cv::Point2d(inlierPrediction->imagePos[0], inlierPrediction->imagePos[1]),
                                  inlierPrediction->covarianceMatrix,
                                  2 * (imageMask.rows + imageMask.cols),
                                  colorBlack,
                                  true );
    }
}
Example #3
0
// --------------------------------------------------------------------------------------------------------------------
// Devuelve los indices (posiciones del arreglo de matches) de los matches que estan por debajo de un threshold.
// Se devuelven estos indices, para aprovechar que los matches, las predicciones y los jacobianos estan en arreglos
// con el mismo orden
// --------------------------------------------------------------------------------------------------------------------
void matchesBelowAThreshold(const VectorFeatureMatch &matches,
                            const VectorImageFeaturePrediction &predictedImageFeatures,
                            double threshold,
                            std::vector<int> &supportIndexesInMatchesVector)
{
    // Para cada feature predicho se busca su match y se calcula la distancia
    for (uint i = 0; i < predictedImageFeatures.size(); ++i)
    {
        ImageFeaturePrediction *prediction = predictedImageFeatures[i];

        bool wasFound = false;
        int matchIndex = 0;
        while (!wasFound && matchIndex < matches.size())
        {
            FeatureMatch *match = matches[matchIndex];

            // Se calcula la distancia y se cuenta si es buen feature.
            if (match->featureIndex == prediction->featureIndex)
            {
                double xDist = match->imagePos[0] - prediction->imagePos[0];
                double yDist = match->imagePos[1] - prediction->imagePos[1];
                double dist = sqrt(xDist*xDist + yDist*yDist);

                if (dist < threshold)
                {
                    // Si es un buen match se guarda el indice en el arreglo de matches para luego
                    // obtener la prediccion y los jacobianos asociados.(que se encuentran en el mismo orden que el arreglo matches)
                    supportIndexesInMatchesVector.push_back(matchIndex);
                }

                wasFound = true;
            }

            matchIndex++;
        }
    }
}
Example #4
0
void updateMapFeatures(const VectorImageFeaturePrediction &predictedDistortedFeatures, const VectorFeatureMatch& inlierMatches, State &state)
#endif
{
    size_t inlierMatchesSize = inlierMatches.size();
    size_t predictedDistortedFeaturesSize = predictedDistortedFeatures.size();
    
    for (int i = 0; i < predictedDistortedFeaturesSize; ++i)
    {
        int featureIndex = predictedDistortedFeatures[i]->featureIndex;
        MapFeature *currMapFeature = state.mapFeatures[featureIndex];
        currMapFeature->timesPredicted++;
    }
    
    for (int i = 0; i < inlierMatchesSize; ++i)
    {
        int featureIndex = inlierMatches[i]->featureIndex;
        MapFeature *currMapFeature = state.mapFeatures[featureIndex];
        currMapFeature->timesMatched++;
    }
#if defined(DEBUG_SHOW_IMAGES)
    // Asigno los valores correctos para la posicion en
    // la imagen de la ultima vez que se vieron los features
    cleanFeaturesLastImagePos(state.mapFeatures);
    updateFeaturesLastImagePos(state.mapFeatures, inlierMatches);
#endif
    
    // Actualizamos los descriptores de los inliers
    for (int i = 0; i < inlierMatchesSize; i++)
    {
        FeatureMatch *currFeatureMatch = inlierMatches[i];
        MapFeature *currMapFeature = state.mapFeatures[currFeatureMatch->featureIndex];
        
        assert(currMapFeature->descriptor.cols == currFeatureMatch->imagePosDescriptor.cols);
        
        currFeatureMatch->imagePosDescriptor.copyTo(currMapFeature->descriptor);
    }    
}
Example #5
0
// --------------------------------------------------------------------------------------------------------------------
// Realiza el ciclo de ransac, donde se obtienen los matches considerados inliers.
// Esta informacion se devuelve, para por fuera, rescatar outliers y poder hacer una mejor estimacion
// En inlierJacobianIndexes se guardan los indices(posicion en el arreglo) de las predicciones,
// ya que se corresponden con la posicion del los jacobianos asociados y
// que son necesarios para hacer el update.
// --------------------------------------------------------------------------------------------------------------------
void ransac( const State &state, Matd &covariance,
             const VectorImageFeaturePrediction &predictedMatchedFeatures,
             const VectorMatd &predictedMatchedJacobians,
             const VectorFeatureMatch &matches,
             VectorFeatureMatch &inlierMatches,
             VectorImageFeaturePrediction &inlierPredictions,
             VectorMatd &inlierJacobians,
             VectorFeatureMatch &outlierMatches )
{
    size_t matchesSize = matches.size();
    if (matches.size() == 0)
    {
        return;
    }

    uint numberOfHipotesis = 1000;

    ExtendedKalmanFilterParameters *ekfParams = ConfigurationManager::getInstance().ekfParams;

    // threshold para buscar los matches 2 * sigma_pixels
    double threshold = ekfParams->ransacThresholdPredictDistance;

    std::vector<int> inlierIndexesInMatchesVector;

    for (uint i = 0; i < numberOfHipotesis && i < matchesSize; ++i)
    {
        // seleccionar matches random
        FeatureMatch *match = NULL;
        selectRandomMatch(matches, i, match);

        // Se toma la prediccion correspondiente el matching
        ImageFeaturePrediction *predictedMatchedFeature = predictedMatchedFeatures[i];

        // Se genera un arreglo para el feature predicho.
        VectorImageFeaturePrediction prediction;
        prediction.push_back(predictedMatchedFeature);

        VectorFeatureMatch matching;
        matching.push_back(match);

        // Se buscan los 2 jacobianos del feature
        VectorMatd jacobians;
        jacobians.push_back(predictedMatchedJacobians[i]);

        // Se hace un update parcial (solo para el estado)
        // se copia el estado actual
        State temporalState(state);

        updateOnlyState(prediction, matching, jacobians, temporalState, covariance);

        // Se predicen TODAS las mediciones con el estado actualizado
        VectorImageFeaturePrediction predictedImageFeatures;
        VectorMapFeature unseenFeatures;
        std::vector<int> mapFeatureIndexes;
        // se pasa mapFeatureIndexes vacio para que sepa que son para todos los features.
        predictMeasurementState(temporalState, temporalState.mapFeatures, mapFeatureIndexes, predictedImageFeatures, unseenFeatures);

        // Encontrar matches debajo de un threshold
        std::vector<int> supportIndexesInMatchesVector;

        matchesBelowAThreshold(matches, predictedImageFeatures, threshold, supportIndexesInMatchesVector);

        // Si la cantidad de matches encontrado es mayor que la que se tiene actualmente, actualizar.
        if (supportIndexesInMatchesVector.size() > inlierIndexesInMatchesVector.size())
        {
#ifdef DEBUG_SHOW_RANSAC_INFO
            std::string windowName = "Inliers en el ciclo de ransac, prediccion hecha en base a un solo match";
            showRansacInliers(matches, predictedImageFeatures, supportIndexesInMatchesVector, match, windowName);
#endif
            // Actualizo el conjunto de inliers
            inlierIndexesInMatchesVector = supportIndexesInMatchesVector;

            // Se actualiza la cantidad de iteraciones
            // 1 - w
            double e = 1.0L - (double)inlierIndexesInMatchesVector.size()/(double)matches.size();

            numberOfHipotesis = static_cast<int>( log(1.0L - ekfParams->ransacAllInliersProbability)/log(1.0L - (1.0L - e)) );
        }
        
        // Libero la memoria auxiliar
        size_t predictedImageFeaturesSize = predictedImageFeatures.size();
        for (uint j = 0; j < predictedImageFeaturesSize; ++j)
        {
            delete predictedImageFeatures[j];
        }
    }

    // Se devuelven los matches, predicciones y jacobianos de los indices (del vector matches) inliers
    inlierMatches.clear();
    inlierPredictions.clear();
    inlierJacobians.clear();
    outlierMatches.clear();

    // Se reserva lugar para que los push_back sean en O(1)
    inlierMatches.reserve(inlierIndexesInMatchesVector.size());
    inlierPredictions.reserve(inlierIndexesInMatchesVector.size());
    inlierJacobians.reserve(inlierIndexesInMatchesVector.size());
    outlierMatches.reserve(matches.size() - inlierIndexesInMatchesVector.size());

    // Se arma una mascara para identificar los lugares de matches donde estan los inliers
    std::vector<bool> inlierMatchesMask(matches.size(), false);

    // FIXME: Esto se puede hacer sin la mascara usando la imformacion de que
    // los inliers mantienen el orden relativo que tienen en el vector "matches"
    for (uint i = 0; i < inlierIndexesInMatchesVector.size(); ++i)
    {
        int index = inlierIndexesInMatchesVector[i];

        inlierMatchesMask[index] = true;
    }

    // Con la mascara se separan los matches inliers y los outliers
    for (uint i = 0; i < inlierMatchesMask.size(); ++i)
    {
        bool is_inlier = inlierMatchesMask[i];

        if (is_inlier)
        {
            inlierMatches.push_back(matches[i]);
            inlierPredictions.push_back(predictedMatchedFeatures[i]);
            inlierJacobians.push_back(predictedMatchedJacobians[i]);
        }
        else
        {
            outlierMatches.push_back(matches[i]);
        }
    }

#ifdef DEBUG
    std::cout << "cantidad de matches: " << matches.size() << std::endl;
    std::cout << "cantidad de inliers: " << inlierMatches.size() << std::endl;
    std::cout << "cantidad de ouliers: " << outlierMatches.size() << 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
}
Example #7
0
void matchPredictedFeatures( const cv::Mat& image,
                             const VectorMapFeature& features,
                             const VectorImageFeaturePrediction& vectorfeaturePrediction,
                             VectorFeatureMatch &matches )
{
    ConfigurationManager &configManager = ConfigurationManager::getInstance();

    cv::Mat imageMask( cv::Mat::zeros(image.rows, image.cols, CV_8UC1) );

    size_t vectorfeaturePredictionSize = vectorfeaturePrediction.size();

    // armo la mascara, para evitar buscar features fuera de los elipses de los features predichos
    for (int i = 0; i < vectorfeaturePredictionSize; ++i)
    {
        ImageFeaturePrediction *featurePrediction = vectorfeaturePrediction[i];
        drawUncertaintyEllipse2D( imageMask,
                                  cv::Point2d(featurePrediction->imagePos[0], featurePrediction->imagePos[1]),
                                  featurePrediction->covarianceMatrix,
                                  2.0L*MAX(configManager.cameraCalibration->pixelsX, configManager.cameraCalibration->pixelsY),
                                  ellipseColorWhite,
                                  true );
    }

    // detectamos features
    std::vector<cv::KeyPoint> imageKeypoints;
    configManager.featureDetector->detect(image, imageKeypoints, imageMask);

    // extraemos descriptores
    cv::Mat descriptors;
    configManager.descriptorExtractor->compute(image, imageKeypoints, descriptors);

    // IMPORTANTE: el size del imageKeypoints se DEBE tomar despues de extraer descriptores
    // ya que este metodo puede eliminar keypoints sobre los que no se haya podido computar
    // los descriptores.
    size_t imageKeypointsSize = imageKeypoints.size();

    for (int i = 0; i < vectorfeaturePredictionSize; ++i)
    {
        cv::Mat mask(cv::Mat::zeros(1, descriptors.rows, CV_8UC1));

        ImageFeaturePrediction *featurePrediction = vectorfeaturePrediction[i];

        MapFeature *currMapFeature= features[featurePrediction->featureIndex];

        cv::Size2f axesSize(0.0f, 0.0f);
        double angle = 0.0L;
        matrix2x2ToUncertaintyEllipse2D(featurePrediction->covarianceMatrix, axesSize, angle);

        for (int j = 0; j < imageKeypointsSize; ++j)
        {
            // Si el feature cae dentro de la elipse, agregarlo como posible candidato
            if ( pointIsInsideEllipse( imageKeypoints[j].pt,
                                       cv::Point2d(featurePrediction->imagePos[0], featurePrediction->imagePos[1]),
                                       axesSize,
                                       angle ) )
            {
                mask.at<uchar>(0, j) = true;
            }
        }

        std::vector<cv::DMatch> currFeatureMatch;
        matchICDescriptors( currMapFeature->descriptor,
                            descriptors,
                            currFeatureMatch,
                            mask );

        // Si encontramos algun match lo agregamos al vector de resultado
        if (currFeatureMatch.size() > 0)
        {
            cv::KeyPoint& matchedKeypoint = imageKeypoints[currFeatureMatch[0].trainIdx];

            // Agregamos un match al resultado
            FeatureMatch *newMatch = new FeatureMatch;
            newMatch->featureIndex = featurePrediction->featureIndex;
            newMatch->imagePos[0] = static_cast<double>(matchedKeypoint.pt.x);
            newMatch->imagePos[1] = static_cast<double>(matchedKeypoint.pt.y);
            newMatch->distance = currFeatureMatch[0].distance;

            descriptors.row(currFeatureMatch[0].trainIdx).copyTo(newMatch->imagePosDescriptor);

            matches.push_back(newMatch);
        }
    }
}