41 #include <visp3/tt/vpTemplateTracker.h>
42 #include <visp3/tt/vpTemplateTrackerBSpline.h>
45 : nbLvlPyr(1), l0Pyr(0), pyrInitialised(false), evolRMS(0), x_pos(), y_pos(),
46 evolRMS_eps(1e-4), ptTemplate(NULL), ptTemplatePyr(NULL), ptTemplateInit(false),
47 templateSize(0), templateSizePyr(NULL), ptTemplateSelect(NULL), ptTemplateSelectPyr(NULL),
48 ptTemplateSelectInit(false), templateSelectSize(0), ptTemplateSupp(NULL), ptTemplateSuppPyr(NULL),
49 ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL), zoneTracked(NULL), zoneTrackedPyr(NULL), pyr_IDes(NULL), H(),
50 Hdesire(), HdesirePyr(), HLM(), HLMdesire(), HLMdesirePyr(), HLMdesireInverse(), HLMdesireInversePyr(), G(),
51 gain(1.), thresholdGradient(40), costFunctionVerification(false), blur(true), useBrent(false), nbIterBrent(3),
52 taillef(7), fgG(NULL), fgdG(NULL), ratioPixelIn(0), mod_i(1), mod_j(1), nbParam(0), lambdaDep(0.001),
53 iterationMax(30), iterationGlobale(0), diverge(false), nbIteration(0), useCompositionnal(true), useInverse(false),
54 Warp(_warp), p(0), dp(), X1(), X2(), dW(), BI(), dIx(), dIy(), zoneRef_()
89 unsigned int NbPointDsZone = 0;
95 for (
int i = 0; i < hauteur_im; i += mod_fi) {
96 for (
int j = 0; j < largeur_im; j += mod_fj) {
123 unsigned int cpt_point = 0;
125 for (
int i = 0; i < hauteur_im; i +=
mod_i) {
127 for (
int j = 0; j < largeur_im; j +=
mod_j) {
147 pt.
val = vpTemplateTrackerBSpline::getSubPixBspline4(GaussI, i, j);
188 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
202 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
214 if (ptTemplateSuppPyr) {
215 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
216 if (ptTemplateSuppPyr[i]) {
218 delete[] ptTemplateSuppPyr[i][point].Bt;
219 delete[] ptTemplateSuppPyr[i][point].BtInit;
220 delete[] ptTemplateSuppPyr[i][point].dBt;
221 delete[] ptTemplateSuppPyr[i][point].d2W;
222 delete[] ptTemplateSuppPyr[i][point].d2Wx;
223 delete[] ptTemplateSuppPyr[i][point].d2Wy;
225 delete[] ptTemplateSuppPyr[i];
228 delete[] ptTemplateSuppPyr;
229 ptTemplateSuppPyr = NULL;
233 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
272 for (
unsigned int point = 0; point <
templateSize; point++) {
281 for (
unsigned int point = 0; point <
templateSize; point++) {
287 if (ptTemplateSupp) {
288 for (
unsigned int point = 0; point <
templateSize; point++) {
289 delete[] ptTemplateSupp[point].Bt;
290 delete[] ptTemplateSupp[point].BtInit;
291 delete[] ptTemplateSupp[point].dBt;
292 delete[] ptTemplateSupp[point].d2W;
293 delete[] ptTemplateSupp[point].d2Wx;
294 delete[] ptTemplateSupp[point].d2Wy;
296 delete[] ptTemplateSupp;
297 ptTemplateSupp = NULL;
346 zoneWarped.
display(I, col, thickness);
388 zoneWarped.
display(I, col, thickness);
417 adpt = alpha * direction;
424 p2 = tp + alpha * direction;
432 double *Cost =
new double[4];
438 double *talpha =
new double[4];
449 for (
unsigned int opt = 0; opt <
nbIterBrent; opt++) {
458 for (
unsigned int i = 0; i < 3; i++) {
459 A[i][0] = talpha[i] * talpha[i];
465 for (
unsigned int i = 0; i < 3; i++)
468 parabol = (A.
t() * A).inverseByLU() * A.
t() * B;
472 if (parabol[0] > 0) {
473 talpha[3] = -0.5 * parabol[1] / parabol[0];
478 int tindic_x_min = 0;
479 int tindic_x_max = 0;
480 for (
int i = 1; i < 3; i++) {
481 if (talpha[i] < talpha[tindic_x_min])
483 if (talpha[i] > talpha[tindic_x_max])
487 if (Cost[tindic_x_max] < Cost[tindic_x_min]) {
489 talpha[3] = talpha[tindic_x_max] + 1.;
496 talpha[3] = talpha[tindic_x_min] - 1.;
507 for (
int i = 1; i < 3; i++) {
508 if (talpha[i] < talpha[indic_x_min])
510 if (talpha[i] > talpha[indic_x_max])
514 if (talpha[3] > talpha[indic_x_max])
515 if ((talpha[3] - talpha[indic_x_max]) > alpha)
516 talpha[3] = talpha[indic_x_max] + 4.;
517 if (talpha[3] < talpha[indic_x_min])
518 if ((talpha[indic_x_min] - talpha[3]) > alpha)
519 talpha[3] = talpha[indic_x_min] - 4.;
543 adpt = talpha[3] * direction;
550 p3 = tp + talpha[3] * direction;
556 int indice_f_max = 0;
557 for (
int i = 1; i < 4; i++)
558 if (Cost[i] > Cost[indice_f_max])
560 if (indice_f_max != 3) {
561 *ptp[indice_f_max] = *ptp[3];
562 Cost[indice_f_max] = Cost[3];
563 talpha[indice_f_max] = talpha[3];
568 int indice_f_min = 0;
569 for (
int i = 0; i < 4; i++)
570 if (Cost[i] < Cost[indice_f_min])
573 alpha = talpha[indice_f_min];
598 ptTemplateSuppPyr =
new vpTemplateTrackerPointSuppMIInv *[
nbLvlPyr];
600 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
602 ptTemplateSuppPyr[i] = NULL;
628 for (
unsigned int i = 1; i <
nbLvlPyr; i++) {
746 ptTemplateSuppPyr[0] = ptTemplateSupp;
752 ptTemplateSuppPyr[0] = ptTemplateSupp;
763 for (
unsigned int i = 1; i <
nbLvlPyr; i++) {
773 ptTemplateSuppPyr[i] = ptTemplateSupp;
779 ptTemplateSuppPyr[i] = ptTemplateSupp;
818 for (
unsigned int i = 1; i <
nbLvlPyr; i++) {
844 for (
int i = (
int)
nbLvlPyr - 1; i >= 0; i--) {
845 if (i >= (
int)
l0Pyr) {
849 ptTemplateSupp = ptTemplateSuppPyr[i];
871 ptTemplateSupp=ptTemplateSuppPyr[0];
904 p_pre_estimation =
p;
913 if (pre_fcost < post_fcost)
914 p = p_pre_estimation;
929 Warp->computeCoeff(param);
935 for (
unsigned int j = 0; j < 3; j++) {
938 Warp->computeDenom(
X1, param);
941 unsigned int index = i * 3 + j;
942 double x_ =
x_pos[index] -
X2[0];
943 double y_ =
y_pos[index] -
X2[1];
961 x_pos.resize(nb_corners);
962 y_pos.resize(nb_corners);
964 Warp->computeCoeff(param);
968 unsigned int i3 = i * 3;
970 for (
unsigned int j = 0; j < 3; j++) {
973 Warp->computeDenom(
X1, param);