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); } }
private void convertFile(DataInputStream ins, FileWriter fout, FeatureCategory type) { BinaryStructureReader bsr = new BinaryStructureReader(ins); PrintWriter pwout; try { pwout = new PrintWriter(fout); } catch (Exception ioex) { ioex.printStackTrace(); return; } try { while (true) { // Read in data int nlabels = bsr.readInt(); bsr.blockBegin(); ArrayList<String> labels = new ArrayList<String>(); for (int i = 0; i < nlabels; i++) { String label = bsr.readString(); labels.add(label); } bsr.blockEnd(); int npoints = bsr.readInt(); ArrayList<double[]> points = new ArrayList<double[]>(); bsr.blockBegin(); for (int i = 0; i < npoints; i++) { points.add(bsr.readDoubles()); } bsr.blockEnd(); ArrayList<Double> features = Features.getFeatures(type, points); String featureString = FEUtil.featuresToString(features); boolean hasLabel = false; for (int i = 0; i < labels.size(); i++) { String featureSymbol = SoarSymbols.getSymbol(type, labels.get(i).toLowerCase()); if (featureSymbol != null) { featureString += String.format(" {%s}\n", featureSymbol); hasLabel = true; break; } } if (hasLabel) { pwout.print(featureString); pwout.flush(); } } } catch (Exception ex) { } }
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"); } } } } }