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(), evolRMS_eps(1e-4), ptTemplate(NULL),
46 ptTemplatePyr(NULL), ptTemplateInit(false), templateSize(0), templateSizePyr(NULL), ptTemplateSelect(NULL),
47 ptTemplateSelectPyr(NULL), ptTemplateSelectInit(false), templateSelectSize(0), ptTemplateSupp(NULL),
48 ptTemplateSuppPyr(NULL), ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL), zoneTracked(NULL), zoneTrackedPyr(NULL),
49 pyr_IDes(NULL), H(), Hdesire(), HdesirePyr(), HLM(), HLMdesire(), HLMdesirePyr(), HLMdesireInverse(),
50 HLMdesireInversePyr(), G(), gain(1.), thresholdGradient(40), costFunctionVerification(false), blur(true),
51 useBrent(false), nbIterBrent(3), taillef(7), fgG(NULL), fgdG(NULL), ratioPixelIn(0), mod_i(1), mod_j(1), nbParam(0),
52 lambdaDep(0.001), iterationMax(30), iterationGlobale(0), diverge(false), nbIteration(0), useCompositionnal(true),
53 useInverse(false), Warp(_warp), p(0), dp(), X1(), X2(), dW(), BI(), dIx(), dIy(), zoneRef_()
87 unsigned int NbPointDsZone = 0;
92 for (
int i = 0; i < hauteur_im; i += mod_fi) {
93 for (
int j = 0; j < largeur_im; j += mod_fj) {
115 unsigned int cpt_point = 0;
117 for (
int i = 0; i < hauteur_im; i +=
mod_i) {
118 for (
int j = 0; j < largeur_im; j +=
mod_j) {
132 pt.
val = vpTemplateTrackerBSpline::getSubPixBspline4(GaussI, i, j);
163 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
177 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
189 if (ptTemplateSuppPyr) {
190 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
191 if (ptTemplateSuppPyr[i]) {
193 delete[] ptTemplateSuppPyr[i][point].Bt;
194 delete[] ptTemplateSuppPyr[i][point].BtInit;
195 delete[] ptTemplateSuppPyr[i][point].dBt;
196 delete[] ptTemplateSuppPyr[i][point].d2W;
197 delete[] ptTemplateSuppPyr[i][point].d2Wx;
198 delete[] ptTemplateSuppPyr[i][point].d2Wy;
200 delete[] ptTemplateSuppPyr[i];
203 delete[] ptTemplateSuppPyr;
204 ptTemplateSuppPyr = NULL;
208 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
247 for (
unsigned int point = 0; point <
templateSize; point++) {
256 for (
unsigned int point = 0; point <
templateSize; point++) {
262 if (ptTemplateSupp) {
263 for (
unsigned int point = 0; point <
templateSize; point++) {
264 delete[] ptTemplateSupp[point].Bt;
265 delete[] ptTemplateSupp[point].BtInit;
266 delete[] ptTemplateSupp[point].dBt;
267 delete[] ptTemplateSupp[point].d2W;
268 delete[] ptTemplateSupp[point].d2Wx;
269 delete[] ptTemplateSupp[point].d2Wy;
271 delete[] ptTemplateSupp;
272 ptTemplateSupp = NULL;
321 zoneWarped.
display(I, col, thickness);
363 zoneWarped.
display(I, col, thickness);
392 adpt = alpha * direction;
399 p2 = tp + alpha * direction;
407 double *Cost =
new double[4];
412 double *talpha =
new double[4];
419 for (
unsigned int opt = 0; opt <
nbIterBrent; opt++) {
421 for (
unsigned int i = 0; i < 3; i++) {
422 A[i][0] = talpha[i] * talpha[i];
427 for (
unsigned int i = 0; i < 3; i++)
430 parabol = (A.
t() * A).inverseByLU() * A.
t() * B;
433 if (parabol[0] > 0) {
434 talpha[3] = -0.5 * parabol[1] / parabol[0];
436 int tindic_x_min = 0;
437 int tindic_x_max = 0;
438 for (
int i = 1; i < 3; i++) {
439 if (talpha[i] < talpha[tindic_x_min])
441 if (talpha[i] > talpha[tindic_x_max])
445 if (Cost[tindic_x_max] < Cost[tindic_x_min]) {
446 talpha[3] = talpha[tindic_x_max] + 1.;
448 talpha[3] = talpha[tindic_x_min] - 1.;
453 for (
int i = 1; i < 3; i++) {
454 if (talpha[i] < talpha[indic_x_min])
456 if (talpha[i] > talpha[indic_x_max])
459 if (talpha[3] > talpha[indic_x_max])
460 if ((talpha[3] - talpha[indic_x_max]) > alpha)
461 talpha[3] = talpha[indic_x_max] + 4.;
462 if (talpha[3] < talpha[indic_x_min])
463 if ((talpha[indic_x_min] - talpha[3]) > alpha)
464 talpha[3] = talpha[indic_x_min] - 4.;
467 adpt = talpha[3] * direction;
474 p3 = tp + talpha[3] * direction;
479 int indice_f_max = 0;
480 for (
int i = 1; i < 4; i++)
481 if (Cost[i] > Cost[indice_f_max])
483 if (indice_f_max != 3) {
484 *ptp[indice_f_max] = *ptp[3];
485 Cost[indice_f_max] = Cost[3];
486 talpha[indice_f_max] = talpha[3];
491 int indice_f_min = 0;
492 for (
int i = 0; i < 4; i++)
493 if (Cost[i] < Cost[indice_f_min])
496 alpha = talpha[indice_f_min];
520 ptTemplateSuppPyr =
new vpTemplateTrackerPointSuppMIInv *[
nbLvlPyr];
522 for (
unsigned int i = 0; i <
nbLvlPyr; i++) {
524 ptTemplateSuppPyr[i] = NULL;
548 for (
unsigned int i = 1; i <
nbLvlPyr; i++) {
649 ptTemplateSuppPyr[0] = ptTemplateSupp;
655 ptTemplateSuppPyr[0] = ptTemplateSupp;
666 for (
unsigned int i = 1; i <
nbLvlPyr; i++) {
674 ptTemplateSuppPyr[i] = ptTemplateSupp;
680 ptTemplateSuppPyr[i] = ptTemplateSupp;
712 for (
unsigned int i = 1; i <
nbLvlPyr; i++) {
719 for (
int i = (
int)
nbLvlPyr - 1; i >= 0; i--) {
720 if (i >= (
int)
l0Pyr) {
724 ptTemplateSupp = ptTemplateSuppPyr[i];
751 p_pre_estimation =
p;
758 if (pre_fcost < post_fcost) {
759 p = p_pre_estimation;
776 Warp->computeCoeff(param);
782 for (
unsigned int j = 0; j < 3; j++) {
785 Warp->computeDenom(
X1, param);
788 unsigned int index = i * 3 + j;
789 double x_ =
x_pos[index] -
X2[0];
790 double y_ =
y_pos[index] -
X2[1];
808 x_pos.resize(nb_corners);
809 y_pos.resize(nb_corners);
811 Warp->computeCoeff(param);
815 unsigned int i3 = i * 3;
817 for (
unsigned int j = 0; j < 3; j++) {
820 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 functionnalities.
error that can be emited by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
const char * getMessage() const
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, bool convolve=false)
static void getGradXGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
static void getGaussianDerivativeKernel(double *filter, unsigned int size, double sigma=0., bool normalize=true)
static void getGaussianKernel(double *filter, unsigned int size, double sigma=0., bool normalize=true)
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
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 emited by the vpTracker class and its derivates.