void addFeaturesToStateAndCovariance( const VectorImageFeatureMeasurement &imageFeatureMeasurement,
State &state,
Matd &stateCovarianceMatrix )
{
size_t imageFeatureMeasurementSize = imageFeatureMeasurement.size();
for (uint i = 0; i < imageFeatureMeasurementSize; ++i)
{
addFeatureToStateAndCovariance(imageFeatureMeasurement[i], state, stateCovarianceMatrix);
}
#ifdef DEBUG
std::cout << "Se agregaron " << imageFeatureMeasurementSize << " nuevos features al mapa." << std::endl;
#endif
}
void EKF::init(const cv::Mat &image)
{
if (_logFile.is_open())
{
time_t seed = time(NULL);
srand(static_cast<uint>(seed));
_logFile << "Random Seed: " << seed << std::endl << std::endl;
_logFile << "~~~~~~~~~~~~ STEP " << _ekfSteps << " ~~~~~~~~~~~~" << std::endl;
}
ExtendedKalmanFilterParameters *ekfParams = ConfigurationManager::getInstance().ekfParams;
initState(state);
state.mapFeatures.reserve(ekfParams->reserveFeaturesDepth);
state.mapFeaturesDepth.reserve(ekfParams->reserveFeaturesDepth);
state.mapFeaturesInvDepth.reserve(ekfParams->reserveFeaturesInvDepth);
initCovariance(stateCovarianceMatrix);
// Detectar features en la imagen
VectorFeatureMatch noMatches; // Al principio no tiene nada ya que no hay matches
VectorImageFeaturePrediction noPredictions;
VectorImageFeatureMeasurement newFeatureMeasurements;
detectNewImageFeatures(image, noPredictions, ekfParams->minMatchesPerImage, newFeatureMeasurements);
#ifdef DEBUG_SHOW_IMAGES
cv::Mat imageWithKeypoints;
image.copyTo(imageWithKeypoints);
for (uint i = 0; i < newFeatureMeasurements.size(); ++i)
{
drawPoint(imageWithKeypoints, newFeatureMeasurements[i]->imagePos, cv::Scalar(0, 0, 255));
}
std::cout << std::endl;
std::string windowName = "Features detectados en la primer imagen (";
std::stringstream convert;
convert << newFeatureMeasurements.size();
windowName += convert.str();
windowName += ")";
cv::namedWindow(windowName);
cv::imshow(windowName, imageWithKeypoints);
cv::waitKey(0);
cv::destroyWindow(windowName);
#endif
// Agregar los features nuevos al estado
addFeaturesToStateAndCovariance(newFeatureMeasurements, state, stateCovarianceMatrix);
size_t newFeatureMeasurementsSize = newFeatureMeasurements.size();
for (uint i = 0; i < newFeatureMeasurementsSize; ++i)
{
delete newFeatureMeasurements[i];
}
if (_logFile.is_open())
{
state.showDetailed(_logFile);
}
}
void detectNewImageFeatures( const cv::Mat image,
const VectorImageFeaturePrediction &featuresPrediction,
uint newImageFeaturesMaxSize,
VectorImageFeatureMeasurement &newImageFeatures )
{
newImageFeatures.clear();
// Calculamos el tamaño de las zonas de la imágen según los parámetros configurados
ConfigurationManager& configManager = ConfigurationManager::getInstance();
//int zonesInARow = exp2f(configManager.ekfParams->detectNewFeaturesImageAreasDivideTimes);
int zonesInARow = 1;
int zoneWidth = image.cols / zonesInARow;
int zoneHeight = image.rows / zonesInARow;
// Construimos la mascara para buscar features solamente en zonas poco densas
cv::Mat imageMask( cv::Mat::ones(image.rows, image.cols, CV_8UC1) * 255 );
buildImageMask( featuresPrediction, imageMask );
// Detectamos features
std::vector<cv::KeyPoint> imageKeypoints;
configManager.featureDetector->detect(image, imageKeypoints, imageMask);
// Extraemos descriptores
cv::Mat descriptors;
configManager.descriptorExtractor->compute(image, imageKeypoints, descriptors);
// Caso particular: la cantidad de features encontrados no supera los pedidos
size_t imageKeypointsSize = imageKeypoints.size();
#ifdef DEBUG
std::cout << "Cantidad de features detectados al agregar nuevos: " << imageKeypointsSize << std::endl;
#endif
if (imageKeypointsSize <= newImageFeaturesMaxSize)
{
double imagePos[2];
for (int i = 0; i < imageKeypointsSize; ++i)
{
cv::KeyPoint &currKeypoint = imageKeypoints[i];
imagePos[0] = currKeypoint.pt.x;
imagePos[1] = currKeypoint.pt.y;
newImageFeatures.push_back( new ImageFeatureMeasurement( imagePos,
descriptors.row(i) ) );
}
}
else
{
// Buscamos nuevos features intentando que esten
// lo mejor distribuidos posible en la imagen
searchFeaturesByZone( featuresPrediction, imageKeypoints, descriptors,
zonesInARow, zoneWidth, zoneHeight,
imageMask,
newImageFeaturesMaxSize, newImageFeatures );
}
#ifdef DEBUG_SHOW_NEW_FEATURES
cv::Mat imageCopy;
image.copyTo(imageCopy);
for (int i = 1; i < zonesInARow; ++i)
{
cv::line(imageCopy, cv::Point(i * zoneWidth, 0), cv::Point(i * zoneWidth, imageCopy.rows), cv::Scalar(0, 255, 0));
cv::line(imageCopy, cv::Point(0, i * zoneHeight), cv::Point(imageCopy.cols, i * zoneHeight), cv::Scalar(0, 255, 0));
}
int featuresPredictionSize = featuresPrediction.size();
for (int i = 0; i < featuresPredictionSize; ++i)
{
ImageFeaturePrediction *currFeaturePrediction = featuresPrediction[i];
drawUncertaintyEllipse2D( imageCopy,
cv::Point2f(currFeaturePrediction->imagePos[0], currFeaturePrediction->imagePos[1]),
currFeaturePrediction->covarianceMatrix,
2 * (image.cols + image.rows),
cv::Scalar(0, 255, 0),
false );
}
cv::namedWindow("Busqueda de nuevos features: mascara");
cv::imshow("Busqueda de nuevos features: mascara", imageMask);
cv::waitKey(0);
for (int i = 0; i < newImageFeatures.size(); ++i)
{
drawPoint(imageCopy, newImageFeatures[i]->imagePos, cv::Scalar(0, 255, 255));
}
cv::namedWindow("Busqueda de nuevos features: imagen con nuevos features");
cv::imshow("Busqueda de nuevos features: imagen con nuevos features", imageCopy);
cv::waitKey(0);
// Se borran todas las ventanas creadas
cv::destroyWindow("Busqueda de nuevos features: mascara");
cv::destroyWindow("Busqueda de nuevos features: imagen con nuevos features");
#endif
}