/**
* This is the function that plots the helicopter AND the object of interest,
* along with its normal.
*
* @param objectPosition Position of an object
* @param objectNormal Normal of an object
* @param quadPosition Position of the quad
* @param quadOrientation Roll, Pitch, and Yaw of the quad.
*/
void WorldPlotter::plotTopView(Point3f objectPosition, Point3f objectNormal,
Point3f quadPosition, Point3f quadOrientation) {
Mat plot = Mat::zeros(plot_size_y, plot_size_x, CV_8UC3);
plotAxes(plot);
// Plot Normal Vector
Point2i object_normal_p1, object_normal_p2;
object_normal_p1.x = objectPosition.x / real_size_x * plot_size_x / 1000.0f
+ plot_size_x / 2;
object_normal_p1.y = objectPosition.y / real_size_y * plot_size_y / 1000.0f
+ plot_size_y / 2;
object_normal_p2.x = object_normal_p1.x - 25 * objectNormal.x;
object_normal_p2.y = object_normal_p1.y - 25 * objectNormal.y;
object_trace.push_back(object_normal_p1);
plotTrace(plot, object_trace, object_color);
line(plot, object_normal_p1, object_normal_p2, normal_color,
normal_thickness);
rectangle(plot,
Point2i(object_normal_p1.x - object_size,
object_normal_p1.y - object_size),
Point2i(object_normal_p1.x + object_size,
object_normal_p1.y + object_size),
object_color, object_thickness);
float x, y, z;
x = quadPosition.x;
y = quadPosition.y;
z = quadPosition.z;
float roll, pitch, yaw;
roll = quadOrientation.x;
pitch = quadOrientation.y;
yaw = quadOrientation.z;
float q_x, q_y;
q_x = (x + 0.1 * cos(yaw)) / real_size_x * plot_size_x
+ plot_size_x / 2;
q_y = (y + 0.1* sin(yaw)) / real_size_y * plot_size_y
+ plot_size_y / 2;
x = x / real_size_x * plot_size_x + plot_size_x / 2;
y = y / real_size_y * plot_size_y + plot_size_y / 2;
quad_trace.push_back(Point2i(x, y));
plotTrace(plot, quad_trace, quad_color);
rectangle(plot, Point2i(x - object_size, y - object_size),
Point2i(x + object_size, y + object_size), quad_color,
object_thickness);
rectangle(plot, Point2i(q_x - 1, q_y - 1), Point2i(q_x + 1, q_y + 1),
quad_color, object_thickness);
line(plot, Point2i(x, y), Point2i(q_x, q_y), normal_color, normal_thickness);
putText(plot, "Object", Point2i(object_normal_p1.x, object_normal_p1.y - 10),
FONT_HERSHEY_PLAIN, 1, text_color);
putText(plot, "Quad", Point2i(x, y - 10), FONT_HERSHEY_PLAIN, 1, text_color);
putText(plot, "iron curtain", Point2i(plot_size_x / 2 - 37, plot_size_y - 9),
FONT_HERSHEY_PLAIN, 1, Scalar(0, 0, 255));
objectPosition *= 0.001;
Vec3f d = cv::Vec3f(objectPosition) - cv::Vec3f(quadPosition);
Point3f distance = cv::Point3f(sqrt(d[0] * d[0] + d[1] * d[1] + d[2] * d[2]),
0, 0);
Point3f new_yaw = cv::Point3f(0, 0, arcTan(objectNormal.x,
objectNormal.y));
float normalization = sqrt(objectNormal.x * objectNormal.x +
objectNormal.y * objectNormal.y);
float scale = -1.5;
float golden_x = objectPosition.x + scale * objectNormal.x / normalization;
float golden_y = objectPosition.y + scale * objectNormal.y / normalization;
//printf("%f %f \n", golden_x, golden_y);
golden_x = golden_x / real_size_x * plot_size_x + plot_size_x / 2;
golden_y = golden_y / real_size_y * plot_size_y + plot_size_y / 2;
cv::Point2i goldenPoint = cv::Point2i(golden_x, golden_y);
cv::circle(plot, goldenPoint, 2, Scalar(0, 0, 255), 2);
cv::Vector<Point3f> coordinates;
vector<string> labels;
coordinates.push_back(objectPosition);
labels.push_back("P.Object pos.");
coordinates.push_back(quadPosition);
labels.push_back("P.Quad pos.");
coordinates.push_back(quadOrientation);
labels.push_back("O.Quad orient.");
coordinates.push_back(distance);
labels.push_back("D.Distance");
coordinates.push_back(new_yaw);
labels.push_back("P.Goal Yaw");
plotCoordinates(NULL, coordinates, labels);
outputPlot = plot;
namedWindow("Top View Plot");
imshow("Top View Plot", plot);
}
int
main(int argc, char *argv[]) {
char *me, *err, *outS;
hestOpt *hopt=NULL;
airArray *mop;
int numGrth, numDtdi, numGrgr, numDtgr, numNrrd, ni;
plotPS pps;
plotParm pparm;
Nrrd **_ndata, **ndata;
mop = airMopNew();
me = argv[0];
hestOptAdd(&hopt, "i", "data", airTypeOther, 1, -1, &_ndata, NULL,
"input nrrd containing data to plot",
&numNrrd, NULL, nrrdHestNrrd);
hestOptAdd(&hopt, "dbox", "minX minY maxX maxY", airTypeDouble,
4, 4, pparm.dbox, NULL,
"bounding box, in data space");
hestOptAdd(&hopt, "bbox", "minX minY maxX maxY", airTypeDouble,
4, 4, pps.bbox, NULL,
"bounding box, in graph space");
hestOptAdd(&hopt, "psc", "PS scale", airTypeDouble, 1, 1, &(pps.psc), "300",
"scaling from graph space to PostScript points");
hestOptAdd(&hopt, "nobg", NULL, airTypeInt, 0, 0, &(pps.nobg), NULL,
"don't fill with background color");
hestOptAdd(&hopt, "bg", "background", airTypeDouble, 3, 3,
&(pps.bgColor), "1 1 1",
"background RGB color; each component in range [0.0,1.0]");
hestOptAdd(&hopt, "grth", "graph thickness", airTypeDouble,
1, -1, &(pparm.graphThick), "0.01",
"thickness of line for graph, or \"0\" for no graph line",
&numGrth);
hestOptAdd(&hopt, "grgr", "graph gray", airTypeDouble,
1, -1, &(pparm.graphGray), "0",
"grayscale to use for graph", &numGrgr);
hestOptAdd(&hopt, "dtdi", "dot diameter", airTypeDouble,
1, -1, &(pparm.dotDiameter), "0.1",
"radius of dot drawn at data points, or \"0\" for no dots",
&numDtdi);
hestOptAdd(&hopt, "dtgr", "dot gray", airTypeDouble,
1, -1, &(pparm.dotGray), "0",
"grayscale to use for dots", &numDtgr);
hestOptAdd(&hopt, "dtid", "dot inner diam frac", airTypeDouble,
1, 1, &(pparm.dotInnerDiameterFraction), "0.0",
"fractional radius of white dot drawn within dot");
hestOptAdd(&hopt, "tihz", "pos", airTypeDouble,
0, -1, &(pparm.horzTick), "",
"locations for tickmarks on horizontal axis",
&(pparm.numHorzTick));
hestOptAdd(&hopt, "tivt", "pos", airTypeDouble,
0, -1, &(pparm.vertTick), "",
"locations for tickmarks on vertical axis",
&(pparm.numVertTick));
hestOptAdd(&hopt, "tiho", "offset", airTypeDouble,
1, 1, &(pparm.horzTickLabelOffset), "0",
"horizontal tick label offset");
hestOptAdd(&hopt, "tivo", "offset", airTypeDouble,
1, 1, &(pparm.vertTickLabelOffset), "0",
"vertical tick label offset");
hestOptAdd(&hopt, "tils", "size", airTypeDouble,
1, 1, &(pparm.tickLabelSize), "0",
"font size for labels on tick marks, or \"0\" for no labels");
hestOptAdd(&hopt, "tith", "tick thickness", airTypeDouble,
1, 1, &(pparm.tickThick), "0.01",
"thickness of lines for tick marks");
hestOptAdd(&hopt, "tiln", "tick length", airTypeDouble,
1, 1, &(pparm.tickLength), "0.08",
"length of lines for tick marks");
hestOptAdd(&hopt, "axth", "axis thickness", airTypeDouble,
1, 1, &(pparm.axisThick), "0.01",
"thickness of lines for axes");
hestOptAdd(&hopt, "axor", "axis origin", airTypeDouble,
2, 2, &(pparm.axisOrig), "0 0",
"origin of lines for axes, in data space");
hestOptAdd(&hopt, "axhl", "horiz axis label", airTypeString,
1, 1, &(pparm.axisHorzLabel), "",
"label on horizontal axis");
hestOptAdd(&hopt, "axvl", "vert axis label", airTypeString,
1, 1, &(pparm.axisVertLabel), "",
"label on vertical axis");
hestOptAdd(&hopt, "o", "output PS", airTypeString,
1, 1, &outS, "out.ps",
"output file to render postscript into");
hestParseOrDie(hopt, argc-1, argv+1, NULL,
me, info, AIR_TRUE, AIR_TRUE, AIR_TRUE);
airMopAdd(mop, hopt, (airMopper)hestOptFree, airMopAlways);
airMopAdd(mop, hopt, (airMopper)hestParseFree, airMopAlways);
if (!( numGrth == numDtdi
&& numDtdi == numGrgr
&& numGrgr == numDtgr )) {
fprintf(stderr, "%s: number of arguments given to grth (%d), dtdi (%d), "
"grgr (%d), dtgr (%d) not all equal\n", me,
numGrth, numDtdi, numGrgr, numDtgr);
airMopError(mop); return 1;
}
if (!( numNrrd == numGrth )) {
fprintf(stderr, "%s: number of nrrds (%d) != number graph options (%d)\n",
me, numNrrd, numGrth);
airMopError(mop); return 1;
}
/* check nrrds */
for (ni=0; ni<numNrrd; ni++) {
if (!( (1 == _ndata[ni]->dim || 2 == _ndata[ni]->dim)
&& nrrdTypeBlock != _ndata[ni]->type )) {
fprintf(stderr, "%s: input nrrd must be 1-D or 2-D array of scalars",
me);
airMopError(mop); return 1;
}
}
ndata = (Nrrd**)calloc(numNrrd, sizeof(Nrrd *));
airMopAdd(mop, ndata, airFree, airMopAlways);
for (ni=0; ni<numNrrd; ni++) {
ndata[ni] = nrrdNew();
airMopAdd(mop, ndata[ni], (airMopper)nrrdNuke, airMopAlways);
if (nrrdConvert(ndata[ni], _ndata[ni], nrrdTypeDouble)) {
airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: couldn't convert input %d to %s:\n%s\n",
me, ni, airEnumStr(nrrdType, nrrdTypeDouble), err);
airMopError(mop); return 1;
}
if (1 == ndata[ni]->dim) {
if (nrrdAxesInsert(ndata[ni], ndata[ni], 0)) {
airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
fprintf(stderr, "%s: couldn't insert axis 0 on nrrd %d:\n%s\n",
me, ni, err);
airMopError(mop); return 1;
}
}
/* currently assuming node centering */
if (!AIR_EXISTS(ndata[ni]->axis[1].min)) {
ndata[ni]->axis[1].min = 0;
}
if (!AIR_EXISTS(ndata[ni]->axis[1].max)) {
ndata[ni]->axis[1].max = ndata[ni]->axis[1].size-1;
}
}
if (!(pps.file = airFopen(outS, stdout, "wb"))) {
fprintf(stderr, "%s: couldn't open output file\n", me);
airMopError(mop); return 1;
}
airMopAdd(mop, pps.file, (airMopper)airFclose, airMopAlways);
plotPreamble(&pps, &pparm);
plotAxes(&pps, &pparm, ndata[0]);
for (ni=0; ni<numNrrd; ni++) {
plotGraph(&pps, &pparm, ndata, ni);
plotDots(&pps, &pparm, ndata, ni);
}
plotEpilog(&pps, &pparm);
airMopOkay(mop);
return 0;
}
/*----------------------------------------------------------------------*/
int main(int argc, char *argv[])
{
SDL_Surface *screen;
int width, height;
Uint8 video_bpp;
Uint32 videoflags;
int done;
SDL_Event event;
Uint32 then, now, frames;
if ( SDL_Init(SDL_INIT_VIDEO) < 0 ) {
fprintf(stderr, "SDL_Init problem: %s", SDL_GetError());
exit(1);
}
atexit(SDL_Quit);
videoflags = SDL_SWSURFACE | SDL_ANYFORMAT;
width = 640;
height = 480;
video_bpp = 0;
while ( argc > 1 ) {
--argc;
if ( strcmp(argv[argc-1], "-width") == 0 ) {
width = atoi(argv[argc]);
--argc;
} else if ( strcmp(argv[argc-1], "-height") == 0 ) {
height = atoi(argv[argc]);
--argc;
} else if ( strcmp(argv[argc-1], "-bpp") == 0 ) {
video_bpp = atoi(argv[argc]);
videoflags &= ~SDL_ANYFORMAT;
--argc;
} else if ( strcmp(argv[argc], "-fast") == 0 ) {
videoflags = FastestFlags(videoflags, width, height, video_bpp);
} else if ( strcmp(argv[argc], "-hw") == 0 ) {
videoflags ^= SDL_HWSURFACE;
} else if ( strcmp(argv[argc], "-flip") == 0 ) {
videoflags ^= SDL_DOUBLEBUF;
} else if ( strcmp(argv[argc], "-fullscreen") == 0 ) {
videoflags ^= SDL_FULLSCREEN;
} else {
fprintf(stderr, "Use: %s [-bpp N] [-hw] [-flip] [-fast] [-fullscreen]\n",
argv[0]);
exit(1);
}
}/*while*/
/*Video mode activation*/
screen = SDL_SetVideoMode(width, height, video_bpp, videoflags);
if (!screen) {
fprintf(stderr, "I can not activate video mode: %dx%d: %s\n",
width, height, SDL_GetError());
exit(2);
}
{
//Define Colors
//1: White
//2: Gray
//3: Dark Gray
//4: Red
//5: Green
//6: Blue
Uint32 c_colors[] = {SDL_MapRGB(screen->format, 255,255,255), SDL_MapRGB(screen->format, 200,200,200),
SDL_MapRGB(screen->format, 64,64,64), SDL_MapRGB(screen->format, 255,0,0),
SDL_MapRGB(screen->format, 0,255,0), SDL_MapRGB(screen->format, 0,0,255)};
plotAxes(screen, c_colors, width, height, -20, 30, -20, 20);
plotLine(screen, c_colors);
SDL_UpdateRect(screen, 0, 0, 0, 0);
frames = 0;
then = SDL_GetTicks();
done = 0;
while( !done ) {
++frames;
while ( SDL_PollEvent(&event) ) {
switch (event.type) {
case SDL_KEYDOWN:
/*break;*/
case SDL_QUIT:
done = 1;
break;
default:
break;
}
}/*while*/
}/*while(!done)*/
}/*END*/
now = SDL_GetTicks();
if ( now > then ) {
printf("%2.2f frames per second\n",
((double)frames*1000)/(now-then));
}
fprintf(stderr, "[END]\n");
return 0;
}/*main*/