Project Tango：确定 IntersectionPointPlaneModelPair 是否与 Gravity 对齐

Question

剧透警告：我不确定我是否以正确的方式使用四元数。

我使用 TangoSupport.fitPlaneModelNearClick(...) 方法得到了 IntersectionPointPlaneModelPair pair。我现在想知道这个平面是否与重力对齐（或多或少）。我的方法是从 pair.planeModel 创建一个 Quaternion (Rajawali)，另一个从 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){
     ...   
}

但是，这不起作用，因为 product 结果与 plane 相同。提前致谢！

Answer 1

我发现，我对 Quaternion 的理解有误。我还发现 this formula 用于平面的角度计算。因此，我将我的实现更改为以下内容：

编辑：新答案（下面是旧答案）

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

并且在 class 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;
}

Project Tango：确定 IntersectionPointPlaneModelPair 是否与 Gravity 对齐

Project Tango: Determine whether an IntersectionPointPlaneModelPair is aligned with Gravity

java

quaternions

google-project-tango

rajawali

编辑：新答案（下面是旧答案）

旧答案