41 #include <visp3/tt/vpTemplateTracker.h> 42 #include <visp3/tt/vpTemplateTrackerBSpline.h> 45 : nbLvlPyr(1), l0Pyr(0), pyrInitialised(false), ptTemplate(NULL), ptTemplatePyr(NULL), ptTemplateInit(false),
46 templateSize(0), templateSizePyr(NULL), ptTemplateSelect(NULL), ptTemplateSelectPyr(NULL),
47 ptTemplateSelectInit(false), templateSelectSize(0), ptTemplateSupp(NULL), ptTemplateSuppPyr(NULL),
48 ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL), zoneTracked(NULL), zoneTrackedPyr(NULL), pyr_IDes(NULL), H(),
49 Hdesire(), HdesirePyr(), HLM(), HLMdesire(), HLMdesirePyr(), HLMdesireInverse(), HLMdesireInversePyr(), G(),
50 gain(1.), thresholdGradient(40), costFunctionVerification(false), blur(true), useBrent(false), nbIterBrent(3),
51 taillef(7), fgG(NULL), fgdG(NULL), ratioPixelIn(0), mod_i(1), mod_j(1), nbParam(0), lambdaDep(0.001),
52 iterationMax(30), iterationGlobale(0), diverge(false), nbIteration(0), useCompositionnal(true), useInverse(false),
53 Warp(_warp), p(0), dp(), X1(), X2(), dW(), BI(), dIx(), dIy(), zoneRef_()
88 unsigned int NbPointDsZone = 0;
94 for (
int i = 0; i < hauteur_im; i += mod_fi) {
95 for (
int j = 0; j < largeur_im; j += mod_fj) {
122 unsigned int cpt_point = 0;
124 for (
int i = 0; i < hauteur_im; i +=
mod_i) {
126 for (
int j = 0; j < largeur_im; j +=
mod_j) {
146 pt.
val = vpTemplateTrackerBSpline::getSubPixBspline4(GaussI, i, j);
187 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
201 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
213 if (ptTemplateSuppPyr) {
214 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
215 if (ptTemplateSuppPyr[i]) {
217 delete[] ptTemplateSuppPyr[i][point].Bt;
218 delete[] ptTemplateSuppPyr[i][point].BtInit;
219 delete[] ptTemplateSuppPyr[i][point].dBt;
220 delete[] ptTemplateSuppPyr[i][point].d2W;
221 delete[] ptTemplateSuppPyr[i][point].d2Wx;
222 delete[] ptTemplateSuppPyr[i][point].d2Wy;
224 delete[] ptTemplateSuppPyr[i];
227 delete[] ptTemplateSuppPyr;
228 ptTemplateSuppPyr = NULL;
232 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
271 for (
unsigned int point = 0; point <
templateSize; point++) {
280 for (
unsigned int point = 0; point <
templateSize; point++) {
286 if (ptTemplateSupp) {
287 for (
unsigned int point = 0; point <
templateSize; point++) {
288 delete[] ptTemplateSupp[point].Bt;
289 delete[] ptTemplateSupp[point].BtInit;
290 delete[] ptTemplateSupp[point].dBt;
291 delete[] ptTemplateSupp[point].d2W;
292 delete[] ptTemplateSupp[point].d2Wx;
293 delete[] ptTemplateSupp[point].d2Wy;
295 delete[] ptTemplateSupp;
296 ptTemplateSupp = NULL;
345 zoneWarped.
display(I, col, thickness);
387 zoneWarped.
display(I, col, thickness);
416 adpt = alpha * direction;
423 p2 = tp + alpha * direction;
431 double *Cost =
new double[4];
437 double *talpha =
new double[4];
448 for (
unsigned int opt = 0; opt <
nbIterBrent; opt++) {
457 for (
unsigned int i = 0; i < 3; i++) {
458 A[i][0] = talpha[i] * talpha[i];
464 for (
unsigned int i = 0; i < 3; i++)
467 parabol = (A.
t() * A).inverseByLU() * A.
t() * B;
471 if (parabol[0] > 0) {
472 talpha[3] = -0.5 * parabol[1] / parabol[0];
477 int tindic_x_min = 0;
478 int tindic_x_max = 0;
479 for (
int i = 1; i < 3; i++) {
480 if (talpha[i] < talpha[tindic_x_min])
482 if (talpha[i] > talpha[tindic_x_max])
486 if (Cost[tindic_x_max] < Cost[tindic_x_min]) {
488 talpha[3] = talpha[tindic_x_max] + 1.;
495 talpha[3] = talpha[tindic_x_min] - 1.;
506 for (
int i = 1; i < 3; i++) {
507 if (talpha[i] < talpha[indic_x_min])
509 if (talpha[i] > talpha[indic_x_max])
513 if (talpha[3] > talpha[indic_x_max])
514 if ((talpha[3] - talpha[indic_x_max]) > alpha)
515 talpha[3] = talpha[indic_x_max] + 4.;
516 if (talpha[3] < talpha[indic_x_min])
517 if ((talpha[indic_x_min] - talpha[3]) > alpha)
518 talpha[3] = talpha[indic_x_min] - 4.;
542 adpt = talpha[3] * direction;
549 p3 = tp + talpha[3] * direction;
555 int indice_f_max = 0;
556 for (
int i = 1; i < 4; i++)
557 if (Cost[i] > Cost[indice_f_max])
559 if (indice_f_max != 3) {
560 *ptp[indice_f_max] = *ptp[3];
561 Cost[indice_f_max] = Cost[3];
562 talpha[indice_f_max] = talpha[3];
567 int indice_f_min = 0;
568 for (
int i = 0; i < 4; i++)
569 if (Cost[i] < Cost[indice_f_min])
572 alpha = talpha[indice_f_min];
597 ptTemplateSuppPyr =
new vpTemplateTrackerPointSuppMIInv *[
nbLvlPyr];
599 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
601 ptTemplateSuppPyr[i] = NULL;
627 for (
unsigned int i = 1; i <
nbLvlPyr; i++) {
745 ptTemplateSuppPyr[0] = ptTemplateSupp;
751 ptTemplateSuppPyr[0] = ptTemplateSupp;
762 for (
unsigned int i = 1; i <
nbLvlPyr; i++) {
772 ptTemplateSuppPyr[i] = ptTemplateSupp;
778 ptTemplateSuppPyr[i] = ptTemplateSupp;
817 for (
unsigned int i = 1; i <
nbLvlPyr; i++) {
843 for (
int i = (
int)nbLvlPyr - 1; i >= 0; i--) {
844 if (i >= (
int)
l0Pyr) {
848 ptTemplateSupp = ptTemplateSuppPyr[i];
870 ptTemplateSupp=ptTemplateSuppPyr[0];
903 p_pre_estimation =
p;
912 if (pre_fcost < post_fcost)
913 p = p_pre_estimation;
static void getGaussPyramidal(const vpImage< unsigned char > &I, vpImage< unsigned char > &GI)
bool ptTemplateSelectInit
Used to indicate that a value is not in the allowed range.
Implementation of a matrix and operations on matrices.
virtual void getParamPyramidDown(const vpColVector &p, vpColVector &pdown)=0
virtual void trackPyr(const vpImage< unsigned char > &I)
bool costFunctionVerification
unsigned int templateSelectSize
unsigned int getWidth() const
void getGaussianBluredImage(const vpImage< unsigned char > &I)
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerPoint ** ptTemplatePyr
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Class to define colors available for display functionnalities.
static void getGaussianKernel(double *filter, unsigned int size, double sigma=0., bool normalize=true)
virtual void initTrackingPyr(const vpImage< unsigned char > &I, vpTemplateTrackerZone &zone)
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
error that can be emited by ViSP classes.
void initClick(const vpImage< unsigned char > &I, bool delaunay=false)
void display(const vpImage< unsigned char > &I, const vpColor &col=vpColor::green, const unsigned int thickness=3)
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
vpMatrix * HLMdesireInversePyr
vpTemplateTracker()
Default constructor.
static void getGradXGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
virtual void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const =0
void copy(const vpTemplateTrackerZone &z)
unsigned int templateSize
virtual void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const =0
void initTracking(const vpImage< unsigned char > &I, vpTemplateTrackerZone &zone)
Error that can be emited by the vpTracker class and its derivates.
vpTemplateTrackerZone * zoneTrackedPyr
virtual ~vpTemplateTracker()
virtual void initHessienDesired(const vpImage< unsigned char > &I)=0
vpTemplateTrackerZone zoneRef_
void setGaussianFilterSize(unsigned int new_taill)
void initFromZone(const vpImage< unsigned char > &I, const vpTemplateTrackerZone &zone)
virtual double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)=0
virtual void initHessienDesiredPyr(const vpImage< unsigned char > &I)
void initClick(const vpImage< unsigned char > &I, bool delaunay=false)
virtual void getParamPyramidUp(const vpColVector &p, vpColVector &pup)=0
const char * getMessage(void) const
unsigned int getNbParam() const
void initFromPoints(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &ip, bool delaunay=false)
void destroy()
Destructor : Memory de-allocation.
void initFromPoints(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &v_ip, bool delaunay=false)
unsigned int * templateSizePyr
vpMatrix HLMdesireInverse
virtual void initPyramidal(unsigned int nbLvl, unsigned int l0)
bool inZone(const int &i, const int &j) const
vpTemplateTrackerZone getPyramidDown() const
void warpZone(const vpTemplateTrackerZone &in, const vpColVector &p, vpTemplateTrackerZone &out)
static void getGaussianDerivativeKernel(double *filter, unsigned int size, double sigma=0., bool normalize=true)
vpTemplateTrackerPointCompo * ptTemplateCompo
Implementation of column vector and the associated operations.
void track(const vpImage< unsigned char > &I)
virtual void trackNoPyr(const vpImage< unsigned char > &I)=0
vpTemplateTrackerZone * zoneTracked
bool ** ptTemplateSelectPyr
unsigned int getHeight() const
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, const bool convolve=false)
void trackRobust(const vpImage< unsigned char > &I)
vpImage< unsigned char > * pyr_IDes
vpTemplateTrackerWarp * Warp
vpTemplateTrackerPointCompo ** ptTemplateCompoPyr
void resize(const unsigned int i, const bool flagNullify=true)
void display(const vpImage< unsigned char > &I, const vpColor &col=vpColor::green, const unsigned int thickness=3)