static double
BLw( /* map world luminance to display brightness */
double Lw
)
{
double b;
if (Lw <= LMIN || (b = Bl(Lw)) <= bwmin+FTINY)
return(Bldmin);
if (b >= bwmax-FTINY)
return(Bldmax);
return(Bldmin + cf(b)*(Bldmax-Bldmin));
}
extern double
crfactor( /* contrast reduction factor */
double Lw
)
{
int i = HISTRES*(Bl(Lw) - bwmin)/(bwmax - bwmin);
double bLw, ratio, Tdb;
if (i <= 0)
return(1.0);
if (i >= HISTRES)
return(1.0);
bLw = BLw(Lw);
ratio = what2do&DO_HSENS ? htcontrs(Lb(bLw))/htcontrs(Lw) : Lb(bLw)/Lw;
Tdb = mhistot * (bwmax - bwmin) / HISTRES;
return(modhist[i]*Lb1(bLw)*(Bldmax-Bldmin)*Bl1(Lw)/(Tdb*ratio));
}
void main()
{
DDRA=255;
DDRB=255;
DDRD=0xf0;
while(1)
{
PORTD=0xef;
_delay_ms(10);
if(((PIND&0x01)==0x00)||c==1)
{
while((PIND&0x0f)==0x0f)
{
c=1;
Al();
}
}
else if(((PIND&0x02)==0x00)||c==2)
{
while((PIND&0x0f)==0x0f)
{
c=2;
Bl();
}
}
else if(((PIND&0x04)==0x00)||c==3)
{
while((PIND&0x0f)==0x0f)
{
c=3;
Cl();
}
}
else if(((PIND&0x08)==0x00)||(c==4))
{
while((PIND&0x0f)==0x0f)
{
c=4;
Dl();
}
}
}
}
extern void
gethisto( /* load precomputed luminance histogram */
FILE *fp
)
{
double histo[MAXPREHIST];
double histart, histep;
double b, lastb, w;
int n;
register int i;
/* load data */
for (i = 0; i < MAXPREHIST &&
fscanf(fp, "%lf %lf", &b, &histo[i]) == 2; i++) {
if (i > 1 && fabs(b - lastb - histep) > .001) {
fprintf(stderr,
"%s: uneven step size in histogram data\n",
progname);
exit(1);
}
if (i == 1)
if ((histep = b - (histart = lastb)) <= FTINY) {
fprintf(stderr,
"%s: illegal step in histogram data\n",
progname);
exit(1);
}
lastb = b;
}
if (i < 2 || !feof(fp)) {
fprintf(stderr,
"%s: format/length error loading histogram (log10L %f at %d)\n",
progname, b, i);
exit(1);
}
n = i;
histart *= LN_10;
histep *= LN_10;
/* find extrema */
for (i = 0; i < n && histo[i] <= FTINY; i++)
;
bwmin = histart + (i-.001)*histep;
for (i = n; i-- && histo[i] <= FTINY; )
;
bwmax = histart + (i+1.001)*histep;
if (bwmax > Bl(LMAX))
bwmax = Bl(LMAX);
if (bwmin < Bl(LMIN))
bwmin = Bl(LMIN);
else /* duplicate bottom bin */
bwmin = bwmax - (bwmax-bwmin)*HISTRES/(HISTRES-1);
/* convert histogram */
bwavg = 0.; histot = 0.;
for (i = 0; i < HISTRES; i++)
bwhist[i] = 0.;
for (i = 0, b = histart; i < n; i++, b += histep) {
if (b < bwmin+FTINY) continue;
if (b >= bwmax-FTINY) break;
w = histo[i];
bwavg += w*b;
bwhist[bwhi(b)] += w;
histot += w;
}
bwavg /= histot;
if (bwmin > Bl(LMIN)+FTINY) { /* add false samples at bottom */
bwhist[1] *= 0.5;
bwhist[0] += bwhist[1];
}
}
extern void
comphist(void) /* create foveal sampling histogram */
{
double l, b, w, lwmin, lwmax;
register int x, y;
/* check for precalculated histogram */
if (what2do&DO_PREHIST)
return;
lwmin = 1e10; /* find extrema */
lwmax = 0.;
for (y = 0; y < fvyr; y++)
for (x = 0; x < fvxr; x++) {
l = plum(fovscan(y)[x]);
if (l < lwmin) lwmin = l;
if (l > lwmax) lwmax = l;
}
lwmax *= 1.01;
if (lwmax > LMAX)
lwmax = LMAX;
bwmax = Bl(lwmax);
if (lwmin < LMIN) {
lwmin = LMIN;
bwmin = Bl(LMIN);
} else { /* duplicate bottom bin */
bwmin = bwmax - (bwmax-Bl(lwmin))*HISTRES/(HISTRES-1);
lwmin = Lb(bwmin);
}
/* (re)compute histogram */
bwavg = 0.;
histot = 0.;
for (x = 0; x < HISTRES; x++)
bwhist[x] = 0.;
/* global average */
if (!(what2do&DO_FIXHIST) || fixfrac < 1.-FTINY)
for (y = 0; y < fvyr; y++)
for (x = 0; x < fvxr; x++) {
l = plum(fovscan(y)[x]);
if (l < lwmin+FTINY) continue;
if (l >= lwmax-FTINY) continue;
b = Bl(l);
w = what2do&DO_CWEIGHT ? centprob(x,y) : 1.;
bwavg += w*b;
bwhist[bwhi(b)] += w;
histot += w;
}
/* average fixation points */
if (what2do&DO_FIXHIST && nfixations > 0) {
if (histot > FTINY)
w = fixfrac/(1.-fixfrac)*histot/nfixations;
else
w = 1.;
for (x = 0; x < nfixations; x++) {
l = plum(fovscan(fixlst[x][1])[fixlst[x][0]]);
if (l < lwmin+FTINY) continue;
if (l >= lwmax-FTINY) continue;
b = Bl(l);
bwavg += w*b;
bwhist[bwhi(b)] += w;
histot += w;
}
}
bwavg /= histot;
if (lwmin > LMIN+FTINY) { /* add false samples at bottom */
bwhist[1] *= 0.5;
bwhist[0] += bwhist[1];
}
}
int main( int argc, char **argv )
{
Hubo_Control hubo("proto-manip-daemon");
ach_channel_t chan_manip_cmd;
int r = ach_open( &chan_manip_cmd, CHAN_HUBO_MANIP, NULL );
daemon_assert( r==ACH_OK, __LINE__ );
hubo_manip_cmd manip;
memset( &manip, 0, sizeof(manip) );
hubo.update();
Eigen::Isometry3d Br, Bl;
Vector6d right, left, zeros; zeros.setZero();
Vector3d rtrans, ltrans, langles, rangles;
hubo.getRightArmAngles(right);
hubo.getLeftArmAngles(left);
hubo.huboArmFK( Br, right, RIGHT );
hubo.huboArmFK( Bl, left, LEFT );
std::cout << "Performed initial FK" << std::endl;
for(int i=0; i<3; i++)
{
manip.translation[RIGHT][i] = Br(i,3);
manip.translation[LEFT][i] = Bl(i,3);
}
std::cout << "Putting first transformation" << std::endl;
ach_put( &chan_manip_cmd, &manip, sizeof(manip) );
size_t fs;
std::cout << "About to start loop" << std::endl;
while( !daemon_sig_quit )
{
hubo.update();
ach_get( &chan_manip_cmd, &manip, sizeof(manip), &fs, NULL, ACH_O_LAST );
for(int i=0; i<3; i++)
{
rtrans(i) = manip.translation[RIGHT][i];
ltrans(i) = manip.translation[LEFT][i];
rangles(i) = manip.eulerAngles[RIGHT][i];
langles(i) = manip.eulerAngles[LEFT][i];
}
// Handle the right arm
Br = Eigen::Matrix4d::Identity();
Br.translate(rtrans);
Br.rotate( Eigen::AngleAxisd(rangles(0), Vector3d(1,0,0)) );
Br.rotate( Eigen::AngleAxisd(rangles(1), Vector3d(0,1,0)) );
Br.rotate( Eigen::AngleAxisd(rangles(2), Vector3d(0,0,1)) );
hubo.huboArmIK( right, Br, zeros, RIGHT );
hubo.setRightArmAngles( right );
// Handle the left arm
Bl = Eigen::Matrix4d::Identity();
Bl.translate(ltrans);
Bl.rotate( Eigen::AngleAxisd(langles(0), Vector3d(1,0,0)) );
Bl.rotate( Eigen::AngleAxisd(langles(1), Vector3d(0,1,0)) );
Bl.rotate( Eigen::AngleAxisd(langles(2), Vector3d(0,0,1)) );
hubo.huboArmIK( left, Bl, zeros, LEFT );
hubo.setLeftArmAngles( left );
// Send commands off to the control daemon
hubo.sendControls();
}
ach_close( &chan_manip_cmd );
}
int
main(
int argc,
char *argv[]
)
{
static RGBPRIMS outprimS;
int i;
#define bool(flg) switch (argv[i][2]) { \
case '\0': what2do ^= flg; break; \
case 'y': case 'Y': case 't': case 'T': \
case '+': case '1': what2do |= flg; break; \
case 'n': case 'N': case 'f': case 'F': \
case '-': case '0': what2do &= ~(flg); break; \
default: goto userr; }
progname = argv[0];
for (i = 1; i < argc && argv[i][0] == '-'; i++)
switch (argv[i][1]) {
case 'h':
bool(DO_HUMAN);
break;
case 'a':
bool(DO_ACUITY);
break;
case 'v':
bool(DO_VEIL);
break;
case 's':
bool(DO_HSENS);
break;
case 'c':
bool(DO_COLOR);
break;
case 'w':
bool(DO_CWEIGHT);
break;
case 'i':
if (i+1 >= argc) goto userr;
fixfrac = atof(argv[++i]);
if (fixfrac > FTINY) what2do |= DO_FIXHIST;
else what2do &= ~DO_FIXHIST;
break;
case 'I':
bool(DO_PREHIST);
break;
case 'l':
bool(DO_LINEAR);
break;
case 'p':
if (i+8 >= argc) goto userr;
outprimS[RED][CIEX] = atof(argv[++i]);
outprimS[RED][CIEY] = atof(argv[++i]);
outprimS[GRN][CIEX] = atof(argv[++i]);
outprimS[GRN][CIEY] = atof(argv[++i]);
outprimS[BLU][CIEX] = atof(argv[++i]);
outprimS[BLU][CIEY] = atof(argv[++i]);
outprimS[WHT][CIEX] = atof(argv[++i]);
outprimS[WHT][CIEY] = atof(argv[++i]);
outprims = outprimS;
break;
case 'e':
if (i+1 >= argc) goto userr;
scalef = atof(argv[++i]);
if ((argv[i][0] == '+') | (argv[i][0] == '-'))
scalef = pow(2.0, scalef);
what2do |= DO_LINEAR;
break;
case 'f':
if (i+1 >= argc) goto userr;
mbcalfile = argv[++i];
break;
case 'm':
if (i+1 >= argc) goto userr;
cwarpfile = argv[++i];
break;
case 'u':
if (i+1 >= argc) goto userr;
ldmax = atof(argv[++i]);
if (ldmax <= FTINY)
goto userr;
break;
case 'd':
if (i+1 >= argc) goto userr;
lddyn = atof(argv[++i]);
break;
case 'x':
if (i+1 >= argc) goto userr;
if ((mapfp = fopen(argv[++i], "w")) == NULL) {
fprintf(stderr,
"%s: cannot open for writing\n",
argv[i]);
exit(1);
}
break;
default:
goto userr;
}
if ((what2do & (DO_FIXHIST|DO_PREHIST)) == (DO_FIXHIST|DO_PREHIST)) {
fprintf(stderr, "%s: only one of -i or -I option\n", progname);
exit(1);
}
if ((mbcalfile != NULL) + (cwarpfile != NULL) +
(outprims != stdprims) > 1) {
fprintf(stderr,
"%s: only one of -p, -m or -f option supported\n",
progname);
exit(1);
}
if ((outprims == stdprims) & (inprims != stdprims))
outprims = inprims;
Bldmin = Bl(ldmax/lddyn);
Bldmax = Bl(ldmax);
if (i >= argc || i+2 < argc)
goto userr;
/* open input file */
if ((infp = fopen(infn=argv[i], "r")) == NULL)
syserror(infn);
/* open output file */
if (i+2 == argc && freopen(argv[i+1], "w", stdout) == NULL)
syserror(argv[i+1]);
SET_FILE_BINARY(infp);
SET_FILE_BINARY(stdout);
getahead(); /* load input header */
printargs(argc, argv, stdout); /* add to output header */
if ((mbcalfile == NULL) & (outprims != stdprims))
fputprims(outprims, stdout);
if ((what2do & (DO_PREHIST|DO_VEIL|DO_ACUITY)) != DO_PREHIST)
getfovimg(); /* get foveal sample image? */
if (what2do&DO_PREHIST) /* get histogram? */
gethisto(stdin);
else if (what2do&DO_FIXHIST) /* get fixation history? */
getfixations(stdin);
mapimage(); /* map the picture */
if (mapfp != NULL) /* write out basic mapping */
putmapping(mapfp);
exit(0);
userr:
fprintf(stderr, "Usage: %s [-{h|a|v|s|c|l|w}[+-]][-I|-i ffrac][-e ev][-p xr yr xg yg xb yb xw yw|-f mbf.cal|-m rgb.cwp][-u Ldmax][-d Lddyn][-x mapfile] inpic [outpic]\n",
progname);
exit(1);
return 1; /* pro forma return */
#undef bool
}
