// --------------------------------------------------------------------------------------------------------------------
// 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]);
}
}
// --------------------------------------------------------------------------------------------------------------------
// 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
}