// TODO(damonkohler): Use an efficient search.
private FrameTransform get(GraphName source, Time time) {
CircularBlockingDeque<LazyFrameTransform> deque = transforms.get(source);
if (deque == null) {
return null;
}
LazyFrameTransform result = null;
synchronized (mutex) {
long offset = 0;
for (LazyFrameTransform lazyFrameTransform : deque) {
if (result == null) {
result = lazyFrameTransform;
offset = Math.abs(time.subtract(result.get().getTime()).totalNsecs());
continue;
}
long newOffset = Math.abs(time.subtract(lazyFrameTransform.get().getTime()).totalNsecs());
if (newOffset < offset) {
result = lazyFrameTransform;
offset = newOffset;
}
}
}
if (result == null) {
return null;
}
return result.get();
}
// @Override
public void onSensorChanged(SensorEvent event) {
if (event.sensor.getType() == Sensor.TYPE_PRESSURE) {
FluidPressure msg = this.publisher.newMessage();
long time_delta_millis = System.currentTimeMillis() - SystemClock.uptimeMillis();
msg.getHeader().setStamp(Time.fromMillis(time_delta_millis + event.timestamp / 1000000));
msg.getHeader().setFrameId("/android/barometric_pressure"); // TODO Make parameter
msg.setFluidPressure(100.0 * event.values[0]); // Reported in hPa, need to output in Pa
msg.setVariance(0.0);
publisher.publish(msg);
}
}
@Override
protected Bitmap processFrame(VideoCapture capture) {
Time[] measureTime = new Time[9];
String[] compDescStrings = {
"Total processFrame",
"Grab a new frame",
"MatToBitmap",
"Publish cameraInfo",
"Create ImageMsg",
"Compress image",
"Transfer to Stream",
"Image.SetData",
"Publish Image",
"Total econds per frame"
};
String[] rawDescStrings = {
"Total processFrame",
"Grab a new frame",
"MatToBitmap",
"Publish cameraInfo",
"Create ImageMsg",
"Pixel to buffer",
"Transfer to Stream",
"Image.SetData",
"Publish Image",
"Total seconds per frame"
};
measureTime[0] = connectedNode.getCurrentTime();
switch (MainActivity.viewMode) {
case MainActivity.VIEW_MODE_GRAY:
// capture.retrieve(mGray, Highgui.CV_CAP_ANDROID_GREY_FRAME);
capture.retrieve(mRgba, Highgui.CV_CAP_ANDROID_GREY_FRAME);
// Imgproc.cvtColor(mGray, mRgba, Imgproc.COLOR_GRAY2RGBA, 4);
break;
case MainActivity.VIEW_MODE_RGBA:
capture.retrieve(mRgba, Highgui.CV_CAP_ANDROID_COLOR_FRAME_RGBA);
// Core.putText(mRgba, "OpenCV + Android", new Point(10, 100), 3, 2, new
// Scalar(255, 0, 0, 255), 3);
break;
case MainActivity.VIEW_MODE_CANNY:
capture.retrieve(mGray, Highgui.CV_CAP_ANDROID_GREY_FRAME);
Imgproc.Canny(mGray, mIntermediateMat, 80, 100);
Imgproc.cvtColor(mIntermediateMat, mRgba, Imgproc.COLOR_GRAY2BGRA, 4);
break;
}
Time currentTime = connectedNode.getCurrentTime();
measureTime[1] = connectedNode.getCurrentTime();
if (bmp == null) bmp = Bitmap.createBitmap(mRgba.cols(), mRgba.rows(), Bitmap.Config.ARGB_8888);
if (MainActivity.imageCompression == MainActivity.IMAGE_TRANSPORT_COMPRESSION_NONE
&& bb == null) {
Log.i(TAG, "Buffer 1");
bb = ByteBuffer.allocate(bmp.getRowBytes() * bmp.getHeight());
Log.i(TAG, "Buffer 2");
bb.clear();
Log.i(TAG, "Buffer 3");
}
try {
Utils.matToBitmap(mRgba, bmp);
measureTime[2] = connectedNode.getCurrentTime();
cameraInfo = cameraInfoPublisher.newMessage();
cameraInfo.getHeader().setFrameId("camera");
cameraInfo.getHeader().setStamp(currentTime);
cameraInfo.setWidth(640);
cameraInfo.setHeight(480);
cameraInfoPublisher.publish(cameraInfo);
measureTime[3] = connectedNode.getCurrentTime();
if (MainActivity.imageCompression >= MainActivity.IMAGE_TRANSPORT_COMPRESSION_PNG) {
// Compressed image
sensor_msgs.CompressedImage image = imagePublisher.newMessage();
if (MainActivity.imageCompression == MainActivity.IMAGE_TRANSPORT_COMPRESSION_PNG)
image.setFormat("png");
else if (MainActivity.imageCompression == MainActivity.IMAGE_TRANSPORT_COMPRESSION_JPEG)
image.setFormat("jpeg");
image.getHeader().setStamp(currentTime);
image.getHeader().setFrameId("camera");
measureTime[4] = connectedNode.getCurrentTime();
ByteArrayOutputStream baos = new ByteArrayOutputStream();
if (MainActivity.imageCompression == MainActivity.IMAGE_TRANSPORT_COMPRESSION_PNG)
bmp.compress(Bitmap.CompressFormat.PNG, 100, baos);
else if (MainActivity.imageCompression == MainActivity.IMAGE_TRANSPORT_COMPRESSION_JPEG)
bmp.compress(Bitmap.CompressFormat.JPEG, MainActivity.imageCompressionQuality, baos);
measureTime[5] = connectedNode.getCurrentTime();
stream.buffer().writeBytes(baos.toByteArray());
measureTime[6] = connectedNode.getCurrentTime();
image.setData(stream.buffer().copy());
measureTime[7] = connectedNode.getCurrentTime();
stream.buffer().clear();
imagePublisher.publish(image);
measureTime[8] = connectedNode.getCurrentTime();
} else {
// Raw image
Log.i(TAG, "Raw image 1");
sensor_msgs.Image rawImage = rawImagePublisher.newMessage();
rawImage.getHeader().setStamp(currentTime);
rawImage.getHeader().setFrameId("camera");
rawImage.setEncoding("rgba8");
rawImage.setWidth(bmp.getWidth());
rawImage.setHeight(bmp.getHeight());
rawImage.setStep(640);
measureTime[4] = connectedNode.getCurrentTime();
Log.i(TAG, "Raw image 2");
bmp.copyPixelsToBuffer(bb);
measureTime[5] = connectedNode.getCurrentTime();
Log.i(TAG, "Raw image 3");
stream.buffer().writeBytes(bb.array());
bb.clear();
measureTime[6] = connectedNode.getCurrentTime();
Log.i(TAG, "Raw image 4");
rawImage.setData(stream.buffer().copy());
stream.buffer().clear();
measureTime[7] = connectedNode.getCurrentTime();
Log.i(TAG, "Raw image 5");
rawImagePublisher.publish(rawImage);
measureTime[8] = connectedNode.getCurrentTime();
Log.i(TAG, "Raw image 6");
}
newTime = connectedNode.getCurrentTime();
stats[9][counter] = (newTime.subtract(oldTime)).nsecs / 1000000.0;
oldTime = newTime;
for (int i = 1; i < 9; i++) {
stats[i][counter] = (measureTime[i].subtract(measureTime[i - 1])).nsecs / 1000000.0;
}
stats[0][counter] = measureTime[8].subtract(measureTime[0]).nsecs / 1000000.0;
counter++;
if (counter == numSamples) {
double[] sts = new double[10];
Arrays.fill(sts, 0.0);
for (int i = 0; i < 10; i++) {
for (int j = 0; j < numSamples; j++) sts[i] += stats[i][j];
sts[i] /= (double) numSamples;
if (MainActivity.imageCompression >= MainActivity.IMAGE_TRANSPORT_COMPRESSION_PNG)
Log.i(TAG, String.format("Mean time for %s:\t\t%4.2fms", compDescStrings[i], sts[i]));
else Log.i(TAG, String.format("Mean time for %s:\t\t%4.2fms", rawDescStrings[i], sts[i]));
}
Log.i(TAG, "\n\n");
counter = 0;
}
return bmp;
} catch (Exception e) {
Log.e(TAG, "Frame conversion and publishing throws an exception: " + e.getMessage());
bmp.recycle();
return null;
}
}
/**
* Returns the transform from the specified source frame to the target frame at a given time;
* returns null if no transformation could be found.
*/
public StampedTransform lookupTransform(String targetFrameID, String sourceFrameID, Time time) {
// resolve the source and target IDs
String resolvedTargetID = assertResolved(tfPrefix, targetFrameID);
String resolvedSourceID = assertResolved(tfPrefix, sourceFrameID);
// if source and target are the same, return the identity transform
if (resolvedSourceID == resolvedTargetID) {
StampedTransform out = StampedTransform.getIdentity();
out.timeStamp = time;
out.frameID = resolvedSourceID;
out.childFrameID = resolvedTargetID;
return out;
}
// lookup source and target frame
Frame sourceFrame = frames.get(resolvedSourceID);
Frame targetFrame = frames.get(resolvedTargetID);
if (sourceFrame == null) {
node.getLog()
.error("Cannot transform: source frame \"" + resolvedSourceID + "\" does not exist.");
return null;
}
if (targetFrame == null) {
node.getLog()
.error("Cannot transform: target frame \"" + resolvedTargetID + "\" does not exist.");
return null;
}
// list that will contain transformations from source frame to some frame F
LinkedList<TransformStorage> inverseTransforms = new LinkedList<TransformStorage>();
// list that will contain transformations from frame F to target frame
LinkedList<TransformStorage> forwardTransforms = new LinkedList<TransformStorage>();
// fill the lists using lookupLists. If it returns FALSE, no transformation could be found.
if (!lookupLists(
targetFrame, sourceFrame, time.totalNsecs(), inverseTransforms, forwardTransforms)) {
// TODO give warning
node.getLog()
.error(
"Cannot transform: source + \""
+ resolvedSourceID
+ "\" and target \""
+ resolvedTargetID
+ "\" are not connected.");
return null;
}
// create an identity transform with the correct time stamp
StampedTransform out = StampedTransform.getIdentity();
out.timeStamp = time;
// multiply all transforms from source frame to frame F TODO: right?
for (TransformStorage t : inverseTransforms) {
out.mul(StorageToStampedTransform(t));
}
// multiply all transforms from frame F to target frame TODO: right?
for (TransformStorage t : forwardTransforms) {
out.mul(StorageToStampedTransform(t).invert(), out);
}
// return transform
return out;
}