// --------------------------------------------------------------------------------------------------------------------
// 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]);
}
}
// ------------------------------------------------------------------------------------------------------------
// Importante: Los rangos rowsToRemove y columnsToRemove deben estar ordenados y no deben solaparse
Matd removeRowsAndColumnsFromMat(Matd matrix, const std::vector< cv::Range > rowsToRemove, const std::vector< cv::Range > columnsToRemove)
{
std::vector< cv::Range > rows;
std::vector< cv::Range > columns;
int resultRows = rangeComplement(rowsToRemove, 0, matrix.rows, rows);
int resultColumns = rangeComplement(columnsToRemove, 0, matrix.cols, columns);
if (resultRows == 0 || resultColumns == 0)
{
return Matd();
}
size_t rowsSize = rows.size();
size_t columnsSize = columns.size();
Matd result( Matd::zeros(resultRows, resultColumns) );
int nextEmptyRow = 0;
for (uint i = 0; i < rowsSize; ++i)
{
int nextEmptyColumn = 0;
const cv::Range& rowsRange = rows[i];
const cv::Range subResultRowsRange(nextEmptyRow, nextEmptyRow + rowsRange.size());
for (uint j = 0; j < columnsSize; ++j)
{
const cv::Range& columnsRange = columns[j];
const cv::Range subResultColumnsRange(nextEmptyColumn, nextEmptyColumn + columnsRange.size());
matrix(rowsRange, columnsRange).copyTo( result(subResultRowsRange, subResultColumnsRange) );
nextEmptyColumn += columnsRange.size();
}
nextEmptyRow += rowsRange.size();
}
return result;
}
void initCovariance( Matd &initialCovariance )
{
ExtendedKalmanFilterParameters *ekfParams = ConfigurationManager::getInstance().ekfParams;
// al principio no hay features en el sistema
Matd(Matd::zeros(13, 13)).copyTo(initialCovariance);
// Se inicia la covarianza para la posicion y orientacion
for (uint i = 0; i < 7; ++i)
{
initialCovariance.at<double>(i, i) = EPSILON;
}
// Se inicia la covarianza para la velocidad lineal y angular
double initialLinearAccelSD2 = ekfParams->initLinearAccelSD * ekfParams->initLinearAccelSD;
double initialAngularAccelSD2 = ekfParams->initAngularAccelSD * ekfParams->initAngularAccelSD;
for (uint i = 0; i < 3; ++i)
{
initialCovariance.at<double>(i + 7, i + 7) = initialLinearAccelSD2;
initialCovariance.at<double>(i + 10, i + 10) = initialAngularAccelSD2;
}
}
void addFeatureToCovarianceMatrix(const double *featureImagePos, State &state, Matd &stateCovarianceMatrix)
{
CameraCalibration *cameraCalib = ConfigurationManager::getInstance().cameraCalibration;
ExtendedKalmanFilterParameters *ekfParams = ConfigurationManager::getInstance().ekfParams;
// jacobianPositionAndOrientation: 6x7 (ignoramos la parte de ceros que aparece en el paper Inverse Depth)
// jacobianHAndRho: 6x3
double jacobianPositionAndOrientation[42] = {0};
double jacobianHAndRho[18] = {0};
computeAddFeatureJacobian(state.orientation,
state.orientationRotationMatrix,
featureImagePos,
jacobianPositionAndOrientation,
jacobianHAndRho);
// subMatrixCovarianceToAdd:3x3 es la matriz que se agrega abajo a la derecha en la covarianza:
//
// P = jacobiano * |covarianzaDelEstado 0 | * jacobiano'
// | 0 subMatrixCovarianceToAdd|
double subMatrixCovarianceToAdd[9] = {0};
subMatrixCovarianceToAdd[0] = cameraCalib->pixelErrorX * cameraCalib->pixelErrorX;
subMatrixCovarianceToAdd[4] = cameraCalib->pixelErrorY * cameraCalib->pixelErrorY;
subMatrixCovarianceToAdd[8] = ekfParams->inverseDepthRhoSD * ekfParams->inverseDepthRhoSD;
Matd covMatFirst7Rows(stateCovarianceMatrix, cv::Range(0, 7), cv::Range(0, stateCovarianceMatrix.cols));
Matd covMatFirst7Cols(stateCovarianceMatrix, cv::Range(0, stateCovarianceMatrix.rows), cv::Range(0, 7));
// Pasamos los jacobianos a Mat de OpenCV para poder realizar los calculos
Matd jacobianPosAndOrientMat(6, 7, jacobianPositionAndOrientation);
Matd jacobianHAndRhoMat(6, 3, jacobianHAndRho);
// Declaramos la matriz que va a guardar los resultados
Matd newStateCovarianceMatrix(stateCovarianceMatrix.rows + 6, stateCovarianceMatrix.cols + 6);
// Submatriz inferior izquierda del resultado (las 6 filas que se agregaron, quitando las ultimas 6 columnas)
Matd covMatNewFeatureVsPreviousState(newStateCovarianceMatrix,
cv::Range(stateCovarianceMatrix.rows, newStateCovarianceMatrix.rows),
cv::Range(0, stateCovarianceMatrix.cols));
// Submatriz superior derecha del resultado (las 6 columnas que se agregaron, quitando las ultimas 6 filas)
Matd covMatPreviousStateVsNewFeature(newStateCovarianceMatrix,
cv::Range(0, stateCovarianceMatrix.rows),
cv::Range(stateCovarianceMatrix.cols, newStateCovarianceMatrix.cols));
// Submatriz inferior derecha del resultado (de 6x6, las ultimas 6 filas y 6 columnas)
Matd covMatNewFeatureCovariance(newStateCovarianceMatrix,
cv::Range(stateCovarianceMatrix.rows, newStateCovarianceMatrix.rows),
cv::Range(stateCovarianceMatrix.cols, newStateCovarianceMatrix.cols));
// Llenamos la matriz de resultado
Matd(jacobianPosAndOrientMat * covMatFirst7Rows).copyTo(covMatNewFeatureVsPreviousState);
Matd(covMatFirst7Cols * jacobianPosAndOrientMat.t()).copyTo(covMatPreviousStateVsNewFeature);
Matd covMatNewFeatVsPrevStateFirst7Cols(covMatNewFeatureVsPreviousState,
cv::Range(0, covMatNewFeatureVsPreviousState.rows),
cv::Range(0, 7));
Matd matFeatureNoise(3, 3, subMatrixCovarianceToAdd);
Matd( covMatNewFeatVsPrevStateFirst7Cols * jacobianPosAndOrientMat.t() +
jacobianHAndRhoMat * matFeatureNoise * jacobianHAndRhoMat.t() ).copyTo(covMatNewFeatureCovariance);
stateCovarianceMatrix.copyTo( Matd( newStateCovarianceMatrix,
cv::Range(0, stateCovarianceMatrix.rows),
cv::Range(0, stateCovarianceMatrix.cols)) );
// Devolvemos la matriz de resultado
stateCovarianceMatrix = newStateCovarianceMatrix;
}
// ------------------------------------------------------------------------------------------------------------
// Funcion que pasa un feature del mapa de inverse depth a depth
void convertToDepth(MapFeature *mapFeature, State &state, Matd &stateCovarianceMatrix)
{
double theta = mapFeature->position[3];
double phi = mapFeature->position[4];
double rho = mapFeature->position[5];
double mi[3] = {0};
double currMapFeatureXYZPos[3] = {0};
makeDirectionalVector(theta, phi, mi);
currMapFeatureXYZPos[0] = mapFeature->position[0] + mi[0]/rho;
currMapFeatureXYZPos[1] = mapFeature->position[1] + mi[1]/rho;
currMapFeatureXYZPos[2] = mapFeature->position[2] + mi[2]/rho;
// Hacer cambios en la matriz de covarianza
double dm_dtheta_div_rho[3] = {0};
double dm_dphi_div_rho[3] = {0};
double neg_mi_div_rho2[3] = {0};
dm_dtheta_div_rho[0] = cos(phi) * cos(theta) / rho;
dm_dtheta_div_rho[2] = -cos(phi) * sin(theta) / rho;
dm_dphi_div_rho[0] = -sin(phi)*sin(theta) / rho;
dm_dphi_div_rho[1] = -cos(phi) / rho;
dm_dphi_div_rho[2] = -sin(phi)*cos(theta) / rho;
neg_mi_div_rho2[0] = -mi[0] / (rho * rho);
neg_mi_div_rho2[1] = -mi[1] / (rho * rho);
neg_mi_div_rho2[2] = -mi[2] / (rho * rho);
//
// jacobian: 3x6
//
Matd jacobian( Matd::zeros(3, 6) );
jacobian[0][0] = 1.0L;
jacobian[1][1] = 1.0L;
jacobian[2][2] = 1.0L;
Matd(3, 1, dm_dtheta_div_rho).copyTo( jacobian(cv::Range(0, 3), cv::Range(3, 4)) );
Matd(3, 1, dm_dphi_div_rho ).copyTo( jacobian(cv::Range(0, 3), cv::Range(4, 5)) );
Matd(3, 1, neg_mi_div_rho2 ).copyTo( jacobian(cv::Range(0, 3), cv::Range(5, 6)) );
//
// stateCovarianceMatrix = | P_{k x k} P_{k x 6} P_{k x n-k-6} |
// | P_{6 x k} P_{6 x 6} P_{6 x n-k-6} | <- P_{6 x n}
// | P_{n-k-6 x k} P_{n-k-6 x 6} P_{n-k-6 x n-k-6} |
//
int covarianceMatrixPos = mapFeature->covarianceMatrixPos;
int oldPosDimension = mapFeature->positionDimension;
cv::Range firstKRange(0, covarianceMatrixPos);
cv::Range featureRange(covarianceMatrixPos, covarianceMatrixPos + oldPosDimension);
cv::Range lastRange(covarianceMatrixPos + oldPosDimension, stateCovarianceMatrix.cols);
cv::Range newCovFeatureRange(covarianceMatrixPos, covarianceMatrixPos + 3);
cv::Range newCovMatLastRange(covarianceMatrixPos + 3, stateCovarianceMatrix.cols - 3);
bool lastFeatureInCovMatrix = lastRange.start == lastRange.end || newCovMatLastRange.start == newCovMatLastRange.end;
Matd stateCovMatrix_6xn( stateCovarianceMatrix, featureRange, cv::Range(0, stateCovarianceMatrix.cols) );
Matd stateCovMatrix_kx6( stateCovarianceMatrix, firstKRange, featureRange );
Matd stateCovMatrix_nk6x6( stateCovarianceMatrix, lastRange, featureRange );
Matd stateCovMatrix_kxk( stateCovarianceMatrix, firstKRange, firstKRange );
Matd stateCovMatrix_nk6xk( stateCovarianceMatrix, lastRange, firstKRange );
Matd stateCovMatrix_kxnk6( stateCovarianceMatrix, firstKRange, lastRange );
Matd stateCovMatrix_nk6xnk6( stateCovarianceMatrix, lastRange, lastRange );
Matd newCovarianceMatrix( Matd::zeros(stateCovarianceMatrix.rows - 3, stateCovarianceMatrix.cols - 3) );
Matd subResult_3xn(jacobian * stateCovMatrix_6xn);
Matd subResult_3xk( subResult_3xn, cv::Range(0, subResult_3xn.rows), firstKRange );
Matd subResult_3x6( subResult_3xn, cv::Range(0, subResult_3xn.rows), featureRange );
Matd(subResult_3x6 * jacobian.t()).copyTo( newCovarianceMatrix(newCovFeatureRange, newCovFeatureRange) );
subResult_3xk.copyTo( newCovarianceMatrix(newCovFeatureRange, firstKRange) );
if (!lastFeatureInCovMatrix)
{
Matd(subResult_3xn, cv::Range(0, subResult_3xn.rows), lastRange).copyTo( newCovarianceMatrix(newCovFeatureRange, newCovMatLastRange) );
}
Matd(stateCovMatrix_kx6 * jacobian.t()).copyTo( newCovarianceMatrix(firstKRange, newCovFeatureRange) );
if (!lastFeatureInCovMatrix)
{
Matd(stateCovMatrix_nk6x6 * jacobian.t()).copyTo( newCovarianceMatrix(newCovMatLastRange, newCovFeatureRange) );
}
stateCovMatrix_kxk.copyTo( newCovarianceMatrix(firstKRange, firstKRange) );
if (!lastFeatureInCovMatrix)
{
stateCovMatrix_nk6xk.copyTo( newCovarianceMatrix(newCovMatLastRange, firstKRange) );
stateCovMatrix_kxnk6.copyTo( newCovarianceMatrix(firstKRange, newCovMatLastRange) );
stateCovMatrix_nk6xnk6.copyTo( newCovarianceMatrix(newCovMatLastRange, newCovMatLastRange) );
}
stateCovarianceMatrix = newCovarianceMatrix;
// Hacer cambios en el state
mapFeature->positionDimension = 3; // XYZ parameters
delete [] mapFeature->position;
mapFeature->position = new double[mapFeature->positionDimension];
mapFeature->position[0] = currMapFeatureXYZPos[0];
mapFeature->position[1] = currMapFeatureXYZPos[1];
mapFeature->position[2] = currMapFeatureXYZPos[2];
mapFeature->featureType = MAPFEATURE_TYPE_DEPTH;
state.mapFeaturesDepth.push_back(mapFeature);
VectorMapFeature::iterator itInvDepthMapFeature = state.mapFeaturesInvDepth.begin();
VectorMapFeature::iterator itInvDepthMapFeatureEnd = state.mapFeaturesInvDepth.end();
while(itInvDepthMapFeature != itInvDepthMapFeatureEnd && mapFeature != *itInvDepthMapFeature)
{
itInvDepthMapFeature++;
}
assert(itInvDepthMapFeature != itInvDepthMapFeatureEnd);
state.mapFeaturesInvDepth.erase(itInvDepthMapFeature);
VectorMapFeature::iterator itMapFeature = state.mapFeatures.begin();
VectorMapFeature::iterator itMapFeatureEnd = state.mapFeatures.end();
while(itMapFeature != itMapFeatureEnd && mapFeature != *itMapFeature)
{
itMapFeature++;
}
assert(itMapFeature != itMapFeatureEnd);
size_t mapFeaturesSize = state.mapFeatures.size();
int newPosDimension = mapFeature->positionDimension;
for(uint i = 0; i < mapFeaturesSize; ++i)
{
int &currMapFeatureCovarianceMatPos = state.mapFeatures[i]->covarianceMatrixPos;
if (currMapFeatureCovarianceMatPos > covarianceMatrixPos)
{
currMapFeatureCovarianceMatPos = currMapFeatureCovarianceMatPos - oldPosDimension + newPosDimension;
}
}
}
void searchFeaturesByZone( const VectorImageFeaturePrediction &featuresPrediction,
std::vector<cv::KeyPoint> &imageKeypoints, cv::Mat &descriptors,
int zonesInARow, int zoneWidth, int zoneHeight,
cv::Mat& imageMask,
int imageFeaturesMaxSize,
VectorImageFeatureMeasurement &newImageFeatures )
{
int zonesCount = zonesInARow * zonesInARow;
// Inicializamos el ZoneInfo para cada zona.
ZoneInfo **zonesInfo = new ZoneInfo *[zonesCount];
for (int i = 0; i < zonesCount; ++i)
{
ZoneInfo *currZoneInfo = new ZoneInfo;
currZoneInfo->candidateImageFeatureMeasurementLeft = 0;
currZoneInfo->predictionsPlusFeaturesCount = 0;
currZoneInfo->zoneId = i;
zonesInfo[i] = currZoneInfo;
}
groupImageFeaturesAndPredictionsByZone( featuresPrediction, imageKeypoints, descriptors,
zoneWidth, zoneHeight, imageMask.cols, imageMask.rows, zonesCount,
zonesInfo );
// Ordenamos el arreglo de ZoneInfo por su cantidad de predicciones tal que su media cae dentro de la zona
qsort(zonesInfo, zonesCount, sizeof(ZoneInfo *), &sortCompare);
// Convertimos el arreglo de ZoneInfo a lista para lograr mayor performance en la eliminacion
std::list< ZoneInfo * > zoneInfoList;
for (int i = 0; i < zonesCount; ++i)
{
zoneInfoList.push_back( zonesInfo[i] );
}
// Agregamos los features al resultado
newImageFeatures.reserve(imageFeaturesMaxSize);
double ellipseSize = ConfigurationManager::getInstance().ekfParams->detectNewFeaturesImageMaskEllipseSize;
Matd newFeatMeasToAddEllipse( (Matd(2, 2) << ellipseSize, 0.0L,
0.0L, ellipseSize) );
int zonesLeft = zonesCount;
while(zonesLeft > 0 && imageFeaturesMaxSize > 0)
{
ZoneInfo *currZoneInfo = zoneInfoList.front();
int &currZoneFeaturesLeft = currZoneInfo->candidateImageFeatureMeasurementLeft;
// Si no hay mas features en la zona, la quitamos de la lista
if (currZoneFeaturesLeft == 0)
{
zoneInfoList.pop_front();
#ifdef DEBUG_SHOW_NEW_FEATURES
std::cout << "Se elimino la zona " << currZoneInfo->zoneId << " por no tener más features candidatos restantes." << std::endl;
std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl << std::endl;
#endif
zonesLeft--;
}
else
{
// Tomamos un feature de forma aleatoria
int randKeypointIdx = rand() % currZoneFeaturesLeft;
ImageFeatureMeasurement *imFeatMeasToAdd = currZoneInfo->candidateImageFeatureMeasurement[randKeypointIdx];
int imFeatMeasToAddX = static_cast<int>(imFeatMeasToAdd->imagePos[0]);
int imFeatMeasToAddY = static_cast<int>(imFeatMeasToAdd->imagePos[1]);
if ( imageMask.at<uchar>(imFeatMeasToAddY, imFeatMeasToAddX) )
{
// Agregamos el feature al resultado y utilizamos el constructor por copia
// Aca si que copiamos el descriptor, para poder devolverlo
newImageFeatures.push_back( new ImageFeatureMeasurement(*imFeatMeasToAdd) );
currZoneInfo->predictionsPlusFeaturesCount++;
#ifdef DEBUG_SHOW_NEW_FEATURES
std::cout << "Agregamos feature en la zona " << currZoneInfo->zoneId << std::endl;
std::cout << "Hay ahora " << currZoneInfo->predictionsPlusFeaturesCount << " predicciones y features" << std::endl;
std::cout << "Quedan " << currZoneInfo->candidateImageFeatureMeasurementLeft << " feature candidatos por agregar" << std::endl;
std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl << std::endl;
#endif
// Reordeno la lista
std::list< ZoneInfo * >::iterator itCurr = zoneInfoList.begin();
std::list< ZoneInfo * >::iterator itNext = zoneInfoList.begin();
std::list< ZoneInfo * >::iterator itEnd = zoneInfoList.end();
itNext++;
// Seguimos recorriendo
while(itNext != itEnd)
{
ZoneInfo *nextZoneInfo = static_cast<ZoneInfo *>(*itNext);
// Si la cantidad de features y predicciones de la zona actual
// supera o iguala a la cantidad de features y predicciones de la siguiente zona en la lista,
// cambio el orden de la zona actual por la siguiente, para asegurar el orden total
// de las zonas con respecto a la cantidad de features
if (currZoneInfo->predictionsPlusFeaturesCount >= nextZoneInfo->predictionsPlusFeaturesCount)
{
(*itNext) = currZoneInfo;
(*itCurr) = nextZoneInfo;
}
else
{
break;
}
itCurr++;
itNext++;
}
// Actualizamos la mascara
drawUncertaintyEllipse2D( imageMask,
cv::Point2d(imFeatMeasToAdd->imagePos[0], imFeatMeasToAdd->imagePos[1]),
newFeatMeasToAddEllipse,
2 * (imageMask.cols + imageMask.rows),
cv::Scalar(0, 0, 0),
true );
// Actualizamos la cantidad de features buscada
imageFeaturesMaxSize--;
}
// Nos aseguramos que el mismo feature no sea reelecto
currZoneFeaturesLeft--;
currZoneInfo->candidateImageFeatureMeasurement[randKeypointIdx] =
currZoneInfo->candidateImageFeatureMeasurement[currZoneFeaturesLeft];
delete imFeatMeasToAdd;
}
}
// Liberamos toda la memoria auxiliar utilizada
for (int i = 0; i < zonesCount; ++i)
{
ZoneInfo *currZoneInfo = zonesInfo[i];
for (int j = 0; j < currZoneInfo->candidateImageFeatureMeasurementLeft; ++j)
{
delete currZoneInfo->candidateImageFeatureMeasurement[j];
}
delete currZoneInfo;
}
delete [] zonesInfo;
}