/*!
Test the quality of the tracking.
The tracking is supposed to fail if less than 10 points are tracked.
\todo Find a efficient way to test the quality.
\throw vpTrackingException::fatalError if the test fails.
*/
void
vpMbKltTracker::testTracking()
{
unsigned int nbTotalPoints = 0;
vpMbtDistanceKltPoints *kltpoly;
// for (unsigned int i = 0; i < faces.size(); i += 1){
for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
kltpoly = *it;
if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 && kltpoly->hasEnoughPoints()){
nbTotalPoints += kltpoly->getCurrentNumberPoints();
}
}
vpMbtDistanceKltCylinder *kltPolyCylinder;
for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
kltPolyCylinder = *it;
if(kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
nbTotalPoints += kltPolyCylinder->getCurrentNumberPoints();
}
if(nbTotalPoints < 10){
std::cerr << "test tracking failed (too few points to realize a good tracking)." << std::endl;
throw vpTrackingException(vpTrackingException::fatalError,
"test tracking failed (too few points to realize a good tracking).");
}
}
void vpTemplateTrackerWarpHomography::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
{
//if((ParamM[2]*vX[0]+ParamM[5]*vX[1]+1)>0)//si dans le plan image reel
if((denom)>0)// FS optimisation
{
vXres[0]=((1+ParamM[0])*vX[0]+ParamM[3]*vX[1]+ParamM[6])*denom;
vXres[1]=(ParamM[1]*vX[0]+(1+ParamM[4])*vX[1]+ParamM[7])*denom;
}
else
throw(vpTrackingException(vpTrackingException::fatalError,"Division by zero in vpTemplateTrackerWarpHomography::warpX()"));
}
void vpTemplateTrackerWarpHomography::warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
{
if((ParamM[2]*vX[0]+ParamM[5]*vX[1]+1)<0)//si dans le plan image reel
{
vXres[0]=((1+ParamM[0])*vX[0]+ParamM[3]*vX[1]+ParamM[6])/(ParamM[2]*vX[0]+ParamM[5]*vX[1]+1);
vXres[1]=(ParamM[1]*vX[0]+(1+ParamM[4])*vX[1]+ParamM[7])/(ParamM[2]*vX[0]+ParamM[5]*vX[1]+1);
}
else
throw(vpTrackingException(vpTrackingException::fatalError,"Division by zero in vpTemplateTrackerWarpHomography::warpXSpecialInv()")) ;
}
void
vpMbKltTracker::computeVVSInteractionMatrixAndResidu(unsigned int shift, vpColVector &R, vpMatrix &L, vpHomography &H,
std::list<vpMbtDistanceKltPoints*> &kltPolygons_, std::list<vpMbtDistanceKltCylinder*> &kltCylinders_,
const vpHomogeneousMatrix &ctTc0_) {
vpMbtDistanceKltPoints *kltpoly;
// for (unsigned int i = 0; i < faces.size(); i += 1){
for(std::list<vpMbtDistanceKltPoints*>::const_iterator it = kltPolygons_.begin(); it != kltPolygons_.end(); ++it){
kltpoly = *it;
if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
kltpoly->hasEnoughPoints()){
vpSubColVector subR(R, shift, 2*kltpoly->getCurrentNumberPoints());
vpSubMatrix subL(L, shift, 0, 2*kltpoly->getCurrentNumberPoints(), 6);
try{
kltpoly->computeHomography(ctTc0_, H);
kltpoly->computeInteractionMatrixAndResidu(subR, subL);
}catch(...){
throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
}
shift += 2*kltpoly->getCurrentNumberPoints();
}
}
vpMbtDistanceKltCylinder *kltPolyCylinder;
for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it = kltCylinders_.begin(); it != kltCylinders_.end(); ++it){
kltPolyCylinder = *it;
if(kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
{
vpSubColVector subR(R, shift, 2*kltPolyCylinder->getCurrentNumberPoints());
vpSubMatrix subL(L, shift, 0, 2*kltPolyCylinder->getCurrentNumberPoints(), 6);
try{
kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0_,subR, subL);
}catch(...){
throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
}
shift += 2*kltPolyCylinder->getCurrentNumberPoints();
}
}
}
/*!
This method choose a gray level (default is 0) used to modify the
"in" dot level in "out" dot level. This gray level is here needed to
stop the recursivity . The pixels of the dot are set to this new
gray level "\out\" when connexe() is called.
\exception vpTrackingException::initializationError : No valid gray
level "out" can be found. This means that the min gray level is set to 0
and the max gray level to 255. This should not occur
*/
void
vpDot::setGrayLevelOut()
{
if (gray_level_min == 0) {
if (gray_level_max == 255) {
// gray_level_min = 0 and gray_level_max = 255: this should not occur
vpERROR_TRACE("Unable to choose a good \"out\" level") ;
throw(vpTrackingException(vpTrackingException::initializationError,
"Unable to choose a good \"out\" level")) ;
}
gray_level_out = static_cast<unsigned char>(gray_level_max + 1u);
}
}
/*!
Resample the ellipse if the number of sample is less than 90% of the
expected value.
\note The expected value is computed thanks to the difference between the smallest and the biggest \f$ \alpha \f$ angles
and the parameter which indicates the number of degrees between
two points (vpMe::sample_step).
\param I : Image in which the ellipse appears.
\exception vpTrackingException::initializationError : Moving edges not initialized.
*/
void
vpMeEllipse::reSample(const vpImage<unsigned char> &I)
{
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
throw(vpTrackingException(vpTrackingException::initializationError,
"Moving edges not initialized")) ;
}
unsigned int n = numberOfSignal() ;
expecteddensity = (alpha2-alpha1) / vpMath::rad((double)me->getSampleStep());
if ((double)n<0.9*expecteddensity){
sample(I) ;
}
}
void
vpMbKltTracker::computeVVSCheckLevenbergMarquardtKlt(const unsigned int iter, const unsigned int nbInfos,
const vpHomogeneousMatrix &cMoPrev, const vpColVector &error_prev, const vpHomogeneousMatrix &ctTc0_Prev,
double &mu, bool &reStartFromLastIncrement) {
if(iter != 0 && m_optimizationMethod == vpMbTracker::LEVENBERG_MARQUARDT_OPT){
if(m_error.sumSquare()/(double)(2*nbInfos) > error_prev.sumSquare()/(double)(2*nbInfos)){
mu *= 10.0;
if(mu > 1.0)
throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
cMo = cMoPrev;
m_error = error_prev;
ctTc0 = ctTc0_Prev;
reStartFromLastIncrement = true;
}
}
}
/*!
Track KLT keypoints using the iterative Lucas-Kanade method with pyramids.
\param I : Input image.
*/
void vpKltOpencv::track(const cv::Mat &I)
{
if(m_points[1].size() == 0)
throw vpTrackingException(vpTrackingException::fatalError, "Not enough key points to track.");
std::vector<float> err;
int flags = 0;
cv::swap(m_prevGray, m_gray);
if (m_initial_guess) {
flags |= cv::OPTFLOW_USE_INITIAL_FLOW;
m_initial_guess = false;
}
else {
std::swap(m_points[1], m_points[0]);
}
//cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
I.copyTo(m_gray);
if(m_prevGray.empty()){
m_gray.copyTo(m_prevGray);
}
std::vector<uchar> status;
cv::calcOpticalFlowPyrLK(m_prevGray, m_gray, m_points[0], m_points[1], status, err, cv::Size(m_winSize, m_winSize),
m_pyrMaxLevel, m_termcrit, flags, m_minEigThreshold);
// Remove points that are lost
for (int i=(int)status.size()-1; i>=0; i--) {
if (status[(size_t)i] == 0) { // point is lost
m_points[0].erase(m_points[0].begin()+i);
m_points[1].erase(m_points[1].begin()+i);
m_points_id.erase(m_points_id.begin()+i);
}
}
}
/*!
Resample the line if the number of sample is less than 80% of the
expected value.
\note The expected value is computed thanks to the length of the
line and the parameter which indicates the number of pixel between
two points (vpMe::sample_step).
\param I : Image in which the line appears.
*/
void
vpMeLine::reSample(const vpImage<unsigned char> &I)
{
double i1,j1,i2,j2 ;
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
throw(vpTrackingException(vpTrackingException::initializationError,
"Moving edges not initialized")) ;
}
project(a,b,c,PExt[0].ifloat,PExt[0].jfloat,i1,j1) ;
project(a,b,c,PExt[1].ifloat,PExt[1].jfloat,i2,j2) ;
// Points extremites
PExt[0].ifloat = i1 ;
PExt[0].jfloat = j1 ;
PExt[1].ifloat = i2 ;
PExt[1].jfloat = j2 ;
double d = sqrt(vpMath::sqr(i1-i2)+vpMath::sqr(j1-j2)) ;
unsigned int n = numberOfSignal() ;
double expecteddensity = d / (double)me->getSampleStep();
if ((double)n<0.9*expecteddensity)
{
double delta_new = delta;
delta = delta_1;
sample(I) ;
delta = delta_new;
// 2. On appelle ce qui n'est pas specifique
{
vpMeTracker::initTracking(I) ;
}
}
}
/*!
Realize the tracking of the object in the image
\throw vpException : if the tracking is supposed to have failed
\param I : the input image
*/
void
vpMbKltTracker::track(const vpImage<unsigned char>& I)
{
unsigned int nbInfos = 0;
unsigned int nbFaceUsed = 0;
try{
preTracking(I, nbInfos, nbFaceUsed);
}
catch(vpException &e){
throw e;
}
if(nbInfos < 4 || nbFaceUsed == 0){
vpERROR_TRACE("\n\t\t Error-> not enough data") ;
throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data");
}
//vpColVector w;
computeVVS(nbInfos, m_w);
if(postTracking(I, m_w))
reinit(I);
}
/*!
Construct a list of vpMeSite moving edges at a particular sampling
step between the two extremities. The two extremities are defined by
the points with the smallest and the biggest \f$ alpha \f$ angle.
\param I : Image in which the ellipse appears.
\exception vpTrackingException::initializationError : Moving edges not initialized.
*/
void
vpMeEllipse::sample(const vpImage<unsigned char> & I)
{
vpCDEBUG(1) <<"begin vpMeEllipse::sample() : "<<std::endl ;
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
throw(vpTrackingException(vpTrackingException::initializationError,
"Moving edges not initialized")) ;
}
int height = (int)I.getHeight() ;
int width = (int)I.getWidth() ;
double n_sample;
//if (me->getSampleStep()==0)
if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
{
std::cout << "In vpMeEllipse::sample: " ;
std::cout << "function called with sample step = 0" ;
//return fatalError ;
}
double j, i;//, j11, i11;
vpImagePoint iP11;
j = i = 0.0 ;
double incr = vpMath::rad(me->getSampleStep()) ; // angle increment en degree
vpColor col = vpColor::red ;
getParameters() ;
// Delete old list
list.clear();
angle.clear();
// sample positions
double k = alpha1 ;
while (k<alpha2)
{
// j = a *cos(k) ; // equation of an ellipse
// i = b *sin(k) ; // equation of an ellipse
j = a *sin(k) ; // equation of an ellipse
i = b *cos(k) ; // equation of an ellipse
// (i,j) are the coordinates on the origin centered ellipse ;
// a rotation by "e" and a translation by (xci,jc) are done
// to get the coordinates of the point on the shifted ellipse
// iP11.set_j( iPc.get_j() + ce *j - se *i );
// iP11.set_i( iPc.get_i() -( se *j + ce *i) );
iP11.set_j( iPc.get_j() + ce *j + se *i );
iP11.set_i( iPc.get_i() - se *j + ce *i );
vpDisplay::displayCross(I, iP11, 5, col) ;
double theta ;
computeTheta(theta, K, iP11) ;
// If point is in the image, add to the sample list
if(!outOfImage(vpMath::round(iP11.get_i()), vpMath::round(iP11.get_j()), 0, height, width))
{
vpMeSite pix ;
pix.init((int)iP11.get_i(), (int)iP11.get_j(), theta) ;
pix.setDisplay(selectDisplay) ;
pix.setState(vpMeSite::NO_SUPPRESSION);
if(vpDEBUG_ENABLE(3))
{
vpDisplay::displayCross(I,iP11, 5, vpColor::blue);
}
list.push_back(pix);
angle.push_back(k);
}
k += incr ;
}
vpMeTracker::initTracking(I) ;
n_sample = (unsigned int)list.size() ;
vpCDEBUG(1) << "end vpMeEllipse::sample() : " ;
vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
}
/*!
Least squares method used to make the tracking more robust. It
ensures that the points taken into account to compute the right
equation belong to the line.
*/
void
vpMeLine::leastSquare()
{
vpMatrix A(numberOfSignal(),2) ;
vpColVector x(2), x_1(2) ;
x_1 = 0;
unsigned int i ;
vpRobust r(numberOfSignal()) ;
r.setThreshold(2);
r.setIteration(0) ;
vpMatrix D(numberOfSignal(),numberOfSignal()) ;
D.eye() ;
vpMatrix DA, DAmemory ;
vpColVector DAx ;
vpColVector w(numberOfSignal()) ;
vpColVector B(numberOfSignal()) ;
w =1 ;
vpMeSite p_me ;
unsigned int iter =0 ;
unsigned int nos_1 = 0 ;
double distance = 100;
if (list.size() <= 2 || numberOfSignal() <= 2)
{
//vpERROR_TRACE("Not enough point") ;
vpCDEBUG(1) << "Not enough point";
throw(vpTrackingException(vpTrackingException::notEnoughPointError,
"not enough point")) ;
}
if ((fabs(b) >=0.9)) // Construction du systeme Ax=B
// a i + j + c = 0
// A = (i 1) B = (-j)
{
nos_1 = numberOfSignal() ;
unsigned int k =0 ;
for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
A[k][0] = p_me.ifloat ;
A[k][1] = 1 ;
B[k] = -p_me.jfloat ;
k++ ;
}
}
while (iter < 4 && distance > 0.05)
{
DA = D*A ;
x = DA.pseudoInverse(1e-26) *D*B ;
vpColVector residu(nos_1);
residu = B - A*x;
r.setIteration(iter) ;
r.MEstimator(vpRobust::TUKEY,residu,w) ;
k = 0;
for (i=0 ; i < nos_1 ; i++)
{
D[k][k] =w[k] ;
k++;
}
iter++ ;
distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
x_1 = x;
}
k =0 ;
for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
if (w[k] < 0.2)
{
p_me.setState(vpMeSite::M_ESTIMATOR);
*it = p_me;
}
k++ ;
}
}
// mise a jour de l'equation de la droite
a = x[0] ;
b = 1 ;
c = x[1] ;
double s =sqrt( vpMath::sqr(a)+vpMath::sqr(b)) ;
a /= s ;
b /= s ;
c /= s ;
}
else // Construction du systeme Ax=B
// i + bj + c = 0
// A = (j 1) B = (-i)
{
nos_1 = numberOfSignal() ;
unsigned int k =0 ;
for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
A[k][0] = p_me.jfloat ;
A[k][1] = 1 ;
B[k] = -p_me.ifloat ;
k++ ;
}
}
while (iter < 4 && distance > 0.05)
{
DA = D*A ;
x = DA.pseudoInverse(1e-26) *D*B ;
vpColVector residu(nos_1);
residu = B - A*x;
r.setIteration(iter) ;
r.MEstimator(vpRobust::TUKEY,residu,w) ;
k = 0;
for (i=0 ; i < nos_1 ; i++)
{
D[k][k] =w[k] ;
k++;
}
iter++ ;
distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
x_1 = x;
}
k =0 ;
for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
if (w[k] < 0.2)
{
p_me.setState(vpMeSite::M_ESTIMATOR);
*it = p_me;
}
k++ ;
}
}
a = 1 ;
b = x[0] ;
c = x[1] ;
double s = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
a /= s ;
b /= s ;
c /= s ;
}
// mise a jour du delta
delta = atan2(a,b) ;
normalizeAngle(delta) ;
}
/*!
Seek along the line defined by its equation, the two extremities of
the line. This function is useful in case of translation of the
line.
\param I : Image in which the line appears.
\exception vpTrackingException::initializationError : Moving edges not initialized.
*/
void
vpMeLine::seekExtremities(const vpImage<unsigned char> &I)
{
vpCDEBUG(1) <<"begin vpMeLine::sample() : "<<std::endl ;
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
throw(vpTrackingException(vpTrackingException::initializationError,
"Moving edges not initialized")) ;
}
int rows = (int)I.getHeight() ;
int cols = (int)I.getWidth() ;
double n_sample;
//if (me->getSampleStep()==0)
if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
{
vpERROR_TRACE("function called with sample step = 0") ;
throw(vpTrackingException(vpTrackingException::fatalError,
"sample step = 0")) ;
}
// i, j portions of the line_p
double diffsi = PExt[0].ifloat-PExt[1].ifloat;
double diffsj = PExt[0].jfloat-PExt[1].jfloat;
double s = vpMath::sqr(diffsi)+vpMath::sqr(diffsj) ;
double di = diffsi/sqrt(s) ; // pas de risque de /0 car d(P1,P2) >0
double dj = diffsj/sqrt(s) ;
double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));
// number of samples along line_p
n_sample = length_p/(double)me->getSampleStep();
double sample_step = (double)me->getSampleStep();
vpMeSite P ;
P.init((int) PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign) ;
P.setDisplay(selectDisplay) ;
unsigned int memory_range = me->getRange() ;
me->setRange(1);
vpImagePoint ip;
for (int i=0 ; i < 3 ; i++)
{
P.ifloat = P.ifloat + di*sample_step ; P.i = (int)P.ifloat ;
P.jfloat = P.jfloat + dj*sample_step ; P.j = (int)P.jfloat ;
if(!outOfImage(P.i, P.j, 5, rows, cols))
{
P.track(I,me,false) ;
if (P.getState() == vpMeSite::NO_SUPPRESSION)
{
list.push_back(P);
if (vpDEBUG_ENABLE(3)) {
ip.set_i( P.i );
ip.set_j( P.j );
vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
}
}
else {
if (vpDEBUG_ENABLE(3)) {
ip.set_i( P.i );
ip.set_j( P.j );
vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
}
}
}
}
P.init((int) PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign) ;
P.setDisplay(selectDisplay) ;
for (int i=0 ; i < 3 ; i++)
{
P.ifloat = P.ifloat - di*sample_step ; P.i = (int)P.ifloat ;
P.jfloat = P.jfloat - dj*sample_step ; P.j = (int)P.jfloat ;
if(!outOfImage(P.i, P.j, 5, rows, cols))
{
P.track(I,me,false) ;
if (P.getState() == vpMeSite::NO_SUPPRESSION)
{
list.push_back(P);
if (vpDEBUG_ENABLE(3)) {
ip.set_i( P.i );
ip.set_j( P.j );
vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
}
}
else {
if (vpDEBUG_ENABLE(3)) {
ip.set_i( P.i );
ip.set_j( P.j );
vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
}
}
}
}
me->setRange(memory_range);
vpCDEBUG(1) <<"end vpMeLine::sample() : " ;
vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
}
/*!
Least squares method used to make the tracking more robust. It
ensures that the points taken into account to compute the right
equation belong to the ellipse.
*/
void
vpMeEllipse::leastSquare()
{
// Construction du systeme Ax=b
//! i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
// A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
unsigned int i ;
vpMeSite p_me ;
unsigned int iter =0 ;
vpColVector b_(numberOfSignal()) ;
vpRobust r(numberOfSignal()) ;
r.setThreshold(2);
r.setIteration(0) ;
vpMatrix D(numberOfSignal(),numberOfSignal()) ;
D.setIdentity() ;
vpMatrix DA, DAmemory ;
vpColVector DAx ;
vpColVector w(numberOfSignal()) ;
w =1 ;
unsigned int nos_1 = numberOfSignal() ;
if (list.size() < 3)
{
vpERROR_TRACE("Not enough point") ;
throw(vpTrackingException(vpTrackingException::notEnoughPointError,
"not enough point")) ;
}
if (circle ==false)
{
vpMatrix A(numberOfSignal(),5) ;
vpColVector x(5);
unsigned int k =0 ;
for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
A[k][0] = vpMath::sqr(p_me.jfloat) ;
A[k][1] = 2 * p_me.ifloat * p_me.jfloat ;
A[k][2] = 2 * p_me.ifloat ;
A[k][3] = 2 * p_me.jfloat ;
A[k][4] = 1 ;
b_[k] = - vpMath::sqr(p_me.ifloat) ;
k++ ;
}
}
while (iter < 4 )
{
DA = D*A ;
vpMatrix DAp ;
x = DA.pseudoInverse(1e-26) *D*b_ ;
vpColVector residu(nos_1);
residu = b_ - A*x;
r.setIteration(iter) ;
r.MEstimator(vpRobust::TUKEY,residu,w) ;
k = 0;
for (i=0 ; i < nos_1 ; i++)
{
D[k][k] =w[k] ;
k++;
}
iter++;
}
k =0 ;
for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
if (w[k] < thresholdWeight)
{
p_me.setState(vpMeSite::M_ESTIMATOR);
*it = p_me;
}
k++ ;
}
}
for(i = 0; i < 5; i ++)
K[i] = x[i];
}
else
{
vpMatrix A(numberOfSignal(),3) ;
vpColVector x(3);
unsigned int k =0 ;
for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
A[k][0] = 2* p_me.ifloat ;
A[k][1] = 2 * p_me.jfloat ;
A[k][2] = 1 ;
b_[k] = - vpMath::sqr(p_me.ifloat) - vpMath::sqr(p_me.jfloat) ;
k++ ;
}
}
while (iter < 4 )
{
DA = D*A ;
vpMatrix DAp ;
x = DA.pseudoInverse(1e-26) *D*b_ ;
vpColVector residu(nos_1);
residu = b_ - A*x;
r.setIteration(iter) ;
r.MEstimator(vpRobust::TUKEY,residu,w) ;
k = 0;
for (i=0 ; i < nos_1 ; i++)
{
D[k][k] =w[k];
k++;
}
iter++;
}
k =0 ;
for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
if (w[k] < thresholdWeight)
{
p_me.setState(vpMeSite::M_ESTIMATOR);
*it = p_me;
}
k++ ;
}
}
for(i = 0; i < 3; i ++)
K[i+2] = x[i];
}
getParameters() ;
}
/*!
Construct a list of vpMeSite moving edges at a particular sampling
step between the two extremities of the line.
\param I : Image in which the line appears.
\exception vpTrackingException::initializationError : Moving edges not initialized.
*/
void
vpMeLine::sample(const vpImage<unsigned char>& I)
{
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
throw(vpTrackingException(vpTrackingException::initializationError,
"Moving edges not initialized")) ;
}
int rows = (int)I.getHeight() ;
int cols = (int)I.getWidth() ;
double n_sample;
if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
{
vpERROR_TRACE("function called with sample step = 0") ;
throw(vpTrackingException(vpTrackingException::fatalError,
"sample step = 0")) ;
}
// i, j portions of the line_p
double diffsi = PExt[0].ifloat-PExt[1].ifloat;
double diffsj = PExt[0].jfloat-PExt[1].jfloat;
double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));
if(std::fabs(length_p)<=std::numeric_limits<double>::epsilon())
throw(vpTrackingException(vpTrackingException::fatalError,"points too close of each other to define a line")) ;
// number of samples along line_p
n_sample = length_p/(double)me->getSampleStep();
double stepi = diffsi/(double)n_sample;
double stepj = diffsj/(double)n_sample;
// Choose starting point
double is = PExt[1].ifloat;
double js = PExt[1].jfloat;
// Delete old list
list.clear();
// sample positions at i*me->getSampleStep() interval along the
// line_p, starting at PSiteExt[0]
vpImagePoint ip;
for(int i=0; i<=vpMath::round(n_sample); i++)
{
// If point is in the image, add to the sample list
if(!outOfImage(vpMath::round(is), vpMath::round(js), 0, rows, cols))
{
vpMeSite pix ; //= list.value();
pix.init((int)is, (int)js, delta, 0, sign) ;
pix.setDisplay(selectDisplay) ;
if(vpDEBUG_ENABLE(3))
{
ip.set_i( is );
ip.set_j( js );
vpDisplay::displayCross(I, ip, 2, vpColor::blue);
}
list.push_back(pix);
}
is += stepi;
js += stepj;
}
vpCDEBUG(1) << "end vpMeLine::sample() : ";
vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
}
/*!
Track moving-edges.
\param I : Image.
\exception vpTrackingException::initializationError : Moving edges not initialized.
*/
void
vpMeTracker::track(const vpImage<unsigned char>& I)
{
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
throw(vpTrackingException(vpTrackingException::initializationError,
"Moving edges not initialized")) ;
}
if (list.empty())
{
vpDERROR_TRACE(2, "Tracking error: too few pixel to track");
throw(vpTrackingException(vpTrackingException::notEnoughPointError,
"too few pixel to track")) ;
}
vpImagePoint ip1, ip2;
nGoodElement=0;
// int d =0;
// Loop through list of sites to track
for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
vpMeSite s = *it;//current reference pixel
// d++ ;
// If element hasn't been suppressed
if(s.getState() == vpMeSite::NO_SUPPRESSION)
{
try{
// vpERROR_TRACE("%d",d ) ;
// vpERROR_TRACE("range %d",me->range) ;
s.track(I,me,true);
}
catch(vpTrackingException)
{
vpERROR_TRACE("catch exception ") ;
s.setState(vpMeSite::THRESHOLD);
}
if(s.getState() != vpMeSite::THRESHOLD)
{
nGoodElement++;
#if (DEBUG_LEVEL2)
{
double a,b ;
a = s.i_1 - s.i ;
b = s.j_1 - s.j ;
if(s.getState() == vpMeSite::NO_SUPPRESSION) {
ip1.set_i( s.i );
ip1.set_j( s.j );
ip2.set_i( s.i+a*5 );
ip2.set_j( s.j+b*5 );
vpDisplay::displayArrow(I, ip1, ip2, vpColor::green) ;
}
}
#endif
}
*it = s;
}
}
}
/*!
Seek along the ellipse edge defined by its equation, the two extremities of
the ellipse (ie the two points with the smallest and the biggest \f$ \alpha \f$ angle.
\param I : Image in which the ellipse appears.
\exception vpTrackingException::initializationError : Moving edges not initialized.
*/
void
vpMeEllipse::seekExtremities(const vpImage<unsigned char> &I)
{
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
throw(vpTrackingException(vpTrackingException::initializationError,
"Moving edges not initialized")) ;
}
int rows = (int)I.getHeight() ;
int cols = (int)I.getWidth() ;
vpImagePoint ip;
unsigned int memory_range = me->getRange() ;
me->setRange(2);
double memory_mu1 = me->getMu1();
me->setMu1(0.5);
double memory_mu2 = me->getMu2();
me->setMu2(0.5);
double incr = vpMath::rad(2.0) ;
if (alpha2-alpha1 < 2*M_PI-vpMath::rad(6.0))
{
vpMeSite P;
double k = alpha1;
double i1,j1;
for (unsigned int i=0 ; i < 3 ; i++)
{
k -= incr;
//while ( k < -M_PI ) { k+=2*M_PI; }
i1 = b *cos(k) ; // equation of an ellipse
j1 = a *sin(k) ; // equation of an ellipse
P.ifloat = iPc.get_i() - se *j1 + ce *i1 ; P.i = (int)P.ifloat ;
P.jfloat = iPc.get_j() + ce *j1 + se *i1 ; P.j = (int)P.jfloat ;
if(!outOfImage(P.i, P.j, 5, rows, cols))
{
P.track(I,me,false) ;
if (P.getState() == vpMeSite::NO_SUPPRESSION)
{
list.push_back(P);
angle.push_back(k);
if (vpDEBUG_ENABLE(3)) {
ip.set_i( P.i );
ip.set_j( P.j );
vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
}
}
else {
if (vpDEBUG_ENABLE(3)) {
ip.set_i( P.i );
ip.set_j( P.j );
vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
}
}
}
}
k = alpha2;
for (unsigned int i=0 ; i < 3 ; i++)
{
k += incr;
//while ( k > M_PI ) { k-=2*M_PI; }
i1 = b *cos(k) ; // equation of an ellipse
j1 = a *sin(k) ; // equation of an ellipse
P.ifloat = iPc.get_i() - se *j1 + ce *i1 ; P.i = (int)P.ifloat ;
P.jfloat = iPc.get_j() + ce *j1 + se *i1 ; P.j = (int)P.jfloat ;
if(!outOfImage(P.i, P.j, 5, rows, cols))
{
P.track(I,me,false) ;
if (P.getState() == vpMeSite::NO_SUPPRESSION)
{
list.push_back(P);
angle.push_back(k);
if (vpDEBUG_ENABLE(3)) {
ip.set_i( P.i );
ip.set_j( P.j );
vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
}
}
else {
if (vpDEBUG_ENABLE(3)) {
ip.set_i( P.i );
ip.set_j( P.j );
vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
}
}
}
}
}
suppressPoints() ;
me->setRange(memory_range);
me->setMu1(memory_mu1);
me->setMu2(memory_mu2);
}
/*!
Compute the two parameters \f$(\rho, \theta)\f$ of the line.
\param I : Image in which the line appears.
*/
void
vpMeLine::computeRhoTheta(const vpImage<unsigned char>& I)
{
//rho = -c ;
//theta = atan2(a,b) ;
rho = fabs(c);
theta = atan2(b,a) ;
while (theta >= M_PI) theta -=M_PI ;
while (theta < 0) theta +=M_PI ;
if(_useIntensityForRho){
/* while(theta < -M_PI) theta += 2*M_PI ;
while(theta >= M_PI) theta -= 2*M_PI ;
// If theta is between -90 and -180 get the equivalent
// between 0 and 90
if(theta <-M_PI/2)
{
theta += M_PI ;
rho *= -1 ;
}
// If theta is between 90 and 180 get the equivalent
// between 0 and -90
if(theta >M_PI/2)
{
theta -= M_PI ;
rho *= -1 ;
}
*/
// convention pour choisir le signe de rho
int i,j ;
i = vpMath::round((PExt[0].ifloat + PExt[1].ifloat )/2) ;
j = vpMath::round((PExt[0].jfloat + PExt[1].jfloat )/2) ;
int end = false ;
int incr = 10 ;
int i1=0,i2=0,j1=0,j2=0 ;
unsigned char v1=0,v2=0 ;
int width_ = (int)I.getWidth();
int height_ = (int)I.getHeight();
update_indices(theta,i,j,incr,i1,i2,j1,j2);
if(i1<0 || i1>=height_ || i2<0 || i2>=height_ ||
j1<0 || j1>=width_ || j2<0 || j2>=width_){
double rho_lim1 = fabs((double)i/cos(theta));
double rho_lim2 = fabs((double)j/sin(theta));
double co_rho_lim1 = fabs(((double)(height_-i))/cos(theta));
double co_rho_lim2 = fabs(((double)(width_-j))/sin(theta));
double rho_lim = std::min(rho_lim1,rho_lim2);
double co_rho_lim = std::min(co_rho_lim1,co_rho_lim2);
incr = (int)std::floor(std::min(rho_lim,co_rho_lim));
if(incr<INCR_MIN){
vpERROR_TRACE("increment is too small") ;
throw(vpTrackingException(vpTrackingException::fatalError,
"increment is too small")) ;
}
update_indices(theta,i,j,incr,i1,i2,j1,j2);
}
while (!end)
{
end = true;
unsigned int i1_ = static_cast<unsigned int>(i1);
unsigned int j1_ = static_cast<unsigned int>(j1);
unsigned int i2_ = static_cast<unsigned int>(i2);
unsigned int j2_ = static_cast<unsigned int>(j2);
v1=I[i1_][j1_];
v2=I[i2_][j2_];
if (abs(v1-v2) < 1)
{
incr-- ;
end = false ;
if (incr==1)
{
std::cout << "In CStraightLine::GetParameters() " ;
std::cout << " Error Tracking " << abs(v1-v2) << std::endl ;
}
}
update_indices(theta,i,j,incr,i1,i2,j1,j2);
}
if (theta >=0 && theta <= M_PI/2)
{
if (v2 < v1)
{
theta += M_PI;
rho *= -1;
}
}
else
{
double jinter;
jinter = -c/b;
if (v2 < v1)
{
theta += M_PI;
if (jinter > 0)
{
rho *= -1;
}
}
else
{
if (jinter < 0)
{
rho *= -1;
}
}
}
if (vpDEBUG_ENABLE(2))
{
vpImagePoint ip, ip1, ip2;
ip.set_i( i );
ip.set_j( j );
ip1.set_i( i1 );
ip1.set_j( j1 );
ip2.set_i( i2 );
ip2.set_j( j2 );
vpDisplay::displayArrow(I, ip, ip1, vpColor::green) ;
vpDisplay::displayArrow(I, ip, ip2, vpColor::red) ;
}
}
}
void vpTemplateTrackerSSDInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
{
double erreur=0;
unsigned int Nbpoint=0;
if(blur)
vpImageFilter::filter(I, BI,fgG,taillef);
vpColVector dpinv(nbParam);
double IW;
double Tij;
unsigned int iteration=0;
int i,j;
double i2,j2;
double alpha=2.;
//vpTemplateTrackerPointtest *pt;
initPosEvalRMS(p);
vpTemplateTrackerPoint *pt;
do
{
Nbpoint=0;
erreur=0;
dp=0;
Warp->computeCoeff(p);
for(unsigned int point=0;point<templateSize;point++)
{
if((!useTemplateSelect)||(ptTemplateSelect[point]))
{
//pt=&ptTemplatetest[point];
pt=&ptTemplate[point];
i=pt->y;
j=pt->x;
X1[0]=j;X1[1]=i;
Warp->computeDenom(X1,p);
Warp->warpX(X1,X2,p);
j2=X2[0];i2=X2[1];
if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
{
Tij=pt->val;
if(!blur)
IW=I.getValue(i2,j2);
else
IW=BI.getValue(i2,j2);
Nbpoint++;
double er=(Tij-IW);
for(unsigned int it=0;it<nbParam;it++)
dp[it]+=er*pt->HiG[it];
erreur+=er*er;
}
}
}
//std::cout << "npoint: " << Nbpoint << std::endl;
if(Nbpoint==0) {
//std::cout<<"plus de point dans template suivi"<<std::endl;
deletePosEvalRMS();
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
dp=gain*dp;
//std::cout<<erreur/Nbpoint<<","<<GetCost(I,p)<<std::endl;
if(useBrent)
{
alpha=2.;
computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
dp=alpha*dp;
}
Warp->getParamInverse(dp,dpinv);
Warp->pRondp(p,dpinv,p);
iteration++;
computeEvalRMS(p);
//std::cout << "iteration: " << iteration << " max: " << iterationMax << std::endl;
//std::cout << "evolRMS: " << evolRMS << " threshold: " << threshold_RMS << std::endl;
}
while(/*( erreur_prec-erreur<50) &&*/ (iteration < iterationMax)&&(evolRMS>threshold_RMS));
nbIteration=iteration;
deletePosEvalRMS();
}
void vpTemplateTrackerMIESM::trackNoPyr(const vpImage<unsigned char> &I)
{
if(!CompoInitialised)
std::cout<<"Compositionnal tracking no initialised\nUse initCompInverse(vpImage<unsigned char> &I) function"<<std::endl;
dW=0;
if(blur)
vpImageFilter::filter(I, BI,fgG,taillef);
vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
/* if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_0 && ApproxHessian!=HESSIAN_NEW && ApproxHessian!=HESSIAN_YOUCEF)
{
getGradX(dIx, d2Ix,fgdG,taillef);
getGradY(dIx, d2Ixy,fgdG,taillef);
getGradY(dIy, d2Iy,fgdG,taillef);
}*/
double erreur=0;
int Nbpoint=0;
int point;
MI_preEstimation=-getCost(I,p);
double lambda=lambdaDep;
double MI=0;
//double MIprec=-1000;
double i2,j2;
double Tij;
double IW;
//int cr,ct;
//double er,et;
vpColVector dpinv(nbParam);
double dx,dy;
double alpha=2.;
int i,j;
unsigned int iteration=0;
do
{
Nbpoint=0;
//MIprec=MI;
MI=0;
erreur=0;
zeroProbabilities();
/////////////////////////////////////////////////////////////////////////
// Inverse
Warp->computeCoeff(p);
for(point=0;point<(int)templateSize;point++)
{
i=ptTemplate[point].y;
j=ptTemplate[point].x;
X1[0]=j;X1[1]=i;
Warp->computeDenom(X1,p);
Warp->warpX(X1,X2,p);
j2=X2[0];i2=X2[1];
if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
{
Nbpoint++;
Tij=ptTemplate[point].val;
if(!blur)
IW=I.getValue(i2,j2);
else
IW=BI.getValue(i2,j2);
//ct=ptTemplateSupp[point].ct;
//et=ptTemplateSupp[point].et;
//cr=(int)((IW*(Nc-1))/255.);
//er=((double)IW*(Nc-1))/255.-cr;
// if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE) // cas à tester AY
// vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
// else
// vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
}
}
if(Nbpoint==0)
{
//std::cout<<"plus de point dans template suivi"<<std::endl;
diverge=true;
MI=0;
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
else
{
computeProba(Nbpoint);
computeMI(MI);
if(hessianComputation!= vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
computeHessien(HInverse);
computeGradient();
GInverse=G;
/////////////////////////////////////////////////////////////////////////
// DIRECT
Nbpoint=0;
//MIprec=MI;
MI=0;
erreur=0;
zeroProbabilities();
Warp->computeCoeff(p);
#ifdef VISP_HAVE_OPENMP
int nthreads = omp_get_num_procs() ;
//std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
omp_set_num_threads(nthreads);
#pragma omp parallel for private(point, Tij,IW,i,j,i2,j2,/*cr,ct,er,et,*/dx,dy) default(shared)
#endif
for(point=0;point<(int)templateSize;point++)
{
i=ptTemplate[point].y;
j=ptTemplate[point].x;
X1[0]=j;X1[1]=i;
Warp->computeDenom(X1,p);
Warp->warpX(i,j,i2,j2,p);
X2[0]=j2;X2[1]=i2;
//Warp->computeDenom(X1,p);
if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
{
Nbpoint++;
Tij=ptTemplate[point].val;
//Tij=Iterateurvecteur->val;
if(!blur)
IW=I.getValue(i2,j2);
else
IW=BI.getValue(i2,j2);
dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
//ct=(int)((IW*(Nc-1))/255.);
//et=((double)IW*(Nc-1))/255.-ct;
//cr=ptTemplateSupp[point].ct;
//er=ptTemplateSupp[point].et;
Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);
double *tptemp=new double[nbParam];
for(unsigned int it=0;it<nbParam;it++)
tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;
//calcul de l'erreur
erreur+=(Tij-IW)*(Tij-IW);
// if(bspline==3)
// vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam);
// else
// vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam);
// vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam,bspline,ApproxHessian,
// hessianComputation==vpHessienType::USE_HESSIEN_DESIRE);
delete[] tptemp;
}
}
computeProba(Nbpoint);
computeMI(MI);
if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
computeHessien(HDirect);
computeGradient();
GDirect=G;
if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
{
H=HDirect+HInverse;
vpMatrix::computeHLM(H,lambda,HLM);
}
G=GDirect-GInverse;
//G=GDirect;
//G=-GInverse;
try
{
if(minimizationMethod==vpTemplateTrackerMIESM::USE_GRADIENT)
dp=-gain*0.3*G;
else
{
switch(hessianComputation)
{
case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
dp=gain*HLMdesireInverse*G;
break;
case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
if(HLM.cond()>HLMdesire.cond())
dp=gain*HLMdesireInverse*G;
else
dp=gain*0.3*HLM.inverseByLU()*G;
break;
default:
dp=gain*0.3*HLM.inverseByLU()*G;
break;
}
}
}
catch(vpException &e)
{
//std::cerr<<"probleme inversion"<<std::endl;
throw(e);
}
if(ApproxHessian==HESSIAN_NONSECOND)
dp=-1.*dp;
if(useBrent)
{
alpha=2.;
computeOptimalBrentGain(I,p,-MI,dp,alpha);
dp=alpha*dp;
}
p+=dp;
iteration++;
}
}
while( /*(MI!=MIprec) &&*/(iteration< iterationMax));
MI_postEstimation=-getCost(I,p);
if(MI_preEstimation>MI_postEstimation)
{
MI_postEstimation = -1;
}
nbIteration=iteration;
}
/*!
Perform the tracking of a dot by connex components.
\param mean_value : Threshold to use for the next call to track()
and corresponding to the mean value of the dot intensity.
\warning The content of the image is modified thanks to
setGrayLevelOut() called before. This method choose a gray level
(default is 0) used to modify the "in" dot level in "out" dot
level. This gray level is here needed to stop the recursivity . The
pixels of the dot are set to this new gray level "\out\".
\return vpDot::out if an error occurs, vpDot::in otherwise.
\sa setGrayLevelOut()
*/
int
vpDot::connexe(vpImage<unsigned char>& I, int u, int v,
unsigned int gray_level_min, unsigned int gray_level_max,
double &mean_value, double &u_cog, double &v_cog, double &n)
{
int width = I.getWidth();
int height= I.getHeight();
// Test if we are in the image
if ( (u < 0) || (v < 0) || (u >= width) || (v >= height) ) {
return vpDot::out ;
}
if (I[v][u] >= gray_level_min && I[v][u] <= gray_level_max)
{
vpImagePoint ip;
ip.set_u(u);
ip.set_v(v);
if (graphics==true)
{
// printf("u %d v %d\n", u, v);
vpDisplay::displayPoint(I, ip, vpColor::green) ;
//vpDisplay::flush(I);
}
ip_edges_list += ip;
u_cog += u ;
v_cog += v ;
n+=1 ;
if (n > nbMaxPoint) {
vpERROR_TRACE("Too many point %lf (%lf%% of image size). "
"This threshold can be modified using the setMaxDotSize() "
"method.",
n, n / (I.getWidth() * I.getHeight()),
nbMaxPoint, maxDotSizePercentage) ;
throw(vpTrackingException(vpTrackingException::featureLostError,
"Dot to big")) ;
}
// Bounding box update
if (u < this->u_min) this->u_min = u;
if (u > this->u_max) this->u_max = u;
if (v < this->v_min) this->v_min = v;
if (v > this->v_max) this->v_max = v;
// if (graphics==true)
// {
// vpRect r(this->u_min, this->v_min,
// this->u_max - this->u_min + 1,
// this->v_max - this->v_min + 1);
// vpDisplay::displayRectangle(I, r, vpColor::white) ;
// }
// Mean value of the dot intensities
mean_value = (mean_value *(n-1) + I[v][u]) / n;
if (compute_moment==true)
{
m00++ ;
m10 += u ;
m01 += v ;
m11 += (u*v) ;
m20 += u*u ;
m02 += v*v ;
}
I[v][u] = this->gray_level_out ;
}
else
{
return vpDot::out ;
}
if ( u-1 >= 0)
{
if (I[v][u-1] >= gray_level_min && I[v][u-1] <= gray_level_max)
connexe(I,u-1,v, gray_level_min, gray_level_max, mean_value,
u_cog,v_cog, n) ;
}
if (u+1 < width)
{
if (I[v][u+1] >= gray_level_min && I[v][u+1] <= gray_level_max)
connexe(I,u+1,v,gray_level_min, gray_level_max, mean_value,
u_cog, v_cog, n) ;
}
if (v-1 >= 0)
{
if (I[v-1][u] >=gray_level_min && I[v-1][u] <= gray_level_max)
connexe(I,u, v-1,gray_level_min, gray_level_max, mean_value,
u_cog, v_cog, n) ;
}
if (v+1 < height)
{
if (I[v+1][u] >=gray_level_min && I[v+1][u] <= gray_level_max)
connexe(I,u,v+1,gray_level_min, gray_level_max, mean_value,
u_cog, v_cog, n) ;
}
if (connexity == CONNEXITY_8) {
if ( (u-1 >= 0) && (v-1 >= 0) )
{
if (I[v-1][u-1] >=gray_level_min && I[v-1][u-1] <= gray_level_max)
connexe(I,u-1,v-1,gray_level_min, gray_level_max, mean_value,
u_cog, v_cog, n) ;
}
if ( (u+1 < width) && (v-1 >= 0 ) )
{
if (I[v-1][u+1] >=gray_level_min && I[v-1][u+1] <= gray_level_max)
connexe(I,u+1,v-1,gray_level_min, gray_level_max, mean_value,
u_cog, v_cog, n) ;
}
if ( (v+1 < height) && (u-1 >= 0) )
{
if (I[v+1][u-1] >=gray_level_min && I[v+1][u-1] <= gray_level_max)
connexe(I,u-1,v+1,gray_level_min, gray_level_max, mean_value,
u_cog, v_cog, n) ;
}
if ( (v+1 < height) && (u+1 < width) )
{
if (I[v+1][u+1] >=gray_level_min && I[v+1][u+1] <= gray_level_max)
connexe(I,u+1,v+1,gray_level_min, gray_level_max, mean_value,
u_cog, v_cog, n) ;
}
}
return vpDot::in ;
}
/*!
Virtual function that is called by lower classes vpMeEllipse, vpMeLine
and vpMeNurbs.
\exception vpTrackingException::initializationError : Moving edges not initialized.
*/
void
vpMeTracker::initTracking(const vpImage<unsigned char>& I)
{
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
throw(vpTrackingException(vpTrackingException::initializationError,
"Moving edges not initialized")) ;
}
// Must set range to 0
unsigned int range_tmp = me->getRange();
me->setRange(init_range);
nGoodElement=0;
int d = 0;
vpImagePoint ip1, ip2;
// Loop through list of sites to track
for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
vpMeSite refp = *it;//current reference pixel
d++ ;
// If element hasn't been suppressed
if(refp.getState() == vpMeSite::NO_SUPPRESSION)
{
try {
refp.track(I,me,false);
}
catch(...)
{
// EM verifier quel signal est de sortie !!!
vpERROR_TRACE("Error caught") ;
throw ;
}
if(refp.getState() == vpMeSite::NO_SUPPRESSION) nGoodElement++;
}
#if (DEBUG_LEVEL2)
{
double a,b ;
a = refp.i_1 - refp.i ;
b = refp.j_1 - refp.j ;
if(refp.getState() == vpMeSite::NO_SUPPRESSION) {
ip1.set_i( refp.i );
ip1.set_j( refp.j );
ip2.set_i( refp.i+a );
ip2.set_j( refp.j+b );
vpDisplay::displayArrow(I, ip1, ip2, vpColor::green) ;
}
}
#endif
*it = refp;
}
/*
if (res != OK)
{
std::cout<< "In vpMeTracker::initTracking(): " ;
switch (res)
{
case ERR_TRACKING:
std::cout << "vpMeTracker::initTracking:Track return ERR_TRACKING " << std::endl ;
break ;
case fatalError:
std::cout << "vpMeTracker::initTracking:Track return fatalError" << std::endl ;
break ;
default:
std::cout << "vpMeTracker::initTracking:Track return error " << res << std::endl ;
}
return res ;
}
*/
me->setRange(range_tmp);
}
void vpTemplateTrackerSSDESM::trackNoPyr(const vpImage<unsigned char> &I)
{
double erreur=0;
unsigned int Nbpoint=0;
if(blur)
vpImageFilter::filter(I, BI,fgG,taillef);
vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
double IW,dIWx,dIWy;
double Tij;
unsigned int iteration=0;
int i,j;
double i2,j2;
double alpha=2.;
do
{
Nbpoint=0;
erreur=0;
dp=0;
HDir=0;
GDir=0;
GInv=0;
Warp->computeCoeff(p);
for(unsigned int point=0;point<templateSize;point++)
{
i=ptTemplate[point].y;
j=ptTemplate[point].x;
X1[0]=j;X1[1]=i;
Warp->computeDenom(X1,p);
Warp->warpX(X1,X2,p);
j2=X2[0];i2=X2[1];
if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
{
//INVERSE
Tij=ptTemplate[point].val;
if(!blur)
IW=I.getValue(i2,j2);
else
IW=BI.getValue(i2,j2);
Nbpoint++;
double er=(Tij-IW);
for(unsigned int it=0;it<nbParam;it++)
GInv[it]+=er*ptTemplate[point].dW[it];
erreur+=er*er;
//DIRECT
//dIWx=dIx.getValue(i2,j2);
//dIWy=dIy.getValue(i2,j2);
dIWx=dIx.getValue(i2,j2)+ptTemplate[point].dx;
dIWy=dIy.getValue(i2,j2)+ptTemplate[point].dy;
//Calcul du Hessien
//Warp->dWarp(X1,X2,p,dW);
Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);
double *tempt=new double[nbParam];
for(unsigned int it=0;it<nbParam;it++)
tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;
for(unsigned int it=0;it<nbParam;it++)
for(unsigned int jt=0;jt<nbParam;jt++)
HDir[it][jt]+=tempt[it]*tempt[jt];
for(unsigned int it=0;it<nbParam;it++)
GDir[it]+=er*tempt[it];
delete[] tempt;
}
}
if(Nbpoint==0) {
//std::cout<<"plus de point dans template suivi"<<std::endl;
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
vpMatrix::computeHLM(HDir,lambdaDep,HLMDir);
try
{
//dp=(HLMInv+HLMDir).inverseByLU()*(GInv+GDir);
//dp=HLMInv.inverseByLU()*GInv+HLMDir.inverseByLU()*GDir;
//dp=HLMInv.inverseByLU()*GInv;
dp=(HLMDir).inverseByLU()*(GDir);
}
catch(vpException &e)
{
//std::cout<<"probleme inversion"<<std::endl;
throw(e);
}
dp=gain*dp;
if(useBrent)
{
alpha=2.;
computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
dp=alpha*dp;
}
//Warp->pRondp(p,dp,p);
p+=dp;
iteration++;
}
while( (iteration < iterationMax));
nbIteration=iteration;
}
/*!
Compute the center of gravity (COG) of the dot using connex
components. We assume the origin pixel (u, v) is in the dot. If
not, the dot is seach around this origin using a spiral search.
\param I : Image to process.
\param u : Starting pixel coordinate along the columns from where the
dot is searched .
\param v : Starting pixel coordinate along the rows from where the
dot is searched .
\warning The content of the image is modified.
\exception vpTrackingException::featureLostError : If the tracking fails.
\sa connexe()
*/
void
vpDot::COG(const vpImage<unsigned char> &I, double& u, double& v)
{
// Set the maximal number of points considering the maximal dot size
// image percentage
nbMaxPoint = (I.getWidth() * I.getHeight()) * maxDotSizePercentage;
// segmentation de l'image apres seuillage
// (etiquetage des composante connexe)
if (compute_moment)
m00 = m11 = m02 = m20 = m10 = m01 = mu11 = mu20 = mu02 = 0;
double u_cog = 0 ;
double v_cog = 0 ;
double npoint = 0 ;
this->mean_gray_level = 0 ;
ip_connexities_list.clear() ;
ip_edges_list.clear();
// Initialise the boundig box
this->u_min = I.getWidth();
this->u_max = 0;
this->v_min = I.getHeight();
this->v_max = 0;
#if 0
// Original version
if ( connexe(I, (unsigned int)u, (unsigned int)v,
gray_level_min, gray_level_max,
mean_gray_level, u_cog, v_cog, npoint) == vpDot::out)
{
bool sol = false ;
unsigned int pas ;
for (pas = 2 ; pas <= 25 ; pas ++ )if (sol==false)
{
for (int k=-1 ; k <=1 ; k++) if (sol==false)
for (int l=-1 ; l <=1 ; l++) if (sol==false)
{
u_cog = 0 ;
v_cog = 0 ;
ip_connexities_list.clear() ;
this->mean_gray_level = 0 ;
if (connexe(I, (unsigned int)(u+k*pas),(unsigned int)(v+l*pas),
gray_level_min, gray_level_max,
mean_gray_level, u_cog, v_cog, npoint) != vpDot::out)
{
sol = true ; u += k*pas ; v += l*pas ;
}
}
}
if (sol == false)
{
vpERROR_TRACE("Dot has been lost") ;
throw(vpTrackingException(vpTrackingException::featureLostError,
"Dot has been lost")) ;
}
}
#else
// If the dot is not found, search around using a spiral
if ( !connexe(I,(unsigned int)u,(unsigned int)v, mean_gray_level, u_cog, v_cog, npoint) )
{
bool sol = false ;
unsigned int right = 1;
unsigned int botom = 1;
unsigned int left = 2;
unsigned int up = 2;
double u_ = u, v_ = v;
unsigned int k;
// Spiral search from the center to find the nearest dot
while( (right < SPIRAL_SEARCH_SIZE) && (sol == false) ) {
for (k=1; k <= right; k++) if(sol==false) {
u_cog = 0 ;
v_cog = 0 ;
ip_connexities_list.clear() ;
ip_edges_list.clear();
this->mean_gray_level = 0 ;
if ( connexe(I, (unsigned int)u_+k, (unsigned int)(v_),mean_gray_level, u_cog, v_cog, npoint) ) {
sol = true; u = u_+k; v = v_;
}
}
u_ += k;
right += 2;
for (k=1; k <= botom; k++) if (sol==false) {
u_cog = 0 ;
v_cog = 0 ;
ip_connexities_list.clear() ;
ip_edges_list.clear();
this->mean_gray_level = 0 ;
if ( connexe(I, (unsigned int)(u_), (unsigned int)(v_+k),mean_gray_level, u_cog, v_cog, npoint) ) {
sol = true; u = u_; v = v_+k;
}
}
v_ += k;
botom += 2;
for (k=1; k <= left; k++) if (sol==false) {
u_cog = 0 ;
v_cog = 0 ;
ip_connexities_list.clear() ;
ip_edges_list.clear();
this->mean_gray_level = 0 ;
if ( connexe(I, (unsigned int)(u_-k), (unsigned int)(v_),mean_gray_level,u_cog, v_cog, npoint) ) {
sol = true ; u = u_-k; v = v_;
}
}
u_ -= k;
left += 2;
for (k=1; k <= up; k++) if(sol==false) {
u_cog = 0 ;
v_cog = 0 ;
ip_connexities_list.clear() ;
ip_edges_list.clear();
this->mean_gray_level = 0 ;
if ( connexe(I, (unsigned int)(u_), (unsigned int)(v_-k),mean_gray_level,u_cog, v_cog, npoint) ) {
sol = true ; u = u_; v = v_-k;
}
}
v_ -= k;
up += 2;
}
if (sol == false) {
vpERROR_TRACE("Dot has been lost") ;
throw(vpTrackingException(vpTrackingException::featureLostError,
"Dot has been lost")) ;
}
}
#endif
/*
vpImagePoint ip;
unsigned int i, j;
std::list<vpImagePoint>::iterator it;
for (it = ip_connexities_list.begin(); it != ip_connexities_list.end(); it ++) {
ip = *it;
i = (unsigned int) ip.get_i();
j = (unsigned int) ip.get_j();
I[i][j] = 255 ;
}*/
u_cog = u_cog/npoint ;
v_cog = v_cog/npoint ;
u = u_cog ;
v = v_cog ;
// Initialize the threshold for the next call to track()
double Ip = pow((double)this->mean_gray_level/255,1/gamma);
if(Ip - (1 - grayLevelPrecision)<0){
gray_level_min = 0 ;
}
else{
gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
if (gray_level_min > 255)
gray_level_min = 255;
}
gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
if (gray_level_max > 255)
gray_level_max = 255;
//vpCTRACE << "gray_level_min: " << gray_level_min << std::endl;
//vpCTRACE << "gray_level_max: " << gray_level_max << std::endl;
if (npoint < 5)
{
vpERROR_TRACE("Dot to small") ;
throw(vpTrackingException(vpTrackingException::featureLostError,
"Dot to small")) ;
}
if (npoint > nbMaxPoint)
{
vpERROR_TRACE("Too many point %lf (%lf%%). Max allowed is %lf (%lf%%). This threshold can be modified using the setMaxDotSize() method.",
npoint, npoint / (I.getWidth() * I.getHeight()),
nbMaxPoint, maxDotSizePercentage) ;
throw(vpTrackingException(vpTrackingException::featureLostError,
"Dot to big")) ;
}
}
bool vpDot::connexe(const vpImage<unsigned char>& I,unsigned int u,unsigned int v,
double &mean_value, double &u_cog, double &v_cog, double &n,std::vector<bool> &checkTab)
{
unsigned int width = I.getWidth();
unsigned int height= I.getHeight();
// Test if we are in the image
if ( (u >= width) || (v >= height) )
{
//std::cout << "out of bound" << std::endl;
return false;
}
if(checkTab[u + v*I.getWidth()])
return true;
vpImagePoint ip;
ip.set_u(u);
ip.set_v(v);
if (I[v][u] >= gray_level_min && I[v][u] <= gray_level_max)
{
checkTab[v*I.getWidth() + u] = true;
ip_connexities_list.push_back(ip);
u_cog += u ;
v_cog += v ;
n+=1 ;
if (n > nbMaxPoint) {
vpERROR_TRACE("Too many point %lf (%lf%% of image size). "
"This threshold can be modified using the setMaxDotSize() "
"method.",
n, n / (I.getWidth() * I.getHeight()),
nbMaxPoint, maxDotSizePercentage) ;
throw(vpTrackingException(vpTrackingException::featureLostError,
"Dot to big")) ;
}
// Bounding box update
if (u < this->u_min) this->u_min = u;
if (u > this->u_max) this->u_max = u;
if (v < this->v_min) this->v_min = v;
if (v > this->v_max) this->v_max = v;
// Mean value of the dot intensities
mean_value = (mean_value *(n-1) + I[v][u]) / n;
if (compute_moment==true)
{
m00++ ;
m10 += u ;
m01 += v ;
m11 += (u*v) ;
m20 += u*u ;
m02 += v*v ;
}
}
else
{
//std::cout << "not in" << std::endl;
return false;
}
bool edge = false;
//if((int)u-1 >= 0)
if(u >= 1)
if(!checkTab[u-1 + v*I.getWidth()])
if(!connexe(I,u-1,v, mean_value,u_cog,v_cog, n, checkTab))
edge = true;
if(u+1 < I.getWidth())
if(!checkTab[u+1+v*I.getWidth()])
if(!connexe(I,u+1,v,mean_value,u_cog, v_cog, n, checkTab))
edge = true;
if(v >= 1)
if(!checkTab[u+(v-1)*I.getWidth()])
if(!connexe(I,u, v-1,mean_value,u_cog, v_cog, n, checkTab))
edge = true;
if(v+1 < I.getHeight())
if(!checkTab[u+(v+1)*I.getWidth()])
if(!connexe(I,u,v+1,mean_value,u_cog, v_cog, n, checkTab))
edge = true;
if (connexityType == CONNEXITY_8) {
if(v >= 1 && u >= 1)
if(!checkTab[u-1+(v-1)*I.getWidth()])
if(!connexe(I,u-1,v-1,mean_value,u_cog, v_cog, n, checkTab))
edge = true;
if(v >= 1 && u+1 < I.getWidth())
if(!checkTab[u+1+(v-1)*I.getWidth()])
if(!connexe(I,u+1,v-1,mean_value,u_cog, v_cog, n, checkTab))
edge = true;
if(v+1 < I.getHeight() && u >= 1)
if(!checkTab[u-1+(v+1)*I.getWidth()])
if(!connexe(I,u-1,v+1,mean_value, u_cog, v_cog, n, checkTab))
edge = true;
if(v+1 < I.getHeight() && u+1 < I.getWidth())
if(!checkTab[u+1+(v+1)*I.getWidth()])
if(!connexe(I,u+1,v+1,mean_value,u_cog, v_cog, n, checkTab))
edge = true;
}
if(edge){
ip_edges_list.push_back(ip);
if (graphics==true)
{
vpImagePoint ip_(ip);
for(unsigned int t=0; t<thickness; t++) {
ip_.set_u(ip.get_u() + t);
vpDisplay::displayPoint(I, ip_, vpColor::red) ;
}
//vpDisplay::flush(I);
}
}
return true;
}
void vpTemplateTracker::trackPyr(const vpImage<unsigned char> &I)
{
//vpTRACE("trackPyr");
vpImage<unsigned char> *pyr_I;
// pyr_I=new vpImage<unsigned char>[nbLvlPyr+1]; // Why +1 ?
pyr_I=new vpImage<unsigned char>[nbLvlPyr]; // Why +1 ?
pyr_I[0]=I;
try
{
vpColVector ptemp(nbParam);
if(nbLvlPyr>1)
{
// vpColVector *p_sauv=new vpColVector[nbLvlPyr];
// for(unsigned int i=0;i<nbLvlPyr;i++)p_sauv[i].resize(nbParam);
// p_sauv[0]=p;
for(unsigned int i=1;i<nbLvlPyr;i++)
{
vpImageFilter::getGaussPyramidal(pyr_I[i-1],pyr_I[i]);
//test getParamPyramidDown
/*vpColVector vX_test(2);vX_test[0]=15.;vX_test[1]=30.;
vpColVector vX_test2(2);
Warp->computeCoeff(p);
Warp->computeDenom(vX_test,p);
Warp->warpX(vX_test,vX_test2,p);
std::cout<<"p = "<<p.t()<<std::endl;*/
//std::cout<<"get p down"<<std::endl;
Warp->getParamPyramidDown(p,ptemp);
p=ptemp;
zoneTracked=&zoneTrackedPyr[i];
// p_sauv[i]=p;
/*std::cout<<"p_down = "<<p.t()<<std::endl;
vpColVector vX_testd(2);vX_testd[0]=15./2.;vX_testd[1]=30./2.;
vpColVector vX_testd2(2);
Warp->computeCoeff(p);
Warp->computeDenom(vX_testd,p);
Warp->warpX(vX_testd,vX_testd2,p);
std::cout<<2.*vX_testd2[0]<<","<<2.*vX_testd2[1]<<" <=> "<<vX_test2[0]<<","<<vX_test2[1]<<std::endl;*/
}
for(int i=(int)nbLvlPyr-1;i>=0;i--)
{
if(i>=(int)l0Pyr)
{
templateSize=templateSizePyr[i];
ptTemplate=ptTemplatePyr[i];
ptTemplateSelect=ptTemplateSelectPyr[i];
ptTemplateSupp=ptTemplateSuppPyr[i];
ptTemplateCompo=ptTemplateCompoPyr[i];
H=HdesirePyr[i];
HLM=HLMdesirePyr[i];
HLMdesireInverse=HLMdesireInversePyr[i];
// zoneTracked=&zoneTrackedPyr[i];
trackRobust(pyr_I[i]);
}
//std::cout<<"get p up"<<std::endl;
// ptemp=p_sauv[i-1];
if (i > 0) {
Warp->getParamPyramidUp(p,ptemp);
p=ptemp;
zoneTracked=&zoneTrackedPyr[i-1];
}
}
#if 0
if(l0Pyr==0)
{
templateSize=templateSizePyr[0];
ptTemplate=ptTemplatePyr[0];
ptTemplateSelect=ptTemplateSelectPyr[0];
ptTemplateSupp=ptTemplateSuppPyr[0];
ptTemplateCompo=ptTemplateCompoPyr[0];
H=HdesirePyr[0];
HLM=HLMdesirePyr[0];
HLMdesireInverse=HLMdesireInversePyr[0];
zoneTracked=&zoneTrackedPyr[0];
trackRobust(pyr_I[0]);
}
if (l0Pyr > 0) {
// for (int l=(int)l0Pyr; l >=0; l--) {
// Warp->getParamPyramidUp(p,ptemp);
// p=ptemp;
// }
zoneTracked=&zoneTrackedPyr[0];
}
#endif
// delete [] p_sauv;
}
else
{
//std::cout<<"reviens a tracker de base"<<std::endl;
trackRobust(I);
}
delete[] pyr_I;
}
catch(vpException &e){
delete[] pyr_I;
throw(vpTrackingException(vpTrackingException::badValue, e.getMessage()));
}
}
void vpTemplateTrackerMIForwardAdditional::trackNoPyr(const vpImage<unsigned char> &I)
{
dW=0;
double erreur=0;
int Nbpoint=0;
if(blur)
vpImageFilter::filter(I, BI,fgG,taillef);
vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
double MI=0,MIprec=-1000;
MI_preEstimation=-getCost(I,p);
double i2,j2;
double Tij;
double IW,dx,dy;
//unsigned
int cr,ct;
double er,et;
double alpha=2.;
int i,j;
unsigned int iteration=0;
initPosEvalRMS(p);
do
{
if(iteration%5==0)
initHessienDesired(I);
Nbpoint=0;
MIprec=MI;
MI=0;
erreur=0;
zeroProbabilities();
Warp->computeCoeff(p);
#ifdef VISP_HAVE_OPENMP
int nthreads = omp_get_num_procs() ;
//std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
omp_set_num_threads(nthreads);
#pragma omp parallel for private(Tij,IW,i,j,i2,j2,cr,ct,er,et,dx,dy) default(shared)
#endif
for(int point=0;point<(int)templateSize;point++)
{
i=ptTemplate[point].y;
j=ptTemplate[point].x;
X1[0]=j;X1[1]=i;
Warp->computeDenom(X1,p);
Warp->warpX(X1,X2,p);
j2=X2[0];i2=X2[1];
if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
{
Nbpoint++;
Tij=ptTemplate[point].val;
//Tij=Iterateurvecteur->val;
if(!blur)
IW=I.getValue(i2,j2);
else
IW=BI.getValue(i2,j2);
dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
ct=(int)((IW*(Nc-1))/255.);
cr=(int)((Tij*(Nc-1))/255.);
et=(IW*(Nc-1))/255.-ct;
er=((double)Tij*(Nc-1))/255.-cr;
//calcul de l'erreur
erreur+=(Tij-IW)*(Tij-IW);
//Calcul de l'histogramme joint par interpolation bilinÃaire (Bspline ordre 1)
Warp->dWarp(X1,X2,p,dW);
//double *tptemp=temp;
double *tptemp=new double[nbParam];;
for(unsigned int it=0;it<nbParam;it++)
tptemp[it] =(dW[0][it]*dx+dW[1][it]*dy);
//*tptemp++ =dW[0][it]*dIWx+dW[1][it]*dIWy;
//std::cout<<cr<<" "<<ct<<" ; ";
if(ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
else if(ApproxHessian==HESSIAN_0 || ApproxHessian==HESSIAN_NEW)
vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
delete[] tptemp;
}
}
if(Nbpoint==0)
{
//std::cout<<"plus de point dans template suivi"<<std::endl;
diverge=true;
MI=0;
deletePosEvalRMS();
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
else
{
computeProba(Nbpoint);
computeMI(MI);
//std::cout<<iteration<<"\tMI= "<<MI<<std::endl;
computeHessien(H);
computeGradient();
vpMatrix::computeHLM(H,lambda,HLM);
try
{
switch(hessianComputation)
{
case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
dp=gain*HLMdesireInverse*G;
break;
case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
if(HLM.cond()>HLMdesire.cond())
dp=gain*HLMdesireInverse*G;
else
dp=gain*0.2*HLM.inverseByLU()*G;
break;
default:
dp=gain*0.2*HLM.inverseByLU()*G;
break;
}
}
catch(vpException &e)
{
//std::cerr<<"probleme inversion"<<std::endl;
deletePosEvalRMS();
throw(e);
}
}
switch(minimizationMethod)
{
case vpTemplateTrackerMIForwardAdditional::USE_LMA:
{
vpColVector p_test_LMA(nbParam);
if(ApproxHessian==HESSIAN_NONSECOND)
p_test_LMA=p-100000.1*dp;
else
p_test_LMA=p+1.*dp;
MI=-getCost(I,p);
double MI_LMA=-getCost(I,p_test_LMA);
if(MI_LMA>MI)
{
p=p_test_LMA;
lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
}
else
{
lambda=(lambda*10.<1e6)?1e6:lambda*10.;
}
}
break;
case vpTemplateTrackerMIForwardAdditional::USE_GRADIENT:
{
dp=-gain*6.0*G;
if(useBrent)
{
alpha=2.;
computeOptimalBrentGain(I,p,-MI,dp,alpha);
dp=alpha*dp;
}
p+=1.*dp;
break;
}
case vpTemplateTrackerMIForwardAdditional::USE_QUASINEWTON:
{
double s_scal_y;
if(iterationGlobale!=0)
{
vpColVector s_quasi=p-p_prec;
vpColVector y_quasi=G-G_prec;
s_scal_y=s_quasi.t()*y_quasi;
//if(s_scal_y!=0)//BFGS
// KQuasiNewton=KQuasiNewton-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y;
//if(s_scal_y!=0)//DFP
if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())
KQuasiNewton=KQuasiNewton+0.001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
}
dp=-KQuasiNewton*G;
p_prec=p;
G_prec=G;
p-=1.01*dp;
}
break;
default:
{
if(ApproxHessian==HESSIAN_NONSECOND)
dp=-0.1*dp;
if(useBrent)
{
alpha=2.;
computeOptimalBrentGain(I,p,-MI,dp,alpha);
//std::cout<<alpha<<std::endl;
dp=alpha*dp;
}
p+=1.*dp;
break;
}
}
computeEvalRMS(p);
iteration++;
iterationGlobale++;
}
while( (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
//while( (MI!=MIprec) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
if(Nbpoint==0) {
//std::cout<<"plus de point dans template suivi"<<std::endl;
deletePosEvalRMS();
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
nbIteration=iteration;
MI_postEstimation=-getCost(I,p);
if(MI_preEstimation>MI_postEstimation)
{
MI_postEstimation = -1;
}
deletePosEvalRMS();
}
void vpTemplateTrackerMIInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
{
if(!CompoInitialised)
std::cout<<"Compositionnal tracking no initialised\nUse InitCompInverse(vpImage<unsigned char> &I) function"<<std::endl;
dW=0;
if(blur)
vpImageFilter::filter(I, BI,fgG,taillef);
int Nbpoint=0;
double lambda=lambdaDep;
double MI=0,MIprec=-1000;
vpColVector p_avant_estimation;p_avant_estimation=p;
MI_preEstimation=-getCost(I,p);
NMI_preEstimation=-getNormalizedCost(I,p);
// std::cout << "MI avant: " << MI_preEstimation << std::endl;
// std::cout << "NMI avant: " << NMI_preEstimation << std::endl;
initPosEvalRMS(p);
vpColVector dpinv(nbParam);
double alpha=2.;
unsigned int iteration=0;
//unsigned int bspline_ = (unsigned int) bspline;
//unsigned int totParam = (bspline_ * bspline_)*(1+nbParam+nbParam*nbParam);
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
int Ncb_ = (int)Ncb;
int nbParam_ = (int)nbParam;
int sizePrtBis = Ncb_*Ncb_;
int sizedPrtBis = Ncb_*Ncb_*nbParam_;
int sized2PrtBis = Ncb_*Ncb_*nbParam_*nbParam_;
#endif
vpMatrix Hnorm(nbParam,nbParam);
do
{
Nbpoint=0;
MIprec=MI;
MI=0;
#ifndef USE_OPENMP_MI_INVCOMP
zeroProbabilities();
#endif
Warp->computeCoeff(p);
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
#pragma omp parallel
#endif
{
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
double *Prtbis = new double[sizePrtBis];
double *dPrtbis = new double[sizedPrtBis];
double *d2Prtbis = new double[sized2PrtBis];
unsigned int indd, indd2;
indd = indd2 = 0;
for(int i=0;i<Ncb_*Ncb_;i++){
Prtbis[i]=0.0;
Prt[i]=0.0;
for(int j=0;j<nbParam_;j++){
dPrtbis[indd]=0.0;
dPrt[indd]=0.0;
indd++;
for(int k=0;k<nbParam_;k++){
d2Prtbis[indd2]=0.0;
d2Prt[indd2]=0.0;
indd2++;
}
}
}
#pragma omp barrier
#pragma omp for reduction(+:Nbpoint)
#endif
for(int point=0;point<(int)templateSize;point++)
{
vpColVector x1(2),x2(2);
double i2,j2;
double IW;
int cr,ct;
double er,et;
x1[0]=(double)ptTemplate[point].x;
x1[1]=(double)ptTemplate[point].y;
Warp->computeDenom(x1,p); // A modif pour parallelisation mais ne pose pas de pb avec warp utilises dans DECSA
Warp->warpX(x1,x2,p);
j2=x2[0];
i2=x2[1];
if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
{
//if(m_ptCurrentMask == NULL ||(m_ptCurrentMask->getWidth() == I.getWidth() && m_ptCurrentMask->getHeight() == I.getHeight() && (*m_ptCurrentMask)[(unsigned int)i2][(unsigned int)j2] > 128))
{
Nbpoint++;
if(!blur)
IW=(double)I.getValue(i2,j2);
else
IW=BI.getValue(i2,j2);
ct=ptTemplateSupp[point].ct;
et=ptTemplateSupp[point].et;
double tmp = IW*(((double)Nc)-1.f)/255.f;
cr=(int)tmp;
er=tmp-(double)cr;
if( (ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE) && (ptTemplateSelect[point] || !useTemplateSelect) )
{
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prtbis, dPrtbis, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam, bspline);
#else
vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prt, dPrt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam, bspline);
#endif
}
else if (ptTemplateSelect[point] || !useTemplateSelect)
{
if(bspline==3){
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prtbis, dPrtbis, d2Prtbis, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
#else
vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
#endif
// {
// // ################### AY : Optim
// if(et>0.5){ct++;}
// if(er>0.5){cr++;}
// int index = (cr*Nc+ct)*totParam;
// double *ptb = &PrtTout[index];
// #endif
// vpTemplateTrackerMIBSpline::PutTotPVBspline3(ptb, er, ptTemplateSupp[point].BtInit, size);
// // ###################
// }
}
else{
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prtbis, dPrtbis, d2Prtbis, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
#else
vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
#endif
}
}
else{
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prtbis, cr, er, ct, et, Ncb ,nbParam, bspline);
#else
vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prt, cr, er, ct, et, Ncb,nbParam, bspline);
#endif
}
}
}
}
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
#pragma omp critical
{
unsigned int indd, indd2;
indd = indd2 = 0;
for(int i=0;i<Ncb*Ncb;i++){
Prt[i]+=Prtbis[i]/(double)Nbpoint;
for(int j=0;j<nbParam_;j++){
dPrt[indd]+=dPrtbis[indd]/(double)Nbpoint;
indd++;
for(int k=0;k<nbParam_;k++){
d2Prt[indd2]+=d2Prtbis[indd2]/(double)Nbpoint;
indd2++;
}
}
}
delete [] Prtbis;
delete [] dPrtbis;
delete [] d2Prtbis;
}
#pragma omp barrier
#endif
}
if(Nbpoint==0)
{
diverge=true;
MI=0;
deletePosEvalRMS();
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
else
{
// computeProba(Nbpoint);
#ifndef USE_OPENMP_MI_INVCOMP
unsigned int indd, indd2;
indd = indd2 = 0;
unsigned int Ncb_ = (unsigned int)Ncb;
for(unsigned int i=0;i<Ncb_*Ncb_;i++){
Prt[i]=Prt[i]/Nbpoint;
for(unsigned int j=0;j<nbParam;j++){
dPrt[indd]=dPrt[indd]/Nbpoint;
indd++;
for(unsigned int k=0;k<nbParam;k++){
d2Prt[indd2]=d2Prt[indd2]/Nbpoint;
indd2++;
}
}
}
#endif
computeMI(MI);
if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE){
computeHessienNormalized(Hnorm);
computeHessien(H);
}
computeGradient();
vpMatrix::computeHLM(H,lambda,HLM);
try
{
switch(hessianComputation)
{
case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
dp=gain*HLMdesireInverse*G;
break;
case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
if(HLM.cond()>HLMdesire.cond())
dp=gain*HLMdesireInverse*G;
else
dp=gain*0.2*HLM.inverseByLU()*G;
break;
default:
dp=gain*0.2*HLM.inverseByLU()*G;
break;
}
}
catch(vpException &e)
{
//std::cerr<<"probleme inversion"<<std::endl;
throw(e);
}
}
switch(minimizationMethod)
{
case vpTemplateTrackerMIInverseCompositional::USE_LMA:
{
vpColVector dp_test_LMA(nbParam);
vpColVector dpinv_test_LMA(nbParam);
vpColVector p_test_LMA(nbParam);
if(ApproxHessian==HESSIAN_NONSECOND)
dp_test_LMA=-100000.1*dp;
else
dp_test_LMA=1.*dp;
Warp->getParamInverse(dp_test_LMA,dpinv_test_LMA);
Warp->pRondp(p,dpinv_test_LMA,p_test_LMA);
MI=-getCost(I,p);
double MI_LMA=-getCost(I,p_test_LMA);
if(MI_LMA>MI)
{
dp=dp_test_LMA;
lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
}
else
{
dp=0;
lambda=(lambda*10.<1e6)?1e6:lambda*10.;
}
}
break;
case vpTemplateTrackerMIInverseCompositional::USE_GRADIENT:
dp=-gain*0.3*G*20;
break;
case vpTemplateTrackerMIInverseCompositional::USE_QUASINEWTON:
{
double s_scal_y;
if(iterationGlobale!=0)
{
vpColVector s_quasi=p-p_prec;
vpColVector y_quasi=G-G_prec;
s_scal_y=s_quasi.t()*y_quasi;
//std::cout<<"mise a jour K"<<std::endl;
/*if(s_scal_y!=0)//BFGS
KQuasiNewton=KQuasiNewton+0.01*(-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y);*/
//if(s_scal_y!=0)//DFP
if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())//DFP
{
KQuasiNewton=KQuasiNewton+0.0001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
//std::cout<<"mise a jour K"<<std::endl;
}
}
dp=gain*KQuasiNewton*G;
//std::cout<<KQuasiNewton<<std::endl<<std::endl;
p_prec=p;
G_prec=G;
//p-=1.01*dp;
}
break;
default:
{
if(useBrent)
{
alpha=2.;
computeOptimalBrentGain(I,p,-MI,dp,alpha);
dp=alpha*dp;
}
if(ApproxHessian==HESSIAN_NONSECOND)
dp=-1.*dp;
break;
}
}
Warp->getParamInverse(dp,dpinv);
Warp->pRondp(p,dpinv,p);
iteration++;
iterationGlobale++;
computeEvalRMS(p);
// std::cout << p.t() << std::endl;
}
while( (!diverge) && (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
//while( (!diverge) && (MI!=MIprec) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
nbIteration=iteration;
if(diverge)
{
if(computeCovariance){
covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
covarianceMatrix = -1;
MI_postEstimation = -1;
NMI_postEstimation = -1;
}
deletePosEvalRMS();
// throw(vpTrackingException(vpTrackingException::badValue, "Tracking failed")) ;
}
else
{
MI_postEstimation=-getCost(I,p);
NMI_postEstimation=-getNormalizedCost(I,p);
// std::cout << "MI apres: " << MI_postEstimation << std::endl;
// std::cout << "NMI apres: " << NMI_postEstimation << std::endl;
if(MI_preEstimation>MI_postEstimation)
{
p=p_avant_estimation;
MI_postEstimation = MI_preEstimation;
NMI_postEstimation = NMI_preEstimation;
covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
covarianceMatrix = -1;
}
deletePosEvalRMS();
if(computeCovariance){
try{
covarianceMatrix = (-H).inverseByLU();
// covarianceMatrix = (-Hnorm).inverseByLU();
}
catch(...){
covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
covarianceMatrix = -1;
MI_postEstimation = -1;
NMI_postEstimation = -1;
deletePosEvalRMS();
// throw(vpMatrixException(vpMatrixException::matrixError, "Covariance not inversible")) ;
}
}
}
}
