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

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