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