void ConjugateGradient( float **A, float *b, float *x, int N, int iterations)
{
// vectors
float r[N];
float w[N];
float z[N];
// scalars
float alpha;
float beta;
float temp[N];
// initialization of residual vector: r
matrixVectorProduct( A, x, temp, N);
vectorDifference( b, temp, r, N);
// w
vectorScale( r, -1, w, N);
// z
matrixVectorProduct( A, w, z, N);
// alpha
alpha = dotProduct( r, w, N) / dotProduct( w, z, N);
// beta
beta = 0.;
// x
vectorScale( w, alpha, temp, N);
vectorSum( x, temp, x, N);
for( int i=0; i<iterations; i++){
vectorScale( z, alpha, temp, N);
vectorDifference( r, temp, r, N);
if( sqrt( dotProduct( r, r, N)) < 1e-10)
return;
// B = (r'*z)/(w'*z);
beta = dotProduct( r, z, N) / dotProduct( w, z, N);
// w = -r + B*w;
vectorScale( w, beta, w, N);
vectorDifference( w, r, w, N);
// z = A*w;
matrixVectorProduct( A, w, z, N);
// a = (r'*w)/(w'*z);
alpha = dotProduct( r, w, N) / dotProduct( w, z, N);
// x = x + a*w;
vectorScale( w, alpha, temp, N);
vectorSum( x, temp, x, N);
}
}
void
applyTransforms(struct trans *tlist, struct GENode *glist) {
struct trans *ptl = tlist;
struct GENode *pgl = NULL;
struct transMatrix m, aux;
struct homoCoord start;
struct homoCoord end;
while(NULL != ptl) {
pgl = glist;
while(NULL != pgl) {
switch(pgl->el.type)
{
case LINE:
initHomoVector(&start, pgl->el.data.line.st.x, pgl->el.data.line.st.y);
initHomoVector(&end, pgl->el.data.line.en.x, pgl->el.data.line.en.y);
break;
default:
break;
}
switch(ptl->tType) {
case TRANSLATION:
initTranslation(&m, ptl->data.t.tx, ptl->data.t.ty);
break;
case SCALING:
initScale(&m, ptl->data.s.sx, ptl->data.s.sy);
if (!(ptl->data.s.px == 0 && ptl->data.s.py == 0))
{
initTranslation(&aux, -1*ptl->data.s.px, -1*ptl->data.s.py);
matrixProduct(&m, m, aux);
initTranslation(&aux, ptl->data.s.px, ptl->data.s.py);
matrixProduct(&m, m, aux);
}
break;
case ROTATION:
initRotation(&m, ptl->data.r.u);
if (!(ptl->data.s.px == 0 && ptl->data.s.py == 0))
{
initTranslation(&aux, -1*ptl->data.r.px, -1*ptl->data.r.py);
matrixProduct(&m, m, aux);
initTranslation(&aux, ptl->data.s.px, ptl->data.s.py);
matrixProduct(&m, m, aux);
}
break;
default:
break;
}
matrixVectorProduct(&start, m, start);
matrixVectorProduct(&end, m, end);
twoDCoord(&pgl->el.data.line.st, start);
twoDCoord(&pgl->el.data.line.en, end);
pgl = pgl->next;
}
ptl = ptl->next;
}
}
void ConjugateGradient3( float **A, float *b, float *x, int N, int iterations)
{
float r[N];
float r2[N];
float p[N];
float q[N];
//float beta[N];
//float alpha[N];
float beta;
float alpha;
float temp[N];
// initialization of residual vector: r_0
matrixVectorProduct( A, x, temp, N);
vectorDifference( b, temp, r, N);
// initialization of p_0
vectorCopy( r, p, N);
for( int i=0; i<iterations; i++){
// q_k
matrixVectorProduct( A, p, q, N);
alpha = dotProduct( r, r, N);
if(i>0){
beta = alpha / dotProduct( r2, r2, N);
vectorScale( p, beta, p, N);
vectorSum( r, p, p, N);
}
alpha /= dotProduct( p, q, N);
// next x
vectorScale( p, alpha, temp, N);
vectorSum( x, temp, x, N);
// old r
vectorCopy( r, r2, N);
// next r
vectorScale( q, -alpha, temp, N);
vectorSum( r, temp, r, N);
//float *pointer ;
}
}
double* feedForward(NeuralNetwork* net, double* inputs) {
unsigned int i, cSize, nSize;
double* cla, *nlp, *nla;
if(!inputs || !isValidNet(net)) {
return NULL;
}
//copy inputs (for later reference)
for(i = 0; i < net->layerSizes[0]; i++) {
net->preActivations[0][i] = inputs[i];
net->activations[0][i] = inputs[i];
}
//feed forward to each successive layer
for(i = 1; i < net->layers; i++) {
cla = net->activations[i - 1];
nlp = net->preActivations[i];
nla = net->activations[i];
cSize = net->layerSizes[i - 1];
nSize = net->layerSizes[i];
//multiply current layer by weights
matrixVectorProduct(net->weights[i - 1], cla, nlp, nSize, cSize);
//add biases wa => wa + b
add(nlp, net->biases[i - 1], nlp, nSize);
//up until now everything is saved in preactivation function
//now the output of the activation function is stored in the activation vector instead
//apply activation function wa + b => f(wa + b)
applyOnEach(nlp, nla, net->activationFunction, nSize);
}
return net->activations[i - 1];
}
void KinematicMotion::addRotation(const double *xAxisNew, const double *yAxisNew,
const double *zAxisNew, bool normalized) {
double e0[3];
double e1[3];
double e2[3];
copyVector(xAxisNew, e0);
copyVector(yAxisNew, e1);
copyVector(zAxisNew, e2);
if (!normalized) {
normalizeVector(e0);
normalizeVector(e1);
normalizeVector(e2);
}
double newRotation[9];
for (int i = 0; i < 3; i++) {
newRotation[i * 3 + 0] = e0[i];
newRotation[i * 3 + 1] = e1[i];
newRotation[i * 3 + 2] = e2[i];
}
//R'*(R*x + t) = R'*R*x + R'*t
matrixProduct(newRotation, rotationMatrix);
matrixVectorProduct(newRotation, translationVector);
}
void KinematicMotion::addRotation(const double *vec0, const double *vec1, bool normalized) { // not unique, but shortest path
// rotation is defined in the CURRENT coordinates system
double e0[3];
copyVector(vec0, e0);
double e1[3];
copyVector(vec1, e1);
if (!normalized) {
normalizeVector(e0);
normalizeVector(e1);
}
// algorithm form Non-linear Modeling and Analysis of Solids and Structures (Steen Krenk) P54
double e2[3];
e2[0] = e0[0] + e1[0];
e2[1] = e0[1] + e1[1];
e2[2] = e0[2] + e1[2];
double e2_square = vectorProduct(e2, e2);
double newRotation[9];
newRotation[0] = 1.0 + 2.0 * e1[0] * e0[0] - 2.0 / e2_square * e2[0] * e2[0];
newRotation[1] = 0.0 + 2.0 * e1[0] * e0[1] - 2.0 / e2_square * e2[0] * e2[1];
newRotation[2] = 0.0 + 2.0 * e1[0] * e0[2] - 2.0 / e2_square * e2[0] * e2[2];
newRotation[3] = 0.0 + 2.0 * e1[1] * e0[0] - 2.0 / e2_square * e2[1] * e2[0];
newRotation[4] = 1.0 + 2.0 * e1[1] * e0[1] - 2.0 / e2_square * e2[1] * e2[1];
newRotation[5] = 0.0 + 2.0 * e1[1] * e0[2] - 2.0 / e2_square * e2[1] * e2[2];
newRotation[6] = 0.0 + 2.0 * e1[2] * e0[0] - 2.0 / e2_square * e2[2] * e2[0];
newRotation[7] = 0.0 + 2.0 * e1[2] * e0[1] - 2.0 / e2_square * e2[2] * e2[1];
newRotation[8] = 1.0 + 2.0 * e1[2] * e0[2] - 2.0 / e2_square * e2[2] * e2[2];
//R'*(R*x + t) = R'*R*x + R'*t
matrixProduct(newRotation, rotationMatrix);
matrixVectorProduct(newRotation, translationVector);
}
void KinematicMotion::addRotation(const double *axis, bool normalized, double angle) {
// rotation is defined in the CURRENT coordinates system
double u[3];
copyVector(axis, u);
if (!normalized) {
normalizeVector(u);
}
// algorithm from http://en.wikipedia.org/wiki/Rotation_matrix
double c = cos(angle);
double s = sin(angle);
double newRotation[9];
newRotation[0] = c + u[0] * u[0] * (1.0 - c);
newRotation[1] = u[0] * u[1] * (1.0 - c) - u[2] * s;
newRotation[2] = u[0] * u[2] * (1.0 - c) + u[1] * s;
newRotation[3] = u[1] * u[0] * (1.0 - c) + u[2] * s;
newRotation[4] = c + u[1] * u[1] * (1.0 - c);
newRotation[5] = u[1] * u[2] * (1.0 - c) - u[0] * s;
newRotation[6] = u[2] * u[0] * (1.0 - c) - u[1] * s;
newRotation[7] = u[2] * u[1] * (1.0 - c) + u[0] * s;
newRotation[8] = c + u[2] * u[2] * (1.0 - c);
//R'*(R*x + t) = R'*R*x + R'*t
matrixProduct(newRotation, rotationMatrix);
matrixVectorProduct(newRotation, translationVector);
}
void KinematicMotion::move(double *coordinates) const {
// rotate first, then translate: x=R*x + T. (Remark: R*(x+T) is wrong)
matrixVectorProduct(rotationMatrix, coordinates);
vectorAddition(coordinates, translationVector);
}
void KinematicMotion::addRotation(const double *_rotationMatrix) {
//R'*(R*x + t) = R'*R*x + R'*t
matrixProduct(_rotationMatrix, rotationMatrix);
matrixVectorProduct(_rotationMatrix, translationVector);
}
void solveHomographiePro(float **imgPts, float **imgPts2, float *h){
//PUNTO 4 --X: 208.142039
//PUNTO 4 --Y: 231.760118
//PUNTO 5 --X: 208.254415
//PUNTO 5 --Y: 165.749664
//PUNTO 6 --X: 140.647865
//PUNTO 6 --Y: 165.646337
//PUNTO 7 --X: 140.951607
//PUNTO 7 --Y: 232.487685
/*
imgPts ---> x,y puntos detectados por el filtro
imgPts2 ---> i,j puntos sinteticos absolutos a partir de los cuales se pretende hacer la transformacion
*/
float ** A;
float ** Ainv;
float * imgPtsmod;
int j;
//Reservo memoria
A=(float **)malloc(24 * sizeof(float *));
for (int i=0;i<24;i++) A[i]=(float *)malloc(8 * sizeof(float));
Ainv=(float **)malloc(24 * sizeof(float *));
for (int i=0;i<24;i++) Ainv[i]=(float *)malloc(8 * sizeof(float));
imgPtsmod=(float *)malloc(24 * sizeof(float));
//Asignacion de valores
j=0;
for (int i=0; i<12; i++) {
A[j][0]=imgPts2[i][0];
A[j][1]=imgPts2[i][1];
A[j][2]=1;
A[j][3]=0;
A[j][4]=0;
A[j][5]=0;
A[j][6]=-imgPts2[i][0]*imgPts[i][0];
A[j][7]=-imgPts2[i][1]*imgPts[i][0];
A[j+1][0]=0;
A[j+1][1]=0;
A[j+1][2]=0;
A[j+1][3]=imgPts2[i][0];
A[j+1][4]=imgPts2[i][1];
A[j+1][5]=1;
A[j+1][6]=-imgPts2[i][0]*imgPts[i][1];
A[j+1][7]=-imgPts2[i][1]*imgPts[i][1];
j=j+2;
}
/*
imgPts2=[i1 j1; i2 j2; i3 j3; i4 j4] ---> 4x2
imgPts2mod=[i1; j1; i2; j2; i3; j3; i4; j4] ---> 8x1
h = Ainv(8x8) * imgPts2mod(8x1)
*/
imgPtsmod[0]=imgPts[0][0];
imgPtsmod[1]=imgPts[0][1];
imgPtsmod[2]=imgPts[1][0];
imgPtsmod[3]=imgPts[1][1];
imgPtsmod[4]=imgPts[2][0];
imgPtsmod[5]=imgPts[2][1];
imgPtsmod[6]=imgPts[3][0];
imgPtsmod[7]=imgPts[3][1];
imgPtsmod[8]=imgPts[4][0];
imgPtsmod[9]=imgPts[4][1];
imgPtsmod[10]=imgPts[5][0];
imgPtsmod[12]=imgPts[5][1];
imgPtsmod[12]=imgPts[6][0];
imgPtsmod[13]=imgPts[6][1];
imgPtsmod[14]=imgPts[7][0];
imgPtsmod[15]=imgPts[7][1];
imgPtsmod[16]=imgPts[8][0];
imgPtsmod[17]=imgPts[8][1];
imgPtsmod[18]=imgPts[9][0];
imgPtsmod[19]=imgPts[9][1];
imgPtsmod[20]=imgPts[10][0];
imgPtsmod[21]=imgPts[10][1];
imgPtsmod[22]=imgPts[11][0];
imgPtsmod[23]=imgPts[11][1];
//inicializo h en 0
for(int i=0;i<24;i++)h[i]=0;
//Resuelvo sistema A*h=imgPts2mod
PseudoInverseGen(A,24,8,Ainv);
// printf("resultado inside matrixVectorProduct\n");
matrixVectorProduct(Ainv,24,imgPtsmod,h);
// //PRINTS
// printf("PUNTOS IMAGE POINTS\n");
// printf("VECTOR imgPts\n");
// for(int i=0;i<4;i++)
// {
// printf("%f\t",imgPts[i][0]);
// printf("%f\t",imgPts[i][1]);
// printf("\n");
// }
//
// printf("PUNTOS INVENTADOS\n");
// for(int i=0;i<4;i++)
// {
// printf("%f\t",imgPts2[i][0]);
// printf("%f\t",imgPts2[i][1]);
// printf("\n");
// }
//
// printf("Vector imgPtsmod\n");
// for(int i=0;i<8;i++)
// {
// printf("%f\t",imgPtsmod[i]);
// printf("\n");
// }
//
printf("MATRIZ A\n");
for(int i=0;i<24;i++)
{
for(j=0;j<8;j++)
printf("%f\t",A[i][j]);
printf("\n");
}
//
//
// printf("Vector h\n");
// for(int i=0;i<8;i++)
// {
// printf("%f\t",h[i]);
// printf("\n");
// }
// printf("FIN PRINT\n");
//Libero memoria
for (int i=0;i<24;i++) free(A[i]);
free(A);
for (int i=0;i<24;i++) free(Ainv[i]);
free(Ainv);
free(imgPtsmod);
}
void solveAffineTransformation(float **imgPts, float **imgPts2, float *h){
//PUNTO 4 --X: 208.142039
//PUNTO 4 --Y: 231.760118
//PUNTO 5 --X: 208.254415
//PUNTO 5 --Y: 165.749664
//PUNTO 6 --X: 140.647865
//PUNTO 6 --Y: 165.646337
//PUNTO 7 --X: 140.951607
//PUNTO 7 --Y: 232.487685
/*
imgPts ---> x,y puntos detectados por el filtro
imgPts2 ---> i,j puntos sinteticos absolutos a partir de los cuales se pretende hacer la transformacion
*/
float ** A;
float ** Ainv;
float * imgPtsmod;
int j;
//Reservo memoria
A=(float **)malloc(6 * sizeof(float *));
for (int i=0;i<6;i++) A[i]=(float *)malloc(6 * sizeof(float));
Ainv=(float **)malloc(6 * sizeof(float *));
for (int i=0;i<6;i++) Ainv[i]=(float *)malloc(6 * sizeof(float));
imgPtsmod=(float *)malloc(6 * sizeof(float));
//Asignacion de valores
j=0;
for (int i=0; i<3; i++) {
A[j][0]=imgPts2[i][0];
A[j][1]=imgPts2[i][1];
A[j][2]=1;
A[j][3]=0;
A[j][4]=0;
A[j][5]=0;
A[j+1][0]=0;
A[j+1][1]=0;
A[j+1][2]=0;
A[j+1][3]=imgPts2[i][0];
A[j+1][4]=imgPts2[i][1];
A[j+1][5]=1;
j=j+2;
}
/*
imgPts2=[i1 j1; i2 j2; i3 j3] ---> 3x2
imgPts2mod=[i1; j1; i2; j2; i3; j3] ---> 6x1
h = Ainv(6x6) * imgPtsmod(6x1)
*/
imgPtsmod[0]=imgPts[0][0];
imgPtsmod[1]=imgPts[0][1];
imgPtsmod[2]=imgPts[1][0];
imgPtsmod[3]=imgPts[1][1];
imgPtsmod[4]=imgPts[2][0];
imgPtsmod[5]=imgPts[2][1];
//inicializo h en 0
for(int i=0;i<6;i++)h[i]=0;
//Resuelvo sistema A*h=imgPts2mod
PseudoInverseGen(A,6,6,Ainv);
printf("resultado inside matrixVectorProduct\n");
matrixVectorProduct(Ainv,6,imgPtsmod,h);
//PRINTS
// printf("PUNTOS IMAGE POINTS\n");
// printf("VECTOR imgPts\n");
// for(int i=0;i<3;i++)
// {
// printf("%f\t",imgPts[i][0]);
// printf("%f\t",imgPts[i][1]);
// printf("\n");
// }
//
// printf("PUNTOS INVENTADOS\n");
// for(int i=0;i<3;i++)
// {
// printf("%f\t",imgPts2[i][0]);
// printf("%f\t",imgPts2[i][1]);
// printf("\n");
// }
//
// printf("Vector imgPtsmod\n");
// for(int i=0;i<6;i++)
// {
// printf("%f\t",imgPtsmod[i]);
// printf("\n");
// }
//
// printf("MATRIZ A\n");
// for(int i=0;i<6;i++)
// {
// for(j=0;j<6;j++)
// printf("%f\t",A[i][j]);
// printf("\n");
// }
//
//
// printf("Vector h\n");
// for(int i=0;i<6;i++)
// {
// printf("%f\t",h[i]);
// printf("\n");
// }
// printf("FIN PRINT\n");
//Libero memoria
for (int i=0;i<6;i++) free(A[i]);
free(A);
for (int i=0;i<6;i++) free(Ainv[i]);
free(Ainv);
free(imgPtsmod);
}
