// compute the vector VEC between a 3D point P0 and a line (
// passing through two 3D points P and S)
double vec_pt_to_line3D(double *P,double *S,double *P0,double *VEC)
{
double diff10[3];
double diff21[3];
double dot_prod_diff10_diff21;
double t,norm_diff21;
double VT[3];
double diffVT0[3];
double norm_diffVT0;
VEC_DIFF(diff10,P,P0);
VEC_DIFF(diff21,S,P);
VEC_LENGTH(norm_diff21,diff21);
VEC_DOT_PRODUCT(dot_prod_diff10_diff21,diff10,diff21);
t=-dot_prod_diff10_diff21 / ( norm_diff21*norm_diff21 );
VEC_COPY(VT,P)
VEC_ACCUM(VT,t,diff21);
VEC_DIFF(diffVT0,VT,P0);
for(int t=0;t<3;t++)
{
VEC[t] = P0[t];
VEC[t+3] = VT[t];
}
VEC_LENGTH(norm_diffVT0,diffVT0);
return norm_diffVT0;
}
// 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 gim_merge_contacts_unique(GDYNAMIC_ARRAY * source_contacts,
GDYNAMIC_ARRAY * dest_contacts)
{
dest_contacts->m_size = 0;
//Traverse the source contacts
GUINT32 source_count = source_contacts->m_size;
if(source_count==0) return;
GIM_CONTACT * psource_contacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,(*source_contacts));
//add the unique contact
GIM_CONTACT * pcontact = 0;
GIM_DYNARRAY_PUSH_EMPTY(GIM_CONTACT,(*dest_contacts));
pcontact = GIM_DYNARRAY_POINTER_LAST(GIM_CONTACT,(*dest_contacts));
//set the first contact
GIM_COPY_CONTACTS(pcontact, psource_contacts);
if(source_count==1) return;
//scale the first contact
VEC_SCALE(pcontact->m_normal,pcontact->m_depth,pcontact->m_normal);
psource_contacts++;
//Average the contacts
GUINT32 i;
for(i=1;i<source_count;i++)
{
VEC_SUM(pcontact->m_point,pcontact->m_point,psource_contacts->m_point);
VEC_ACCUM(pcontact->m_normal,psource_contacts->m_depth,psource_contacts->m_normal);
psource_contacts++;
}
GREAL divide_average = 1.0f/((GREAL)source_count);
VEC_SCALE(pcontact->m_point,divide_average,pcontact->m_point);
pcontact->m_depth = VEC_DOT(pcontact->m_normal,pcontact->m_normal)*divide_average;
GIM_SQRT(pcontact->m_depth,pcontact->m_depth);
VEC_NORMALIZE(pcontact->m_normal);
/*GREAL normal_len;
VEC_INV_LENGTH(pcontact->m_normal,normal_len);
VEC_SCALE(pcontact->m_normal,normal_len,pcontact->m_normal);
//Deep = LEN(normal)/SQRT(source_count)
GIM_SQRT(divide_average,divide_average);
pcontact->m_depth = divide_average/normal_len;
*/
}
void PositBranches(int NbPts, double **centeredImage, double** worldPts, double**objectMat, double** Rot1, double** Rot2, double* Trans){
int i,j;
double r1T[4],r2T[4],U[3],u[3],IVect[3],JVect[3],row1[3],row2[3],row3[3];
double I0I0, J0J0, I0J0;
double scale;
double NU;
int firstNonCol;
double delta,lambda,mu,q,zi,zmin1,zmin2;
// if (false) {
// printf("\nIMAGE POINTS A LA ENTRADA DE POSITBRANCHES:\n");
// for (i=0; i<NbPts; i++) {
// printf("%g\t%g\n",centeredImage[i][0],centeredImage[i][1]);
// }
//
// }
double b[3];
double r1Taux[3]={0,0,0};
double r2Taux[3]={0,0,0};
double r11Taux[3]={0,0,0};
double r22Taux[3]={0,0,0};
for (i=0; i<NbPts; i++) {
b[0]=worldPts[i][0];
b[1]=worldPts[i][1];
b[2]=1;
VEC_ACCUM(r11Taux,centeredImage[i][0],b);
VEC_ACCUM(r22Taux,centeredImage[i][1],b);
}
VEC_DOT_MAT_3X3(r1Taux,r11Taux,objectMat);
VEC_DOT_MAT_3X3(r2Taux,r22Taux,objectMat)
r1T[0]=r1Taux[0];
r1T[1]=r1Taux[1];
r1T[2]=0;
r1T[3]=r1Taux[2];
r2T[0]=r2Taux[0];
r2T[1]=r2Taux[1];
r2T[2]=0;
r2T[3]=r2Taux[2];
I0I0=r1T[0]*r1T[0]+r1T[1]*r1T[1]+r1T[2]*r1T[2];
J0J0=r2T[0]*r2T[0]+r2T[1]*r2T[1]+r2T[2]*r2T[2];
I0J0=r1T[0]*r2T[0]+r1T[1]*r2T[1]+r1T[2]*r2T[2];
// if (false){
// printf("I0I0J0J0I0J0\n");
// printf("%g\t%g\t%g\n",I0I0,J0J0,I0J0);
// }
/*Computation of u, unit vector normal to the image plane*/
firstNonCol=2;
NU=0.0;
while (NU==0.0)
{
U[0]=worldPts[1][1]*worldPts[firstNonCol][2]-worldPts[1][2]*worldPts[firstNonCol][1];
U[1]=worldPts[1][2]*worldPts[firstNonCol][0]-worldPts[1][0]*worldPts[firstNonCol][2];
U[2]=worldPts[1][0]*worldPts[firstNonCol][1]-worldPts[1][1]*worldPts[firstNonCol][0];
NU=sqrt(U[0]*U[0]+U[1]*U[1]+U[2]*U[2]);
firstNonCol++;
}
for (i=0;i<3;i++) u[i]=U[i]/NU;
/*Computation of mu and lambda*/
// delta=sqrt((J0J0-I0I0)*(J0J0-I0I0)+4*(I0J0*I0J0));
// if ((I0I0-J0J0)>=0) q=atan((-2*I0J0)/((J0J0-I0I0)*(J0J0-I0I0)));
// else q=atan((-2*I0J0)/((J0J0-I0I0)*(J0J0-I0I0)))+MY_PI/2;
// {
// lambda=sqrt(delta)*cos(q/2);
// if (lambda==0.0) mu=0.0;
// else mu=sqrt(delta)*sin(q/2);
// }
delta=(J0J0-I0I0)*(J0J0-I0I0)+4*(I0J0*I0J0);
if ((I0I0-J0J0)>=0) q=-(I0I0-J0J0+sqrt(delta))/2;
else q=-(I0I0-J0J0-sqrt(delta))/2;
if (q>=0)
{
lambda=sqrt(q);
if (lambda==0.0) mu=0.0;
else mu=-I0J0/sqrt(q);
}
else
{
lambda=sqrt(-(I0J0*I0J0)/q);
if (lambda==0.0) mu=sqrt(I0I0-J0J0);
else mu=-I0J0/sqrt(-(I0J0*I0J0)/q);
}
// printf("\nlambda=%f\t mu=%f\n",lambda,mu);
/*First Rotation Matrix computation*/
for (i=0;i<3;i++)
{
IVect[i]=r1T[i]+lambda*u[i];
JVect[i]=r2T[i]+mu*u[i];
}
scale=sqrt(sqrt(IVect[0]*IVect[0]+IVect[1]*IVect[1]+IVect[2]*IVect[2])*sqrt(JVect[0]*JVect[0]+JVect[1]*JVect[1]+JVect[2]*JVect[2]));
for (i=0;i<3;i++)
{
row1[i]=IVect[i]/scale;
row2[i]=JVect[i]/scale;
}
row3[0]=row1[1]*row2[2]-row1[2]*row2[1];
row3[1]=row2[0]*row1[2]-row1[0]*row2[2];
row3[2]=row1[0]*row2[1]-row1[1]*row2[0];
for (i=0;i<3;i++)
{
Rot1[0][i]=row1[i];
Rot1[1][i]=row2[i];
Rot1[2][i]=row3[i];
}
// printf("\nRot1\n");
// for (i=0;i<3;i++){
// for (j=0;j<3;j++) printf("%f\t",Rot1[i][j]);
// printf("\n");
// }
//
/*Second Rotation matrix computation*/
for (i=0;i<3;i++)
{
IVect[i]=r1T[i]-lambda*u[i];
JVect[i]=r2T[i]-mu*u[i];
}
for (i=0;i<3;i++)
{
row1[i]=IVect[i]/scale;
row2[i]=JVect[i]/scale;
}
row3[0]=row1[1]*row2[2]-row1[2]*row2[1];
row3[1]=row2[0]*row1[2]-row1[0]*row2[2];
row3[2]=row1[0]*row2[1]-row1[1]*row2[0];
for (i=0;i<3;i++)
{
Rot2[0][i]=row1[i];
Rot2[1][i]=row2[i];
Rot2[2][i]=row3[i];
}
// printf("\nRot2\n");
// for (i=0;i<3;i++){
// for (j=0;j<3;j++) printf("%f\t",Rot2[i][j]);
// printf("\n");
// }
/* computation of translation*/
Trans[0]=r1T[3]/scale;
Trans[1]=r2T[3]/scale;
Trans[2]=1/scale;
// printf("\nTrans\n");
// for (j=0;j<3;j++){
// printf("%f\t",Trans[j]);
// }
// printf("\n");
/*calculation of the minimum depths zi of the model points in the camera coordinate system, for the first pose*/
for (i=0;i<NbPts;i++)
{
zi=Trans[2]+(Rot1[2][0]*worldPts[i][0]+
Rot1[2][1]*worldPts[i][1]+
Rot1[2][2]*worldPts[i][2]);
if (i==0) zmin1=zi;
if (zi<zmin1) zmin1=zi;
}
/*calculation of the minimum depths zi of the model points in the camera coordinate system, for the second pose*/
for (i=0;i<NbPts;i++)
{
zi=Trans[2]+(Rot2[2][0]*worldPts[i][0]+
Rot2[2][1]*worldPts[i][1]+
Rot2[2][2]*worldPts[i][2]);
if (i==0) zmin2=zi;
if (zi<zmin2) zmin2=zi; //Afshin: I corrected this. It was zmin1 by mistake
// if (zi<zmin1) zmin2=zi; //Afshin: This was the original code
}
/*In case of incoherent pose, the first element of the rotation matrix is set to 2.*/
/*(object points behind image plane)*/
if (zmin1<0||zmin1!=zmin1) Rot1[0][0]=2;
if (zmin2<0||zmin2!=zmin2) Rot2[0][0]=2;
}
