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