bool lookForFaces()
{
yarp::sig::Vector pxl(2,0.0);
yarp::sig::Vector pxr(2,0.0);
while (true)
{
imageInR = imagePortInR -> read(false);
imageInL = imagePortInL -> read(false);
if (imageInL)
{
ImageOf<PixelBgr> imageOutL;
detectorL->loop(imageInL,imageOutL);
imagePortOutL.write(imageOutL);
}
if (imageInR)
{
ImageOf<PixelBgr> imageOutR;
detectorR->loop(imageInR,imageOutR);
imagePortOutR.write(imageOutR);
}
printMessage(2,"counterL: %i counterR: %i\n",detectorL->counter,detectorR->counter);
if (detectorL->counter > certainty &&
detectorR->counter > certainty)
{
pxl=detectorL->getCenterOfFace();
pxr=detectorR->getCenterOfFace();
printMessage(1,"I have found a face! pxl: %s pxr: %s\n",pxl.toString().c_str(),pxr.toString().c_str());
if (pxl(0)!=0.0 && pxr(0)!=0.0)
{
igaze->lookAtStereoPixels(pxl,pxr);
}
return true;
}
Time::delay(0.1);
}
return false;
}
void generateTarget()
{
// translational target part: a circular trajectory
// in the yz plane centered in [-0.3,-0.1,0.1] with radius=0.1 m
// and frequency 0.1 Hz
// xd[0]=-0.3;
// xd[1]=-0.1+0.2*cos(2.0*M_PI*0.1*(t-t0));
// xd[2]=+0.1+0.2*sin(2.0*M_PI*0.1*(t-t0));
// we keep the orientation of the left arm constant:
// we want the middle finger to point forward (end-effector x-axis)
// with the palm turned down (end-effector y-axis points leftward);
// to achieve that it is enough to rotate the root frame of pi around z-axis
od[0]=0.0; od[1]=0.0; od[2]=1.0; od[3]=M_PI;
//adding code for taget location through vision
//incoming image from both eyes
Bottle *target = incomingPort.read();
Vector r(2), l(2);
r[0] = target->get(0).asDouble();
r[1] = target->get(1).asDouble();
l[0] = target->get(3).asDouble();
l[1] = target->get(4).asDouble();
if(r[0]==0.0 && r[1]==0.0 && l[0]==0.0 && l[1]==0.0){
xd[0]=-0.3;
xd[1]=-.1;
xd[2]=.1;
}
else{
igaze->lookAtStereoPixels(l,r);
igaze->triangulate3DPoint(l,r,xd);
}
}
