40 #include <visp3/core/vpException.h>
41 #include <visp3/tt_mi/vpTemplateTrackerMI.h>
42 #include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
81 :
vpTemplateTracker(_warp), hessianComputation(USE_HESSIEN_NORMAL), ApproxHessian(HESSIAN_NEW), lambda(0), temp(NULL),
82 Prt(NULL), dPrt(NULL), Pt(NULL), Pr(NULL), d2Prt(NULL), PrtTout(NULL), dprtemp(NULL), PrtD(NULL), dPrtD(NULL),
83 influBspline(0), bspline(3), Nc(8), Ncb(0), d2Ix(), d2Iy(), d2Ixy(), MI_preEstimation(0), MI_postEstimation(0),
84 NMI_preEstimation(0), NMI_postEstimation(0), covarianceMatrix(), computeCovariance(false)
105 Pt =
new double[
Ncb];
106 Pr =
new double[
Ncb];
141 Pt =
new double[
Ncb];
142 Pr =
new double[
Ncb];
153 unsigned int Ncb_ = (
unsigned int)
Ncb;
154 unsigned int Nc_ = (
unsigned int)
Nc;
155 unsigned int influBspline_ = (
unsigned int)
influBspline;
157 memset(
Prt, 0, Ncb_ * Ncb_ *
sizeof(
double));
158 memset(
PrtD, 0, Nc_ * Nc_ * influBspline_ *
sizeof(
double));
160 Warp->computeCoeff(tp);
161 for (
unsigned int point = 0; point <
templateSize; point++) {
165 Warp->computeDenom(
X1, tp);
170 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
179 double Nc_1 = (
Nc - 1.) / 255.;
180 double IW_Nc = IW * Nc_1;
181 double Tij_Nc = Tij * Nc_1;
182 int cr =
static_cast<int>(IW_Nc);
183 int ct =
static_cast<int>(Tij_Nc);
184 double er = IW_Nc - cr;
185 double et = Tij_Nc - ct;
189 vpTemplateTrackerMIBSpline::PutPVBsplineD(
PrtD, cr, er, ct, et,
Nc, 1.,
bspline);
196 for (
int r = 0; r <
Nc; r++)
197 for (
int t = 0; t <
Nc; t++) {
202 Prt[r2 *
Ncb + t2] += *pt;
210 for (
unsigned int r = 0; r < Ncb_; r++) {
211 for (
unsigned int t = 0; t < Ncb_; t++) {
212 Prt[r * Ncb_ + t] /= Nbpoint;
216 memset(
Pr, 0, Ncb_ *
sizeof(
double));
217 memset(
Pt, 0, Ncb_ *
sizeof(
double));
218 for (
unsigned int r = 0; r < Ncb_; r++) {
219 for (
unsigned int t = 0; t < Ncb_; t++) {
220 Pr[r] +=
Prt[r * Ncb_ + t];
221 Pt[r] +=
Prt[t * Ncb_ + r];
225 for (
unsigned int r = 0; r < Ncb_; r++) {
226 if (std::fabs(
Pr[r]) > std::numeric_limits<double>::epsilon()) {
227 MI -=
Pr[r] * log(
Pr[r]);
229 if (std::fabs(
Pt[r]) > std::numeric_limits<double>::epsilon()) {
230 MI -=
Pt[r] * log(
Pt[r]);
232 for (
unsigned int t = 0; t < Ncb_; t++) {
233 unsigned int r_Ncb_t_ = r * Ncb_ + t;
234 if (std::fabs(
Prt[r_Ncb_t_]) > std::numeric_limits<double>::epsilon()) {
235 MI +=
Prt[r_Ncb_t_] * log(
Prt[r_Ncb_t_]);
255 double Prt_[256][256];
257 memset(Pr_, 0, 256 *
sizeof(
double));
258 memset(Pt_, 0, 256 *
sizeof(
double));
259 memset(Prt_, 0, 256 * 256 *
sizeof(
double));
261 for (
unsigned int point = 0; point <
templateSize; point++) {
265 Warp->computeDenom(
X1, tp);
270 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
273 int Tij_ =
static_cast<int>(Tij);
275 IW = I[(int)i2][(
int)j2];
279 int IW_ =
static_cast<int>(IW);
288 for (
int r = 0; r < 256; r++) {
291 if (std::fabs(Pr_[r]) > std::numeric_limits<double>::epsilon()) {
292 MI -= Pr_[r] * log(Pr_[r]);
294 if (std::fabs(Pt_[r]) > std::numeric_limits<double>::epsilon()) {
295 MI -= Pt_[r] * log(Pt_[r]);
297 for (
int t = 0; t < 256; t++) {
298 Prt_[r][t] /= Nbpoint;
299 if (std::fabs(Prt_[r][t]) > std::numeric_limits<double>::epsilon()) {
300 denom -= (Prt_[r][t] * log(Prt_[r][t]));
305 if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
340 unsigned int Nc_ =
static_cast<unsigned int>(
Nc);
341 unsigned int Ncb_ =
static_cast<unsigned int>(
Ncb);
342 unsigned int bspline_ =
static_cast<unsigned int>(
bspline);
344 for (
unsigned int r = 0; r < Nc_; r++) {
345 for (
unsigned int t = 0; t < Nc_; t++) {
346 for (
unsigned int r2 = 0; r2 < bspline_; r2++) {
347 unsigned int r2_r_Ncb_ = (r2 + r) * Ncb_;
348 for (
unsigned int t2 = 0; t2 < bspline_; t2++) {
349 unsigned int t2_t_ = t2 + t;
350 unsigned int r2_r_Ncb_t2_t_nbParam_ = (r2_r_Ncb_ + t2_t_) *
nbParam;
351 Prt[r2_r_Ncb_ + t2_t_] += *pt++;
352 for (
unsigned int ip = 0; ip <
nbParam; ip++) {
353 dPrt[r2_r_Ncb_t2_t_nbParam_ + ip] += *pt++;
354 unsigned int ip_nbParam_ = ip *
nbParam;
355 for (
unsigned int it = 0; it <
nbParam; it++) {
356 d2Prt[r2_r_Ncb_t2_t_nbParam_ *
nbParam + ip_nbParam_ + it] += *pt++;
367 unsigned int indd, indd2;
369 for (
volatile int i = 0; i <
Ncb *
Ncb; i++) {
370 Prt[i] =
Prt[i] / nbpoint;
371 for (
unsigned int j = 0; j <
nbParam; j++) {
372 dPrt[indd] /= nbpoint;
374 for (
unsigned int k = 0; k <
nbParam; k++) {
375 d2Prt[indd2] /= nbpoint;
384 unsigned int Ncb_ = (
unsigned int)
Ncb;
387 memset(
Pr, 0, Ncb_ *
sizeof(
double));
388 memset(
Pt, 0, Ncb_ *
sizeof(
double));
389 for (
unsigned int r = 0; r < Ncb_; r++) {
390 unsigned int r_Nbc_ = r * Ncb_;
391 for (
unsigned int t = 0; t < Ncb_; t++) {
392 Pr[r] +=
Prt[r_Nbc_ + t];
393 Pt[r] +=
Prt[r + Ncb_ * t];
398 for (
unsigned int r = 0; r < Ncb_; r++) {
399 if (std::fabs(
Pr[r]) > std::numeric_limits<double>::epsilon()) {
400 MI -=
Pr[r] * log(
Pr[r]);
402 if (std::fabs(
Pt[r]) > std::numeric_limits<double>::epsilon()) {
403 MI -=
Pt[r] * log(
Pt[r]);
405 unsigned int r_Nbc_ = r * Ncb_;
406 for (
unsigned int t = 0; t < Ncb_; t++) {
407 unsigned int r_Nbc_t_ = r_Nbc_ + t;
408 if (std::fabs(
Prt[r_Nbc_t_]) > std::numeric_limits<double>::epsilon()) {
409 MI +=
Prt[r_Nbc_t_] * log(
Prt[r_Nbc_t_]);
417 double seuilevitinf = 1e-200;
420 unsigned int Ncb_ =
static_cast<unsigned int>(
Ncb);
423 for (
unsigned int t = 0; t < Ncb_; t++) {
424 if (
Pt[t] > seuilevitinf) {
425 for (
unsigned int r = 0; r < Ncb_; r++) {
426 if (
Prt[r * Ncb_ + t] > seuilevitinf) {
427 unsigned int r_Ncb_t_ = r * Ncb_ + t;
428 unsigned int r_Ncb_t_nbParam_ = r_Ncb_t_ *
nbParam;
429 for (
unsigned int it = 0; it <
nbParam; it++) {
433 dtemp = 1. + log(
Prt[r * Ncb_ + t] /
Pt[t]);
435 double Prt_Pt_ = 1. /
Prt[r_Ncb_t_] - 1. /
Pt[t];
436 unsigned int r_Ncb_t_nbParam2_ = r_Ncb_t_ * nbParam2;
437 for (
unsigned int it = 0; it <
nbParam; it++) {
438 unsigned int r_Ncb_t_nbParam2_it_nbParam_ = r_Ncb_t_nbParam2_ + it *
nbParam;
439 for (
unsigned int jt = 0; jt <
nbParam; jt++) {
444 Hessian[it][jt] +=
d2Prt[r_Ncb_t_nbParam2_it_nbParam_ + jt] * dtemp;
457 double seuilevitinf = 1e-200;
458 double u = 0, v = 0, B = 0;
467 for (
unsigned int i = 0; i <
nbParam; i++) {
473 std::fill(
m_du.begin(),
m_du.end(), 0);
474 std::fill(
m_dv.begin(),
m_dv.end(), 0);
475 std::fill(
m_A.begin(),
m_A.end(), 0);
476 std::fill(
m_dB.begin(),
m_dB.end(), 0);
477 for (
unsigned int it = 0; it <
nbParam; it++) {
478 std::fill(
m_d2u[0].begin(),
m_d2u[0].end(), 0);
479 std::fill(
m_d2v[0].begin(),
m_d2v[0].end(), 0);
480 std::fill(
m_dA[0].begin(),
m_dA[0].end(), 0);
485 unsigned int Ncb_ =
static_cast<unsigned int>(
Ncb);
488 for (
unsigned int t = 0; t < Ncb_; t++) {
489 if (
Pt[t] > seuilevitinf) {
490 for (
unsigned int r = 0; r < Ncb_; r++) {
491 unsigned int r_Ncb_t_ = r * Ncb_ + t;
492 if (
Prt[r_Ncb_t_] > seuilevitinf) {
493 unsigned int r_Ncb_t_nbParam_ = r_Ncb_t_ *
nbParam;
494 for (
unsigned int it = 0; it <
nbParam; it++) {
498 double log_Pt_Pr_ = log(
Pt[t] *
Pr[r]);
499 double log_Prt_ = log(
Prt[r_Ncb_t_]);
501 u +=
Prt[r_Ncb_t_] * log_Pt_Pr_;
503 v +=
Prt[r_Ncb_t_] * log_Prt_;
505 double log_Prt_1_ = 1 + log(
Prt[r_Ncb_t_]);
506 for (
unsigned int it = 0; it <
nbParam; it++) {
512 double Prt_ = 1.0 /
Prt[r_Ncb_t_];
513 unsigned int r_Ncb_t_nbParam2_ = r_Ncb_t_ * nbParam2;
514 for (
unsigned int it = 0; it <
nbParam; it++) {
516 unsigned int r_Ncb_t_nbParam2_it_nbParam_ = r_Ncb_t_nbParam2_ + it *
nbParam;
517 for (
unsigned int jt = 0; jt <
nbParam; jt++) {
518 unsigned int r_Ncb_t_nbParam2_it_nbParam_jt_ = r_Ncb_t_nbParam2_it_nbParam_ + jt;
519 m_d2u[it][jt] +=
d2Prt[r_Ncb_t_nbParam2_it_nbParam_jt_] * log_Pt_Pr_ + dprtemp_it2_;
520 m_d2v[it][jt] +=
d2Prt[r_Ncb_t_nbParam2_it_nbParam_jt_] * log_Prt_1_ + dprtemp_it2_;
530 for (
unsigned int it = 0; it <
nbParam; it++) {
535 double A_it_dB_it_ =
m_A[it] *
m_dB[it];
536 for (
unsigned int jt = 0; jt <
nbParam; jt++) {
540 Hessian[it][jt] = (
m_dA[it][jt] * B - A_it_dB_it_) / B2;
547 double seuilevitinf = 1e-200;
549 unsigned int Ncb_ =
static_cast<unsigned int>(
Ncb);
551 for (
unsigned int t = 0; t < Ncb_; t++) {
552 if (
Pt[t] > seuilevitinf) {
553 for (
unsigned int r = 0; r < Ncb_; r++) {
554 unsigned int r_Ncb_t_ = r * Ncb_ + t;
555 if (
Prt[r_Ncb_t_] > seuilevitinf) {
556 unsigned int r_Ncb_t_nbParam_ = r_Ncb_t_ *
nbParam;
557 for (
unsigned int it = 0; it <
nbParam; it++) {
561 dtemp = 1. + log(
Prt[r_Ncb_t_] /
Pt[t]);
563 for (
unsigned int it = 0; it <
nbParam; it++) {
574 unsigned int Ncb_ =
static_cast<unsigned int>(
Ncb);
575 unsigned int Nc_ =
static_cast<unsigned int>(
Nc);
576 unsigned int influBspline_ =
static_cast<unsigned int>(
influBspline);
578 unsigned int Ncb2_ = Ncb_ * Ncb_ *
sizeof(double);
579 unsigned int Ncb2_nbParam_ = Ncb2_ *
nbParam;
580 unsigned int Ncb2_nbParam2_ = Ncb2_nbParam_ *
nbParam;
582 memset(
Prt, 0, Ncb2_);
583 memset(
dPrt, 0, Ncb2_nbParam_);
584 memset(
d2Prt, 0, Ncb2_nbParam2_);
590 unsigned int tNcb =
static_cast<unsigned int>(nc + bspline_);
591 unsigned int tinfluBspline =
static_cast<unsigned int>(bspline_ * bspline_);
592 unsigned int nc_ =
static_cast<unsigned int>(nc);
594 double *tPrtD =
new double[nc_ * nc_ * tinfluBspline];
595 double *tPrt =
new double[tNcb * tNcb];
596 double *tPr =
new double[tNcb];
597 double *tPt =
new double[tNcb];
600 volatile int Nbpoint = 0;
606 memset(tPrt, 0, tNcb * tNcb *
sizeof(
double));
607 memset(tPrtD, 0, nc_ * nc_ * tinfluBspline *
sizeof(
double));
609 Warp->computeCoeff(tp);
610 for (
unsigned int point = 0; point <
templateSize; point++) {
614 Warp->computeDenom(
X1, tp);
619 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth()) - 1) {
628 int cr =
static_cast<int>((IW * (nc - 1)) / 255.);
629 int ct =
static_cast<int>((Tij * (nc - 1)) / 255.);
630 double er = (IW * (nc - 1)) / 255. - cr;
631 double et = (Tij * (nc - 1)) / 255. - ct;
635 vpTemplateTrackerMIBSpline::PutPVBsplineD(tPrtD, cr, er, ct, et, nc, 1., bspline_);
639 int tNcb_ = (int)tNcb;
640 int tinfluBspline_ = (int)tinfluBspline;
641 for (
int r = 0; r < nc; r++)
642 for (
int t = 0; t < nc; t++) {
643 for (
volatile int i = 0; i < tinfluBspline_; i++) {
645 r2 = r + i / bspline_;
646 t2 = t + i % bspline_;
647 tPrt[r2 * tNcb_ + t2] += *pt;
661 for (
unsigned int r = 0; r < tNcb; r++)
662 for (
unsigned int t = 0; t < tNcb; t++)
663 tPrt[r * tNcb + t] = tPrt[r * tNcb + t] / Nbpoint;
665 memset(tPr, 0, tNcb *
sizeof(
double));
666 for (
unsigned int r = 0; r < tNcb; r++) {
667 for (
unsigned int t = 0; t < tNcb; t++)
668 tPr[r] += tPrt[r * tNcb + t];
672 memset(tPt, 0, (
size_t)(tNcb *
sizeof(
double)));
673 for (
unsigned int t = 0; t < tNcb; t++) {
674 for (
unsigned int r = 0; r < tNcb; r++)
675 tPt[t] += tPrt[r * tNcb + t];
677 for (
unsigned int r = 0; r < tNcb; r++)
678 if (std::fabs(tPr[r]) > std::numeric_limits<double>::epsilon())
679 MI -= tPr[r] * log(tPr[r]);
681 for (
unsigned int t = 0; t < tNcb; t++)
682 if (std::fabs(tPt[t]) > std::numeric_limits<double>::epsilon())
683 MI -= tPt[t] * log(tPt[t]);
685 for (
unsigned int r = 0; r < tNcb; r++)
686 for (
unsigned int t = 0; t < tNcb; t++)
687 if (std::fabs(tPrt[r * tNcb + t]) > std::numeric_limits<double>::epsilon())
688 MI += tPrt[r * tNcb + t] * log(tPrt[r * tNcb + t]);
707 volatile int Nbpoint = 0;
708 unsigned int Tij, IW;
714 Warp->computeCoeff(tp);
715 for (
unsigned int point = 0; point <
templateSize; point++) {
719 Warp->computeDenom(
X1, tp);
724 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth()) - 1) {
729 IW =
static_cast<unsigned int>(I.
getValue(i2, j2));
731 IW =
static_cast<unsigned int>(GaussI.
getValue(i2, j2));
742 Prt256 = Prt256 / Nbpoint;
743 Pr256 = Pr256 / Nbpoint;
744 Pt256 = Pt256 / Nbpoint;
748 for (
unsigned int t = 0; t < 256; t++) {
749 for (
unsigned int r = 0; r < 256; r++) {
750 if (std::fabs(Prt256[r][t]) > std::numeric_limits<double>::epsilon())
751 MI += Prt256[r][t] * log(Prt256[r][t]);
753 if (std::fabs(Pt256[t]) > std::numeric_limits<double>::epsilon())
754 MI += -Pt256[t] * log(Pt256[t]);
755 if (std::fabs(Pr256[t]) > std::numeric_limits<double>::epsilon())
756 MI += -Pr256[t] * log(Pr256[t]);
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emited by ViSP classes.
@ divideByZeroError
Division by zero.
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, bool convolve=false)
unsigned int getWidth() const
Type getValue(unsigned int i, unsigned int j) const
unsigned int getHeight() const
Implementation of a matrix and operations on matrices.
vpHessienApproximationType ApproxHessian
void computeHessienNormalized(vpMatrix &H)
std::vector< std::vector< double > > m_d2v
vpHessienType hessianComputation
void computeMI(double &MI)
virtual ~vpTemplateTrackerMI()
std::vector< double > m_dB
std::vector< double > m_du
void computeProba(int &nbpoint)
vpTemplateTrackerMI()
Default constructor.
void computeHessien(vpMatrix &H)
std::vector< double > m_dv
std::vector< std::vector< double > > m_d2u
double getNormalizedCost(const vpImage< unsigned char > &I, const vpColVector &tp)
std::vector< std::vector< double > > m_dA
void setBspline(const vpBsplineType &newbs)
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
double getMI256(const vpImage< unsigned char > &I, const vpColVector &tp)
std::vector< double > m_A
virtual void warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)=0
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerWarp * Warp
unsigned int templateSize
Error that can be emited by the vpTracker class and its derivates.