// --------------------------------------------------------------------------------------------------------------------
// 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]);
}
}
void updateFeaturesLastImagePos(VectorMapFeature &predictedFeatures, const VectorFeatureMatch &matches)
{
size_t matchesSize = matches.size();
for (int i = 0; i < matchesSize; ++i)
{
predictedFeatures[matches[i]->featureIndex]->lastImagePos[0] = matches[i]->imagePos[0];
predictedFeatures[matches[i]->featureIndex]->lastImagePos[1] = matches[i]->imagePos[1];
}
}
// --------------------------------------------------------------------------------------------------------------------
// 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++;
}
}
}
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);
}
}
// --------------------------------------------------------------------------------------------------------------------
// 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 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);
}
}
}
