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 Buffer getBuffer(String name) {
Buffer b = bufferMap.get(name);
if (b == null) {
b = new Buffer(name);
synchronized (buffers) {
bufferMap.put(name, b);
buffers.add(b);
Collections.sort(buffers);
}
}
return b;
}
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 synchronized void addFront(VisObject vo) {
front.add(vo);
}
public synchronized void clear() {
back.clear();
front.clear();
}
public synchronized void addBack(VisObject vo) {
back.add(vo);
}
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");
}
}
}
}
}
