47 #include <visp3/vision/vpLevenbergMarquartd.h> 48 #include <visp3/vision/vpPose.h> 52 #define MINIMUM 0.000001 54 #define DEBUG_LEVEL1 0 81 #define MIJ(m, i, j, s) ((m) + ((long)(i) * (long)(s)) + (long)(j)) 85 static double XI[NBPTMAX], YI[NBPTMAX];
86 static double XO[NBPTMAX], YO[NBPTMAX], ZO[NBPTMAX];
89 #define MINIMUM 0.000001 91 void eval_function(
int npt,
double *xc,
double *f);
92 void fcn(
int m,
int n,
double *xc,
double *fvecc,
double *jac,
int ldfjac,
int iflag);
94 void eval_function(
int npt,
double *xc,
double *f)
105 for (i = 0; i < npt; i++) {
106 double x = rd[0][0] * XO[i] + rd[0][1] * YO[i] + rd[0][2] * ZO[i] + xc[0];
107 double y = rd[1][0] * XO[i] + rd[1][1] * YO[i] + rd[1][2] * ZO[i] + xc[1];
108 double z = rd[2][0] * XO[i] + rd[2][1] * YO[i] + rd[2][2] * ZO[i] + xc[2];
109 f[i] = x / z - XI[i];
110 f[npt + i] = y / z - YI[i];
141 void fcn(
int m,
int n,
double *xc,
double *fvecc,
double *jac,
int ldfjac,
int iflag)
148 printf(
"pas assez de points\n");
152 eval_function(npt, xc, fvecc);
153 else if (iflag == 2) {
163 double tt = sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]);
171 double mco = 1.0 - co;
174 for (
int i = 0; i < npt; i++) {
180 double rx = rd[0][0] * x + rd[0][1] * y + rd[0][2] * z + xc[0];
181 double ry = rd[1][0] * x + rd[1][1] * y + rd[1][2] * z + xc[1];
182 double rz = rd[2][0] * x + rd[2][1] * y + rd[2][2] * z + xc[2];
187 double drxt = (si * u1 * u3 + co * u2) * z + (si * u1 * u2 - co * u3) * y + (si * u1 * u1 - si) * x;
188 double drxu1 = mco * u3 * z + mco * u2 * y + 2 * mco * u1 * x;
189 double drxu2 = si * z + mco * u1 * y;
190 double drxu3 = mco * u1 * z - si * y;
192 double dryt = (si * u2 * u3 - co * u1) * z + (si * u2 * u2 - si) * y + (co * u3 + si * u1 * u2) * x;
193 double dryu1 = mco * u2 * x - si * z;
194 double dryu2 = mco * u3 * z + 2 * mco * u2 * y + mco * u1 * x;
195 double dryu3 = mco * u2 * z + si * x;
197 double drzt = (si * u3 * u3 - si) * z + (si * u2 * u3 + co * u1) * y + (si * u1 * u3 - co * u2) * x;
198 double drzu1 = si * y + mco * u3 * x;
199 double drzu2 = mco * u3 * y - si * x;
200 double drzu3 = 2 * mco * u3 * z + mco * u2 * y + mco * u1 * x;
205 double dxit = drxt / rz - rx * drzt / (rz * rz);
207 double dyit = dryt / rz - ry * drzt / (rz * rz);
209 double dxiu1 = drxu1 / rz - drzu1 * rx / (rz * rz);
210 double dyiu1 = dryu1 / rz - drzu1 * ry / (rz * rz);
212 double dxiu2 = drxu2 / rz - drzu2 * rx / (rz * rz);
213 double dyiu2 = dryu2 / rz - drzu2 * ry / (rz * rz);
215 double dxiu3 = drxu3 / rz - drzu3 * rx / (rz * rz);
216 double dyiu3 = dryu3 / rz - drzu3 * ry / (rz * rz);
222 *MIJ(jac, 0, i, ldfjac) = 1 / rz;
223 *MIJ(jac, 1, i, ldfjac) = 0.0;
224 *MIJ(jac, 2, i, ldfjac) = -rx / (rz * rz);
226 *MIJ(jac, 3, i, ldfjac) = u1 * dxit + (1 - u1 * u1) * dxiu1 / tt - u1 * u2 * dxiu2 / tt - u1 * u3 * dxiu3 / tt;
227 *MIJ(jac, 4, i, ldfjac) = u2 * dxit - u1 * u2 * dxiu1 / tt + (1 - u2 * u2) * dxiu2 / tt - u2 * u3 * dxiu3 / tt;
229 *MIJ(jac, 5, i, ldfjac) = u3 * dxit - u1 * u3 * dxiu1 / tt - u2 * u3 * dxiu2 / tt + (1 - u3 * u3) * dxiu3 / tt;
231 *MIJ(jac, 3, i, ldfjac) = 0.0;
232 *MIJ(jac, 4, i, ldfjac) = 0.0;
233 *MIJ(jac, 5, i, ldfjac) = 0.0;
235 *MIJ(jac, 0, npt + i, ldfjac) = 0.0;
236 *MIJ(jac, 1, npt + i, ldfjac) = 1 / rz;
237 *MIJ(jac, 2, npt + i, ldfjac) = -ry / (rz * rz);
239 *MIJ(jac, 3, npt + i, ldfjac) =
240 u1 * dyit + (1 - u1 * u1) * dyiu1 / tt - u1 * u2 * dyiu2 / tt - u1 * u3 * dyiu3 / tt;
241 *MIJ(jac, 4, npt + i, ldfjac) =
242 u2 * dyit - u1 * u2 * dyiu1 / tt + (1 - u2 * u2) * dyiu2 / tt - u2 * u3 * dyiu3 / tt;
243 *MIJ(jac, 5, npt + i, ldfjac) =
244 u3 * dyit - u1 * u3 * dyiu1 / tt - u2 * u3 * dyiu2 / tt + (1 - u3 * u3) * dyiu3 / tt;
246 *MIJ(jac, 3, npt + i, ldfjac) = 0.0;
247 *MIJ(jac, 4, npt + i, ldfjac) = 0.0;
248 *MIJ(jac, 5, npt + i, ldfjac) = 0.0;
265 std::cout <<
"begin CCalcuvpPose::PoseLowe(...) " << std::endl;
270 int info, ipvt[NBR_PAR];
272 double f[2 * NBPTMAX], sol[NBR_PAR];
273 double tol, jac[NBR_PAR][2 * NBPTMAX], wa[2 * NBPTMAX + 50];
279 lwa = 2 * NBPTMAX + 50;
280 ldfjac = 2 * NBPTMAX;
281 tol = std::numeric_limits<double>::epsilon();
290 for (
unsigned int i = 0; i < 3; i++) {
297 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
306 tst_lmder = lmder1(&fcn, m, n, sol, f, &jac[0][0], ldfjac, tol, &info, ipvt, lwa, wa);
307 if (tst_lmder == -1) {
308 std::cout <<
" in CCalculPose::PoseLowe(...) : ";
309 std::cout <<
"pb de minimisation, returns FATAL_ERROR";
313 for (
unsigned int i = 0; i < 3; i++)
316 for (
unsigned int i = 0; i < 3; i++) {
327 std::cout <<
"end CCalculPose::PoseLowe(...) " << std::endl;
Implementation of an homogeneous matrix and operations on such kind of matrices.
double get_oY() const
Get the point Y coordinate in the object frame.
double get_y() const
Get the point y coordinate in the image plane.
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Class that defines what is a point.
Implementation of a rotation matrix and operations on such kind of matrices.
void insert(const vpRotationMatrix &R)
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
double get_oZ() const
Get the point Z coordinate in the object frame.
void poseLowe(vpHomogeneousMatrix &cMo)
Compute the pose using the Lowe non linear approach it consider the minimization of a residual using ...
void extract(vpRotationMatrix &R) const
double get_x() const
Get the point x coordinate in the image plane.
double get_oX() const
Get the point X coordinate in the object frame.
Implementation of a rotation vector as axis-angle minimal representation.