Esempio n. 1
0
// --------------------------------------------------------------------------------------------------------------------
// Rescata los ouliers que se consideran buenos
// IMPORTANTE: Asume que outlierMatches, outlierMatchFeaturePrediction y (por fuera outlierMatchFeaturePredictionJacobians)
// tienen la misma cantidad de elementos y se corresponden en cada posicion los unos con los otros.
// --------------------------------------------------------------------------------------------------------------------
void rescueOutliers(const VectorFeatureMatch &outlierMatches,
                    const VectorImageFeaturePrediction &outlierMatchFeaturePrediction,
                    const VectorMatd &outlierMatchFeaturePredictionJacobians,
                    VectorFeatureMatch &rescuedMatches,
                    VectorImageFeaturePrediction &rescuedPredictions,
                    VectorMatd &rescuedJacobians)
{
    ExtendedKalmanFilterParameters *ekfParams = ConfigurationManager::getInstance().ekfParams;
    std::vector<int> rescuedMatchesIndexes;

    size_t outlierMatchesSize = outlierMatches.size();
    for (uint i = 0; i < outlierMatchesSize; ++i)
    {
        FeatureMatch *match = outlierMatches[i];
        ImageFeaturePrediction *prediction = outlierMatchFeaturePrediction[i];

        Matd dist = (Matd(2, 1) << match->imagePos[0] - prediction->imagePos[0],
                                   match->imagePos[1] - prediction->imagePos[1]);

        // Se verifica que sea un buen match
        // http://es.wikipedia.org/wiki/Propagaci%C3%B3n_de_errores
        // Se propaga el error desde la funcion que calcula la prediccion a la funcion
        // que calcula la distancia entre la prediccion y el match
        // entonces, el calculo de esa distancia, tiene que tener un error menor a ransacChi2Threshold
        // Osea que es una forma de decir que la confianza que se tiene sobre dicha distancia es muy grande
        // y queda en funcion de la matriz de covarianza de la prediccion
        if ( Matd( dist.t() * prediction->covarianceMatrix.inv() * dist)[0][0] < ekfParams->ransacChi2Threshold )
        {
            rescuedMatchesIndexes.push_back(i);
        }
    }

    // Se agregan los resultados para los indices rescatados.
    rescuedMatches.clear();
    rescuedPredictions.clear();
    rescuedJacobians.clear();

    // Se reserva lugar para que los push_back sean en O(1)
    rescuedMatches.reserve(rescuedMatchesIndexes.size());
    rescuedPredictions.reserve(rescuedMatchesIndexes.size());
    rescuedJacobians.reserve(rescuedMatchesIndexes.size());

    for (uint i = 0; i < rescuedMatchesIndexes.size(); ++i)
    {
        int index = rescuedMatchesIndexes[i];

        rescuedMatches.push_back(outlierMatches[index]);
        rescuedPredictions.push_back(outlierMatchFeaturePrediction[index]);
        rescuedJacobians.push_back(outlierMatchFeaturePredictionJacobians[index]);
    }

}
Esempio n. 2
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
}