void changeInverseDepthToDepth( const double *inverseDepthPoint, double *depthPoint )
{
double rho = inverseDepthPoint[5];
double directionalVector[3] = {0};
makeDirectionalVector(inverseDepthPoint[3], inverseDepthPoint[4], directionalVector);
depthPoint[0] = inverseDepthPoint[0] + directionalVector[0]/rho;
depthPoint[1] = inverseDepthPoint[1] + directionalVector[1]/rho;
depthPoint[2] = inverseDepthPoint[2] + directionalVector[2]/rho;
}
// ------------------------------------------------------------------------------------------------------------
// 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;
}
}
}