[英]Project Tango: Determine whether an IntersectionPointPlaneModelPair is aligned with Gravity
Spoiler Alert: I am not sure whether or not I am using Quaternions in the correct way. 剧透警报:我不确定我是否以正确的方式使用四元数。
I have an IntersectionPointPlaneModelPair pair
from using the TangoSupport.fitPlaneModelNearClick(...)
method. 我使用TangoSupport.fitPlaneModelNearClick(...)
方法获得了一个IntersectionPointPlaneModelPair pair
。 I would now like to find out whether or not this plane is aligned with Gravity (more or less). 现在,我想确定此平面是否与“重力”对齐(或多或少)。 My approach was to create a Quaternion
( Rajawali ) from the pair.planeModel
and another from ScenePoseCalculator.TANGO_WORLD_UP
and a rotation of 0.0
, multiply them and determine the angle between the original and the product: 我的方法是创建一个Quaternion
( 拉贾瓦利从) pair.planeModel
从和另一个ScenePoseCalculator.TANGO_WORLD_UP
和的旋转0.0
,它们相乘并确定原始和产品之间的角度:
IntersectionPointPlaneModelPair pair= TangoSupport.fitPlaneModelNearClick(...);
double x = 0.05; // subject to change
double[] p = pair.planeModel;
Quaternion plane = new Quaternion(p[0], p[1], p[2], p[3]);
plane.normalize();
Quaternion gravity = Quaternion(ScenePoseCalculator.TANGO_WORLD_UP.clone(), 0.0);
Quaternion product = plane.multiply(gravity);
if (plane.angleBetween(product) > x){
...
}
However, this does not work, because the product
turned out to be identical to the plane
. 但是,这不起作用,因为结果证明product
与plane
相同。 Thanks in advance! 提前致谢!
I found out, that I was having a wrong understanding of Quaternion
s. 我发现我对Quaternion
的理解有误。 I also found this formula for angle calculation of planes. 我还找到了用于平面角度计算的公式 。 Therefore I changed my implementation to be the following: 因此,我将实现更改为以下内容:
private boolean isAlignedWithGravity(IntersectionPointPlaneModelPair candidate,
TangoPoseData devicePose, double maxDeviation) {
Matrix4 adfTdevice = ScenePoseCalculator.tangoPoseToMatrix(devicePose);
Vector3 gravityVector = ScenePoseCalculator.TANGO_WORLD_UP.clone();
adfTdevice.clone().multiply(mExtrinsics.getDeviceTDepthCamera()).inverse().
rotateVector(gravityVector);
double[] gravity = new double[]{gravityVector.x, gravityVector.y, gravityVector.z};
double angle = VectorUtilities.getAngleBetweenVectors(candidate.planeModel, gravity);
// vectors should be perpendicular => 90° => PI / 2 in radians
double target = Math.PI / 2;
return (Math.abs(target - angle) <= maxDeviation);
}
And in a class VectorUtilities
: 在VectorUtilities
类中:
/**
* Calculates the angle between two planes according to http://www.wolframalpha
* .com/input/?i=dihedral+angle
*/
public static double getAngleBetweenVectors(double[] a, double[] b) {
double numerator = 0;
for (int i = 0; i < Math.min(a.length, b.length); i++){
numerator += a[i] * b[i];
}
double denominator = getLength(a) * getLength(b);
return Math.acos(numerator / denominator);
}
public static double getLength(double[] vector) {
double sum = 0.0;
for (double dimension : vector) {
sum += (dimension * dimension);
}
return Math.sqrt(sum);
}
private boolean isAlignedWithGravity(IntersectionPointPlaneModelPair pair,
TangoPoseData devicePose) {
Matrix4 adfTdevice = ScenePoseCalculator.tangoPoseToMatrix(devicePose);
Vector3 gravityVector = ScenePoseCalculator.TANGO_WORLD_UP.clone();
adfTdevice.clone().multiply(mExtrinsics.getDeviceTDepthCamera()).inverse().
rotateVector(gravityVector);
double[] gravity = new double[]{gravityVector.x, gravityVector.y, gravityVector.z};
double angle = getAngleBetweenPlanes(pair.planeModel, gravity);
Log.d(TAG, "angle: " + angle);
if (angle < 0.1) {
return false;
}
return true;
}
/**
* Calculates the angle between two planes according to http://mathworld.wolfram
* .com/DihedralAngle.html
*/
private double getAngleBetweenPlanes(double[] a, double[] b) {
double numerator = Math.abs(a[0] * b[0] + a[1] * b[1] + a[2] * b[2]);
double aFactor = Math.sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
double bFactor = Math.sqrt(b[0] * b[0] + b[1] * b[1] + b[2] * b[2]);
double denumerator = aFactor * bFactor;
double result = Math.acos(numerator / denumerator);
return result;
}
声明:本站的技术帖子网页,遵循CC BY-SA 4.0协议,如果您需要转载,请注明本站网址或者原文地址。任何问题请咨询:yoyou2525@163.com.