Пример #1
0
  public static void main(String[] args) {
    GetOpt opts = new GetOpt();

    opts.addBoolean('h', "help", false, "Show this help screen");
    opts.addString('i', "infile", null, "Input .pts file");
    opts.addString('o', "outfile", null, "Output feature vector file");
    opts.addString('t', "type", "shape", "Type of features to extract: {color, shape}");

    if (!opts.parse(args)) {
      System.err.println("ERR: " + opts.getReason());
      System.exit(1);
    }
    if (opts.getBoolean("help")) {
      opts.doHelp();
      System.exit(1);
    }

    PtsFileConverter pfc = new PtsFileConverter(opts);
  }
Пример #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");
          }
        }
      }
    }
  }
Пример #3
0
public class BallDetector extends Thread {
  Context ctx;
  Device kinect;

  JFrame controlFrame;
  JFrame colorFrame;
  JFrame depthFrame;

  // JImage rgbJim;
  // JImage depthJim;

  JButton startTracking;
  JButton resetProjectile;
  JButton resetDepth;
  boolean tracking = false;
  boolean log = false;

  ParameterGUI pg;

  BufferedImage rgbImg;
  BufferedImage depthImg;

  Statistics BALL;
  boolean[] validImageValue;

  static double x_param = 0d;
  static int y_param = 0;

  LCM lcm;

  final boolean colorAnalyze = false;
  final boolean colorAnalyze2 = true;
  boolean display = false;

  volatile long globalTime = 0;
  volatile long startTime = System.nanoTime();

  final boolean verbose = false;

  volatile boolean newImage = false;
  public Point3D botStart;

  BallTracker finder;

  KinectRGBVideo colorStream;
  Object colorMonitor;
  KinectDepthVideo depthStream;
  Object depthMonitor;

  BallDetector(boolean _display) {

    ctx = Freenect.createContext();
    if (ctx.numDevices() > 0) {
      kinect = ctx.openDevice(0);
    } else {
      System.err.println("WARNING: No kinects detected");
      return;
    }
    display = _display;

    controlFrame = new JFrame("Controls");
    controlFrame.setLayout(new GridLayout(5, 1));
    controlFrame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
    pg = new ParameterGUI();
    pg.addIntSlider("maxDepth", "max depth", 800, 2047, 1050);
    pg.addIntSlider("blobThresh", "blob thresh", 1, 500, 125);
    pg.addIntSlider("thresh", "thresh", 1, 100, 10);
    pg.addIntSlider("frames", "frames", 1, 1000, 1);
    pg.addListener(
        new ParameterListener() {
          public void parameterChanged(ParameterGUI _pg, String name) {
            if (name.equals("thresh")) {
              KinectDepthVideo.THRESH = _pg.gi(name);
            } else if (name.equals("frames")) {
              KinectDepthVideo.MAX_FRAMES = _pg.gi(name);
            } else if (name.equals("maxDepth")) {
              KinectDepthVideo.MAX_DEPTH = _pg.gi(name);
            }
          }
        });
    controlFrame.add(pg, 0, 0);

    startTracking = new JButton("Start Tracking Balls");
    startTracking.addActionListener(
        new ActionListener() {

          public void actionPerformed(ActionEvent e) {
            if (!tracking) {
              tracking = true;
              colorStream.pause();
              depthStream.pause();
              startTracking.setText("Stop Tracking");
            } else {
              tracking = false;
              colorStream.resume();
              depthStream.resume();
              startTracking.setText("Start Tracking Balls");
            }
          }
        });
    controlFrame.add(startTracking, 1, 0);

    resetProjectile = new JButton("Reset Projectile");
    resetProjectile.addActionListener(
        new ActionListener() {
          public void actionPerformed(ActionEvent e) {
            try {
              lcm.publish("6_RESET", "reset");
            } catch (IOException ex) {
              System.out.println("can't publish reset");
            }
          }
        });
    controlFrame.add(resetProjectile, 2, 0);
    resetDepth = new JButton("Reset Depth Avgs");
    resetDepth.addActionListener(
        new ActionListener() {
          public void actionPerformed(ActionEvent e) {
            DepthClearer ic = new DepthClearer(pg);
            ic.start();
          }
        });
    controlFrame.add(resetDepth, 3, 0);
    JPanel scoreButtons = new JPanel(new GridLayout(1, 3));
    JButton addHuman = new JButton("human++");
    addHuman.addActionListener(
        new ActionListener() {
          public void actionPerformed(ActionEvent e) {
            try {
              lcm.publish("6_SCORE_HUMAN", "bish");
            } catch (IOException ex) {
              System.out.println("can't publish score");
            }
          }
        });
    JButton addRobot = new JButton("robot++");
    addRobot.addActionListener(
        new ActionListener() {
          public void actionPerformed(ActionEvent e) {
            try {
              lcm.publish("6_SCORE_ROBOT", "bish");
            } catch (IOException ex) {
              System.out.println("can't publish score");
            }
          }
        });
    JButton resetScores = new JButton("reset scores");
    resetScores.addActionListener(
        new ActionListener() {
          public void actionPerformed(ActionEvent e) {
            try {
              lcm.publish("6_SCORE_RESET", "bish");
            } catch (IOException ex) {
              System.out.println("can't publish score");
            }
          }
        });
    scoreButtons.add(addHuman, 0, 0);
    scoreButtons.add(addRobot, 0, 1);
    scoreButtons.add(resetScores, 0, 2);
    controlFrame.add(scoreButtons, 4, 0);
    controlFrame.setSize(800, 600);
    controlFrame.setVisible(true);

    colorFrame = new JFrame("color feed");
    colorMonitor = new Object();
    colorStream = new KinectRGBVideo(kinect, colorMonitor, display);
    colorFrame.setDefaultCloseOperation(JFrame.DO_NOTHING_ON_CLOSE);
    colorFrame.addWindowListener(new RGBClose());
    colorFrame.setSize(KinectVideo.WIDTH, KinectVideo.HEIGHT);
    colorFrame.setContentPane(colorStream);
    colorFrame.setVisible(true);

    depthFrame = new JFrame("depth feed");
    depthMonitor = new Object();
    depthStream = new KinectDepthVideo(kinect, depthMonitor, display);
    depthFrame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
    depthFrame.setSize(KinectVideo.WIDTH, KinectVideo.HEIGHT);
    depthFrame.setContentPane(depthStream);
    depthFrame.setVisible(true);

    rgbImg = new BufferedImage(640, 480, BufferedImage.TYPE_INT_ARGB);
    depthImg = new BufferedImage(640, 480, BufferedImage.TYPE_INT_ARGB);

    validImageValue = new boolean[KinectVideo.WIDTH * KinectVideo.HEIGHT];
    try {
      lcm = new LCM("udpm://239.255.76.67:7667?ttl=1");
    } catch (IOException e) {
      lcm = LCM.getSingleton();
    }
    BALL = new Statistics();

    finder = new BallTracker(KinectVideo.WIDTH, KinectVideo.HEIGHT, false);

    if (display) {
      depthImg = depthStream.getFrame();
      rgbImg = colorStream.getFrame();
    }
    // get robot position from click
    depthStream.addMouseListener(
        new MouseAdapter() {
          public void mouseClicked(MouseEvent e) {
            Point botPix = e.getPoint();
            botStart = depthStream.getWorldCoords(botPix);
            botStart.z += 0.08;
            System.out.println("botStart: " + botStart.toString());
            depthStream.showSubtraction();
            depthStream.botLoc = botPix;
          }
        });
    DepthClearer ic = new DepthClearer(pg);
    ic.start();
  }

  // stops rgb feed if user closes rgb window
  public class RGBClose implements WindowListener {

    public void windowClosing(WindowEvent e) {
      kinect.stopVideo();
      colorFrame.dispose();
    }

    public void windowOpened(WindowEvent e) {}

    public void windowClosed(WindowEvent e) {}

    public void windowActivated(WindowEvent e) {}

    public void windowDeactivated(WindowEvent e) {}

    public void windowIconified(WindowEvent e) {}

    public void windowDeiconified(WindowEvent e) {}
  }

  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");
          }
        }
      }
    }
  }

  public float raw_depth_to_meters(int raw_depth) {
    if (raw_depth < 2047) {
      return (1.0f / (raw_depth * -0.0030711016f + 3.3309495161f)) + .05f;
    }
    return 0;
  }

  // utility functions to copy/paste later
  public int getDepth(ByteBuffer bb, int index) {
    int depth = 0;
    byte byte1 = bb.get(index * 2);
    byte byte2 = bb.get(index * 2 + 1);
    depth = byte2 & 0x3;
    depth = depth << 8;
    depth = depth | (byte1 & 0xFF);
    return depth & 0x3FF;
  }

  public void setLog(boolean b) {
    log = b;
  }

  public class DepthClearer extends Thread {
    ParameterGUI pg;

    public DepthClearer(ParameterGUI _pg) {
      pg = _pg;
    }
    // set max_frames to 1 for 5 seconds then move up to 200
    public void run() {
      pg.si("frames", 1);
      try {
        Thread.sleep(5000);
      } catch (Exception e) {
        e.printStackTrace();
      }
      for (int i = 1; i <= 100; i++) {
        pg.si("frames", 2 * i);
        try {
          Thread.sleep(10);
        } catch (Exception e) {
          e.printStackTrace();
        }
      }
    }
  }

  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);
    }
  }

  public static void main(String[] args) {
    //		Projectile proj = new Projectile();
    BallDetector kv = new BallDetector(true);
    //		for(int i = 0; i < args.length; i++)
    //		{
    //			if(args[i].equals("log"))
    //				kv.setLog(true);
    //		}
    kv.start();
  }
}