bool updateModule()
{
if (imgOutPort.getOutputCount()>0)
{
if (ImageOf<PixelBgr> *pImgBgrIn=imgInPort.read(false))
{
Vector xa,oa;
iarm->getPose(xa,oa);
Matrix Ha=axis2dcm(oa);
Ha.setSubcol(xa,0,3);
Vector v(4,0.0); v[3]=1.0;
Vector c=Ha*v;
v=0.0; v[0]=0.05; v[3]=1.0;
Vector x=Ha*v;
v=0.0; v[1]=0.05; v[3]=1.0;
Vector y=Ha*v;
v=0.0; v[2]=0.05; v[3]=1.0;
Vector z=Ha*v;
v=solution; v.push_back(1.0);
Vector t=Ha*v;
Vector pc,px,py,pz,pt;
int camSel=(eye=="left")?0:1;
igaze->get2DPixel(camSel,c,pc);
igaze->get2DPixel(camSel,x,px);
igaze->get2DPixel(camSel,y,py);
igaze->get2DPixel(camSel,z,pz);
igaze->get2DPixel(camSel,t,pt);
CvPoint point_c=cvPoint((int)pc[0],(int)pc[1]);
CvPoint point_x=cvPoint((int)px[0],(int)px[1]);
CvPoint point_y=cvPoint((int)py[0],(int)py[1]);
CvPoint point_z=cvPoint((int)pz[0],(int)pz[1]);
CvPoint point_t=cvPoint((int)pt[0],(int)pt[1]);
cvCircle(pImgBgrIn->getIplImage(),point_c,4,cvScalar(0,255,0),4);
cvCircle(pImgBgrIn->getIplImage(),point_t,4,cvScalar(255,0,0),4);
cvLine(pImgBgrIn->getIplImage(),point_c,point_x,cvScalar(0,0,255),2);
cvLine(pImgBgrIn->getIplImage(),point_c,point_y,cvScalar(0,255,0),2);
cvLine(pImgBgrIn->getIplImage(),point_c,point_z,cvScalar(255,0,0),2);
cvLine(pImgBgrIn->getIplImage(),point_c,point_t,cvScalar(255,255,255),2);
tip.clear();
tip.addInt(point_t.x);
tip.addInt(point_t.y);
imgOutPort.prepare()=*pImgBgrIn;
imgOutPort.write();
}
}
return true;
}
bool updateModule()
{
ImageOf<PixelRgb> *imgIn=imgInPort.read(true);
if (imgIn==NULL)
return false;
ImageOf<PixelRgb> &imgOut=imgOutPort.prepare();
imgOut=*imgIn;
cv::Mat img=cv::cvarrToMat(imgOut.getIplImage());
Vector xa,oa;
iarm->getPose(xa,oa);
Matrix Ha=axis2dcm(oa);
xa.push_back(1.0);
Ha.setCol(3,xa);
Vector pc;
igaze->get2DPixel(camSel,xa,pc);
cv::Point point_c((int)pc[0],(int)pc[1]);
cv::circle(img,point_c,4,cv::Scalar(0,255,0),4);
Vector analogs,encs(nEncs),joints;
if (ianalog!=NULL)
ianalog->read(analogs);
iencs->getEncoders(encs.data());
for (int i=0; i<3; i++)
{
if (ianalog!=NULL)
finger[i].getChainJoints(encs,analogs,joints);
else
finger[i].getChainJoints(encs,joints);
finger[i].setAng(CTRL_DEG2RAD*joints);
}
for (int fng=0; fng<3; fng++)
{
deque<cv::Point> point_f;
for (int i=-1; i<(int)finger[fng].getN(); i++)
{
Vector fc;
igaze->get2DPixel(camSel,Ha*(i<0?finger[fng].getH0().getCol(3):
finger[fng].getH(i,true).getCol(3)),fc);
point_f.push_front(cv::Point((int)fc[0],(int)fc[1]));
cv::circle(img,point_f.front(),3,cv::Scalar(0,0,255),4);
if (i>=0)
{
cv::line(img,point_f.front(),point_f.back(),cv::Scalar(255,255,255),2);
point_f.pop_back();
}
else
cv::line(img,point_c,point_f.front(),cv::Scalar(255,0,0),2);
}
}
imgOutPort.writeStrict();
return true;
}
