List of usage examples for org.opencv.core Core multiply
public static void multiply(Mat src1, Scalar src2, Mat dst)
From source file:Questao1.java
void multiplicacao() { System.loadLibrary(Core.NATIVE_LIBRARY_NAME); Core.multiply(image1, image2, output); Imgcodecs.imwrite("multiplicacao.jpg", output); showResult("multiplicacao.jpg"); }
From source file:arlocros.ArMarkerPoseEstimator.java
License:Apache License
private void start(final ConnectedNode connectedNode) { // load OpenCV shared library System.loadLibrary(Core.NATIVE_LIBRARY_NAME); // read configuration variables from the ROS Runtime (configured in the // launch file) log = connectedNode.getLog();/*from w ww. ja v a 2s. c o m*/ // Read Marker Config markerConfig = MarkerConfig.createFromConfig(parameter.markerConfigFile(), parameter.patternDirectory()); camp = getCameraInfo(connectedNode, parameter); // start to listen to transform messages in /tf in order to feed the // Transformer and lookup transforms final TransformationService transformationService = TransformationService.create(connectedNode); // Subscribe to Image Subscriber<sensor_msgs.Image> subscriberToImage = connectedNode.newSubscriber(parameter.cameraImageTopic(), sensor_msgs.Image._TYPE); ComputePose computePose = null; try { final Mat cameraMatrix = CameraParams.getCameraMatrix(camp); final MatOfDouble distCoeffs = CameraParams.getDistCoeffs(camp); computePose = ComputePose.create(markerConfig, new Size(camp.width(), camp.height()), cameraMatrix, distCoeffs, this.parameter.visualization()); } catch (NyARException e) { logger.info("Cannot initialize ComputePose", e); } catch (FileNotFoundException e) { logger.info("Cannot find file when initialize ComputePose", e); } final ComputePose poseProcessor = computePose; final Publisher<tf2_msgs.TFMessage> tfPublisherCamToMarker = connectedNode.newPublisher("tf", tf2_msgs.TFMessage._TYPE); final Publisher<tf2_msgs.TFMessage> tfPublisherMapToOdom = connectedNode.newPublisher("tf", tf2_msgs.TFMessage._TYPE); logger.info("My instance id is " + parameter.instanceId()); if (heartbeatMonitor != null) { logger.info("Start waiting for arlocros id: " + (parameter.instanceId() - 1)); while (true) { final Time currentTime = connectedNode.getCurrentTime(); final Time lastHeartbeatTime = heartbeatMonitor.getLastTimeReceivedMessage(); if (lastHeartbeatTime != null) { final Duration duration = currentTime.subtract(lastHeartbeatTime); if (duration.totalNsecs() > 3.0E8) { logger.info("Not received any heartbeat for 300ms. Start running."); break; } } } } subscriberToImage.addMessageListener(new MessageListener<sensor_msgs.Image>() { @Override public void onNewMessage(sensor_msgs.Image message) { // if (!message.getEncoding().toLowerCase().equals("rgb8")) { log.error("Sorry, " + message.getEncoding() + " Image encoding is not supported! EXITING"); System.exit(-1); } if (camp != null) { try { // final Mat image = Utils.matFromImage(message); // uncomment to add more contrast to the image final Mat thresholdedImage = Utils.tresholdContrastBlackWhite(image, parameter.filterBlockSize(), parameter.subtractedConstant(), parameter.invertBlackWhiteColor()); image.release(); // Mat cannyimg = new Mat(image.height(), image.width(), // CvType.CV_8UC3); // Imgproc.Canny(image, cannyimg, 10, 100); // Imshow.show(cannyimg); // image.convertTo(image, -1, 1.5, 0); // setup camera matrix and return vectors // compute pose final Mat rvec = new Mat(3, 1, CvType.CV_64F); final MatOfDouble tvec = new MatOfDouble(1.0, 1.0, 1.0); final boolean hasPose = poseProcessor.computePose(rvec, tvec, thresholdedImage); if (!hasPose) { return; } thresholdedImage.release(); // publish pose final QuaternionHelper q = new QuaternionHelper(); // convert rotation vector result of solvepnp to rotation matrix Mat R = new Mat(3, 3, CvType.CV_32FC1); Calib3d.Rodrigues(rvec, R); // see publishers before for documentation final Mat tvec_map_cam = new MatOfDouble(1.0, 1.0, 1.0); R = R.t(); final double bankX = Math.atan2(-R.get(1, 2)[0], R.get(1, 1)[0]); final double headingY = Math.atan2(-R.get(2, 0)[0], R.get(0, 0)[0]); final double attitudeZ = Math.asin(R.get(1, 0)[0]); q.setFromEuler(bankX, headingY, attitudeZ); Core.multiply(R, new Scalar(-1), R); Core.gemm(R, tvec, 1, new Mat(), 0, tvec_map_cam, 0); R.release(); final org.ros.rosjava_geometry.Quaternion rotation = new org.ros.rosjava_geometry.Quaternion( q.getX(), q.getY(), q.getZ(), q.getW()); final double x = tvec_map_cam.get(0, 0)[0]; final double y = tvec_map_cam.get(1, 0)[0]; final double z = tvec_map_cam.get(2, 0)[0]; tvec_map_cam.release(); final org.ros.rosjava_geometry.Vector3 translation = new org.ros.rosjava_geometry.Vector3(x, y, z); final org.ros.rosjava_geometry.Transform transform_map_cam = new org.ros.rosjava_geometry.Transform( translation, rotation); // odom to camera_rgb_optical_frame final GraphName sourceFrame = GraphName.of(parameter.cameraFrameName()); final GraphName targetFrame = GraphName.of("base_link"); org.ros.rosjava_geometry.Transform transform_cam_base = null; if (transformationService.canTransform(targetFrame, sourceFrame)) { try { transform_cam_base = transformationService.lookupTransform(targetFrame, sourceFrame); } catch (Exception e) { log.error(ExceptionUtils.getStackTrace(e)); log.info("Cloud not get transformation from " + parameter.cameraFrameName() + " to " + "base_link! " + "However, will continue.."); // cancel this loop..no result can be computed return; } } else { log.info("Cloud not get transformation from " + parameter.cameraFrameName() + " to " + "base_link!" + " However, " + "will continue.."); // cancel this loop..no result can be computed return; } // multiply results org.ros.rosjava_geometry.Transform current_pose = org.ros.rosjava_geometry.Transform .identity(); current_pose = current_pose.multiply(transform_map_cam); current_pose = current_pose.multiply(transform_cam_base); if (current_pose.getTranslation().getZ() < 0.5) { return; } // check for plausibility of the pose by checking if movement // exceeds max speed (defined) of the robot if (parameter.badPoseReject()) { Time current_timestamp = connectedNode.getCurrentTime(); // TODO Unfortunately, we do not have the tf timestamp at // hand here. So we can only use the current timestamp. double maxspeed = 5; boolean goodpose = false; // if (current_pose != null && current_timestamp != null) { if ((last_pose != null && last_timestamp != null) && !Double.isNaN(last_pose.getTranslation().getX())) { // check speed of movement between last and current pose double distance = PoseCompare.distance(current_pose, last_pose); double timedelta = PoseCompare.timedelta(current_timestamp, last_timestamp); if ((distance / timedelta) < maxspeed) { if (smoothing) { double xold = last_pose.getTranslation().getX(); double yold = last_pose.getTranslation().getY(); double zold = last_pose.getTranslation().getZ(); double xnew = current_pose.getTranslation().getX(); double ynew = current_pose.getTranslation().getY(); double znew = current_pose.getTranslation().getZ(); final org.ros.rosjava_geometry.Vector3 smoothTranslation = new org.ros.rosjava_geometry.Vector3( (xold * 2 + xnew) / 3, (yold * 2 + ynew) / 3, (zold * 2 + znew) / 3); current_pose = new org.ros.rosjava_geometry.Transform(smoothTranslation, current_pose.getRotationAndScale()); last_pose = current_pose; } last_pose = current_pose; last_timestamp = current_timestamp; goodpose = true; } else { log.info("distance " + distance + " time: " + timedelta + " --> Pose rejected"); log.info("current pose: " + current_pose.getTranslation().getX() + " " + current_pose.getTranslation().getY() + " " + current_pose.getTranslation().getZ()); log.info("last pose: " + last_pose.getTranslation().getX() + " " + last_pose.getTranslation().getY() + " " + last_pose.getTranslation().getZ()); } } else { last_pose = current_pose; last_timestamp = current_timestamp; } // } // bad pose rejection if (!goodpose) { return; } } // set information to message final geometry_msgs.PoseStamped posestamped = posePublisher.newMessage(); Pose pose = posestamped.getPose(); Quaternion orientation = pose.getOrientation(); Point point = pose.getPosition(); point.setX(current_pose.getTranslation().getX()); point.setY(current_pose.getTranslation().getY()); point.setZ(current_pose.getTranslation().getZ()); orientation.setW(current_pose.getRotationAndScale().getW()); orientation.setX(current_pose.getRotationAndScale().getX()); orientation.setY(current_pose.getRotationAndScale().getY()); orientation.setZ(current_pose.getRotationAndScale().getZ()); // frame_id too posestamped.getHeader().setFrameId("map"); posestamped.getHeader().setStamp(connectedNode.getCurrentTime()); posePublisher.publish(posestamped); mostRecentPose.set(posestamped); // publishCamFrameToMarkerFrame(rvec, tvec, tfPublisherCamToMarker, connectedNode); // publishMapToOdom( // rvec, tvec, transformationService, tfPublisherMapToOdom, connectedNode); rvec.release(); tvec.release(); } catch (Exception e) { logger.info("An exception occurs.", e); } } } }); }
From source file:arlocros.ArMarkerPoseEstimator.java
License:Apache License
private void publishMapToOdom(Mat rvec, Mat tvec, TransformationService transformationService, Publisher<tf2_msgs.TFMessage> tfPublisherMapToOdom, ConnectedNode connectedNode) { // compute transform map to odom from map to // camera_rgb_optical_frame and odom to camera_rgb_optical_frame // map to camera_rgb_optical_frame Mat tvec_map_cam = new MatOfDouble(1.0, 1.0, 1.0); QuaternionHelper q = new QuaternionHelper(); // get rotation matrix R from solvepnp output rotation vector // rvec/*from w w w . j a v a 2 s . co m*/ Mat R = new Mat(3, 3, CvType.CV_32FC1); Calib3d.Rodrigues(rvec, R); // transpose R, because we need the transformation from // world(map) to camera R = R.t(); // get rotation around X,Y,Z from R in radiants double bankX = Math.atan2(-R.get(1, 2)[0], R.get(1, 1)[0]); double headingY = Math.atan2(-R.get(2, 0)[0], R.get(0, 0)[0]); double attitudeZ = Math.asin(R.get(1, 0)[0]); q.setFromEuler(bankX, headingY, attitudeZ); // compute translation vector from world (map) to cam // tvec_map_cam Core.multiply(R, new Scalar(-1), R); // R=-R Core.gemm(R, tvec, 1, new Mat(), 0, tvec_map_cam, 0); // tvec_map_cam=R*tvec R.release(); org.ros.rosjava_geometry.Quaternion rotation = new org.ros.rosjava_geometry.Quaternion(q.getX(), q.getY(), q.getZ(), q.getW()); double x = tvec_map_cam.get(0, 0)[0]; double y = tvec_map_cam.get(1, 0)[0]; double z = tvec_map_cam.get(2, 0)[0]; tvec_map_cam.release(); // create a Transform Object that hold the transform map to cam org.ros.rosjava_geometry.Vector3 translation = new org.ros.rosjava_geometry.Vector3(x, y, z); org.ros.rosjava_geometry.Transform transform_map_cam = new org.ros.rosjava_geometry.Transform(translation, rotation); // odom to camera_rgb_optical_frame GraphName sourceFrame = GraphName.of(parameter.cameraFrameName()); GraphName targetFrame = GraphName.of("odom"); org.ros.rosjava_geometry.Transform transform_cam_odom = null; if (transformationService.canTransform(targetFrame, sourceFrame)) { try { transform_cam_odom = transformationService.lookupTransform(targetFrame, sourceFrame); } catch (Exception e) { log.error(ExceptionUtils.getStackTrace(e)); log.info("Cloud not get transformation from " + parameter.cameraFrameName() + " to " + "odom! " + "" + "" + "" + "" + "However, " + "will continue.."); return; } } else { log.info("Cloud not get transformation from " + parameter.cameraFrameName() + " to " + "odom! " + "However, will " + "continue.."); // cancel this loop..no result can be computed return; } // multiply results org.ros.rosjava_geometry.Transform result = org.ros.rosjava_geometry.Transform.identity(); result = result.multiply(transform_map_cam); result = result.multiply(transform_cam_odom); // set information to ROS message TFMessage tfMessage = tfPublisherMapToOdom.newMessage(); TransformStamped transformStamped = connectedNode.getTopicMessageFactory() .newFromType(geometry_msgs.TransformStamped._TYPE); Transform transform = transformStamped.getTransform(); Quaternion orientation = transform.getRotation(); Vector3 vector = transform.getTranslation(); vector.setX(result.getTranslation().getX()); vector.setY(result.getTranslation().getY()); vector.setZ(result.getTranslation().getZ()); orientation.setW(result.getRotationAndScale().getW()); orientation.setX(result.getRotationAndScale().getX()); orientation.setY(result.getRotationAndScale().getY()); orientation.setZ(result.getRotationAndScale().getZ()); transformStamped.getHeader().setFrameId("map"); transformStamped.setChildFrameId("odom"); transformStamped.getHeader().setStamp(connectedNode.getCurrentTime()); // frame_id too tfMessage.getTransforms().add(transformStamped); tfPublisherMapToOdom.publish(tfMessage); }
From source file:ch.zhaw.facerecognitionlibrary.PreProcessor.BrightnessCorrection.GammaCorrection.java
License:Open Source License
public PreProcessor preprocessImage(PreProcessor preProcessor) { List<Mat> images = preProcessor.getImages(); List<Mat> processed = new ArrayList<Mat>(); for (Mat img : images) { img.convertTo(img, CvType.CV_32F); Core.divide(img, INT_MAX, img);//from w w w. j ava2 s . co m Core.pow(img, gamma, img); Core.multiply(img, INT_MAX, img); img.convertTo(img, CvType.CV_8U); processed.add(img); } preProcessor.setImages(processed); return preProcessor; }
From source file:com.github.rosjava_catkin_package_a.ARLocROS.ARLoc.java
License:Apache License
@Override public void onStart(final ConnectedNode connectedNode) { // load OpenCV shared library System.loadLibrary(Core.NATIVE_LIBRARY_NAME); // read configuration variables from the ROS Runtime (configured in the // launch file) log = connectedNode.getLog();/*from w ww.j av a2 s. c om*/ log.info("Reading parameters"); this.parameter = Parameter.createFromParameterTree(connectedNode.getParameterTree()); // Read Marker Config markerConfig = MarkerConfig.createFromConfig(parameter.markerConfigFile(), parameter.patternDirectory()); // setup rotation vector and translation vector storing output of the // localization rvec = new Mat(3, 1, CvType.CV_64F); tvec = new MatOfDouble(1.0, 1.0, 1.0); camp = getCameraInfo(connectedNode, parameter); // start to listen to transform messages in /tf in order to feed the // Transformer and lookup transforms final TransformationService transformationService = TransformationService.create(connectedNode); // Subscribe to Image Subscriber<sensor_msgs.Image> subscriberToImage = connectedNode.newSubscriber(parameter.cameraImageTopic(), sensor_msgs.Image._TYPE); ComputePose computePose = null; try { final Mat cameraMatrix = CameraParams.getCameraMatrix(camp); final MatOfDouble distCoeffs = CameraParams.getDistCoeffs(camp); computePose = ComputePose.create(markerConfig, new Size(camp.width(), camp.height()), cameraMatrix, distCoeffs, this.parameter.visualization()); } catch (NyARException e) { logger.info("Cannot initialize ComputePose", e); } catch (FileNotFoundException e) { logger.info("Cannot find file when initialize ComputePose", e); } final ComputePose poseProcessor = computePose; subscriberToImage.addMessageListener(new MessageListener<sensor_msgs.Image>() { @Override public void onNewMessage(sensor_msgs.Image message) { // if (!message.getEncoding().toLowerCase().equals("rgb8")) { log.error("Sorry, " + message.getEncoding() + " Image encoding is not supported! EXITING"); System.exit(-1); } if (camp != null) { try { // image = Utils.matFromImage(message); // uncomment to add more contrast to the image //Utils.tresholdContrastBlackWhite(image, 600); Imgproc.threshold(image, image, 200, 255, Imgproc.THRESH_BINARY); // Mat cannyimg = new Mat(image.height(), image.width(), // CvType.CV_8UC3); // Imgproc.Canny(image, cannyimg, 10, 100); // Imshow.show(cannyimg); // image.convertTo(image, -1, 1.5, 0); // setup camera matrix and return vectors // compute pose if (poseProcessor.computePose(rvec, tvec, image)) { // notify publisher threads (pose and tf, see below) synchronized (tvec) { tvec.notifyAll(); } } } catch (Exception e) { e.printStackTrace(); } } } }); // publish tf CAMERA_FRAME_NAME --> MARKER_FRAME_NAME final Publisher<tf2_msgs.TFMessage> tfPublisherCamToMarker = connectedNode.newPublisher("tf", tf2_msgs.TFMessage._TYPE); connectedNode.executeCancellableLoop(new CancellableLoop() { @Override protected void loop() throws InterruptedException { synchronized (tvec) { tvec.wait(); } QuaternionHelper q = new QuaternionHelper(); /* * http://euclideanspace.com/maths/geometry/rotations/ * conversions/matrixToEuler/index.htm * http://stackoverflow.com/questions/12933284/rodrigues-into- * eulerangles-and-vice-versa * * heading = atan2(-m20,m00) attitude = asin(m10) bank = * atan2(-m12,m11) */ // convert output rotation vector rvec to rotation matrix R Mat R = new Mat(3, 3, CvType.CV_32FC1); Calib3d.Rodrigues(rvec, R); // get rotations around X,Y,Z from rotation matrix R double bankX = Math.atan2(-R.get(1, 2)[0], R.get(1, 1)[0]); double headingY = Math.atan2(-R.get(2, 0)[0], R.get(0, 0)[0]); double attitudeZ = Math.asin(R.get(1, 0)[0]); // convert Euler angles to quarternion q.setFromEuler(bankX, headingY, attitudeZ); // set information to message TFMessage tfmessage = tfPublisherCamToMarker.newMessage(); TransformStamped posestamped = connectedNode.getTopicMessageFactory() .newFromType(geometry_msgs.TransformStamped._TYPE); Transform transform = posestamped.getTransform(); Quaternion orientation = transform.getRotation(); Vector3 point = transform.getTranslation(); point.setX(tvec.get(0, 0)[0]); point.setY(tvec.get(1, 0)[0]); point.setZ(tvec.get(2, 0)[0]); orientation.setW(q.getW()); orientation.setX(q.getX()); orientation.setY(q.getY()); orientation.setZ(q.getZ()); posestamped.getHeader().setFrameId(parameter.cameraFrameName()); posestamped.setChildFrameId(parameter.markerFrameName()); posestamped.getHeader().setStamp(connectedNode.getCurrentTime()); // frame_id too tfmessage.getTransforms().add(posestamped); tfPublisherCamToMarker.publish(tfmessage); } }); // publish Markers final Publisher<visualization_msgs.Marker> markerPublisher = connectedNode.newPublisher("markers", visualization_msgs.Marker._TYPE); connectedNode.executeCancellableLoop(new CancellableLoop() { @Override protected void loop() throws InterruptedException { // publish markers every 500ms Thread.sleep(500); // get marker points from markerConfig, each marker has 4 // vertices List<Point3> points3dlist = markerConfig.getUnordered3DPointList(); int i = 0; for (Point3 p : points3dlist) { Marker markermessage = markerPublisher.newMessage(); // FIXME If the markers are published into an existing frame // (e.g. map or odom) the node will consume very high CPU // and will fail after a short time. The markers are // probably published in the wrong way. markermessage.getHeader().setFrameId(parameter.markerFrameName()); markermessage.setId(i); i++; markermessage.setType(visualization_msgs.Marker.SPHERE); markermessage.setAction(visualization_msgs.Marker.ADD); // position double x = p.x; markermessage.getPose().getPosition().setX(x); double y = p.y; markermessage.getPose().getPosition().setY(y); double z = p.z; markermessage.getPose().getPosition().setZ(z); // orientation markermessage.getPose().getOrientation().setX(0); markermessage.getPose().getOrientation().setY(0); markermessage.getPose().getOrientation().setZ(0); markermessage.getPose().getOrientation().setW(1); // patterntSize markermessage.getScale().setX(0.1); markermessage.getScale().setY(0.1); markermessage.getScale().setZ(0.1); // color markermessage.getColor().setA(1); markermessage.getColor().setR(1); markermessage.getColor().setG(0); markermessage.getColor().setB(0); markerPublisher.publish(markermessage); } } }); // publish tf map --> odom final Publisher<tf2_msgs.TFMessage> tfPublisherMapToOdom = connectedNode.newPublisher("tf", tf2_msgs.TFMessage._TYPE); connectedNode.executeCancellableLoop(new CancellableLoop() { @Override protected void loop() throws InterruptedException { // since this is an infinite loop, wait to be notified if new // image was processed synchronized (tvec) { tvec.wait(); } // compute transform map to odom from map to // camera_rgb_optical_frame and odom to camera_rgb_optical_frame // map to camera_rgb_optical_frame Mat tvec_map_cam = new MatOfDouble(1.0, 1.0, 1.0); QuaternionHelper q = new QuaternionHelper(); // get rotation matrix R from solvepnp output rotation vector // rvec Mat R = new Mat(3, 3, CvType.CV_32FC1); Calib3d.Rodrigues(rvec, R); // transpose R, because we need the transformation from // world(map) to camera R = R.t(); // get rotation around X,Y,Z from R in radiants double bankX = Math.atan2(-R.get(1, 2)[0], R.get(1, 1)[0]); double headingY = Math.atan2(-R.get(2, 0)[0], R.get(0, 0)[0]); double attitudeZ = Math.asin(R.get(1, 0)[0]); q.setFromEuler(bankX, headingY, attitudeZ); // compute translation vector from world (map) to cam // tvec_map_cam Core.multiply(R, new Scalar(-1), R); // R=-R Core.gemm(R, tvec, 1, new Mat(), 0, tvec_map_cam, 0); // tvec_map_cam=R*tvec org.ros.rosjava_geometry.Quaternion rotation = new org.ros.rosjava_geometry.Quaternion(q.getX(), q.getY(), q.getZ(), q.getW()); double x = tvec_map_cam.get(0, 0)[0]; double y = tvec_map_cam.get(1, 0)[0]; double z = tvec_map_cam.get(2, 0)[0]; // create a Transform Object that hold the transform map to cam org.ros.rosjava_geometry.Vector3 translation = new org.ros.rosjava_geometry.Vector3(x, y, z); org.ros.rosjava_geometry.Transform transform_map_cam = new org.ros.rosjava_geometry.Transform( translation, rotation); // odom to camera_rgb_optical_frame GraphName sourceFrame = GraphName.of(parameter.cameraFrameName()); GraphName targetFrame = GraphName.of("odom"); org.ros.rosjava_geometry.Transform transform_cam_odom = null; if (transformationService.canTransform(targetFrame, sourceFrame)) { try { transform_cam_odom = transformationService.lookupTransform(targetFrame, sourceFrame); } catch (Exception e) { e.printStackTrace(); log.info("Cloud not get transformation from " + parameter.cameraFrameName() + " to " + "odom! " + "However, " + "will continue.."); return; } } else { log.info("Cloud not get transformation from " + parameter.cameraFrameName() + " to " + "odom! " + "However, will " + "continue.."); // cancel this loop..no result can be computed return; } // multiply results org.ros.rosjava_geometry.Transform result = org.ros.rosjava_geometry.Transform.identity(); result = result.multiply(transform_map_cam); result = result.multiply(transform_cam_odom); // set information to ROS message TFMessage tfMessage = tfPublisherMapToOdom.newMessage(); TransformStamped transformStamped = connectedNode.getTopicMessageFactory() .newFromType(geometry_msgs.TransformStamped._TYPE); Transform transform = transformStamped.getTransform(); Quaternion orientation = transform.getRotation(); Vector3 vector = transform.getTranslation(); vector.setX(result.getTranslation().getX()); vector.setY(result.getTranslation().getY()); vector.setZ(result.getTranslation().getZ()); orientation.setW(result.getRotationAndScale().getW()); orientation.setX(result.getRotationAndScale().getX()); orientation.setY(result.getRotationAndScale().getY()); orientation.setZ(result.getRotationAndScale().getZ()); transformStamped.getHeader().setFrameId("map"); transformStamped.setChildFrameId("odom"); transformStamped.getHeader().setStamp(connectedNode.getCurrentTime()); // frame_id too tfMessage.getTransforms().add(transformStamped); tfPublisherMapToOdom.publish(tfMessage); // System.exit(0); } }); // Publish Pose final Publisher<geometry_msgs.PoseStamped> posePublisher = connectedNode .newPublisher(parameter.poseTopicName(), geometry_msgs.PoseStamped._TYPE); connectedNode.executeCancellableLoop(new CancellableLoop() { @Override protected void loop() throws InterruptedException { // since this is an infinite loop, wait here to be notified if // new image was processed synchronized (tvec) { tvec.wait(); } final QuaternionHelper q = new QuaternionHelper(); // convert rotation vector result of solvepnp to rotation matrix Mat R = new Mat(3, 3, CvType.CV_32FC1); Calib3d.Rodrigues(rvec, R); // see publishers before for documentation final Mat tvec_map_cam = new MatOfDouble(1.0, 1.0, 1.0); R = R.t(); final double bankX = Math.atan2(-R.get(1, 2)[0], R.get(1, 1)[0]); final double headingY = Math.atan2(-R.get(2, 0)[0], R.get(0, 0)[0]); final double attitudeZ = Math.asin(R.get(1, 0)[0]); q.setFromEuler(bankX, headingY, attitudeZ); Core.multiply(R, new Scalar(-1), R); Core.gemm(R, tvec, 1, new Mat(), 0, tvec_map_cam, 0); final org.ros.rosjava_geometry.Quaternion rotation = new org.ros.rosjava_geometry.Quaternion( q.getX(), q.getY(), q.getZ(), q.getW()); final double x = tvec_map_cam.get(0, 0)[0]; final double y = tvec_map_cam.get(1, 0)[0]; final double z = tvec_map_cam.get(2, 0)[0]; final org.ros.rosjava_geometry.Vector3 translation = new org.ros.rosjava_geometry.Vector3(x, y, z); final org.ros.rosjava_geometry.Transform transform_map_cam = new org.ros.rosjava_geometry.Transform( translation, rotation); // odom to camera_rgb_optical_frame final GraphName sourceFrame = GraphName.of(parameter.cameraFrameName()); final GraphName targetFrame = GraphName.of("base_link"); org.ros.rosjava_geometry.Transform transform_cam_base = null; if (transformationService.canTransform(targetFrame, sourceFrame)) { try { transform_cam_base = transformationService.lookupTransform(targetFrame, sourceFrame); } catch (Exception e) { e.printStackTrace(); log.info("Cloud not get transformation from " + parameter.cameraFrameName() + " to " + "base_link! " + "However, will continue.."); // cancel this loop..no result can be computed return; } } else { log.info("Cloud not get transformation from " + parameter.cameraFrameName() + " to " + "base_link!" + " However, " + "will continue.."); // cancel this loop..no result can be computed return; } // multiply results org.ros.rosjava_geometry.Transform current_pose = org.ros.rosjava_geometry.Transform.identity(); current_pose = current_pose.multiply(transform_map_cam); current_pose = current_pose.multiply(transform_cam_base); // check for plausibility of the pose by checking if movement // exceeds max speed (defined) of the robot if (parameter.badPoseReject()) { Time current_timestamp = connectedNode.getCurrentTime(); // TODO Unfortunately, we do not have the tf timestamp at // hand here. So we can only use the current timestamp. double maxspeed = 5; boolean goodpose = false; // if (current_pose != null && current_timestamp != null) { if (last_pose != null && last_timestamp != null) { // check speed of movement between last and current pose double distance = PoseCompare.distance(current_pose, last_pose); double timedelta = PoseCompare.timedelta(current_timestamp, last_timestamp); if ((distance / timedelta) < maxspeed) { if (smoothing) { double xold = last_pose.getTranslation().getX(); double yold = last_pose.getTranslation().getY(); double zold = last_pose.getTranslation().getZ(); double xnew = current_pose.getTranslation().getX(); double ynew = current_pose.getTranslation().getY(); double znew = current_pose.getTranslation().getZ(); final org.ros.rosjava_geometry.Vector3 smoothTranslation = new org.ros.rosjava_geometry.Vector3( (xold * 2 + xnew) / 3, (yold * 2 + ynew) / 3, (zold * 2 + znew) / 3); current_pose = new org.ros.rosjava_geometry.Transform(smoothTranslation, current_pose.getRotationAndScale()); last_pose = current_pose; } last_pose = current_pose; last_timestamp = current_timestamp; goodpose = true; } else { log.info("distance " + distance + " time: " + timedelta + " --> Pose rejected"); } } else { last_pose = current_pose; last_timestamp = current_timestamp; } // } // bad pose rejection if (!goodpose) { return; } } // set information to message geometry_msgs.PoseStamped posestamped = posePublisher.newMessage(); Pose pose = posestamped.getPose(); Quaternion orientation = pose.getOrientation(); Point point = pose.getPosition(); point.setX(current_pose.getTranslation().getX()); point.setY(current_pose.getTranslation().getY()); point.setZ(current_pose.getTranslation().getZ()); orientation.setW(current_pose.getRotationAndScale().getW()); orientation.setX(current_pose.getRotationAndScale().getX()); orientation.setY(current_pose.getRotationAndScale().getY()); orientation.setZ(current_pose.getRotationAndScale().getZ()); // frame_id too posestamped.getHeader().setFrameId("map"); posestamped.getHeader().setStamp(connectedNode.getCurrentTime()); posePublisher.publish(posestamped); } }); }
From source file:com.shootoff.camera.Camera.java
License:Open Source License
public static Mat colorTransfer(Mat source, Mat target) { Mat src = new Mat(); Mat dst = new Mat(); Imgproc.cvtColor(source, src, Imgproc.COLOR_BGR2Lab); Imgproc.cvtColor(target, dst, Imgproc.COLOR_BGR2Lab); ArrayList<Mat> src_channels = new ArrayList<Mat>(); ArrayList<Mat> dst_channels = new ArrayList<Mat>(); Core.split(src, src_channels);//from w ww . j ava2 s .co m Core.split(dst, dst_channels); for (int i = 0; i < 3; i++) { MatOfDouble src_mean = new MatOfDouble(), src_std = new MatOfDouble(); MatOfDouble dst_mean = new MatOfDouble(), dst_std = new MatOfDouble(); Core.meanStdDev(src_channels.get(i), src_mean, src_std); Core.meanStdDev(dst_channels.get(i), dst_mean, dst_std); dst_channels.get(i).convertTo(dst_channels.get(i), CvType.CV_64FC1); Core.subtract(dst_channels.get(i), dst_mean, dst_channels.get(i)); Core.divide(dst_std, src_std, dst_std); Core.multiply(dst_channels.get(i), dst_std, dst_channels.get(i)); Core.add(dst_channels.get(i), src_mean, dst_channels.get(i)); dst_channels.get(i).convertTo(dst_channels.get(i), CvType.CV_8UC1); } Core.merge(dst_channels, dst); Imgproc.cvtColor(dst, dst, Imgproc.COLOR_Lab2BGR); return dst; }
From source file:com.wallerlab.compcellscope.calcDPCTask.java
License:BSD License
protected Long doInBackground(Mat... matrix_list) { //int count = urls.length; Mat in1 = matrix_list[0];// www . j a va 2 s .c o m Mat in2 = matrix_list[1]; Mat outputMat = matrix_list[2]; Mat Mat1 = new Mat(in1.width(), in1.height(), in1.type()); Mat Mat2 = new Mat(in2.width(), in2.height(), in2.type()); in1.copyTo(Mat1); in2.copyTo(Mat2); Imgproc.cvtColor(Mat1, Mat1, Imgproc.COLOR_RGBA2GRAY, 1); Imgproc.cvtColor(Mat2, Mat2, Imgproc.COLOR_RGBA2GRAY, 1); Mat output = new Mat(Mat1.width(), Mat1.height(), CvType.CV_8UC4); Mat dpcSum = new Mat(Mat1.width(), Mat1.height(), CvType.CV_32FC1); Mat dpcDifference = new Mat(Mat1.width(), Mat1.height(), CvType.CV_32FC1); Mat dpcImgF = new Mat(Mat1.width(), Mat1.height(), CvType.CV_32FC1); /* Log.d(TAG,String.format("Mat1 format is %.1f-%.1f, type: %d",Mat1.size().width,Mat1.size().height,Mat1.type())); Log.d(TAG,String.format("Mat2 format is %.1f-%.1f, type: %d",Mat2.size().width,Mat2.size().height,Mat2.type())); */ // Convert to Floats Mat1.convertTo(Mat1, CvType.CV_32FC1); Mat2.convertTo(Mat2, CvType.CV_32FC1); Core.add(Mat1, Mat2, dpcSum); Core.subtract(Mat1, Mat2, dpcDifference); Core.divide(dpcDifference, dpcSum, dpcImgF); Core.add(dpcImgF, new Scalar(1.0), dpcImgF); // Normalize to 0-2.0 Core.multiply(dpcImgF, new Scalar(110), dpcImgF); // Normalize to 0-255 dpcImgF.convertTo(output, CvType.CV_8UC1); // Convert back into RGB Imgproc.cvtColor(output, output, Imgproc.COLOR_GRAY2RGBA, 4); dpcSum.release(); dpcDifference.release(); dpcImgF.release(); Mat1.release(); Mat2.release(); Mat maskedImg = Mat.zeros(output.rows(), output.cols(), CvType.CV_8UC4); int radius = maskedImg.width() / 2 + 25; Core.circle(maskedImg, new Point(maskedImg.width() / 2, maskedImg.height() / 2), radius, new Scalar(255, 255, 255), -1, 8, 0); output.copyTo(outputMat, maskedImg); output.release(); maskedImg.release(); return null; }
From source file:com.wallerlab.compcellscope.MultiModeViewActivity.java
License:BSD License
public Mat calcDPC(Mat in1, Mat in2, Mat out) { Mat Mat1 = new Mat(in1.width(), in1.height(), in1.type()); Mat Mat2 = new Mat(in2.width(), in2.height(), in2.type()); in1.copyTo(Mat1);// w w w.j a v a 2 s . c o m in2.copyTo(Mat2); Imgproc.cvtColor(Mat1, Mat1, Imgproc.COLOR_RGBA2GRAY, 1); Imgproc.cvtColor(Mat2, Mat2, Imgproc.COLOR_RGBA2GRAY, 1); Mat output = new Mat(Mat1.width(), Mat1.height(), CvType.CV_8UC4); Mat dpcSum = new Mat(Mat1.width(), Mat1.height(), CvType.CV_32FC1); Mat dpcDifference = new Mat(Mat1.width(), Mat1.height(), CvType.CV_32FC1); Mat dpcImgF = new Mat(Mat1.width(), Mat1.height(), CvType.CV_32FC1); /* Log.d(TAG,String.format("Mat1 format is %.1f-%.1f, type: %d",Mat1.size().width,Mat1.size().height,Mat1.type())); Log.d(TAG,String.format("Mat2 format is %.1f-%.1f, type: %d",Mat2.size().width,Mat2.size().height,Mat2.type())); */ // Convert to Floats Mat1.convertTo(Mat1, CvType.CV_32FC1); Mat2.convertTo(Mat2, CvType.CV_32FC1); Core.add(Mat1, Mat2, dpcSum); Core.subtract(Mat1, Mat2, dpcDifference); Core.divide(dpcDifference, dpcSum, dpcImgF); Core.add(dpcImgF, new Scalar(1.0), dpcImgF); // Normalize to 0-2.0 Core.multiply(dpcImgF, new Scalar(110), dpcImgF); // Normalize to 0-255 dpcImgF.convertTo(output, CvType.CV_8UC1); // Convert back into RGB Imgproc.cvtColor(output, output, Imgproc.COLOR_GRAY2RGBA, 4); dpcSum.release(); dpcDifference.release(); dpcImgF.release(); Mat1.release(); Mat2.release(); Mat maskedImg = Mat.zeros(output.rows(), output.cols(), CvType.CV_8UC4); int radius = maskedImg.width() / 2 + 25; Core.circle(maskedImg, new Point(maskedImg.width() / 2, maskedImg.height() / 2), radius, new Scalar(255, 255, 255), -1, 8, 0); output.copyTo(out, maskedImg); output.release(); maskedImg.release(); return out; }
From source file:com.wallerlab.processing.tasks.ComputeRefocusTask.java
License:BSD License
private Bitmap[] computeFocus(float z) { int width = mDataset.WIDTH - 2 * mDataset.XCROP; int height = mDataset.HEIGHT - 2 * mDataset.YCROP; Mat result = new Mat(height, width, CvType.CV_32FC4); Mat result8 = new Mat(height, width, CvType.CV_8UC4); Mat dpc_result_tb = new Mat(height, width, CvType.CV_32FC4); Mat dpc_result_tb8 = new Mat(height, width, CvType.CV_8UC4); Mat dpc_result_lr = new Mat(height, width, CvType.CV_32FC4); Mat dpc_result_lr8 = new Mat(height, width, CvType.CV_8UC4); Mat img;/*from w ww . j a v a 2s. c o m*/ Mat img32 = new Mat(height, width, CvType.CV_32FC4); Mat shifted; for (int idx = 0; idx < mDataset.fileCount; idx++) { img = ImageUtils.toMat(BitmapFactory.decodeByteArray(fileByteList[idx], 0, fileByteList[idx].length)); img = img.submat(mDataset.YCROP, mDataset.HEIGHT - mDataset.YCROP, mDataset.XCROP, mDataset.WIDTH - mDataset.XCROP); img.convertTo(img32, result.type()); // Grab actual hole number from filename String fName = mDataset.fileList[idx].toString(); String hNum = fName.substring(fName.indexOf("_scanning_") + 10, fName.indexOf(".jpeg")); int holeNum = Integer.parseInt(hNum); //Log.d(TAG,String.format("BF Scan Header is: %s", hNum)); // Calculate these based on array coordinates int xShift = (int) Math.round(z * tanh_lit[holeNum]); int yShift = (int) Math.round(z * tanv_lit[holeNum]); shifted = ImageUtils.circularShift(img32, yShift, xShift); if (mDataset.leftList.contains(holeNum)) //add LHS { Core.add(dpc_result_lr, shifted, dpc_result_lr); } else //subtract RHS { Core.subtract(dpc_result_lr, shifted, dpc_result_lr); } if (mDataset.topList.contains(holeNum)) //add Top { Core.add(dpc_result_tb, shifted, dpc_result_tb); } else //subtract bottom { Core.subtract(dpc_result_tb, shifted, dpc_result_tb); } Core.add(result, shifted, result); float progress = ((idx + 1) / (float) mDataset.fileCount); onProgressUpdate((int) (progress * 100), -1); Log.d(TAG, String.format("progress: %f", progress)); } Core.MinMaxLocResult minMaxLocResult1 = Core.minMaxLoc(result.reshape(1)); result.convertTo(result8, CvType.CV_8UC4, 255 / minMaxLocResult1.maxVal); Core.MinMaxLocResult minMaxLocResult2 = Core.minMaxLoc(dpc_result_lr.reshape(1)); dpc_result_lr.convertTo(dpc_result_lr8, CvType.CV_8UC4, 255 / (minMaxLocResult2.maxVal - minMaxLocResult2.minVal), -minMaxLocResult2.minVal * 255.0 / (minMaxLocResult2.maxVal - minMaxLocResult2.minVal)); Core.MinMaxLocResult minMaxLocResult3 = Core.minMaxLoc(dpc_result_tb.reshape(1)); dpc_result_tb.convertTo(dpc_result_tb8, CvType.CV_8UC4, 255 / (minMaxLocResult3.maxVal - minMaxLocResult3.minVal), -minMaxLocResult3.minVal * 255.0 / (minMaxLocResult3.maxVal - minMaxLocResult3.minVal)); /* Log.d(TAG,String.format("result_min: %f, max: %f",minMaxLocResult1.minVal,minMaxLocResult1.maxVal)); Log.d(TAG,String.format("lr_min: %f, max: %f",minMaxLocResult2.minVal,minMaxLocResult2.maxVal)); Log.d(TAG,String.format("tb_min: %f, max: %f",minMaxLocResult3.minVal,minMaxLocResult3.maxVal)); */ // remove transparency in DPC images Scalar alphaMask = new Scalar(new double[] { 1.0, 1.0, 1.0, 255.0 }); Core.multiply(dpc_result_lr8, alphaMask, dpc_result_lr8); Core.multiply(dpc_result_tb8, alphaMask, dpc_result_tb8); if (!mDataset.USE_COLOR_DPC) { Imgproc.cvtColor(dpc_result_lr8, dpc_result_lr8, Imgproc.COLOR_BGR2GRAY); Imgproc.cvtColor(dpc_result_tb8, dpc_result_tb8, Imgproc.COLOR_BGR2GRAY); } /* // Cut off edges in DPC images Point centerPt = new Point(); centerPt.x = Math.round((float)width/2.0); centerPt.y = Math.round((float)height/2.0); Mat circleMat = new Mat(dpc_result_lr8.size(), dpc_result_lr8.type()); Scalar color = new Scalar(255); Core.circle(circleMat, centerPt, 200, color); //Core.bitwise_and(circleMat, dpc_result_lr8, dpc_result_lr8); //Core.bitwise_and(circleMat, dpc_result_tb8, dpc_result_tb8); * * */ Bitmap[] outputBitmaps = new Bitmap[3]; outputBitmaps[0] = ImageUtils.toBitmap(result8); outputBitmaps[1] = ImageUtils.toBitmap(dpc_result_lr8); outputBitmaps[2] = ImageUtils.toBitmap(dpc_result_tb8); return outputBitmaps; }
From source file:ctPrincipal.Operacoes.java
String realizarOperacoes(int op) { String resultImgOutput = ""; switch (op) { case 1://and imagemBinaria();//from w ww . j a v a2s. co m Core.bitwise_and(image1bin, image2bin, output); normalizarBinario(); Imgcodecs.imwrite("OutputImg/and.jpg", output); resultImgOutput = "OutputImg/and.jpg"; break; case 2://or imagemBinaria(); Core.bitwise_or(image1bin, image2bin, output); normalizarBinario(); Imgcodecs.imwrite("OutputImg/or.jpg", output); resultImgOutput = "OutputImg/or.jpg"; break; case 3://xor imagemBinaria(); Core.bitwise_xor(image1bin, image2bin, output); normalizarBinario(); Imgcodecs.imwrite("OutputImg/xor.jpg", output); resultImgOutput = "OutputImg/xor.jpg"; break; case 4://not Core.bitwise_not(image1bin, output); Imgcodecs.imwrite("OutputImg/not.jpg", output); resultImgOutput = "OutputImg/not.jpg"; break; case 5://soma Core.add(image1, image2, output); Imgcodecs.imwrite("OutputImg/soma.jpg", output); resultImgOutput = "OutputImg/soma.jpg"; break; case 6://subtracao Core.subtract(image1, image2, output); Imgcodecs.imwrite("OutputImg/subtracao.jpg", output); resultImgOutput = "OutputImg/subtracao.jpg"; break; case 7:// multiplicacao Core.multiply(image1, image2, output); Imgcodecs.imwrite("OutputImg/multiplicacao.jpg", output); resultImgOutput = "OutputImg/multiplicacao.jpg"; break; case 8://divisao Core.divide(image1, image2, output); Imgcodecs.imwrite("OutputImg/divisao.jpg", output); resultImgOutput = "OutputImg/divisao.jpg"; break; } return resultImgOutput; }