ARCore Pose and Aruco estimatePoseSingleMarkers - android

I have a server function that detects and estimates a pose of aruco's marker from an image.
Using the function estimatePoseSingleMarkers I found the rotation and translation vector.
I need to use this value in an Android app with ARCore to create a Pose.
The documentation says that Pose needs two float array (rotation and translation): https://developers.google.com/ar/reference/java/arcore/reference/com/google/ar/core/Pose.
float[] newT = new float[] { t[0], t[1], t[2] };
Quaternion q = Quaternion.axisAngle(new Vector3(r[0], r[1], r[2]), 90);
float[] newR = new float[]{ q.x, q.y, q.z, q.w };
Pose pose = new Pose(newT, newR);
The position of the 3D object placed in this pose is totally random.
What am I doing wrong?
This is a snapshot from server image after estimate and draw axis. The image I receive is rotated of 90°, not sure if it relates to anything.

cv::aruco::estimatePoseSingleMarkers (link) returns rotation vector in Rodrigues format. Following the doc
w = norm( r ) // angle of rotation in radians
r = r/w // unit axis of rotation
thus
float w = sqrt( r[0]*r[0] + r[1]*r[1] + r[2]*r[2] );
// handle w==0.0 separately
// Get a new Quaternion using an axis/angle (degrees) to define the rotation
Quaternion q = Quaternion.axisAngle(new Vector3(r[0]/w, r[1]/w, r[2]/w), w * 180.0/3.14159 );
should work except for the right angle rotation mentioned. That is, if the lens parameters are fed to estimatePoseSingleMarkers correctly or up to certain accuracy.

Related

Azimuth mirrored after remapping axis coordinate system

I am trying to remap a device that has an alternate coordinate system.
The sensor is reporting values that are rotated 90° around the X axis. The format is a Quaternion in standard Android Rotation Vector notation. If I use the data unmodified I can hold the device 90° offset and successfully call getOrientation via:
private void updateOrientationFromVector(float[] rotationVector) {
float[] rotationMatrix = new float[9];
SensorManager.getRotationMatrixFromVector(rotationMatrix, rotationVector);
final int worldAxisForDeviceAxisX = SensorManager.AXIS_X;
final int worldAxisForDeviceAxisY = SensorManager.AXIS_Z;
float[] adjustedRotationMatrix = new float[9];
SensorManager.remapCoordinateSystem(rotationMatrix, worldAxisForDeviceAxisX,
worldAxisForDeviceAxisY, adjustedRotationMatrix);
// Transform rotation matrix into azimuth/pitch/roll
float[] orientation = new float[3];
SensorManager.getOrientation(adjustedRotationMatrix, orientation);
// Convert radians to degrees
float azimuth = orientation[0] * 57;
float pitch = orientation[1] * 57;
float roll = orientation[2] * 57;
// Normalize for readability
if(azimuth < 0) {
azimuth = azimuth + 360;
}
Log.d("Orientation", "Azimuth: " + azimuth + "° Pitch: " + pitch + "° Roll: " + roll + "°);
}
This code works fine for all normal Android devices. If I hold a reference phone in front of me as shown, the data is converted properly and shows my correct bearings. But when I use this test device, I must hold it at the wrong orientation to show me the correct bearings.
I want to pre-process the data from this test device to rotate the axes so the this device matches all other Android devices. This will let the display logic be generic.
Unfortunately I have tried many different techniques and none are working.
First, I tried to use a the Android calls again:
private fun rotateQuaternionAxes(rotationVector :FloatArray) : FloatArray {
val rotationMatrix = FloatArray(9)
SensorManager.getRotationMatrixFromVector(rotationMatrix, rotationVector)
val worldAxisForDeviceAxisX = SensorManager.AXIS_X
val worldAxisForDeviceAxisY = SensorManager.AXIS_Z
val adjustedRotationMatrix = FloatArray(9)
SensorManager.remapCoordinateSystem(rotationMatrix, worldAxisForDeviceAxisX, worldAxisForDeviceAxisY, adjustedRotationMatrix)
val axisRemappedData = Quaternion.fromRotationMatrix(adjustedRotationMatrix)
val rotationData = floatArrayOf(
axisRemappedData.x,
axisRemappedData.y,
axisRemappedData.z,
axisRemappedData.w
)
return rotationData
}
My private Quaternion.fromRotationMatrix is not show here, and came from euclideanspace.com
When I pre-process my rotation data with this, the logic works for everything, except north and south are swapped! East and west are correct, and my pitch and roll are correct.
So I decided to follow the suggestions for Rotating a Quaternion on 1-Axis with the following code:
private fun rotateQuaternionAxes(rotationVector :FloatArray) : FloatArray {
// https://stackoverflow.com/questions/4436764/rotating-a-quaternion-on-1-axis
// Device X+ is towards power button; Y+ is toward camera; Z+ towards nav buttons
// So rotate the reported data 90 degrees around X and the axes move appropriately
val sensorQuaternion: Quaternion = Quaternion(rotationVector[0], rotationVector[1], rotationVector[2], rotationVector[3])
val manipulationQuaternion = Quaternion.axisAngle(-1.0f, 0.0f, 0.0f, 90.0f)
val axisRemappedData = Quaternion.multiply(sensorQuaternion, manipulationQuaternion)
val rotationData = floatArrayOf(
axisRemappedData.x,
axisRemappedData.y,
axisRemappedData.z,
axisRemappedData.w
)
//LogUtil.debug("Orientation Orig: $sensorQuaternion Rotated: $axisRemappedData")
return rotationData
}
This does the exact same thing! Everything is fine, except north and south are mirrored, leaving east and west correct.
My Quaternion math came from sceneform-android-sdk and I double-checked it against several online sources.
I also tried simply changing my data by just grabbing the same data differently according to Convert quaternion to a different coordinate system.
private fun rotateQuaternionAxes(rotationVector :FloatArray) : FloatArray {
// No change:
//val rotationData = floatArrayOf(x_val, y_val, z_val, w_val)
val x_val = rotationVector[0]
val y_val = rotationVector[1]
val z_val = rotationVector[2]
val w_val = rotationVector[3]
val rotationData = floatArrayOf(x_val, z_val, -y_val, w_val)
return rotationData
}
This was not even close. I played with the axes and ended up finding rotationData = floatArrayOf(-z_val, -x_val, y_val, w_val) was had correct pitch and roll, but the azimuth was completely non-functional. So I've abandoned a simple remapping as an option.
Since the Android remapCoordinateSystem and the quaternion math give the same result, they seem mathematically equivalent. And multiple sources indicate they should accomplish what I'm trying to do.
Can any one explain why remapping my axes would swap the north/south? I believe I am getting a quaternion reflection instead of rotation. There is no physical point on the device that tracks the direction it shows.
Answer
As you said, it looks like you are expecting your data to be on the East-North-Up (ENU) Frame of Reference (FoR) but you are seeing data on an East-Down-North (EDN) FoR. The link you cited to convert quaternion to another coordinate system converts from an ENU to a NDW FoR - which evidently is not what you are looking for.
There are two ways you can solve this. Either use another rotation matrix, or swap your variables. Using another rotation matrix means doing more computation - but if you really want to learn how to do this, you can check out my self-plug introduction to quaternions for reference frame rotations.
The easiest way would be to swap your variables by recognizing that your X axis is not changing, but your expected Y is measured in z' and your expected Z is measured in -y'. Where X,Y,Z are the expected FoR, and x',y',z' are the actual measured FoR. The following "swaps" should allow you to get the same behavior as your other Android devices:
x_expected = x_actual
y_expected = z_actual
z_expected = -y_actual
!!! HOWEVER !!! If your measurements are given in quaternions, then you will have to use a rotation matrix. If your measurements are given as X,Y,Z measurements, you can get away with the swap provided above.
ENU/NED/NDW Notation
East-North-Up and all other similar axes notations are defined by the order of the coordinate system, expressed as X, then Y, and lastly Z, with respect to a Global inertial (static) Frame of Reference. I've defined your expected coordinate system as if you were to lay your phone flat on the ground with the screen of the phone facing the sky and the top of your phone pointing Northward.

Rotate plane used to find world coordinates

I am trying to find world coordinates from screen coordinates on a Plane where mCamera is a PerspectiveCamera.
public Vector3 getWorldCoordinates(float x, float y) {
// Use an imaginary plane at z=0 to intersect ray
Plane plane = new Plane();
plane.set(0, 0, 1, 0);
Ray ray = mCamera.getPickRay(x, y);
Vector3 pos = new Vector3();
Intersector.intersectRayPlane(ray, plane, pos);
return pos;
}
How can I modify this routine to rotate the Plane x degrees on the X axis before finding the coordinates?
Is there any simple built in libgdx routines used to rotate the plane such as for the Camera using Camera.rotate(Vector3.X, degrees)?
Set a Vector3 to the orientation you want and use that to set the Plane. So in your case
vector3.set(0, 0, 1);
vector3.rotateX(0.5f); // for example
plane.set(vector3.x, vector3.y, vector3.z, 0);
If you are doing this on every frame, you might want to consider instantiating your vector and plane one time in the class constructor and reusing them so you don't occasionally trigger the GC, which can cause stutters.

How to get coordinates of touch screen points in ogl?

I am drawing map using OpenGL. I am getting map drawn after reading XML files and setting corresponding buffer. This map contains streets, highways and boundary. What i want is whenever i touch the map, the color of the specific layer should be changed.
The issue i am facing is this whenever i touch on the screen i am just getting the the screen pixel of the point where i touched. I want to convert this point into OpenGL coordinates so that i can match this point with the Map drawn and can highlight the selected point.
How to convert this point into OpenGL coordinates?
You need to unproject screen point into an OpenGL world space:
vec3 UnProjectPoint( const vec3& Point, const max4& Projection, const mat4& ModelView )
{
vec4 R( Point, 1.0f );
R.x = 2.0f * R.x - 1.0f;
R.y = 2.0f * R.y - 1.0f;
R.y = -R.y;
R.z = 1.0f;
R = Projection.GetInversed() * R;
R = ModelView.GetInversed() * R;
return R.ToVec3();
}
You can transform screen co-ords to opengl using a transform matrix and your camera position.
See: https://stackoverflow.com/a/11716990/1369222
Better override onTouchEvent(MotionEvent e) of GLSurfaceView class and use the code below in the Renderer class in onSurfaceChanged(GL10 gl, int width, int height) method.
GLU.gluOrtho2D(gl,0,width,0,height);
The above code will map the screen coordinates to the openGL SurfaceView screen and you can put the points easily on the screen. But this will be only in the 2D view.

integrating JPCT-ae with QCAR(vuforia)

i know what i am going to ask is already discussed sometimes but after going through all of them i can't found my complete answer so i am asking a new question
when i tried integrating JPCT-ae with QCAR all goes well as expected, i got my modelview matrix from renderframe from jni and successfully transferred that in java to jpct model is shown perfectly as expected. but when i tried to pass this matrix to JPCT world camera my model disappear.
my code:in onsurfacechanged:
world = new World();
world.setAmbientLight(20, 20, 20);
sun = new Light(world);
sun.setIntensity(250, 250, 250);
cube = Primitives.getCube(1);
cube.calcTextureWrapSpherical();
cube.strip();
cube.build();
world.addObject(cube);
cam = world.getCamera();
cam.moveCamera(Camera.CAMERA_MOVEOUT, 10);
cam.lookAt(cube.getTransformedCenter());
SimpleVector sv = new SimpleVector();
sv.set(cube.getTransformedCenter());
sv.y -= 100;
sv.z -= 100;
sun.setPosition(sv);
MemoryHelper.compact();
and in ondraw:
com.threed.jpct.Matrix mResult = new com.threed.jpct.Matrix();
mResult.setDump(modelviewMatrix ); //modelviewMatrix i get from Qcar
cube.setRotationMatrix(mResult);
cam.setBack(mResult);
fb.clear(back);
world.renderScene(fb);
world.draw(fb);
fb.display();
after some research i found that QCAR uses a right-handed coordinate system meaning that the X positive goes right, the Y positive goes up and the Z positive comes out of screen but in JPCT coordinate system the X positive goes right, the Y positive goes down and the Z positive goes into the screen.
Qcar coordinate system:
i know that matrix QCar is giving is a 4*4 matrix having 3*3 rotational values and translation vector .
i am posting matrices to be more clear:
modelviewmatrix:
1.512537 -159.66255 -10.275316 0.0
-89.86529 -1.1592013 4.7839375 0.0
-8.619186 10.179538 -159.44305 0.0
59.182976 93.205956 437.2832 1.0
modelviewmatrix after reverse using cam.setBack(modelviewmatrix.invert(modelviewmatrix)) :
5.9083453E-5 -0.01109448 -3.3668696E-4 0.0
0.0040540528 -3.8752193E-4 0.0047518034 0.0
-0.004756433 -4.6811014E-4 0.0040459237 0.0
0.7533285 0.4116795 2.7063704 0.9999999
if i remove 13,14 and 15 matrix element assuming 3*3 rotation matrix...model is rotated properly but translation(in and out movement of image) is not there
finally i dont know what changes translation vector is needed.
so please suggest me what i am missing here?
QCAR::Matrix44F inverseMatrix = SampleMath::Matrix44FInverse(modelViewMatrix);
QCAR::Matrix44F invTransposeMatrix = SampleMath::Matrix44FTranspose(inverseMatrix);
then pass the invTransposeMatrix value to java
env->SetFloatArrayRegion(modelviewArray, 0, 16, invTransposeMatrix.data);
env->CallVoidMethod(obj, method, modelviewArray);

Rotating the Canvas impacts TouchEvents

I have a map application using an in-house map engine on Android. I'm working on a rotating Map view that rotates the map based on the phone's orientation using the Sensor Service. All works fine with the exception of dragging the map when the phone is pointing other than North. For example, if the phone is facing West, dragging the Map up still moves the Map to the South versus East as would be expected. I'm assuming translating the canvas is one possible solution but I'm honestly not sure the correct way to do this.
Here is the code I'm using to rotate the Canvas:
public void dispatchDraw(Canvas canvas)
{
canvas.save(Canvas.MATRIX_SAVE_FLAG);
// mHeading is the orientation from the Sensor
canvas.rotate(-mHeading, origin[X],origin[Y]);
mCanvas.delegate = canvas;
super.dispatchDraw(mCanvas);
canvas.restore();
}
What is the best approach to make dragging the map consistent regardless of the phones orientation? The sensormanager has a "remapcoordinates()" method but it's not clear that this will resolve my problem.
You can trivially get the delta x and delta y between two consecutive move events. To correct these values for your canvas rotation you can use some simple trignometry:
void correctPointForRotate(PointF delta, float rotation) {
// Get the angle of movement (0=up, 90=right, 180=down, 270=left)
double a = Math.atan2(-delta.x,delta.y);
a = Math.toDegrees(a); // a now ranges -180 to +180
a += 180;
// Adjust angle by amount the map is rotated around the center point
a += rotation;
a = Math.toRadians(a);
// Calculate new corrected panning deltas
double hyp = Math.sqrt(x*x + y*y);
delta.x = (float)(hyp * Math.sin(a));
delta.y = -(float)(hyp * Math.cos(a));
}

Categories

Resources