/**
Returns the distance from this segment to point qq.
*/
float Cline::d2point(Cpoint *qq)
{
float t1, iPx, iPy;
Cpoint iPoint;
Cpoint *ww = new Cpoint((qq->get_xx()-aa.get_xx()),(qq->get_yy()-aa.get_yy())); //ww is the two coordinate vector going from aa to qq
if (vv.scalar(ww)<=0)
{
delete ww;
iPoint = &aa; //aa is the closest point to qq
return (aa.d2point(qq));
}
ww->set_point((qq->get_xx()-bb.get_xx()),(qq->get_yy()-bb.get_yy())); //ww is the two coordinate vector going from bb to qq
if (vv.scalar(ww)>=0)
{
delete ww;
iPoint = &bb;//bb is the closest point to qq
return (bb.d2point(qq));
}
delete ww;
t1=(qq->get_xx()-aa.get_xx())*(bb.get_xx()-aa.get_xx())+(qq->get_yy()-aa.get_yy())*(bb.get_yy()-aa.get_yy());
t1=t1/(length*length);
//sets iP as normal projection
iPx=aa.get_xx()+t1*vv.get_xx();
iPy=aa.get_yy()+t1*vv.get_yy();
iPoint.set_point(iPx,iPy);
return (iPoint.d2point(qq));
}
/**
Returns the distance from this segment to point qq. Evaluate first if distance is from one of the limiting points (aa or bb). If not, evaluate distance from the segment. Sets to *iPoint the closest point of the segment to qq.
*/
int Cline::d2point(const Cpoint & qq, Cpoint & iPoint, float & dist) const
{
//float t1, iPx, iPy;
float dotProd;
//cout << endl << endl << "************* d2point *************" << endl;
//cout << "qq=" << qq << endl;
//ww is the two coordinate vector going from bb to qq
Cpoint ww((qq.get_xx()-bb.get_xx()),(qq.get_yy()-bb.get_yy()));
//check if BB is the closest point
dotProd = vv.scalar(ww);
if (dotProd >= 0)
{
iPoint=bb;//bb is the closest point to qq
dist = bb.d2point(qq);
return iBB;
}
//ww is the two coordinate vector going from aa to qq
ww.set_point((qq.get_xx()-aa.get_xx()),(qq.get_yy()-aa.get_yy()));
//check if AA is the closest point
dotProd = vv.scalar(ww);
if (dotProd <= 0)
{
iPoint = aa; //aa is the closest point to qq
dist = aa.d2point(qq);
return iAA;
}
//point between AA and BB
//compute projection of ww onto vv
dotProd = dotProd/(length*length);
iPoint.set_point(aa.get_xx()+dotProd*vv.get_xx(), aa.get_yy()+dotProd*vv.get_yy());
/*
cout << "iPoint=" << iPoint << endl;
Cline auxLine1(aa, iPoint);
Cline auxLine2(iPoint, bb);
cout << "SEGMENT!" << endl;
printLine();
cout << "\tdir1=" << getDirection() << endl;
cout << "auxLine1(aa, iPoint)=" << endl;
auxLine1.printLine();
cout << "\tdir1=" << auxLine1.getDirection() << endl;
cout << "auxLine2(iPoint, bb)=" << endl;
auxLine2.printLine();
cout << "\tdir2=" << auxLine2.getDirection() << endl;
*/
dist = iPoint.d2point(qq);
return iAB;
}
void CpeopleTracker::computeOcclusions()
{
std::list<CpersonTarget>::iterator iiT, jjT;
Cline iiLine;
Cpoint jjTpoint;
filterEstimate fEst;
bool occlusionFound;
double ddij;
//for each target iiT, compute potential occlusions caused by other targets jjT
for (iiT=targetList.begin();iiT!=targetList.end();iiT++)
{
//builds iiLine, from the robot to target ii
iiT->getEstimate(fEst);
iiLine.set_point_coord(0,0,fEst.position.getX(),fEst.position.getY());
//resets flag
occlusionFound = false;
//jjT targets will cause potential occlusions
for (jjT=targetList.begin();jjT!=targetList.end();jjT++)
{
if (iiT!=jjT)
{
jjT->getEstimate(fEst);
jjTpoint.set_point(fEst.position.getX(),fEst.position.getY());
ddij = iiLine.d2point(&jjTpoint);
if ( ddij < jjT->getPersonRadius()*1.2 )
{
occlusionFound = true;
break; //iterate to the next target iiT if an occlusion is found
}
}
}
//set status and probability of occlusion for target iiT (ToDo: improve step function with a smoother one)
if ( occlusionFound )
{
iiT->pOcclusion = 1; //0.9
iiT->setStatus(IN_OCCLUSION,true);
}
else
{
iiT->pOcclusion = 0;
iiT->setStatus(IN_OCCLUSION,false);
}
}
}
void CpeopleTracker::createFilters()
{
CpersonTarget *newTarget;
std::list<Cpoint3dObservation>::iterator iiD;
std::list<CpersonTarget>::iterator jjT;
std::list<CpersonTarget> newFilters;
double assocProb;
bool associated;
unsigned int ii;
Cline iiLine;
Cpoint jjTpoint;
filterEstimate fEst;
double ddij;
bool detectionInOcclusion;
//check for unassociated detections of LEG detector & create new filters for unassociated LEG detections
//createNewFilters();
for (iiD=laserDetSet.begin(),ii=0;iiD!=laserDetSet.end();iiD++,ii++)
{
associated = false;
for (jjT=targetList.begin();jjT!=targetList.end();jjT++)
{
assocProb = jjT->aProbs[LEGS].at(ii);
//std::cout << "Det: " << ii << "; Filter: " << jjT->getTargetId() << std::endl;
//std::cout << " assocP = " << assocProb << std::endl;
if ( assocProb > params.minAssociationProb ) //iiD detection has been associated, at least, to filter jjT
{
associated = true;
break;
}
}
if (!associated) //iiD has not been associated to any target/filter, so we launch a new filter
{
//first check if some target is causing occlusion of iiD detection
detectionInOcclusion = false;
iiLine.set_point_coord(0,0,iiD->point.getX(),iiD->point.getY());
for (jjT=targetList.begin();jjT!=targetList.end();jjT++)
{
jjT->getEstimate(fEst);
jjTpoint.set_point(fEst.position.getX(),fEst.position.getY());
ddij = iiLine.d2point(&jjTpoint);
if ( ddij < jjT->getPersonRadius()*1.2 )
{
detectionInOcclusion = true;
break; //iterate to the next target iiT if an occlusion is found
}
}
//if no occlusion, then initialize a new target by creating a new filter
if (!detectionInOcclusion)
{
newTarget = new CpersonTarget(nextTargetId);
newTarget->setParameters(filterParams);
nextTargetId++;
newTarget->init(*iiD);
newTarget->updateEstimate();
newFilters.push_back(*newTarget);
delete newTarget;
//std::cout << "createFilters(): new target with id: "<< newTarget->getTargetId() << std::endl;
}
}
}
targetList.splice(targetList.end(),newFilters);//adds new filters to the targetList maintained by this Tracker
newFilters.clear();
}
