margBlob margBlobCorrector::lensCorrectBlob(margBlob& inBlob) {
margBlob corrBlob; // Blob to be returned
float minX = inW;
float minY = inH;
float maxX = 0;
float maxY = 0;
// Adapted UndistortPoint Method
for (int i = 0; i < inBlob.nPts; i++) {
corrBlob.pts.push_back(undistortPoint(inBlob.pts[i].x, inBlob.pts[i].y));
minX = MIN(minX, corrBlob.pts[i].x);
minY = MIN(minY, corrBlob.pts[i].y);
maxX = MAX(maxX, corrBlob.pts[i].x);
maxY = MAX(maxY, corrBlob.pts[i].y);
}
corrBlob.boundingRect.x = minX;
corrBlob.boundingRect.y = minY;
corrBlob.boundingRect.width = maxX - corrBlob.boundingRect.x;
corrBlob.boundingRect.height= maxY - corrBlob.boundingRect.y;
corrBlob.centroid = undistortPoint(inBlob.centroid.x, inBlob.centroid.y);
corrBlob.nPts = inBlob.nPts;
corrBlob.exposure = inBlob.exposure;
corrBlob.area = inBlob.area;
return corrBlob;
}
void addFeatureToStateAndCovariance( const ImageFeatureMeasurement *imageFeatureMeasurement,
State &state,
Matd &stateCovarianceMatrix )
{
CameraCalibration *cameraCalib = ConfigurationManager::getInstance().cameraCalibration;
ExtendedKalmanFilterParameters *ekfParams = ConfigurationManager::getInstance().ekfParams;
double featurePos[6] = {0.0L};
// x y z
for (int i = 0; i < 3; ++i)
{
featurePos[i] = state.position[i];
}
double undistortedImagePoint[2] = {0.0L};
undistortPoint(imageFeatureMeasurement->imagePos, undistortedImagePoint);
// Se retroproyecta el punto dedistorsionado
double retroProjectedPoint[3] = {0.0L};
retroProjectedPoint[0] = -(cameraCalib->cx - undistortedImagePoint[0]) / cameraCalib->fx;
retroProjectedPoint[1] = -(cameraCalib->cy - undistortedImagePoint[1]) / cameraCalib->fy;
retroProjectedPoint[2] = 1.0L;
// Pasamos al eje de coordenadas del mundo
multiplyRotationMatrixByVector(state.orientationRotationMatrix, retroProjectedPoint, retroProjectedPoint);
// Extraemos la informacion
double fdx = retroProjectedPoint[0];
double fdy = retroProjectedPoint[1];
double fdz = retroProjectedPoint[2];
// theta
featurePos[3] = atan2(fdx, fdz);
// phi
featurePos[4] = atan2(-fdy, sqrt(fdx*fdx + fdz*fdz));
// rho
featurePos[5] = ekfParams->initInvDepthRho;
// Agregamos el feature al estado
MapFeature *newMapFeature = new MapFeature( featurePos,
6,
stateCovarianceMatrix.cols,
imageFeatureMeasurement->descriptorData,
MAPFEATURE_TYPE_INVERSE_DEPTH );
state.addFeature(newMapFeature);
// Agregamos el MapFeaturea la matriz de covarianza
addFeatureToCovarianceMatrix(imageFeatureMeasurement->imagePos, state, stateCovarianceMatrix);
#ifdef DEBUG_SHOW_IMAGES
newMapFeature->lastImagePos[0] = imageFeatureMeasurement->imagePos[0];
newMapFeature->lastImagePos[1] = imageFeatureMeasurement->imagePos[1];
#endif
}
void computeAddFeatureJacobian(const double *orientation,
const double *rotationMatrixWC,
const double *featureImagePos,
double *jacobianPositionAndOrientationSubmatrix,
double *jacobianHAndRhoSubmatrix)
{
CameraCalibration *cameraCalib = ConfigurationManager::getInstance().cameraCalibration;
double undistortedPoint[2] = {0};
undistortPoint(featureImagePos, undistortedPoint);
// Feature con respecto a la camara pasado al espacio proyectivo
double x_c = -(cameraCalib->cx - undistortedPoint[0]) / cameraCalib->fx;
double y_c = -(cameraCalib->cy - undistortedPoint[1]) / cameraCalib->fy;
double z_c = 1.0L;
double xyz_c[3] = {0};
xyz_c[0] = x_c;
xyz_c[1] = y_c;
xyz_c[2] = z_c;
double xyz_w[3] = {0};
multiplyRotationMatrixByVector(rotationMatrixWC, xyz_c, xyz_w);
double x_w = xyz_w[0];
double y_w = xyz_w[1];
double z_w = xyz_w[2];
double xx_plus_zz = x_w*x_w + z_w*z_w;
double dtheta_dgw[3] = {0};
dtheta_dgw[0] = z_w / xx_plus_zz;
dtheta_dgw[1] = 0;
dtheta_dgw[2] = -x_w / xx_plus_zz;
double sqrt_xx_plus_zz = sqrt(x_w*x_w + z_w*z_w);
double norm_sq = xx_plus_zz + y_w*y_w;
double dphi_dgw[3] = {0};
dphi_dgw[0] = x_w * y_w / (norm_sq * sqrt_xx_plus_zz);
dphi_dgw[1] = -sqrt_xx_plus_zz / norm_sq;
dphi_dgw[2] = z_w * y_w / (norm_sq * sqrt_xx_plus_zz);
double dgw_dqwr[12] = {0};
makeJacobianOfQuaternionToRotationMatrix(orientation, xyz_c, dgw_dqwr);
double dtheta_dqwr[4] = {0};
matrixMult(dtheta_dgw, 1, 3, dgw_dqwr, 4, dtheta_dqwr);
double dphi_dqwr[4] = {0};
matrixMult(dphi_dgw, 1, 3, dgw_dqwr, 4, dphi_dqwr);
// int derivativeResultRows = 6;
int derivativeResultCols = 7;
for (int i = 0; i < 3; ++i)
{
jacobianPositionAndOrientationSubmatrix[i*derivativeResultCols + i] = 1.0L;
}
for (int i = 0; i < 4; ++i)
{
// 4ta fila, columnas 3, 4, 5 y 6
jacobianPositionAndOrientationSubmatrix[3*derivativeResultCols + i+3] = dtheta_dqwr[i];
// 5ta fila, columnas 3, 4, 5 y 6
jacobianPositionAndOrientationSubmatrix[4*derivativeResultCols + i+3] = dphi_dqwr[i];
}
// Para las siguientes lineas de codigo conviene ver el archivo de MATLAB add_a_feature_covariance_inverse_depth.m
// Multiplicacion de dyprima_dgw * dgw_dgc.
// subresult_dgw_dgc: 2x3
double subresult_dgw_dgc[6] = {0};
matrixMult(dtheta_dgw, 1, 3, rotationMatrixWC, 3, &subresult_dgw_dgc[0]);
matrixMult(dphi_dgw, 1, 3, rotationMatrixWC, 3, &subresult_dgw_dgc[3]);
// multiplicacion de dyprima_dgw * dgw_dgc * dgc_dhu.
double fku_inv = 1.0L / cameraCalib->fx;
double fkv_inv = 1.0L / cameraCalib->fy;
// dgc_dhu: 3x2
double dgc_dhu[6] = {fku_inv, 0, 0, fkv_inv, 0, 0};
// subresult_dgc_dhu: 2x2
double subresult_dgc_dhu[4] = {0};
matrixMult(subresult_dgw_dgc, 2, 3, dgc_dhu, 2, subresult_dgc_dhu);
double dhu_dhd[4] = {0};
computeUndistortPointJacobian(featureImagePos, dhu_dhd);
// jacobianHAndRhoSubmatrix:6x3 de la forma
// 0 0 0
// 0 0 0
// 0 0 0
// a b 0
// c d 0
// 0 0 1
// subresult_dhu_dhd: 2x2
double subresult_dhu_dhd[4] = {0};
matrixMult(subresult_dgc_dhu, 2, 2, dhu_dhd, 2, subresult_dhu_dhd);
jacobianHAndRhoSubmatrix[9] = subresult_dhu_dhd[0];
jacobianHAndRhoSubmatrix[10] = subresult_dhu_dhd[1];
jacobianHAndRhoSubmatrix[12] = subresult_dhu_dhd[2];
jacobianHAndRhoSubmatrix[13] = subresult_dhu_dhd[3];
jacobianHAndRhoSubmatrix[17] = 1.0L;
}
