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