Here is my fvec(). I've cut some line from it (//...) , they are very similar, so I think they are irrelevant.
Indexes of fi are continuous.
Do you see mistake?
Mat is opencv's Mat.
Code:
static void fvec(const real_1d_array &x, real_1d_array &fi, void *ptr){
Process *pr=(Process *) ptr;
double q[4][4] = { { x[0],x[1],x[2],x[3] },{ x[4],x[5],x[6],x[7] },{ x[8],x[9],x[10],x[11] },{0,0,0,1} };
Mat A = Mat(4,4, CV_64FC1);
for(int i = 0; i<4; ++i)
for(int j = 0; j < 4; ++j){
A.at<double>(i,j) = q[i][j];
}
if(determinant(A) < 0.0){
fi[0]=alglib::maxrealnumber;
return;
}
fi[0] = q[0][0] * pr->q_p->x + q[0][1] * pr->q_p->y + q[0][2] * pr->q_p->z + q[0][3] * pr->q_p->mu - pr->eqr[0][0];
fi[1] = pow(q[0][0], 2.0) * pr->q_p->x2
+ 2 * q[0][0] * q[0][1] * pr->q_p->xy
+ 2 * q[0][0] * q[0][2] * pr->q_p->xz
+ 2 * q[0][0] * q[0][3] * pr->q_p->x
+ pow(q[0][1], 2.0) * pr->q_p->y2
+ 2 * q[0][1] * q[0][2] * pr->q_p->yz
+ 2 * q[0][1] * q[0][3] * pr->q_p->y
+ pow(q[0][2], 2.0) * pr->q_p->z2
+ 2 * q[0][2] * q[0][3] * pr->q_p->z
+ pow(q[0][3], 2.0) * pr->q_p->mu
- pr->eqr[1][0];
fi[2] = 3 * q[0][0] * pow(q[0][2], 2.0) * pr->q_p->xz2
+ 3 * q[0][1] * pow(q[0][2], 2.0) * pr->q_p->yz2
+ 3 * pow(q[0][0], 2.0) * q[0][1] * pr->q_p->x2y
+ 3 * pow(q[0][1], 2.0) * q[0][2] * pr->q_p->y2z
+ 3 * pow(q[0][0], 2.0) * q[0][2] * pr->q_p->x2z
+ 3 * q[0][0] * pow(q[0][1], 2.0) * pr->q_p->xy2
+ 6 * q[0][0] * q[0][1] * q[0][3] * pr->q_p->xy
+ 6 * q[0][0] * q[0][1] * q[0][2] * pr->q_p->xyz
+ 3 * pow(q[0][1], 2.0) * q[0][3] * pr->q_p->y2
+ 3 * q[0][1] * pow(q[0][3], 2.0) * pr->q_p->y
+ 3 * pow(q[0][2], 2.0) * q[0][3] * pr->q_p->z2
+ 3 * q[0][0] * pow(q[0][3], 2.0) * pr->q_p->x
+ 3 * q[0][2] * pow(q[0][3], 2.0) * pr->q_p->z
+ 3 * pow(q[0][0], 2.0) * q[0][3] * pr->q_p->x2
+ pow(q[0][1], 3.0) * pr->q_p->y3
+ pow(q[0][3], 3.0) * pr->q_p->mu
+ 6 * q[0][0] * q[0][2] * q[0][3] * pr->q_p->xz
+ 6 * q[0][1] * q[0][2] * q[0][3] * pr->q_p->yz
+ pow(q[0][0], 3.0) * pr->q_p->x3
+ pow(q[0][2], 3.0) * pr->q_p->z3
- pr->eqr[2][0];
fi[3] = q[1][0] * pr->q_p->x + q[1][1] * pr->q_p->y + q[1][2] * pr->q_p->z + q[1][3] * pr->q_p->mu - pr->eqr[0][1];
//...
fi[18] = q[0][0] * q[1][0] * q[2][1] * pr->q_p->x2y
+ q[0][1] * q[1][0] * q[2][3] * pr->q_p->xy
+ q[0][1] * q[1][1] * q[2][0] * pr->q_p->xy2
+ q[0][1] * q[1][1] * q[2][2] * pr->q_p->y2z
+ q[0][1] * q[1][2] * q[2][0] * pr->q_p->xyz
+ q[0][1] * q[1][2] * q[2][3] * pr->q_p->yz
+ q[0][1] * q[1][3] * q[2][0] * pr->q_p->xy
+ q[0][1] * q[1][3] * q[2][2] * pr->q_p->yz
+ q[0][0] * q[1][1] * q[2][3] * pr->q_p->xy
+ q[0][1] * q[1][2] * q[2][1] * pr->q_p->y2z
+ q[0][1] * q[1][2] * q[2][2] * pr->q_p->yz2
+ q[0][1] * q[1][0] * q[2][2] * pr->q_p->xyz
+ q[0][0] * q[1][0] * q[2][2] * pr->q_p->x2z
+ q[0][0] * q[1][1] * q[2][0] * pr->q_p->x2y
+ q[0][0] * q[1][1] * q[2][1] * pr->q_p->xy2
+ q[0][0] * q[1][1] * q[2][2] * pr->q_p->xyz
+ q[0][0] * q[1][2] * q[2][0] * pr->q_p->x2z
+ q[0][0] * q[1][2] * q[2][1] * pr->q_p->xyz
+ q[0][0] * q[1][2] * q[2][2] * pr->q_p->xz2
+ q[0][1] * q[1][1] * q[2][3] * pr->q_p->y2
+ q[0][3] * q[1][2] * q[2][3] * pr->q_p->z
+ q[0][1] * q[1][1] * q[2][1] * pr->q_p->y3
+ q[0][0] * q[1][0] * q[2][0] * pr->q_p->x3
+ q[0][0] * q[1][0] * q[2][3] * pr->q_p->x2
+ q[0][0] * q[1][3] * q[2][0] * pr->q_p->x2
+ q[0][0] * q[1][3] * q[2][3] * pr->q_p->x
+ q[0][2] * q[1][2] * q[2][3] * pr->q_p->z2
+ q[0][3] * q[1][0] * q[2][3] * pr->q_p->x
+ q[0][3] * q[1][1] * q[2][1] * pr->q_p->y2
+ q[0][3] * q[1][1] * q[2][3] * pr->q_p->y
+ q[0][2] * q[1][3] * q[2][2] * pr->q_p->z2
+ q[0][1] * q[1][3] * q[2][1] * pr->q_p->y2
+ q[0][1] * q[1][3] * q[2][3] * pr->q_p->y
+ q[0][2] * q[1][2] * q[2][2] * pr->q_p->z3
+ q[0][3] * q[1][3] * q[2][0] * pr->q_p->x
+ q[0][2] * q[1][3] * q[2][3] * pr->q_p->z
+ q[0][3] * q[1][0] * q[2][0] * pr->q_p->x2
+ q[0][3] * q[1][3] * q[2][3] * pr->q_p->mu
+ q[0][3] * q[1][3] * q[2][2] * pr->q_p->z
+ q[0][3] * q[1][3] * q[2][1] * pr->q_p->y
+ q[0][3] * q[1][2] * q[2][2] * pr->q_p->z2
+ q[0][0] * q[1][2] * q[2][3] * pr->q_p->xz
+ q[0][0] * q[1][3] * q[2][1] * pr->q_p->xy
+ q[0][0] * q[1][3] * q[2][2] * pr->q_p->xz
+ q[0][1] * q[1][0] * q[2][0] * pr->q_p->x2y
+ q[0][1] * q[1][0] * q[2][1] * pr->q_p->xy2
+ q[0][2] * q[1][0] * q[2][1] * pr->q_p->xyz
+ q[0][2] * q[1][0] * q[2][2] * pr->q_p->xz2
+ q[0][3] * q[1][0] * q[2][1] * pr->q_p->xy
+ q[0][3] * q[1][0] * q[2][2] * pr->q_p->xz
+ q[0][3] * q[1][1] * q[2][0] * pr->q_p->xy
+ q[0][3] * q[1][1] * q[2][2] * pr->q_p->yz
+ q[0][3] * q[1][2] * q[2][0] * pr->q_p->xz
+ q[0][3] * q[1][2] * q[2][1] * pr->q_p->yz
+ q[0][2] * q[1][0] * q[2][3] * pr->q_p->xz
+ q[0][2] * q[1][1] * q[2][0] * pr->q_p->xyz
+ q[0][2] * q[1][1] * q[2][1] * pr->q_p->y2z
+ q[0][2] * q[1][1] * q[2][2] * pr->q_p->yz2
+ q[0][2] * q[1][1] * q[2][3] * pr->q_p->yz
+ q[0][2] * q[1][2] * q[2][0] * pr->q_p->xz2
+ q[0][2] * q[1][2] * q[2][1] * pr->q_p->yz2
+ q[0][2] * q[1][3] * q[2][0] * pr->q_p->xz
+ q[0][2] * q[1][3] * q[2][1] * pr->q_p->yz
+ q[0][2] * q[1][0] * q[2][0] * pr->q_p->x2z
- pr->p_p->xyz;
Mat B = A.inv();
for(int i = 0; i<4; ++i)
for(int j = 0; j < 4; ++j){
q[i][j] = B.at<double>(i,j);
}
fi[19] = q[0][0] * pr->p_p->x + q[0][1] * pr->p_p->y + q[0][2] * pr->p_p->z + q[0][3] * pr->p_p->mu - pr->ieqr[0][0];
fi[20] = pow(q[0][0], 2.0) * pr->p_p->x2 + 2 * q[0][0] * q[0][1] * pr->p_p->xy
+ 2 * q[0][0] * q[0][2] * pr->p_p->xz
+ 2 * q[0][0] * q[0][3] * pr->p_p->x
+ pow(q[0][1], 2.0) * pr->p_p->y2
+ 2 * q[0][1] * q[0][2] * pr->p_p->yz
+ 2 * q[0][1] * q[0][3] * pr->p_p->y
+ pow(q[0][2], 2.0) * pr->p_p->z2
+ 2 * q[0][2] * q[0][3] * pr->p_p->z
+ pow(q[0][3], 2.0) * pr->p_p->mu
- pr->ieqr[1][0];
//...
fi[37] = q[0][0] * q[1][0] * q[2][1] * pr->p_p->x2y
+ q[0][1] * q[1][0] * q[2][3] * pr->p_p->xy
+ q[0][1] * q[1][1] * q[2][0] * pr->p_p->xy2
+ q[0][1] * q[1][1] * q[2][2] * pr->p_p->y2z
+ q[0][1] * q[1][2] * q[2][0] * pr->p_p->xyz
+ q[0][1] * q[1][2] * q[2][3] * pr->p_p->yz
+ q[0][1] * q[1][3] * q[2][0] * pr->p_p->xy
+ q[0][1] * q[1][3] * q[2][2] * pr->p_p->yz
+ q[0][0] * q[1][1] * q[2][3] * pr->p_p->xy
+ q[0][1] * q[1][2] * q[2][1] * pr->p_p->y2z
+ q[0][1] * q[1][2] * q[2][2] * pr->p_p->yz2
+ q[0][1] * q[1][0] * q[2][2] * pr->p_p->xyz
+ q[0][0] * q[1][0] * q[2][2] * pr->p_p->x2z
+ q[0][0] * q[1][1] * q[2][0] * pr->p_p->x2y
+ q[0][0] * q[1][1] * q[2][1] * pr->p_p->xy2
+ q[0][0] * q[1][1] * q[2][2] * pr->p_p->xyz
+ q[0][0] * q[1][2] * q[2][0] * pr->p_p->x2z
+ q[0][0] * q[1][2] * q[2][1] * pr->p_p->xyz
+ q[0][0] * q[1][2] * q[2][2] * pr->p_p->xz2
+ q[0][1] * q[1][1] * q[2][3] * pr->p_p->y2
+ q[0][3] * q[1][2] * q[2][3] * pr->p_p->z
+ q[0][1] * q[1][1] * q[2][1] * pr->p_p->y3
+ q[0][0] * q[1][0] * q[2][0] * pr->p_p->x3
+ q[0][0] * q[1][0] * q[2][3] * pr->p_p->x2
+ q[0][0] * q[1][3] * q[2][0] * pr->p_p->x2
+ q[0][0] * q[1][3] * q[2][3] * pr->p_p->x
+ q[0][2] * q[1][2] * q[2][3] * pr->p_p->z2
+ q[0][3] * q[1][0] * q[2][3] * pr->p_p->x
+ q[0][3] * q[1][1] * q[2][1] * pr->p_p->y2
+ q[0][3] * q[1][1] * q[2][3] * pr->p_p->y
+ q[0][2] * q[1][3] * q[2][2] * pr->p_p->z2
+ q[0][1] * q[1][3] * q[2][1] * pr->p_p->y2
+ q[0][1] * q[1][3] * q[2][3] * pr->p_p->y
+ q[0][2] * q[1][2] * q[2][2] * pr->p_p->z3
+ q[0][3] * q[1][3] * q[2][0] * pr->p_p->x
+ q[0][2] * q[1][3] * q[2][3] * pr->p_p->z
+ q[0][3] * q[1][0] * q[2][0] * pr->p_p->x2
+ q[0][3] * q[1][3] * q[2][3] * pr->p_p->mu
+ q[0][3] * q[1][3] * q[2][2] * pr->p_p->z
+ q[0][3] * q[1][3] * q[2][1] * pr->p_p->y
+ q[0][3] * q[1][2] * q[2][2] * pr->p_p->z2
+ q[0][0] * q[1][2] * q[2][3] * pr->p_p->xz
+ q[0][0] * q[1][3] * q[2][1] * pr->p_p->xy
+ q[0][0] * q[1][3] * q[2][2] * pr->p_p->xz
+ q[0][1] * q[1][0] * q[2][0] * pr->p_p->x2y
+ q[0][1] * q[1][0] * q[2][1] * pr->p_p->xy2
+ q[0][2] * q[1][0] * q[2][1] * pr->p_p->xyz
+ q[0][2] * q[1][0] * q[2][2] * pr->p_p->xz2
+ q[0][3] * q[1][0] * q[2][1] * pr->p_p->xy
+ q[0][3] * q[1][0] * q[2][2] * pr->p_p->xz
+ q[0][3] * q[1][1] * q[2][0] * pr->p_p->xy
+ q[0][3] * q[1][1] * q[2][2] * pr->p_p->yz
+ q[0][3] * q[1][2] * q[2][0] * pr->p_p->xz
+ q[0][3] * q[1][2] * q[2][1] * pr->p_p->yz
+ q[0][2] * q[1][0] * q[2][3] * pr->p_p->xz
+ q[0][2] * q[1][1] * q[2][0] * pr->p_p->xyz
+ q[0][2] * q[1][1] * q[2][1] * pr->p_p->y2z
+ q[0][2] * q[1][1] * q[2][2] * pr->p_p->yz2
+ q[0][2] * q[1][1] * q[2][3] * pr->p_p->yz
+ q[0][2] * q[1][2] * q[2][0] * pr->p_p->xz2
+ q[0][2] * q[1][2] * q[2][1] * pr->p_p->yz2
+ q[0][2] * q[1][3] * q[2][0] * pr->p_p->xz
+ q[0][2] * q[1][3] * q[2][1] * pr->p_p->yz
+ q[0][2] * q[1][0] * q[2][0] * pr->p_p->x2z
- pr->q_p->xyz;
}
Thanks for your help