void Read(const char *fname = NULL)
{
if (!active)
{
printf("Catalog loading from %s...\n",
(fname!=NULL)?fname:ORIENT_SRC);
YARPImageOf<YarpPixelFloat> orient_pre;
YARPImageFile::Read((fname!=NULL)?fname:ORIENT_SRC,
orient_pre,YARPImageFile::FORMAT_NUMERIC);
orient.Resize(4,orient_pre.GetHeight());
for (int i=0; i<orient_pre.GetHeight(); i++)
{
for (int j=0; j<4; j++)
{
orient(j,i) = orient_pre(j,i);
}
}
printf("Catalog loaded\n");
active = 1;
}
}
void main(int argc, char *argv[])
{
ParseCommandLine (argc, argv);
YARPImageOf<YarpPixelBGR> workImgL; // just a reference.
YARPImageOf<YarpPixelBGR> workImgR; // just a reference.
YARPImageOf<YarpPixelMono> lpImg;
lpImg.Resize (Size, Size);
YARPImageOf<YarpPixelMono> cartesianl;
YARPImageOf<YarpPixelRGB> in_cartl;
YARPImageOf<YarpPixelRGB> logpolarl;
YARPImageOf<YarpPixelMono> m_storeleft;
YARPImageOf<YarpPixelMono> cartesianr;
YARPImageOf<YarpPixelRGB> in_cartr;
YARPImageOf<YarpPixelRGB> logpolarr;
YARPImageOf<YarpPixelMono> m_storeright;
cartesianl.Resize (Size, Size);
in_cartl.Resize (Size, Size);
logpolarl.Resize (NECC, NANG);
m_storeleft.Resize (NECC, NANG);
cartesianr.Resize (Size, Size);
in_cartr.Resize (Size, Size);
logpolarr.Resize (NECC, NANG);
m_storeright.Resize (NECC, NANG);
YARPImageOf<YarpPixelMono> m_back_histo;
m_back_histo.Resize (15, 10);
YARPImageOf<YarpPixelMono> m_histo;
m_histo.Resize (15, 10);
VisionThread2Status m_status = VTS_WaitForTarget;
int m_counter = 0;
YARPLogPolar lp (NECC, NANG, rfMin, Size);
YARPLpClrSegmentation m_color_segmentation (NECC, NANG, rfMin, Size);
YARPLpBoxer m_boxer (NECC, NANG, rfMin, Size);
FiveBoxesInARow target_boxes_l;
FiveBoxesInARow target_boxes_r;
inImgPortL.Register(in_channell);
inImgPortR.Register(in_channelr);
outImgPortL.Register(out_channell);
outImgPortR.Register(out_channelr);
histoPort.Register(histo_channel);
out_position_l.Register (position_channel_l);
out_position_r.Register (position_channel_r);
while (1)
{
inImgPortL.Read();
YARPGenericImage& inImgL = inImgPortL.Content(); // Lifetime until next Read()
workImgL.Refer(inImgL);
inImgPortR.Read();
YARPGenericImage& inImgR = inImgPortR.Content(); // Lifetime until next Read()
workImgR.Refer(inImgR);
lp.Cart2LpSwap (workImgL, logpolarl);
lp.Cart2LpSwap (workImgR, logpolarr);
switch (m_status)
{
case VTS_WaitForTarget:
if (m_color_segmentation.InitialDetection (logpolarl, logpolarr, m_storeleft, m_storeright))
m_status = VTS_ColorSegmentationInit;
break;
case VTS_ColorSegmentationInit:
{
if (m_color_segmentation.StartupSegmentation (logpolarl, logpolarr, m_storeleft, m_storeright))
{
m_status = VTS_ColorSegmentation;
m_counter = 0;
}
else
m_status = VTS_WaitForTarget;
}
break;
case VTS_ColorSegmentation:
{
// wait 6 steps before aborting in case of bad target...
bool can_continue = m_color_segmentation.ColorSegmentation (logpolarl, logpolarr, m_storeleft, m_storeright);
if (!can_continue)
{
// wait some cycles...
m_counter++;
if (m_counter > 20) // || !good)
{
// move to motion segmentation.
m_status = VTS_WaitForTarget;
m_color_segmentation.Reset ();
}
}
else
{
m_counter = 0;
}
}
break;
case VTS_ResetHistograms:
m_color_segmentation.ResetHistograms (); // continues aborting the segmentation.
case VTS_AbortSegmentation:
m_color_segmentation.AbortSegmentation ();
m_status = VTS_WaitForTarget;
break;
}
m_boxer.Apply (m_storeleft);
lp.Lp2Cart (m_storeleft, cartesianl);
m_boxer.DrawBoxes (cartesianl);
YARPBox *b = m_boxer.GetBoxes();
YARPBoxCopier::Copy (b[0], target_boxes_l.box1);
YARPBoxCopier::Copy (b[1], target_boxes_l.box2);
YARPBoxCopier::Copy (b[2], target_boxes_l.box3);
YARPBoxCopier::Copy (b[3], target_boxes_l.box4);
YARPBoxCopier::Copy (b[4], target_boxes_l.box5);
m_boxer.Apply (m_storeright);
lp.Lp2Cart (m_storeright, cartesianr);
m_boxer.DrawBoxes (cartesianr);
YARPBoxCopier::Copy (b[0], target_boxes_r.box1);
YARPBoxCopier::Copy (b[1], target_boxes_r.box2);
YARPBoxCopier::Copy (b[2], target_boxes_r.box3);
YARPBoxCopier::Copy (b[3], target_boxes_r.box4);
YARPBoxCopier::Copy (b[4], target_boxes_r.box5);
out_position_l.Content() = target_boxes_l;
out_position_l.Write();
out_position_r.Content() = target_boxes_r;
out_position_r.Write();
m_color_segmentation.GetMotionHistogramAsImage(m_histo);
m_color_segmentation.GetWholeHistogramAsImage(m_back_histo);
YARPImageUtils::PasteInto (m_histo, 0, 0, 5, lpImg);
YARPImageUtils::PasteInto (m_back_histo, 0, 60, 5, lpImg);
YARPGenericImage& outImgL = outImgPortL.Content(); // Lasts until next Write()
outImgL.Refer(cartesianl);
outImgPortL.Write();
YARPGenericImage& outImgR = outImgPortR.Content(); // Lasts until next Write()
outImgR.Refer(cartesianr);
outImgPortR.Write();
YARPGenericImage& outhImg = histoPort.Content();
outhImg.Refer(lpImg);
histoPort.Write();
}
}
int
YARPFlowTracker::ComputeRotation (
YARPImageOf<YarpPixelMono>& mask,
int *vx,
int *vy,
int ox,
int oy,
CVisDVector& trsf,
int thr)
{
const int border = 1;
trsf = 0;
trsf(1) = 1;
YARPImageOf<YarpPixelMono> tmp;
tmp.Resize (mask.GetWidth(), mask.GetHeight());
tmp.Zero();
double avex = 0;
double avey = 0;
double average = 0;
int count = 0;
// compute average displacement.
for (int i = border; i < oy-border; i++)
for (int j = border; j < ox-border; j++)
{
if (vx[i*ox+j] <= OOVERFLOW &&
vy[i*ox+j] <= OOVERFLOW &&
mask (j*BLOCKINC+BLOCKSIZE/2, i*BLOCKINC+BLOCKSIZE/2) != 0
&&
(fabs(vx[i*ox+j]) > 0 || fabs(vy[i*ox+j]) > 0)
)
{
avex += vx[i*ox+j];
avey += vy[i*ox+j];
average += sqrt(vx[i*ox+j]*vx[i*ox+j]+vy[i*ox+j]*vy[i*ox+j]);
count++;
}
}
if (count > 0)
{
avex /= count;
avey /= count;
average /= count;
}
//
if (count >= thr)
{
CVisDMatrix A (count * 2, 4);
CVisDMatrix At (4, count * 2);
CVisDMatrix sqA (4, 4);
CVisDVector sqB (4);
CVisDVector b (count * 2);
CVisDVector solution(4);
count = 1;
for (int i = border; i < oy-border; i++)
for (int j = border; j < ox-border; j++)
{
if (vx[i*ox+j] <= OOVERFLOW &&
vy[i*ox+j] <= OOVERFLOW &&
mask (j*BLOCKINC+BLOCKSIZE/2, i*BLOCKINC+BLOCKSIZE/2) != 0
&&
(fabs(vx[i*ox+j]) > 0 || fabs(vy[i*ox+j]) > 0)
)
{
A(count,1) = j*BLOCKINC+BLOCKSIZE/2;
A(count,2) = i*BLOCKINC+BLOCKSIZE/2;
A(count,3) = 1;
A(count,4) = 0;
b(count) = j*BLOCKINC+BLOCKSIZE/2+vx[i*ox+j];
count++;
A(count,1) = i*BLOCKINC+BLOCKSIZE/2;
A(count,2) = -(j*BLOCKINC+BLOCKSIZE/2);
A(count,3) = 0;
A(count,4) = 1;
b(count) = i*BLOCKINC+BLOCKSIZE/2+vy[i*ox+j];
count++;
}
}
// solve by LU.
At = A.Transposed ();
sqA = At * A;
sqB = At * b;
VisDMatrixLU (sqA, sqB, solution);
trsf = solution;
// apply tranformation to mask.
double& aa = solution(1);
double& bb = solution(2);
double& t1 = solution(3);
double& t2 = solution(4);
for (int i = 0; i < mask.GetHeight(); i++)
for (int j = 0; j < mask.GetWidth(); j++)
{
if (mask (j, i) != 0)
{
int dx = int(aa * j + bb * i + t1 +.5);
int dy = int(-bb * j + aa * i + t2 + .5);
tmp.SafePixel (dx, dy) = 255;
}
}
mask = tmp;
return 0;
}
else
return -2;
return -1;
}
int YARPImageFile::Read(const char *src, YARPGenericImage& dest, int format)
{
if (format!=YARPImageFile::FORMAT_NUMERIC)
{
return ImageRead(dest,src);
}
int hh = 0, ww = 0;
{
ifstream fin(src);
int blank = 1;
int curr = 0;
while (!fin.eof())
{
int ch = fin.get();
//if (ch!='\n') printf("[%c]",ch);
if (ch==' ' || ch == '\t' || ch == '\r' || ch == '\n' || fin.eof())
{
if (!blank)
{
if (curr==0)
{
hh++;
}
curr++;
if (curr>ww)
{
ww = curr;
//printf("%d\n", ww);
}
}
blank = 1;
if (ch=='\n')
{
curr = 0;
}
}
else
{
blank = 0;
}
}
}
//printf("yyy dim %d %d\n", hh, ww);
YARPImageOf<YarpPixelFloat> flt;
flt.Resize(ww,hh);
hh = 0; ww = 0;
{
char buf[256];
int idx = 0;
ifstream fin(src);
int blank = 1;
int curr = 0;
while (!fin.eof())
{
int ch = fin.get();
if (ch==' ' || ch == '\t' || ch == '\r' || ch == '\n' || fin.eof())
{
if (!blank)
{
if (curr==0)
{
hh++;
}
curr++;
if (curr>ww)
{
ww = curr;
}
buf[idx] = '\0';
flt(curr-1,hh-1) = float(atof(buf));
idx = 0;
}
blank = 1;
if (ch=='\n')
{
curr = 0;
}
}
else
{
buf[idx] = ch;
idx++;
assert(idx<sizeof(buf));
blank = 0;
}
}
}
dest.CastCopy(flt);
return 0;
//return ImageRead(dest,src);
//return 0;
}
int main(int argc, char* argv[])
{
int boardN = 0;
int ret = 0;
int rhoSize = 256;
int thetaSize = 180;
YARPImageOf<YarpPixelMono> hough_img;
char c;
printf("\n[eyeCalib] Setting up the sensor..");
ret = _grabber.initialize(boardN, _sizeX, _sizeY);
if (ret == YARP_FAIL)
{
printf("[eyeCalib] ERROR in _grabber.initialize(). Quitting.\n");
exit (1);
}
initializeHead();
printf("\n[eyeCalib] Allocating images and filter (%d x %d)...", _sizeX, _sizeY);
for (int i=0; i<N_IMAGES; i++)
_imgBuffer[i].Resize (_sizeX, _sizeY);
_susanImg.Resize (_sizeX, _sizeY);
_susanFlt.resize(_sizeX, _sizeY);
printf("\n[eyeCalib] Allocating Hough Filter (theta %d, rho %d)..", thetaSize, rhoSize);
_houghFlt.resize(thetaSize, rhoSize);
char fileName[255];
FILE *outFile = NULL;
double orientation;
double posVector[4];
double vel;
printf("\n[eyeCalib] Do you want to save results to file ? [Y/n] ");
c = getch();
if (c != 'n')
{
printf("\n[eyeCalib] File name ? ");
scanf("%s", fileName);
printf("[eyeCalib] Append data (Y/n)?");
c = getch();
if (c == 'n')
{
outFile = fopen(fileName,"w");
fprintf(outFile, "velocity;J0;J1;J2;J3;orientation\n");
}
else
{
outFile = fopen(fileName,"a");
}
if (!outFile)
{
printf("[eyeCalib] ERROR opening the file %s. Saving aboorted.\n", fileName);
}
}
printf("\n[eyeCalib] Interactive calibration or Load data form file? [I/l] ");
c = getch();
if ( c == 'l')
{
printf("\n[eyeCalib] File to load? ");
scanf("%s", fileName);
FILE *dataFile = NULL;
dataFile = fopen(fileName,"r");
if (dataFile == NULL)
{
printf("[eyeCalib] ERROR opening the file %s. Quitting.\n", fileName);
releaseSensor();
if (outFile != NULL)
fclose(outFile);
exit(1);
}
bool ret = false;
while (readLine(dataFile, &vel, posVector) == true)
{
printf("\n[eyeCalib] Moving [%.2lf %.2lf %.2lf %.2lf * %.2lf]..", posVector[0], posVector[1], posVector[2], posVector[3], vel);
printf("\n[eyeCalib] Hit any key when Position has been reached..");
moveHead(vel,posVector);
c = getch();
orientation = calibrate();
printf("\n[eyeCalib] Main orientation is %.3frad (%.3fdeg) - variance %.2f", orientation, _rad2deg(orientation));
if (outFile != NULL)
fprintf(outFile, "%lf;%lf;%lf;%lf;%lf;%lf\n", vel, posVector[0], posVector[1], posVector[2], posVector[3], orientation);
}
fclose(dataFile);
}
else
{
do
{
printf("\n[eyeCalib] Avaible Joints:\n\t0 - Left Pan\n\t1 - Left Tilt\n\t2 - Right Pan\n\t3 - Left Tilt");
printf("\n[EyeCalib] Move to? [J0;J1;J2;J3] ");
scanf("%lf;%lf;%lf;%lf", &(posVector[0]), &(posVector[1]), &(posVector[2]), &(posVector[3]));
printf("\n[eyeCalib] Move with velocity? ");
scanf("%lf", &vel);
printf("\n[eyeCalib] Moving [%.2lf %.2lf %.2lf %.2lf * %.2lf]..", posVector[0], posVector[1], posVector[2], posVector[3], vel);
printf("\n[eyeCalib] Hit any key when Position has been reached..");
moveHead(vel, posVector);
c = getch();
orientation = calibrate();
printf("\n[eyeCalib] Main orientation is %.3frad (%.3fdeg) - variance %.2f", orientation, _rad2deg(orientation));
if (outFile != NULL)
fprintf(outFile, "%lf;%lf;%lf;%lf;%lf;%lf\n", vel, posVector[0], posVector[1], posVector[2], posVector[3], orientation);
printf("\n[eyeCalib] Another loop ? [Y/n] ");
c = getch();
} while (c != 'n');
}
if (outFile != NULL)
fclose(outFile);
releaseSensor();
releaseHead();
printf("\n[eyeCalib] Bye.\n");
return 0;
}
