예제 #1
0
/**
 * 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);
}
예제 #2
0
파일: plot.c 프로젝트: SCIInstitute/Cleaver
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;
}
예제 #3
0
파일: plot.c 프로젝트: coffenbacher/EDLPlot
/*----------------------------------------------------------------------*/
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*/