Exemplo n.º 1
0
// find closest 3D point from from a set of 3D lines
void find_point_opt(type_sight *sights_list, int N,
		    double *point_opt, double *outerr)
{
	double prod1[3][3];
	double prod2[3];
	double Id[3][3];
	double mat_sum[3][3]={{0,0,0},
					   {0,0,0},
					   {0,0,0}};
	double prod_sum[3]={0,0,0};
	
	for(int i=0; i<N; i++)
	  {
	    OUTER_PRODUCT_3X3(prod1,sights_list[i].v,sights_list[i].v);
	    IDENTIFY_MATRIX_3X3(Id);
	    ACCUM_SCALE_MATRIX_3X3(Id,-1,prod1); // Now Id actually contains a mat diff
	    ACCUM_SCALE_MATRIX_3X3(mat_sum,1,Id); // accumulates it into matrix 'mat_sum'
	    MAT_DOT_VEC_3X3(prod2,Id,sights_list[i].s); // Id still contains a mat diff
	    VEC_ACCUM(prod_sum,1,prod2); // accumulates the above result
	  }
	
	double mat_sum_inv[3][3];
	double det;
	INVERT_3X3(mat_sum_inv,det,mat_sum);
	
	// find the optimal point
	MAT_DOT_VEC_3X3(point_opt,mat_sum_inv,prod_sum);
}
Exemplo n.º 2
0
void kalman_update_3x3(kalman_state_n* state, float* measurement,float** A, float** H)
{
    float** auxMat;
    auxMat=(float**)malloc(3*sizeof(float*));
    for (int i=0; i<3; i++) auxMat[i]=malloc(3*sizeof(float));
    float* auxVet;
    auxVet=(float*)malloc(3*sizeof(float));
   
    float pTrsp[3][3],Atrsp[3][3];
    
    /*predicted p*/
    TRANSPOSE_MATRIX_3X3(Atrsp, A);
    MATRIX_PRODUCT_3X3(auxMat, state->p, Atrsp);
    MATRIX_PRODUCT_3X3(state->p, A, auxMat);
    ACCUM_SCALE_MATRIX_3X3(state->p, 1, state->q)
    /*predicted x*/
    MAT_DOT_VEC_3X3(state->x, A, state->x);
//    state->x=xaux;

    /*kalman gain*/
    float Htrsp[3][3], S[3][3], Sinv[3][3], B[3][3], det;
    TRANSPOSE_MATRIX_3X3(Htrsp, H);
    TRANSPOSE_MATRIX_3X3(pTrsp, state->p);
    MATRIX_PRODUCT_3X3(auxMat, pTrsp, Htrsp);
    MATRIX_PRODUCT_3X3(S, H, auxMat);
    ACCUM_SCALE_MATRIX_3X3(S, 1.0, state->r);
 
    MATRIX_PRODUCT_3X3(B, H, pTrsp);
 
    INVERT_3X3(Sinv, det, S);
    MATRIX_PRODUCT_3X3(auxMat, Sinv, B);
    TRANSPOSE_MATRIX_3X3(state->k, auxMat);
//    MAT_PRINT_3X3(state->k);

    /*estimated x*/    
    MAT_DOT_VEC_3X3(auxVet, H, state->x);
    VEC_DIFF(auxVet, measurement, auxVet);
    MAT_DOT_VEC_3X3(auxVet, state->k, auxVet);
    VEC_SUM(state->x, state->x, auxVet);
    
    /*estimated P*/
    MATRIX_PRODUCT_3X3(auxMat, H, state->p);
    MATRIX_PRODUCT_3X3(auxMat, state->k, auxMat);
    ACCUM_SCALE_MATRIX_3X3(state->p, -1.0, auxMat);
   
    /*out*/
    MAT_DOT_VEC_3X3(measurement, H, state->x);
    
    for (int i=0; i<3; i++) free(auxMat[i]);
    free(auxMat);
    free(auxVet);
}
Exemplo n.º 3
0
void CoplanarPosit(int NbPts, double **imgPts, double** worldPts, double focalLength, double center[2], double** Rot, double* Trans){
    
    
    long int i,j;
    double** Rot1;
    double** Rot2;
    double** RotFinal1;
    double** RotFinal2;
    double* TransFinal1;
    double* TransFinal2;
    double  E1,Ehvmax1,E2,Ehvmax2;
    long int Ep1,Ep2;
    
    
    
    /* allocation for Rot1*/
    Rot1=(double **)malloc(3* sizeof(double *));
    for (i=0;i<3;i++) Rot1[i]=(double *)malloc(3 * sizeof(double));
    /* end alloc*/
    
    /* allocation for Rot2*/
    Rot2=(double **)malloc(3* sizeof(double *));
    for (i=0;i<3;i++) Rot2[i]=(double *)malloc(3 * sizeof(double));
    /* end alloc*/
    
    /* allocation for RotFinal1*/
    RotFinal1=(double **)malloc(3* sizeof(double *));
    for (i=0;i<3;i++) RotFinal1[i]=(double *)malloc(3 * sizeof(double));
    /* end alloc*/
    
    /* allocation for RotFinal2*/
    RotFinal2=(double **)malloc(3* sizeof(double *));
    for (i=0;i<3;i++) RotFinal2[i]=(double *)malloc(3 * sizeof(double));
    /* end alloc*/
    
    /* allocation for TransFinal1*/
    TransFinal1=(double *)malloc(3* sizeof(double));
    /* end alloc*/
    
    /* allocation for TransFinal2*/
    TransFinal2=(double *)malloc(3* sizeof(double));
    /* end alloc*/
    
    
    
    /* allocation for homogeneousWorldPts*/
    double** homogeneousWorldPts;
    homogeneousWorldPts=(double **)malloc(NbPts* sizeof(double *));
    for (i=0;i<NbPts;i++) homogeneousWorldPts[i]=(double *)malloc(4 * sizeof(double));
    /* end alloc*/
    
    /* Homogeneus world points -  append a 1 to each 3-vector. An Nx4 matrix.*/
    for (i=0;i<NbPts;i++){
        homogeneousWorldPts[i][0] = worldPts[i][0];
        homogeneousWorldPts[i][1] = worldPts[i][1];
        homogeneousWorldPts[i][2] = worldPts[i][2];
        homogeneousWorldPts[i][3] = 1;
    }
    
//    if (false) {
//        printf("IMAGEPTS ANTES DE ENTRAR A POSITBRANCHES\n");
//        for (i=0; i<NbPts; i++) {
//            printf("%g\t%g\n",imgPts[i][0],imgPts[i][1]);
//        }
//    }
    /* allocation for centeredImage*/
    double** centeredImage;
    centeredImage=(double**)malloc(NbPts*sizeof(double*));
    for (i=0; i<NbPts; i++) centeredImage[i]=(double*)malloc(2*sizeof(double));
    /* end alloc*/
    
    for (i=0;i<NbPts;i++){
        centeredImage[i][0]=(imgPts[i][0]-center[0])/focalLength;
        centeredImage[i][1]=(imgPts[i][1]-center[1])/focalLength;
        
    }
    
//    if (false) {
//        printf("CENTERED IMAGEPTS ANTES DE ENTRAR A POSITBRANCHES\n");
//        for (i=0; i<NbPts; i++) {
//            printf("%g\t%g\n",centeredImage[i][0],centeredImage[i][1]);
//        }
//    }
    
    
    //    /* objectMat alloc*/
    //    double** objectMat;
    //    objectMat=(double **)malloc(4 * sizeof(double*));
    //    for (i=0; i<4; i++) objectMat[i]=(double *)malloc(NbPts* sizeof(double));
    //    /* end*/
    
    /* objectMat alloc*/
    double** objectMat;
    objectMat=(double **)malloc(3 * sizeof(double*));
    for (i=0; i<3; i++) objectMat[i]=(double *)malloc(3* sizeof(double));
    /* end*/
    
    for (i=0;i<3;i++) {
        for (j=0;j<3;j++) objectMat[i][j]=0;
    }
    
//    if (false) {
//        printf("PUNTOS ANTES DE ENTRAR A PSEUDOINVERSE\n");
//        for (i=0; i<NbPts; i++) {
//            printf("%g\t%g\t%g\t%g\n",homogeneousWorldPts[i][0],homogeneousWorldPts[i][1],homogeneousWorldPts[i][2],homogeneousWorldPts[i][3]);
//        }
//    }
    
    double v[3];
    double m[3][3]={{0,0,0},{0,0,0},{0,0,0}};
    double det;
    for(i=0;i<NbPts;i++){
        v[0]=homogeneousWorldPts[i][0];
        v[1]=homogeneousWorldPts[i][1];
        v[2]=homogeneousWorldPts[i][3];
        ACCUM_OUTER_PRODUCT_3X3(m,v,v);
    }
    INVERT_3X3(objectMat,det,m);
//    MAT_PRINT_3X3(objectMat);
    
    PositBranches(NbPts, centeredImage, worldPts, objectMat, Rot1, Rot2, Trans);
    
//    if (true) {
//        
//        printf("\nRotacion1 antes de iterar: \n");
//        printf("%f\t %f\t %f\n",Rot1[0][0],Rot1[0][1],Rot1[0][2]);
//        printf("%f\t %f\t %f\n",Rot1[1][0],Rot1[1][1],Rot1[1][2]);
//        printf("%f\t %f\t %f\n",Rot1[2][0],Rot1[2][1],Rot1[2][2]);
//        printf("Traslacion: \n");
//        printf("%f\t %f\t %f\n",Trans[0],Trans[1],Trans[2]);
//        
//        printf("\nRotacion2 antes de iterar: \n");
//        printf("%f\t %f\t %f\n",Rot2[0][0],Rot2[0][1],Rot2[0][2]);
//        printf("%f\t %f\t %f\n",Rot2[1][0],Rot2[1][1],Rot2[1][2]);
//        printf("%f\t %f\t %f\n",Rot2[2][0],Rot2[2][1],Rot2[2][2]);
//        printf("Traslacion: \n");
//        printf("%f\t %f\t %f\n",Trans[0],Trans[1],Trans[2]);
//    }
    
    if ((Rot1[0][0])!=2.0) /*pose1 a priori possible*/
    {
        /*printf("\nBranche 1");*/
        PositLoop(NbPts, centeredImage, homogeneousWorldPts, objectMat, focalLength, center,Rot1, Trans, RotFinal1, TransFinal1);     /*ITERATIONS a partir de la premiere pose fournie*/
        /*PosCopl (BRANCHE 1)*/
    }
    
    if ((Rot2[0][0])!=2.0) /*pose1 a priori possible*/
    {
        /*printf("\nBranche 2");*/
        PositLoop(NbPts, centeredImage, homogeneousWorldPts, objectMat, focalLength, center, Rot2, Trans, RotFinal2, TransFinal2);        /*ITERATIONS a partir de la premiere pose fournie*/
        /*PosCopl (BRANCHE 2)*/
    }
    
    
    if ((RotFinal1[0][0]!=2)&&(RotFinal2[0][0]!=2))
    {
        ErrorC(NbPts,centeredImage,worldPts,focalLength,center,RotFinal1,TransFinal1,&E1,&Ep1,&Ehvmax1);
        ErrorC(NbPts,centeredImage,worldPts,focalLength,center,RotFinal2,TransFinal2,&E2,&Ep2,&Ehvmax2);
        
        if (E1<E2)
        {
            for (i=0;i<3;i++)
            {
                Trans[i]=TransFinal1[i];
                for (j=0;j<3;j++) Rot[i][j]=RotFinal1[i][j];
            }
        }
        else
        {
            for (i=0;i<3;i++)
            {
                Trans[i]=TransFinal2[i];
                for (j=0;j<3;j++) Rot[i][j]=RotFinal2[i][j];
            }
        }
    }
    
    if ((RotFinal1[0][0]!=2)&&(RotFinal2[0][0]==2))
    {
        /*Error(np,coplImage,copl,fLength,POSITRot1,POSITTrans1,&E1,&Ep1,&Ehvmax1);*/
        /*Error(np,coplImage,copl,fLength,POSITRot2,POSITTrans2,&E2,&Ep2,&Ehvmax2);*/
        /*printf("\nErhvmax1=%f Erhvmax2=%f\n",Ehvmax1,Ehvmax2); */
        for (i=0;i<3;i++)
        {
            Trans[i]=TransFinal1[i];
            for (j=0;j<3;j++) Rot[i][j]=RotFinal1[i][j];
        }
    }
    if ((RotFinal1[0][0]==2)&&(RotFinal2[0][0]!=2))
    {
        /*Error(np,coplImage,copl,fLength,POSITRot1,POSITTrans1,&E1,&Ep1,&Ehvmax1);*/
        /*Error(np,coplImage,copl,fLength,POSITRot2,POSITTrans2,&E2,&Ep2,&Ehvmax2);*/
        /*printf("\nErhvmax1=%f Erhvmax2=%f\n",Ehvmax1,Ehvmax2);*/
        for (i=0;i<3;i++)
        {
            Trans[i]=TransFinal2[i];
            for (j=0;j<3;j++) Rot[i][j]=RotFinal2[i][j];
        }
    }
    
    /* if ((POSITRot1[0][0]==2)&&(POSITRot2[0][0]==2))...CAS A IMPLEMENTER (n'apparait jamais)*/
    
    /*desallocations*/
    for (i=0;i<NbPts;i++) {
        free(homogeneousWorldPts[i]);
        free(centeredImage[i]);
    }
    free(homogeneousWorldPts);
    free(centeredImage);
    
    for (i=0;i<3;i++) {
        free(Rot1[i]);
        free(Rot2[i]);
        free(RotFinal1[i]);
        free(RotFinal2[i]);
    }
    free(Rot1);
    free(Rot2);
    free(RotFinal1);
    free(RotFinal2);
    
    free(TransFinal1);
    free(TransFinal2);
    
    for (i=0;i<3;i++) free(objectMat[i]);
    free(objectMat);
    
}