40 #include <visp3/tt/vpTemplateTracker.h>
41 #include <visp3/tt/vpTemplateTrackerBSpline.h>
44 : nbLvlPyr(1), l0Pyr(0), pyrInitialised(false), evolRMS(0), x_pos(), y_pos(), evolRMS_eps(1e-4), ptTemplate(NULL),
45 ptTemplatePyr(NULL), ptTemplateInit(false), templateSize(0), templateSizePyr(NULL), ptTemplateSelect(NULL),
46 ptTemplateSelectPyr(NULL), ptTemplateSelectInit(false), templateSelectSize(0), ptTemplateSupp(NULL),
47 ptTemplateSuppPyr(NULL), ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL), zoneTracked(NULL), zoneTrackedPyr(NULL),
48 pyr_IDes(NULL), H(), Hdesire(), HdesirePyr(), HLM(), HLMdesire(), HLMdesirePyr(), HLMdesireInverse(),
49 HLMdesireInversePyr(), G(), gain(1.), thresholdGradient(40), costFunctionVerification(false), blur(true),
50 useBrent(false), nbIterBrent(3), taillef(7), fgG(NULL), fgdG(NULL), ratioPixelIn(0), mod_i(1), mod_j(1), nbParam(0),
51 lambdaDep(0.001), iterationMax(30), iterationGlobale(0), diverge(false), nbIteration(0), useCompositionnal(true),
52 useInverse(false), Warp(_warp), p(0), dp(), X1(), X2(), dW(), BI(), dIx(), dIy(), zoneRef_()
86 unsigned int NbPointDsZone = 0;
91 for (
int i = 0; i < hauteur_im; i += mod_fi) {
92 for (
int j = 0; j < largeur_im; j += mod_fj) {
114 unsigned int cpt_point = 0;
116 for (
int i = 0; i < hauteur_im; i +=
mod_i) {
117 for (
int j = 0; j < largeur_im; j +=
mod_j) {
131 pt.
val = vpTemplateTrackerBSpline::getSubPixBspline4(GaussI, i, j);
162 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
176 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
188 if (ptTemplateSuppPyr) {
189 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
190 if (ptTemplateSuppPyr[i]) {
192 delete[] ptTemplateSuppPyr[i][point].Bt;
193 delete[] ptTemplateSuppPyr[i][point].BtInit;
194 delete[] ptTemplateSuppPyr[i][point].dBt;
195 delete[] ptTemplateSuppPyr[i][point].d2W;
196 delete[] ptTemplateSuppPyr[i][point].d2Wx;
197 delete[] ptTemplateSuppPyr[i][point].d2Wy;
199 delete[] ptTemplateSuppPyr[i];
202 delete[] ptTemplateSuppPyr;
203 ptTemplateSuppPyr = NULL;
207 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
246 for (
unsigned int point = 0; point <
templateSize; point++) {
255 for (
unsigned int point = 0; point <
templateSize; point++) {
261 if (ptTemplateSupp) {
262 for (
unsigned int point = 0; point <
templateSize; point++) {
263 delete[] ptTemplateSupp[point].Bt;
264 delete[] ptTemplateSupp[point].BtInit;
265 delete[] ptTemplateSupp[point].dBt;
266 delete[] ptTemplateSupp[point].d2W;
267 delete[] ptTemplateSupp[point].d2Wx;
268 delete[] ptTemplateSupp[point].d2Wy;
270 delete[] ptTemplateSupp;
271 ptTemplateSupp = NULL;
320 zoneWarped.
display(I, col, thickness);
362 zoneWarped.
display(I, col, thickness);
391 adpt = alpha * direction;
398 p2 = tp + alpha * direction;
406 double *Cost =
new double[4];
411 double *talpha =
new double[4];
418 for (
unsigned int opt = 0; opt <
nbIterBrent; opt++) {
420 for (
unsigned int i = 0; i < 3; i++) {
421 A[i][0] = talpha[i] * talpha[i];
426 for (
unsigned int i = 0; i < 3; i++)
429 parabol = (A.
t() * A).inverseByLU() * A.
t() * B;
432 if (parabol[0] > 0) {
433 talpha[3] = -0.5 * parabol[1] / parabol[0];
435 int tindic_x_min = 0;
436 int tindic_x_max = 0;
437 for (
int i = 1; i < 3; i++) {
438 if (talpha[i] < talpha[tindic_x_min])
440 if (talpha[i] > talpha[tindic_x_max])
444 if (Cost[tindic_x_max] < Cost[tindic_x_min]) {
445 talpha[3] = talpha[tindic_x_max] + 1.;
447 talpha[3] = talpha[tindic_x_min] - 1.;
452 for (
int i = 1; i < 3; i++) {
453 if (talpha[i] < talpha[indic_x_min])
455 if (talpha[i] > talpha[indic_x_max])
458 if (talpha[3] > talpha[indic_x_max])
459 if ((talpha[3] - talpha[indic_x_max]) > alpha)
460 talpha[3] = talpha[indic_x_max] + 4.;
461 if (talpha[3] < talpha[indic_x_min])
462 if ((talpha[indic_x_min] - talpha[3]) > alpha)
463 talpha[3] = talpha[indic_x_min] - 4.;
466 adpt = talpha[3] * direction;
473 p3 = tp + talpha[3] * direction;
478 int indice_f_max = 0;
479 for (
int i = 1; i < 4; i++)
480 if (Cost[i] > Cost[indice_f_max])
482 if (indice_f_max != 3) {
483 *ptp[indice_f_max] = *ptp[3];
484 Cost[indice_f_max] = Cost[3];
485 talpha[indice_f_max] = talpha[3];
490 int indice_f_min = 0;
491 for (
int i = 0; i < 4; i++)
492 if (Cost[i] < Cost[indice_f_min])
495 alpha = talpha[indice_f_min];
519 ptTemplateSuppPyr =
new vpTemplateTrackerPointSuppMIInv *[
nbLvlPyr];
521 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
523 ptTemplateSuppPyr[i] = NULL;
547 for (
unsigned int i = 1; i <
nbLvlPyr; i++) {
648 ptTemplateSuppPyr[0] = ptTemplateSupp;
654 ptTemplateSuppPyr[0] = ptTemplateSupp;
665 for (
unsigned int i = 1; i <
nbLvlPyr; i++) {
673 ptTemplateSuppPyr[i] = ptTemplateSupp;
679 ptTemplateSuppPyr[i] = ptTemplateSupp;
711 for (
unsigned int i = 1; i <
nbLvlPyr; i++) {
718 for (
int i = (
int)
nbLvlPyr - 1; i >= 0; i--) {
719 if (i >= (
int)
l0Pyr) {
723 ptTemplateSupp = ptTemplateSuppPyr[i];
750 p_pre_estimation =
p;
757 if (pre_fcost < post_fcost) {
758 p = p_pre_estimation;
775 Warp->computeCoeff(param);
781 for (
unsigned int j = 0; j < 3; j++) {
784 Warp->computeDenom(
X1, param);
787 unsigned int index = i * 3 + j;
788 double x_ =
x_pos[index] -
X2[0];
789 double y_ =
y_pos[index] -
X2[1];
807 x_pos.resize(nb_corners);
808 y_pos.resize(nb_corners);
810 Warp->computeCoeff(param);
814 unsigned int i3 = i * 3;
816 for (
unsigned int j = 0; j < 3; j++) {
819 Warp->computeDenom(
X1, param);
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)
Class to define RGB colors available for display functionalities.
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
const char * getMessage() const
static void getGradYGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIy, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size)
static void getGradXGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIx, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size)
static void getGaussianDerivativeKernel(FilterType *filter, unsigned int size, FilterType sigma=0., bool normalize=true)
static void getGaussianKernel(FilterType *filter, unsigned int size, FilterType sigma=0., bool normalize=true)
static void filter(const vpImage< unsigned char > &I, vpImage< FilterType > &If, const vpArray2D< FilterType > &M, bool convolve=false)
static void getGaussPyramidal(const vpImage< unsigned char > &I, vpImage< unsigned char > &GI)
void destroy()
Destructor : Memory de-allocation.
unsigned int getWidth() const
unsigned int getHeight() const
Implementation of a matrix and operations on matrices.
vpColVector getCorner(unsigned int i) const
virtual void getParamPyramidUp(const vpColVector &p, vpColVector &p_up)=0
unsigned int getNbParam() const
virtual void getParamInverse(const vpColVector &p, vpColVector &p_inv) const =0
void warpZone(const vpTemplateTrackerZone &in, const vpColVector &p, vpTemplateTrackerZone &out)
virtual void warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)=0
virtual void getParamPyramidDown(const vpColVector &p, vpColVector &p_down)=0
virtual void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &p12) const =0
vpTemplateTrackerZone getPyramidDown() const
void initFromPoints(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &ip, bool delaunay=false)
bool inZone(const int &i, const int &j) const
void getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const
unsigned int getNbTriangle() const
void copy(const vpTemplateTrackerZone &z)
void display(const vpImage< unsigned char > &I, const vpColor &col=vpColor::green, unsigned int thickness=3)
void initClick(const vpImage< unsigned char > &I, bool delaunay=false)
void display(const vpImage< unsigned char > &I, const vpColor &col=vpColor::green, unsigned int thickness=3)
void initTracking(const vpImage< unsigned char > &I, vpTemplateTrackerZone &zone)
vpTemplateTracker()
Default constructor.
vpMatrix HLMdesireInverse
unsigned int templateSelectSize
virtual void initHessienDesiredPyr(const vpImage< unsigned char > &I)
vpTemplateTrackerPoint ** ptTemplatePyr
void initFromPoints(const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &v_ip, bool delaunay=false)
void computeEvalRMS(const vpColVector &p)
virtual void trackNoPyr(const vpImage< unsigned char > &I)=0
unsigned int * templateSizePyr
void initFromZone(const vpImage< unsigned char > &I, const vpTemplateTrackerZone &zone)
vpTemplateTrackerZone * zoneTrackedPyr
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
vpTemplateTrackerZone zoneRef_
bool ** ptTemplateSelectPyr
vpImage< unsigned char > * pyr_IDes
std::vector< double > y_pos
vpMatrix * HLMdesireInversePyr
std::vector< double > x_pos
virtual double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)=0
void track(const vpImage< unsigned char > &I)
bool costFunctionVerification
vpTemplateTrackerPointCompo ** ptTemplateCompoPyr
virtual void trackPyr(const vpImage< unsigned char > &I)
void getGaussianBluredImage(const vpImage< unsigned char > &I)
void initPosEvalRMS(const vpColVector &p)
virtual void initHessienDesired(const vpImage< unsigned char > &I)=0
virtual ~vpTemplateTracker()
vpTemplateTrackerPoint * ptTemplate
virtual void initTrackingPyr(const vpImage< unsigned char > &I, vpTemplateTrackerZone &zone)
virtual void initPyramidal(unsigned int nbLvl, unsigned int l0)
void setGaussianFilterSize(unsigned int new_taill)
vpTemplateTrackerWarp * Warp
bool ptTemplateSelectInit
void trackRobust(const vpImage< unsigned char > &I)
void initClick(const vpImage< unsigned char > &I, bool delaunay=false)
unsigned int templateSize
vpTemplateTrackerPointCompo * ptTemplateCompo
vpTemplateTrackerZone * zoneTracked
Error that can be emitted by the vpTracker class and its derivatives.