com.google.ar.core.Pose Java Examples

@JavascriptInterface
public void registerAnchor(final String requestId, final float[] matrix) {
    if (requestId == null || matrix == null) {
        return;
    }

    mSurfaceView.queueEvent(new Runnable() {
        @Override
        public void run() {
            Pose anchorPose = Pose.makeTranslation(matrix[12], matrix[13], matrix[14]);
            Anchor anchor = mSession.createAnchor(anchorPose);

            final String scriptString = "ARCoreBridge.registerAnchorResponse('" + requestId + "', '" + String.valueOf(anchor.hashCode()) + "');";
            evaluateWebViewJavascript(scriptString);
        }
    });
}
 

private void calculateVelocity(Pose centerPose, float deltaSeconds) {
  Pose anchorPose = arFragment.getArSceneView().getSession().getAllAnchors().iterator().next()
      .getPose();
  Vector3 currPos =
      new Vector3(
          centerPose.tx() - anchorPose.tx(),
          centerPose.ty() - anchorPose.ty(),
          centerPose.tz() - anchorPose.tz());
  delTime += deltaSeconds;

  if (lastPos == null) {
    lastPos = currPos;
  } else if (delTime >= INTERVAL_TIME_SECONDS) {
    // Calculate velocity in meters per second.
    Vector3 displacement = Vector3.subtract(currPos, lastPos);
    float velocityValue = displacement.length() / delTime;
    velocitySensor.setNextVelocity(velocityValue);
    // TODO(b/135678092): Add a string resource for the following
    velocityText.setText(String.format(Locale.getDefault(), "%.2f m/s", velocityValue));
    delTime = 0;
    lastPos = currPos;
  }
}
 

private void calculateVelocityEveryFrame(Pose centerPose, float deltaSeconds) {
  Pose anchorPose =
      arFragment.getArSceneView().getSession().getAllAnchors().iterator().next().getPose();
  Vector3 currPos =
      new Vector3(
          centerPose.tx() - anchorPose.tx(),
          centerPose.ty() - anchorPose.ty(),
          centerPose.tz() - anchorPose.tz());
  textUpdateTime += deltaSeconds;

  if (lastPos != null) {
    // Calculate velocity in meters per second.
    Vector3 displacement = Vector3.subtract(currPos, lastPos);
    float velocityValue = displacement.length() / deltaSeconds;
    velocitySensor.setNextVelocity(velocityValue);

    if (textUpdateTime >= TEXT_UPDATE_TIME_SECONDS) {
      // TODO(b/135678092): Add a string resource for the following
      velocityText.setText(String.format(Locale.getDefault(), "%.2f m/s", velocityValue));
      textUpdateTime = 0;
    }
  }
  lastPos = currPos;
}
 

private void averageVelocityEveryFrame(Pose centerPose, float deltaSeconds) {
  Pose anchorPose =
      arFragment.getArSceneView().getSession().getAllAnchors().iterator().next().getPose();
  Vector3 currPos =
      new Vector3(
          centerPose.tx() - anchorPose.tx(),
          centerPose.ty() - anchorPose.ty(),
          centerPose.tz() - anchorPose.tz());
  positions.add(currPos);
  currIndex++;
  textUpdateTime += deltaSeconds;

  if (currIndex >= INTERVAL_FRAMES) {
    // Calculate velocity over the past second.
    Vector3 displacement = Vector3.subtract(currPos, positions.get(currIndex - INTERVAL_FRAMES));
    float velocityValue = displacement.length() / INTERVAL_TIME_SECONDS;
    velocitySensor.setNextVelocity(velocityValue);

    if (textUpdateTime >= TEXT_UPDATE_TIME_SECONDS) {
      // TODO(b/135678092): Add a string resource for the following
      velocityText.setText(String.format(Locale.getDefault(), "%.2f m/s", velocityValue));
      textUpdateTime = 0;
    }
  }
}
 

private void calculateSpeed(Pose centerPose, float deltaSeconds) {
  Pose anchorPose =
      arFragment.getArSceneView().getSession().getAllAnchors().iterator().next().getPose();
  Vector3 currPos =
      new Vector3(
          centerPose.tx() - anchorPose.tx(),
          centerPose.ty() - anchorPose.ty(),
          centerPose.tz() - anchorPose.tz());
  delTime += deltaSeconds;

  if (lastPos != null) {
    float distance = Vector3.subtract(currPos, lastPos).length();
    totalDistance += distance;
  }
  lastPos = currPos;

  if (delTime >= INTERVAL_TIME_SECONDS) {
    // Calculate velocity in meters per second.
    float speedValue = totalDistance / delTime;
    velocitySensor.setNextVelocity(speedValue);
    // TODO(b/135678092): Add a string resource for the following
    velocityText.setText(String.format(Locale.getDefault(), "%.2f m/s", speedValue));
    delTime = 0;
    totalDistance = 0;
  }
}
 

/**
 * Transform a vector3f FROM anchor coordinates TO world coordinates
 */
public static Vector3f TransformPointFromPose(Vector3f point, Pose anchorPose) {
    float[] position = new float[3];
    position[0] = point.x;
    position[1] = point.y;
    position[2] = point.z;

    position = anchorPose.transformPoint(position);
    return new Vector3f(position[0], position[1], position[2]);
}
 

/**
 * Transform a vector3f TO anchor coordinates FROM world coordinates
 */
public static Vector3f TransformPointToPose(Vector3f point, Pose anchorPose) {
    // Recenter to anchor
    float[] position = new float[3];
    position[0] = point.x;
    position[1] = point.y;
    position[2] = point.z;

    position = anchorPose.inverse().transformPoint(position);
    return new Vector3f(position[0], position[1], position[2]);
}
 

public static float calculateDistanceToPlane(Pose planePose, Pose cameraPose) {
  float[] normal = new float[3];
  float cameraX = cameraPose.tx();
  float cameraY = cameraPose.ty();
  float cameraZ = cameraPose.tz();
  // Get transformed Y axis of plane's coordinate system.
  planePose.getTransformedAxis(1, 1.0f, normal, 0);
  // Compute dot product of plane's normal with vector from camera to plane center.
  return (cameraX - planePose.tx()) * normal[0]
      + (cameraY - planePose.ty()) * normal[1]
      + (cameraZ - planePose.tz()) * normal[2];
}
 

public void addAnchor(String identifier, String matrix) {
	if (_session == null) {
		DeviceLog.warning("Session is null. Not adding anchor.");
		return;
	}

	String[] floats = matrix.split(",");
	if (floats.length != 16) {
		DeviceLog.warning("Matrix doesn't have 16 elements. Not adding anchor.");
		return;
	}

	float[] anchorMatrix = new float[16];
	for (int i = 0; i < 16; i++) {
		try {
			anchorMatrix[i] = Float.parseFloat(floats[i]);
		} catch (NumberFormatException ignored) {
			DeviceLog.warning("Cannot parse matrix. Not adding anchor.");
			return;
		}
	}

	float quaternion[] = new float[4];
	matrix4x4ToQuaternion(anchorMatrix, quaternion);
	float translation[] = new float[3];
	matrix4x4ToTranslation(anchorMatrix, translation);

	Pose pose = new Pose(translation, quaternion);
	Anchor a = _session.createAnchor(pose);
	_anchors.put(identifier, a);
}
 

@Override
public void onUpdate(FrameTime frameTime) {
    Frame frame = arFragment.getArSceneView().getArFrame();

    Log.d("API123", "onUpdateframe... current anchor node " + (currentAnchorNode == null));


    if (currentAnchorNode != null) {
        Pose objectPose = currentAnchor.getPose();
        Pose cameraPose = frame.getCamera().getPose();

        float dx = objectPose.tx() - cameraPose.tx();
        float dy = objectPose.ty() - cameraPose.ty();
        float dz = objectPose.tz() - cameraPose.tz();

        ///Compute the straight-line distance.
        float distanceMeters = (float) Math.sqrt(dx * dx + dy * dy + dz * dz);
        tvDistance.setText("Distance from camera: " + distanceMeters + " metres");


        /*float[] distance_vector = currentAnchor.getPose().inverse()
                .compose(cameraPose).getTranslation();
        float totalDistanceSquared = 0;
        for (int i = 0; i < 3; ++i)
            totalDistanceSquared += distance_vector[i] * distance_vector[i];*/
    }
}
 

public void createAnchor() {
    runOnUiThread(new Runnable() {
        @Override
        public void run() {
            Pose pose = mFrame.getCamera().getPose();

            try {
                mAnchor = mSession.createAnchor(pose);
            } catch (NotTrackingException e) {
                Log.e(TAG, "Cannot create anchor when not tracking", e);
                mTrackingIndicator.addListener(new TrackingIndicator.DisplayListener() {
                    @Override
                    public void onErrorDisplaying() {
                        // Do nothing, can't set anchor
                    }

                    @Override
                    public void onErrorRemoved() {
                        mTrackingIndicator.removeListener(this);
                        createAnchor();
                    }
                });
                return;
            }

            mPairSessionManager.onAnchorCreated();
            if (mStrokes.size() > 0) {
                for (int i = 0; i < mStrokes.size(); i++) {
                    mStrokes.get(i).offsetToPose(pose);
                    if (mStrokes.get(i).hasFirebaseReference())
                        mPairSessionManager.updateStroke(mStrokes.get(i));
                    else
                        mPairSessionManager.addStroke(mStrokes.get(i));
                }
                mLineShaderRenderer.bNeedsUpdate.set(true);
            }

            mPairSessionManager.setAnchor(mAnchor);
        }
    });
}
 

public void offsetToPose(Pose pose) {
    for (int i = 0; i < points.size(); i++) {
        Vector3f p = LineUtils.TransformPointToPose(points.get(i), pose);
        points.set(i, p);
    }
}
 

public void offsetFromPose(Pose pose) {
    for (int i = 0; i < points.size(); i++) {
        Vector3f p = LineUtils.TransformPointFromPose(points.get(i), pose);
        points.set(i, p);
    }
}
 

public void drawMarkers(Frame frame) {
    for (LocationMarker locationMarker : mLocationMarkers) {

        try {
            // Get the current pose of an Anchor in world space. The Anchor pose is updated
            // during calls to session.update() as ARCore refines its estimate of the world.

            float translation[] = new float[3];
            float rotation[] = new float[4];
            locationMarker.anchor.getPose().getTranslation(translation, 0);
            frame.getCamera().getPose().getRotationQuaternion(rotation, 0);

            Pose rotatedPose = new Pose(translation, rotation);
            rotatedPose.toMatrix(mAnchorMatrix, 0);

            int markerDistance = (int) Math.ceil(
                    uk.co.appoly.arcorelocation.utils.LocationUtils.distance(
                            locationMarker.latitude,
                            deviceLocation.currentBestLocation.getLatitude(),
                            locationMarker.longitude,
                            deviceLocation.currentBestLocation.getLongitude(),
                            0,
                            0)
            );

            // Limit the distance of the Anchor within the scene.
            // Prevents uk.co.appoly.arcorelocation.rendering issues.
            int renderDistance = markerDistance;
            if (renderDistance > distanceLimit)
                renderDistance = distanceLimit;


            float[] projectionMatrix = new float[16];
            frame.getCamera().getProjectionMatrix(projectionMatrix, 0, 0.1f, 100.0f);

            // Get camera matrix and draw.
            float[] viewMatrix = new float[16];
            frame.getCamera().getViewMatrix(viewMatrix, 0);

            // Make sure marker stays the same size on screen, no matter the distance
            float scale = 3.0F / 10.0F * (float) renderDistance;

            // Distant markers a little smaller
            if(markerDistance > 3000)
                scale *= 0.75F;

            // Compute lighting from average intensity of the image.
            final float lightIntensity = frame.getLightEstimate().getPixelIntensity();

            locationMarker.renderer.updateModelMatrix(mAnchorMatrix, scale);
            locationMarker.renderer.draw(viewMatrix, projectionMatrix, lightIntensity);

        } catch (Exception e) {
            e.printStackTrace();
        }
    }
}
 

public void refreshAnchorsIfRequired(Frame frame) {
    if (anchorsNeedRefresh) {
        anchorsNeedRefresh = false;

        for (int i = 0; i < mLocationMarkers.size(); i++) {
            try {

                int markerDistance = (int) Math.round(
                        LocationUtils.distance(
                                mLocationMarkers.get(i).latitude,
                                deviceLocation.currentBestLocation.getLatitude(),
                                mLocationMarkers.get(i).longitude,
                                deviceLocation.currentBestLocation.getLongitude(),
                                0,
                                0)
                );

                float markerBearing = deviceOrientation.currentDegree + (float) LocationUtils.bearing(
                        deviceLocation.currentBestLocation.getLatitude(),
                        deviceLocation.currentBestLocation.getLongitude(),
                        mLocationMarkers.get(i).latitude,
                        mLocationMarkers.get(i).longitude);

                markerBearing = markerBearing + bearingAdjustment;
                markerBearing = markerBearing % 360;

                double rotation = Math.floor(markerBearing);
                rotation = rotation * Math.PI / 180;

                int renderDistance = markerDistance;

                // Limit the distance of the Anchor within the scene.
                // Prevents uk.co.appoly.arcorelocation.rendering issues.
                if (renderDistance > distanceLimit)
                    renderDistance = distanceLimit;

                // Adjustment to add markers on horizon, instead of just directly in front of camera
                double heightAdjustment = Math.round(renderDistance * (Math.tan(Math.toRadians(deviceOrientation.pitch))));

                // Raise distant markers for better illusion of distance
                // Hacky - but it works as a temporary measure
                int cappedRealDistance = markerDistance > 500 ?  500 : markerDistance;
                if (renderDistance != markerDistance)
                    heightAdjustment += 0.01F * (cappedRealDistance - renderDistance);

                float x = 0;
                float z = -renderDistance;

                float zRotated = (float) (z * Math.cos(rotation) - x * Math.sin(rotation));
                float xRotated = (float) -(z * Math.sin(rotation) + x * Math.cos(rotation));

                // Current camera height
                float y = frame.getCamera().getDisplayOrientedPose().ty();

                // Don't immediately assign newly created anchor in-case of exceptions
                Anchor newAnchor = mSession.createAnchor(
                        frame.getCamera().getPose()
                                .compose(Pose.makeTranslation(xRotated, y + (float) heightAdjustment, zRotated)));

                mLocationMarkers.get(i).anchor = newAnchor;

                mLocationMarkers.get(i).renderer.createOnGlThread(mContext, markerDistance);

            } catch (Exception e) {
                e.printStackTrace();
            }

        }
    }
}