/**
Returns the distance squared 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.
*/
float Cline::d2point2(Cpoint *qq, Cpoint *iPoint)
{
float t1, iPx, iPy;
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.d2point2(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; //aa is the closest point to qq
return (bb.d2point2(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->d2point2(qq));
}
/**
Sets aa and bb points of the segment.
*/
int Cline::set_line(const Cpoint &paa, const Cpoint &pbb)
{
aa.set_point(paa.get_xx(),paa.get_yy());
bb.set_point(pbb.get_xx(),pbb.get_yy());
length=aa.d2point(&bb);
set_vv();
return 0;
}
Cline::Cline(const Cpoint &paa, const Cpoint &pbb, const float &hght, const bool & inOut)
{
aa.set_point(paa.get_xx(),paa.get_yy());
bb.set_point(pbb.get_xx(),pbb.get_yy());
length=aa.d2point(&bb);
set_vv();
height=hght;
inOutDoor=inOut;
description[0]='\0';
}
/**
Segment Constructor with aa and bb coordinates
*/
Cline::Cline(const Cpoint &paa, const Cpoint &pbb)
{
aa.set_point(paa.get_xx(),paa.get_yy());
bb.set_point(pbb.get_xx(),pbb.get_yy());
length=aa.d2point(&bb);
set_vv();
height=0;
inOutDoor=0;
description[0]='\0';
}
/** Computes a point over the line that are at x distance of a to b*/
Cpoint Cline::chunk(float d)
{
Cpoint increment;
//adjust();
if (d > length) return bb;
increment.setPolar(d, getRads());
return (aa + increment);
}
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);
}
}
}
/** @brief We drag our finger on the screen to another location.
* @param p [in] New location of our finger.
* @return true on success.
*/
bool ChandWriter::onPaintingMove( const Cpoint &point)
{
Cpoint p =point-(m_rect.leftTop()*8);
if ( m_surface ==NULL)
{
return false;
}
if ( !m_started)
{
m_started =true;
addPoint( p.x, p.y);
#ifdef USE_SDL2
m_graphics->setRenderArea( m_surface);
m_graphics->pixel( p.x, p.y);
m_graphics->setRenderArea( NULL);
#else
CtextSurface::pixel( m_surface, p);
#endif
m_lastPoint =p;
m_index =1;
}
else
{
// Not for small distances.
if ( p.distance( m_lastPoint) >m_minimum_distance && ++m_index<1000)
{
// New line detected.
addPoint( p.x, p.y);
#ifdef USE_SDL2
m_graphics->setRenderArea( m_surface);
m_graphics->line( m_lastPoint.x, m_lastPoint.y, p.x, p.y);
m_graphics->setRenderArea( NULL);
#else
CtextSurface::line( m_surface, m_lastPoint, p);
#endif
// pixel(%d,%d) to (%d,%d)", m_lastPoint.x, m_lastPoint.y, p.x, p.y);
m_lastPoint =p;
}
}
Cimage::onPaint(0);
onUpdate();
return true;
}
unsigned short int Cline::intersectionsWithCircle(Cpoint c, double r, vector<Cpoint> &intersections)
{
intersections.clear();
// move circle to (0,0)
//Cpoint p1 = aa - c; cout << "DEBUG: p1 is " << p1 << endl;
//Cpoint p2 = bb - c; cout << "DEBUG: p2 is " << p2 << endl;
Cpoint p1(aa.get_xx() - c.get_xx(), aa.get_yy() - c.get_yy()); //cout << "DEBUG: p1 is " << p1 << endl;
Cpoint p2(bb.get_xx() - c.get_xx(), bb.get_yy() - c.get_yy()); //cout << "DEBUG: p1 is " << p1 << endl;
// discriminant
double dx = p2.get_xx() - p1.get_xx();
double dy = p2.get_yy() - p1.get_yy();
double dr_sq = dx*dx + dy*dy;
double D = p1.get_xx() * p2.get_yy() - p2.get_xx() * p1.get_yy();
double disc = r*r * dr_sq - D*D;
if(disc < 0)
return 0;
else if(disc == 0)
{
double x = D * dy / dr_sq;
double y = -D * dx / dr_sq;
// check if point belongs to segment
Cpoint i = Cpoint(x + c.get_xx(), y + c.get_yy());
if( i.d2point2(&aa) <= aa.d2point2(&bb) && i.d2point2(&bb) <= aa.d2point2(&bb))
intersections.push_back( i );
return intersections.size();
}
else
{
int sign_dy = (dy >= 0) ? 1 : -1;
double x1 = ( D * dy + sign_dy * dx * sqrt(disc) ) / dr_sq;
double x2 = ( D * dy - sign_dy * dx * sqrt(disc) ) / dr_sq;
double abs_dy = ( dy >= 0 ) ? dy : -dy;
double y1 = ( -D * dx + abs_dy * sqrt(disc) ) / dr_sq;
double y2 = ( -D * dx - abs_dy * sqrt(disc) ) / dr_sq;
Cpoint i1 = Cpoint(x1 + c.get_xx(), y1 + c.get_yy());
Cpoint i2 = Cpoint(x2 + c.get_xx(), y2 + c.get_yy());
if( i1.d2point2(&aa) <= aa.d2point2(&bb) && i1.d2point2(&bb) <= aa.d2point2(&bb))
intersections.push_back( i1 );
if( i2.d2point2(&aa) <= aa.d2point2(&bb) && i2.d2point2(&bb) <= aa.d2point2(&bb))
intersections.push_back( i2 );
return intersections.size();
}
}
bool Cline::chunk(float d, Cpoint &p)
{
//adjust();
if (d > length)
{
p = bb;
return false;
}
else
{
p.setPolar(d, getRads()); // increment
p = p + aa;
return true;
}
}
/**
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::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();
}
