コード例 #1
0
ファイル: margBlobCorrector.cpp プロジェクト: amintz/M1.0
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;	
	
}
コード例 #2
0
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
}
コード例 #3
0
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;
}