int main(){
int i = 0;
int mapCount = 0, clearMapCount = 0, dumpCount=0;
int revFrameCount = 0;
#ifdef USE_NORTH
targetsGPS[maxTargets].lat = ADVANCED5LAT;
targetsGPS[maxTargets].lon = ADVANCED5LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED6LAT;
targetsGPS[maxTargets].lon = ADVANCED6LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED7LAT;
targetsGPS[maxTargets].lon = ADVANCED7LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED8LAT;
targetsGPS[maxTargets].lon = ADVANCED8LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED2LAT;
targetsGPS[maxTargets].lon = ADVANCED2LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED1LAT;
targetsGPS[maxTargets].lon = ADVANCED1LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED3LAT;
targetsGPS[maxTargets].lon = ADVANCED3LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED12LAT;
targetsGPS[maxTargets].lon = ADVANCED12LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED4LAT;
targetsGPS[maxTargets].lon = ADVANCED4LON;
maxTargets++;
#else
targetsGPS[maxTargets].lat = ADVANCED4LAT;
targetsGPS[maxTargets].lon = ADVANCED4LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED1LAT;
targetsGPS[maxTargets].lon = ADVANCED1LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED2LAT;
targetsGPS[maxTargets].lon = ADVANCED2LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED3LAT;
targetsGPS[maxTargets].lon = ADVANCED3LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED11LAT;
targetsGPS[maxTargets].lon = ADVANCED11LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED8LAT;
targetsGPS[maxTargets].lon = ADVANCED8LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED7LAT;
targetsGPS[maxTargets].lon = ADVANCED7LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED6LAT;
targetsGPS[maxTargets].lon = ADVANCED6LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED11LAT;
targetsGPS[maxTargets].lon = ADVANCED11LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED5LAT;
targetsGPS[maxTargets].lon = ADVANCED5LON;
maxTargets++;
#endif
maxTargetIndex=maxTargets-1;
for(i=0;i<maxTargets;i++){// this is converting all GPS point data to XY data.
targetListXY[i].x = GPSX(targetsGPS[i].lon, startLongitude);
targetListXY[i].y = GPSY(targetsGPS[i].lat, startLatitude);
}
currentXY.x = GPSX(gpsvar.longitude,startLongitude);// converts current robot X location compared to start longitude
currentXY.y = GPSY(gpsvar.latitude,startLatitude);// converts current robot Y location compared to start latitude
targetXY = targetListXY[currentTargetIndex];//sets first target GPS point
nextTargetIndex = (currentTargetIndex + 1)%maxTargets;//sets next target GPS point
nextXY = targetListXY[nextTargetIndex];// ??
previousXY.x = GPSX(startLongitude, startLongitude);// why?
previousXY.y = GPSY(startLatitude, startLatitude);//Why?
initRoboteq(); /* Initialize roboteq */
initGuide();//what is guide?
#ifdef USE_VISION // if USE_vision is defined, then initialize vision.
initVision();
#endif //USE_VISION
#ifdef USE_GPS// if USE_GPS is defined, then initialize GPS.
initGPS();
initParser();
#endif //USE_GPS
#ifdef USE_LIDAR// if USE_LIDAR is defined, then initialize LIDAR.
initObjects();
initSICK();
#endif //USE_LIDAR
#ifdef DEBUG_VISUALIZER// if defined, then use visualizer.
initVisualizer();
#endif //DEBUG_VISUALIZER
#ifdef USE_MAP//////>>>>>>>>>>>????
initMap(0,0,0);
#endif //USE_MAP
#ifdef DUMP_GPS// dump GPS data into file
FILE *fp;
fp = fopen("gpsdump.txt", "w");
#endif // DUMP_GPS
while(1){
double dir = 1.0;
double speed = 0.0, turn = 0.0;
static double turnBoost = 0.750;//Multiplier for turn. Adjust to smooth jerky motions. Usually < 1.0
static int lSpeed = 0, rSpeed = 0;//Wheel Speed Variables
if (joystick() != 0) {// is joystick is connected
if (joy0.buttons & LB_BTN) {// deadman switch, but what does joy0.buttons do?????????????????????????????????
speed = -joy0.axis[1]; //Up is negative on joystick negate so positive when going forward
turn = joy0.axis[0];
lSpeed = (int)((speed + turnBoost*turn)*maxSpeed);//send left motor speed
rSpeed = (int)((speed - turnBoost*turn)*maxSpeed);//send right motor speed
}else{ //stop the robot
rSpeed=lSpeed=0;
}
if(((joy0.buttons & B_BTN)||autoOn)&& (saveImage==0)){//what is the single & ???????????????????
saveImage =DEBOUNCE_FOR_SAVE_IMAGE;//save each image the camera takes, save image is an int declared in vision_nav.h
}else{
if (saveImage) saveImage--; // turn off if button wasn't pressed?
}
if(joy0.buttons & RB_BTN){//turn on autonmous mode if start??? button is pressed
autoOn = 1;
mode=1;
}
if(joy0.buttons & Y_BTN){ // turn off autonomous mode
autoOn = 0;
mode =0;
}
lastButtons = joy0.buttons;//is this just updating buttons?
} else{
// printf("No Joystick Found!\n");
rSpeed=lSpeed=0;
}
//
// printf("3: %f %f\n",BASIC3LAT,BASIC3LON);
// printf("4: %f %f\n",BASIC4LAT,BASIC4LON);
// printf("5: %f %f\n",BASIC5LAT,BASIC5LON);
// getchar();
#ifdef AUTO_SWAP//what is this
if((currentTargetIndex>1&&targetIndexMem!=currentTargetIndex)||!autoOn||!mode==3){
startTime=currentTime=(float)(clock()/CLOCKS_PER_SEC);
targetIndexMem = currentTargetIndex;
}else{
currentTime=(float)(clock()/CLOCKS_PER_SEC);
}
totalTime = currentTime-startTime;
if(totalTime>=SWAPTIME&&autoOn){
swap();
targetIndexMem = 0;
}
#endif //AUTO_SWAP
#ifdef USE_GPS
readGPS();
currentXY.x = GPSX(gpsvar.longitude,startLongitude);
currentXY.y = GPSY(gpsvar.latitude,startLatitude);
robotTheta = ADJUST_RADIANS(DEG2RAD(gpsvar.course));
#else
currentXY.x = 0.0;
currentXY.y = 0.0;
robotTheta = 0.0;
#endif //USE_GPS
if(autoOn&&!flagPointSet){//this whole thing?????
flagXY.x=currentXY.x+FLAG_X_ADJUST;
flagXY.y=currentXY.y;
flagPointSet=1;
startAutoTime=currentAutoTime=(float)(clock()/CLOCKS_PER_SEC);
}
if(autoOn){
currentAutoTime=(float)(clock()/CLOCKS_PER_SEC);
totalAutoTime = currentAutoTime-startAutoTime;
if(totalAutoTime>=MODE2DELAY){
mode1TimeUp=1;//what is mode1 time up?
}
printf("TIMEING\n");
}
// if(currentTargetIndex <= OPEN_FIELD_INDEX || currentTargetIndex >= maxTargetIndex){
if(currentTargetIndex <= OPEN_FIELD_INDEX){//if you are on your last target, then set approaching thresh, and dest thresh to larger values?
//OPEN_FIELD_INDEX is set to 0 above...?
approachingThresh=4.0;
destinationThresh=3.0;
}else{//otherwise set your thresholds to a bit closer.
// destinationThresh=1.0;
destinationThresh=0.75;
approachingThresh=2.5;
}
//mode1 = lane tracking and obstacle avoidance. mode 2 = vision, lane tracking, but guide to gps. its not primary focus.
//mode3= gps mode in open field, but vision is toned down to not get distracted by random grass.
//mode 4= flag tracking
if(guide(currentXY, targetXY, previousXY, nextXY, robotTheta, robotWidth, 1)&& !allTargetsReached){//If target reached and and not all targets reached
printf("REACHED TARGET\n");
initGuide();// reset PID control stuff. problably resets all control variables.
previousXY = targetXY;//update last target
if(currentTargetIndex == maxTargetIndex){ //seeing if you are done with all targets.
allTargetsReached = 1;
}else{//otherwise update all the target information
currentTargetIndex = (currentTargetIndex + 1);
nextTargetIndex = (currentTargetIndex + 1)% maxTargets;
targetXY = targetListXY[currentTargetIndex];
nextXY = targetListXY[nextTargetIndex];
}
}
if((autoOn&&(currentTargetIndex == 0&&!approachingTarget&&!mode1TimeUp))||allTargetsReached){
//if autonomous, and on first target, and not not approaching target, and not mode 1 time up, or reached last target.
mode =1;//wtf is mode
distanceMultiplier = 50;//wthis is how heavily to rely on vision
} else if((autoOn&¤tTargetIndex == 0&&mode1TimeUp)||(autoOn&&approachingTarget&&(currentTargetIndex<=OPEN_FIELD_INDEX||currentTargetIndex>=maxTargetIndex-END_LANE_INDEX))){
mode =2;
distanceMultiplier = 50;
} else if((autoOn&¤tTargetIndex!=0)){
mode =3;
distanceMultiplier = 12;
}
flagPointDistance = D((currentXY.x-flagXY.x),(currentXY.y-flagXY.y));// basically the distance formula, but to what? what flags GPS point?
if(allTargetsReached&&flagPointDistance<FLAG_DIST_THRESH){
mode =4;// what is mode
}
#ifdef FLAG_TESTING
/*FLAG TESTING*/
mode=4;
#endif //FLAG_TESTING
/*Current Target Heading PID Control Adjustment*/
cvar.lookAhead = 0.00;//?
cvar.kP = 0.20; cvar.kI = 0.000; cvar.kD = 0.15;
turn = cvar.turn;
int bestVisGpsMask = 99;
int h = 0;
double minVisGpsTurn = 9999;
for(h=0;h<11;h++){
if(fabs((cvar.turn-turn_angle[h]))<minVisGpsTurn){
minVisGpsTurn=fabs((cvar.turn-turn_angle[h]));
bestVisGpsMask = h;
}
}
bestGpsMask = bestVisGpsMask;
// printf("bvg: %d \n", bestVisGpsMask);
// printf("vgt: %f cv3: %f\n", minVisGpsTurn,cvar3.turn);
#ifdef USE_VISION
// double visTurnBoost = 0.50;
double visTurnBoost = 1.0;
if(imageProc(mode) == -1) break;
if(mode==1||mode==2){
turn = turn_angle[bestmask];
turn *= visTurnBoost;
}else if(mode==3 && fabs(turn_angle[bestmask])>0.70){
turn = turn_angle[bestmask];
turn *= visTurnBoost;
}
#endif //USE_VISION
#ifdef USE_LIDAR
updateSick();
// findObjects();
#endif //USE_LIDAR
#ifdef USE_COMBINED_BUFFER//??????????
#define WORSTTHRESH 10
#define BESTTHRESH 3
if(mode==4){
#ifdef USE_NORTH
turn = (0.5*turn_angle[bestBlueMask]+0.5*turn_angle[bestRedMask]);
#else
turn = (0.65*turn_angle[bestBlueMask]+0.35*turn_angle[bestRedMask]);
#endif
turn *= 0.75;
}
combinedTargDist = cvar.targdist;
if(((approachingTarget||inLastTarget)&¤tTargetIndex>OPEN_FIELD_INDEX
&¤tTargetIndex<maxTargetIndex-END_LANE_INDEX)||(MAG(howbad[worstmask]-howbad[bestmask]))<BESTTHRESH||mode==4){
getCombinedBufferAngles(0,0);//Don't Use Vision Radar Data
}else{
getCombinedBufferAngles(0,1);//Use Vision Radar Data
}
if(combinedBufferAngles.left != 0 || combinedBufferAngles.right !=0){
if(mode == 1 || mode==2 || mode==3 || mode==4){
// if(mode == 1 || mode==2 || mode==3){
// if(mode==2 || mode==3){
// if(mode==3){
if(fabs(combinedBufferAngles.right)==fabs(combinedBufferAngles.left)){
double revTurn;
double revDistLeft, revDistRight;
int revIdx;
if(fabs(turn)<0.10) dir = -1.0;
if(fabs(combinedBufferAngles.left)>1.25) dir = -1.0;
if(dir<0){
revIdx = 540-RAD2DEG(combinedBufferAngles.left)*4;
revIdx = MIN(revIdx,1080);
revIdx = MAX(revIdx,0);
revDistLeft = LMSdata[revIdx];
revIdx = 540-RAD2DEG(combinedBufferAngles.right)*4;
revIdx = MIN(revIdx,1080);
revIdx = MAX(revIdx,0);
revDistRight = LMSdata[revIdx];
if(revDistLeft>=revDistRight){
revTurn = combinedBufferAngles.left;
}else {
revTurn = combinedBufferAngles.right;
}
turn = revTurn;
}else{
turn = turn_angle[bestmask];
}
} else if(fabs(combinedBufferAngles.right-turn)<fabs(combinedBufferAngles.left-turn)){
// } else if(turn<=0){
turn = combinedBufferAngles.right;
}else {
turn = combinedBufferAngles.left;
}
}
}
#endif //USE_COMBINED_BUFFER
if(dir<0||revFrameCount!=0){
dir = -1.0;
revFrameCount = (revFrameCount+1)%REVFRAMES;
}
// turn *= dir;
turn = SIGN(turn) * MIN(fabs(turn), 1.0);
speed = 1.0/(1.0+1.0*fabs(turn))*dir;
speed = SIGN(speed) * MIN(fabs(speed), 1.0);
if(!autoOn){
maxSpeed = 60;
targetIndexMem = 0;
}else if(dir<0){
maxSpeed = 30;
}else if(mode<=2||(mode==3 && fabs(turn_angle[bestmask])>0.25)){
maxSpeed = 60 - 25*fabs(turn);
// maxSpeed = 70 - 35*fabs(turn);
// maxSpeed = 90 - 50*fabs(turn);
// maxSpeed = 100 - 65*fabs(turn);
}else if(mode==4){
maxSpeed = 45-20*fabs(turn);
}else{
maxSpeed = 85 - 50*fabs(turn);
// maxSpeed = 100 - 65*fabs(turn);
// maxSpeed = 110 - 70*fabs(turn);
// maxSpeed = 120 - 85*fabs(turn);
}
if(autoOn){
lSpeed = (speed + turnBoost*turn) * maxSpeed;
rSpeed = (speed - turnBoost*turn) * maxSpeed;
}
#ifdef DEBUG_MAIN
printf("s:%.4f t: %.4f m: %d vt:%f dir:%f tmr: %f\n", speed, turn, mode, turn_angle[bestmask], flagPointDistance, totalAutoTime);
#endif //DEBUG_MAIN
#ifdef DUMP_GPS
if(dumpCount==0){
if (fp != NULL) {
fprintf(fp, "%f %f %f %f %f\n",gpsvar.latitude,gpsvar.longitude, gpsvar.course, gpsvar.speed, gpsvar.time);
}
}
dumpCount = dumpCount+1%DUMPGPSDELAY;
#endif //DUMP_GPS
#ifdef DEBUG_TARGET
debugTarget();
#endif //DEBUG_TARGET
#ifdef DEBUG_GUIDE
debugGuide();
#endif //DEBUG_GUIDE
#ifdef DEBUG_GPS
debugGPS();
#endif //DEBUG_GPS
#ifdef DEBUG_LIDAR
debugSICK();
#endif //DEBUG_LIDAR
#ifdef DEBUG_BUFFER
debugCombinedBufferAngles();
#endif //DEBUG_BUFFE
#ifdef DEBUG_VISUALIZER
robotX = currentXY.x;
robotY = currentXY.y;
robotTheta = robotTheta;//redundant I know....
targetX = targetXY.x;
targetY = targetXY.y;
// should probably pass the above to the function...
paintPathPlanner(robotX,robotY,robotTheta);
showPlot();
#endif //VISUALIZER
#ifdef USE_MAP
if(mapCount==0){
// mapRobot(currentXY.x,currentXY.y,robotTheta);
if(clearMapCount==0) clearMapSection(currentXY.x,currentXY.y,robotTheta);
else clearMapCount = (clearMapCount+1)%CLEARMAPDELAY;
mapVSICK(currentXY.x,currentXY.y,robotTheta);
// mapVSICK(0,0,0);
#ifdef USE_LIDAR
mapSICK(currentXY.x,currentXY.y,robotTheta);
#endif
showMap();
// printf("MAPPING\n");
}
mapCount= (mapCount+1)%MAPDELAY;
#endif //USE_MAP
sendSpeed(lSpeed,rSpeed);
Sleep(5);
}
#ifdef DUMP_GPS
fclose(fp);
#endif
return 0;
}
int
main(int argc, const char *argv[]) {
const char *me;
hestOpt *hopt;
hestParm *hparm;
airArray *mop;
char **ninStr, *err, *outS, doneStr[13];
Nrrd *nin0, *nin, *nrgb, *nout, *nhist[2], *npreout, *nhproj[3];
float *rgb;
float *out, *preout, *hist[2], maxSum,
upSample, overSampleScale;
unsigned int size0, sX, sY, sH, ninLen, ti, overSampleNum;
NrrdResampleContext *rsmc;
NrrdKernelSpec *ksp;
me = argv[0];
mop = airMopNew();
hopt = NULL;
hparm = hestParmNew();
airMopAdd(mop, hparm, (airMopper)hestParmFree, airMopAlways);
hparm->respFileEnable = AIR_TRUE;
hestOptAdd(&hopt, "i", "images", airTypeString, 1, -1, &ninStr, NULL,
"input image sequence", &ninLen, NULL, NULL);
hestOptAdd(&hopt, "sh", "histo size", airTypeUInt, 1, 1, &sH, "500",
"histogram size");
hestOptAdd(&hopt, "k", "kern", airTypeOther, 1, 1, &ksp,
"tent", "kernel for upsampling images",
NULL, NULL, nrrdHestKernelSpec);
hestOptAdd(&hopt, "us", "upsampling", airTypeFloat, 1, 1, &upSample,
"1", "amount of upsampling of image");
hestOptAdd(&hopt, "osn", "# oversmp", airTypeUInt, 1, 1, &overSampleNum,
"1", "number of sample per (upsampled) pixel");
hestOptAdd(&hopt, "osc", "scaling", airTypeFloat, 1, 1, &overSampleScale,
"1", "scaling with oversampling");
hestOptAdd(&hopt, "ms", "max sum", airTypeFloat, 1, 1, &maxSum,
"10", "per-hue histogram summation is non-linearly and "
"asymptotically clamped to this maximum");
hestOptAdd(&hopt, "o", "nout", airTypeString, 1, 1, &outS, "-",
"output filename", NULL);
hestParseOrDie(hopt, argc-1, argv+1, hparm,
me, mchistInfo, AIR_TRUE, AIR_TRUE, AIR_TRUE);
airMopAdd(mop, hopt, (airMopper)hestOptFree, airMopAlways);
airMopAdd(mop, hopt, (airMopper)hestParseFree, airMopAlways);
if (0 == overSampleNum) {
fprintf(stderr, "%s: overSampleNum must be > 0\n", me);
airMopError(mop);
return 1;
}
nin0 = nrrdNew();
airMopAdd(mop, nin0, (airMopper)nrrdNuke, airMopAlways);
if (nrrdLoad(nin0, ninStr[0], NULL)) {
airMopAdd(mop, err=biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: couldn't load first image:\n%s", me, err);
airMopError(mop);
return 1;
}
if (!( (3 == nin0->axis[0].size || 4 == nin0->axis[0].size)
&& 3 == nin0->dim
&& nrrdTypeUChar == nin0->type )) {
fprintf(stderr, "%s: 1st image not 3D (3-or-4)-by-X-by-Y %s array "
"(got %u-D %s array)\n", me,
airEnumStr(nrrdType, nrrdTypeUChar),
nin0->dim,
airEnumStr(nrrdType, nin0->type));
airMopError(mop);
return 1;
}
rsmc = nrrdResampleContextNew();
airMopAdd(mop, rsmc, (airMopper)nrrdResampleContextNix, airMopAlways);
size0 = AIR_CAST(unsigned int, nin0->axis[0].size);
sX = AIR_CAST(unsigned int, upSample*nin0->axis[1].size);
sY = AIR_CAST(unsigned int, upSample*nin0->axis[2].size);
nrgb = nrrdNew();
airMopAdd(mop, nrgb, (airMopper)nrrdNuke, airMopAlways);
if (nrrdResampleDefaultCenterSet(rsmc, nrrdCenterCell)
|| nrrdResampleInputSet(rsmc, nin0)
|| nrrdResampleKernelSet(rsmc, 1, ksp->kernel, ksp->parm)
|| nrrdResampleKernelSet(rsmc, 2, ksp->kernel, ksp->parm)
|| nrrdResampleSamplesSet(rsmc, 1, sX)
|| nrrdResampleSamplesSet(rsmc, 2, sY)
|| nrrdResampleRangeFullSet(rsmc, 1)
|| nrrdResampleRangeFullSet(rsmc, 2)
|| nrrdResampleTypeOutSet(rsmc, nrrdTypeFloat)
|| nrrdResampleRenormalizeSet(rsmc, AIR_TRUE)
|| nrrdResampleExecute(rsmc, nrgb)) {
airMopAdd(mop, err=biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: error resampling slice:\n%s", me, err);
airMopError(mop);
return 1;
}
nhist[0] = nrrdNew();
airMopAdd(mop, nhist[0], (airMopper)nrrdNuke, airMopAlways);
nhist[1] = nrrdNew();
airMopAdd(mop, nhist[1], (airMopper)nrrdNuke, airMopAlways);
if (nrrdMaybeAlloc_va(nhist[0], nrrdTypeFloat, 2,
AIR_CAST(size_t, sH),
AIR_CAST(size_t, sH))
|| nrrdMaybeAlloc_va(nhist[1], nrrdTypeFloat, 2,
AIR_CAST(size_t, sH),
AIR_CAST(size_t, sH))) {
airMopAdd(mop, err=biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: error allocating histos:\n%s", me, err);
airMopError(mop);
return 1;
}
nhist[0]->axis[0].min = nhist[0]->axis[1].min = 0.0;
nhist[0]->axis[0].max = nhist[0]->axis[1].max = 1.0;
nhist[1]->axis[0].min = nhist[1]->axis[1].min = 0.0;
nhist[1]->axis[0].max = nhist[1]->axis[1].max = 1.0;
nhproj[0] = nrrdNew();
airMopAdd(mop, nhproj[0], (airMopper)nrrdNix, airMopAlways);
nhproj[1] = nrrdNew();
airMopAdd(mop, nhproj[1], (airMopper)nrrdNix, airMopAlways);
nhproj[2] = nrrdNew();
airMopAdd(mop, nhproj[2], (airMopper)nrrdNix, airMopAlways);
printf("working ... ");
hist[0] = AIR_CAST(float *, nhist[0]->data);
hist[1] = AIR_CAST(float *, nhist[1]->data);
nin = nrrdNew();
airMopAdd(mop, nin, (airMopper)nrrdNuke, airMopAlways);
npreout = nrrdNew();
airMopAdd(mop, npreout, (airMopper)nrrdNuke, airMopAlways);
if (nrrdMaybeAlloc_va(npreout, nrrdTypeFloat, 3,
AIR_CAST(size_t, 3),
AIR_CAST(size_t, sH),
AIR_CAST(size_t, ninLen))) {
airMopAdd(mop, err=biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: error allocating pre-output:\n%s", me, err);
airMopError(mop);
return 1;
}
preout = AIR_CAST(float *, npreout->data);
for (ti=0; ti<ninLen; ti++) {
printf("%s", airDoneStr(0, ti, ninLen, doneStr)); fflush(stdout);
if (nrrdLoad(nin, ninStr[ti], NULL)) {
airMopAdd(mop, err=biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: couldn't load image[%u]:\n%s", me, ti, err);
airMopError(mop);
return 1;
}
if (!nrrdSameSize(nin0, nin, AIR_TRUE)) {
airMopAdd(mop, err=biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: nin[%u] not like nin[0]:\n%s", me, ti, err);
airMopError(mop);
return 1;
}
if (nrrdResampleInputSet(rsmc, nin)
|| nrrdResampleExecute(rsmc, nrgb)) {
airMopAdd(mop, err=biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: trouble resampling nin[%u]:\n%s", me, ti, err);
airMopError(mop);
return 1;
}
if (nrrdWrap_va(nhproj[0], preout + 0*sH, nrrdTypeFloat, 1, AIR_CAST(size_t, sH)) ||
nrrdWrap_va(nhproj[1], preout + 1*sH, nrrdTypeFloat, 1, AIR_CAST(size_t, sH)) ||
nrrdWrap_va(nhproj[2], preout + 2*sH, nrrdTypeFloat, 1, AIR_CAST(size_t, sH))) {
airMopAdd(mop, err=biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: trouble wrapping output[%u]:\n%s", me, ti, err);
airMopError(mop);
return 1;
}
rgb = AIR_CAST(float *, nrgb->data);
imageProc(nhproj, nhist, sH,
rgb, size0, sX*sY,
overSampleNum, overSampleScale);
preout += 3*sH;
}
printf("%s\n", airDoneStr(0, ti, ninLen, doneStr)); fflush(stdout);
nout = nrrdNew();
airMopAdd(mop, nout, (airMopper)nrrdNuke, airMopAlways);
if (nrrdMaybeAlloc_va(nout, nrrdTypeFloat, 3,
AIR_CAST(size_t, 3),
AIR_CAST(size_t, sH),
AIR_CAST(size_t, ninLen))) {
airMopAdd(mop, err=biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: error allocating output:\n%s", me, err);
airMopError(mop);
return 1;
}
out = AIR_CAST(float *, nout->data);
preout = AIR_CAST(float *, npreout->data);
for (ti=0; ti<ninLen; ti++) {
unsigned int hi;
float hh, vv, ss, scl;
for (hi=0; hi<sH; hi++) {
hh = AIR_AFFINE(0, hi, sH, 0, 1);
if (!preout[hi + 2*sH]) {
ELL_3V_SET(out + 3*hi, 0, 0, 0);
} else {
ss = preout[hi + 2*sH];
scl = ss/(maxSum + ss);
vv = scl*preout[hi + 1*sH];
dyeHSVtoRGB(out + 0 + 3*hi, out + 1 + 3*hi, out + 2 + 3*hi,
hh, preout[hi + 0*sH], vv);
}
}
out += 3*sH;
preout += 3*sH;
}
if (nrrdSave(outS, nout, NULL)) {
airMopAdd(mop, err=biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: error saving output:\n%s", me, err);
airMopError(mop);
return 1;
}
airMopOkay(mop);
return 0;
}
