Пример #1
0
 public void plotProjection(ArrayList<double[]> pballs) {
   for (int i = 0; i < pballs.size(); i++) {
     double xyzt[] = pballs.get(i);
     // switch y and z;
     double temp = xyzt[2];
     xyzt[2] = xyzt[1];
     xyzt[1] = temp;
     Point3D realWorld = new Point3D(xyzt[0], xyzt[1], xyzt[2]);
     // Point pixCoords = depthgetPixFromWorld(realWorld);
     // depthImg.setRGB(pixCoords.x,pixCoords.y,0xFFFFFFFF);
   }
 }
Пример #2
0
  public void run() {
    while (true) {
      BALL = null;
      ball_t ballLCM = new ball_t();

      synchronized (depthMonitor) {
        try {
          depthMonitor.wait();
        } catch (Exception e) {
          e.printStackTrace();
        }
      }
      ballLCM.nanoTime = depthStream.latestTimestamp;
      ArrayList<Statistics> blobs;
      depthStream.getReadLock().lock();
      try {
        blobs = finder.analyze2(depthStream.getValidImageArray());
      } finally {
        depthStream.getReadLock().unlock();
      }
      Statistics ball = null;
      Statistics robot = null;
      int minSize = pg.gi("blobThresh");

      // find robot and ball by blob size
      Collections.sort(blobs, ComparatorFactory.getStatisticsCompareSize());
      // find robot and ball by y pixel
      // Collections.sort(blobs, ComparatorFactory.getStatisticsCompareYPix());
      //			if (tracking) {
      //				System.out.println("num blobs: " + blobs.size());
      //				System.out.println("biggest blob size: " + blobs.get(0).N);
      //			}
      //			for (Statistics blob : blobs) {
      //				if (blob.N > 10) {
      //					System.out.println("blob size: " + blob.N);
      //				}
      //				else {
      //					break;
      //				}
      //			}
      if (blobs.size() == 1) {
        Statistics first = blobs.get(0);
        if (first.N > minSize) {
          ball = first;
        }
      } else if (blobs.size() >= 2) {
        Statistics first = blobs.get(0);
        Statistics second = blobs.get(1);
        if (first.N > minSize) {
          ball = first;
        }
        if (second.N > minSize) {
          robot = first;
          ball = second;
        }
      }

      // System.out.println("balls points " + depthStream.trajectory.size());

      // if not tracking keep kv.depthStream.trajectory to just one index
      if (!tracking) {
        depthStream.trajectory.clear();
      }

      if (ball != null) {
        depthStream.trajectory.add(ball);

        Point depthPix = ball.center();
        Point depthCoord = new Point();
        depthCoord.x = depthPix.x - KinectVideo.C_X;
        depthCoord.y = KinectVideo.C_Y - depthPix.y;

        // System.out.println("avg depth " + ball.Uz());

        // get depth from average depth of blob
        double realDepth = raw_depth_to_meters(ball.Uz());
        Point3D coord = depthStream.getWorldCoords(depthCoord, realDepth);
        if (depthPix != null) {
          for (int y = depthPix.y - 3; y < depthPix.y + 3; y++) {
            for (int x = depthPix.x - 3; x < depthPix.x + 3; x++) {
              try {
                depthImg.setRGB(x, y, 0xFFFF0000);
              } catch (Exception e) {
                // System.out.println(x + " " + y);
              }
              ;
            }
          }
          // if (tracking) {
          // 	//save image
          // 	depthStream.getReadLock().lock();
          // 	try {
          // 		File imgFile = new File("image" + ballNum++ + ".png");
          // 		try {
          // 			ImageIO.write(depthImg, "png", imgFile);
          // 		}
          // 		catch(Exception e) {
          // 			System.out.println("can't save img");
          // 		}
          // 	}
          // 	finally {
          // 		depthStream.getReadLock().unlock();
          // 	}
          // }

          if (tracking) {
            ballLCM.x = coord.x;
            ballLCM.y = coord.y;
            ballLCM.z = coord.z;
            // if(tracking)
            System.out.println("updating new ball (" + System.currentTimeMillis() + ")");
            //						if (ballLCM.x > CatchController.TARGET_MAX_X)
            lcm.publish("6_BALL", ballLCM);
            //						else
            //							System.out.println("ball past target zone");
          }
        }
      }
    }
  }