ios - 从单应性或使用 solvePnP() 函数估计相机姿态

Question

我正在尝试在一张照片上构建静态增强现实场景，其中平面和图像上的共面点之间有 4 个定义的对应关系。

这是一个分步流程:

1. 用户使用设备的相机添加图像。假设它包含一个以某种视角捕获的矩形。
2. 用户定义矩形的物理尺寸，它位于水平面(就 SceneKit 而言是 YOZ)。假设它的中心是世界的原点 (0, 0, 0)，那么我们可以很容易地找到每个角的 (x,y,z)。
3. 用户在图像坐标系中为矩形的每个角定义 uv 坐标。
4. SceneKit 场景是用一个相同大小的矩形创建的，并且在相同的视角下可见。
5. 可以在场景中添加和移动其他节点。

我还测量了 iPhone 摄像头相对于 A4 纸中心的位置。所以对于这个镜头，位置是 (0, 14, 42.5)，以厘米为单位测量。我的 iPhone 也稍微倾斜到 table 上(5-10 度)

使用此数据，我设置了 SCNCamera 以获得第三张图像上蓝色平面的所需视角:

let camera = SCNCamera()
camera.xFov = 66
camera.zFar = 1000
camera.zNear = 0.01

cameraNode.camera = camera
cameraAngle = -7 * CGFloat.pi / 180
cameraNode.rotation = SCNVector4(x: 1, y: 0, z: 0, w: Float(cameraAngle))
cameraNode.position = SCNVector3(x: 0, y: 14, z: 42.5)

这将给我一个引用，以便与我的结果进行比较。

为了使用 SceneKit 构建 AR，我需要:

1. 调整 SCNCamera 的视野，使其与真实相机的视野相匹配。
2. 使用世界点 (x,0,z) 和图像点 (u, v) 之间的 4 个对应关系计算相机节点的位置和旋转

H - 单应性； K - 内矩阵； <强>[R | t] - 外部矩阵

我尝试了两种方法来找到相机的变换矩阵:使用 OpenCV 的 solvePnP 和基于 4 个共面点的单应性手动计算。

手动方法:

<强>1。找出单应性

这一步成功完成，因为世界原点的 UV 坐标似乎是正确的。

<强>2。内在矩阵

为了获得 iPhone 6 的固有矩阵，我使用了 this应用程序，从 100 张分辨率为 640*480 的图像中得到以下结果:

假设输入图像的宽高比为 4:3，我可以根据分辨率缩放上述矩阵

我不确定，但感觉这里有一个潜在问题。我使用 cv::calibrationMatrixValues 检查 fovx 计算出的内在矩阵，结果是 ~50°，而它应该接近 60°。

<强>3。相机位姿矩阵

func findCameraPose(homography h: matrix_float3x3, size: CGSize) -> matrix_float4x3? {
    guard let intrinsic = intrinsicMatrix(imageSize: size),
        let intrinsicInverse = intrinsic.inverse else { return nil }

    let l1 = 1.0 / (intrinsicInverse * h.columns.0).norm
    let l2 = 1.0 / (intrinsicInverse * h.columns.1).norm
    let l3 = (l1+l2)/2

    let r1 = l1 * (intrinsicInverse * h.columns.0)
    let r2 = l2 * (intrinsicInverse * h.columns.1)
    let r3 = cross(r1, r2)

    let t = l3 * (intrinsicInverse * h.columns.2)

    return matrix_float4x3(columns: (r1, r2, r3, t))
}

结果:

因为我测量了这个特定图像的大致位置和方向，所以我知道变换矩阵，它会给出预期的结果并且它是完全不同的:

我也有点担心引用旋转矩阵的 2-3 个元素，它是 -9.1，而它应该接近于零，因为旋转非常轻微。

OpenCV 方法:

有一个solvePnP OpenCV 中的函数可以解决这类问题，所以我尝试使用它而不是重新发明轮子。

Objective-C++ 中的 OpenCV:

typedef struct CameraPose {
    SCNVector4 rotationVector;
    SCNVector3 translationVector; 
} CameraPose;

+ (CameraPose)findCameraPose: (NSArray<NSValue *> *) objectPoints imagePoints: (NSArray<NSValue *> *) imagePoints size: (CGSize) size {

    vector<Point3f> cvObjectPoints = [self convertObjectPoints:objectPoints];
    vector<Point2f> cvImagePoints = [self convertImagePoints:imagePoints withSize: size];

    cv::Mat distCoeffs(4,1,cv::DataType<double>::type, 0.0);
    cv::Mat rvec(3,1,cv::DataType<double>::type);
    cv::Mat tvec(3,1,cv::DataType<double>::type);
    cv::Mat cameraMatrix = [self intrinsicMatrixWithImageSize: size];

    cv::solvePnP(cvObjectPoints, cvImagePoints, cameraMatrix, distCoeffs, rvec, tvec);

    SCNVector4 rotationVector = SCNVector4Make(rvec.at<double>(0), rvec.at<double>(1), rvec.at<double>(2), norm(rvec));
    SCNVector3 translationVector = SCNVector3Make(tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2));
    CameraPose result = CameraPose{rotationVector, translationVector};

    return result;
}

+ (vector<Point2f>) convertImagePoints: (NSArray<NSValue *> *) array withSize: (CGSize) size {
    vector<Point2f> points;
    for (NSValue * value in array) {
        CGPoint point = [value CGPointValue];
        points.push_back(Point2f(point.x - size.width/2, point.y - size.height/2));
    }
    return points;
}

+ (vector<Point3f>) convertObjectPoints: (NSArray<NSValue *> *) array {
    vector<Point3f> points;
    for (NSValue * value in array) {
        CGPoint point = [value CGPointValue];
        points.push_back(Point3f(point.x, 0.0, -point.y));
    }
    return points;
}

+ (cv::Mat) intrinsicMatrixWithImageSize: (CGSize) imageSize {
    double f = 0.84 * max(imageSize.width, imageSize.height);
    Mat result(3,3,cv::DataType<double>::type);
    cv::setIdentity(result);
    result.at<double>(0) = f;
    result.at<double>(4) = f;
    return result;
}

在 Swift 中的用法:

func testSolvePnP() {
    let source = modelPoints().map { NSValue(cgPoint: $0) }
    let destination = perspectivePicker.currentPerspective.map { NSValue(cgPoint: $0)}

    let cameraPose = CameraPoseDetector.findCameraPose(source, imagePoints: destination, size: backgroundImageView.size);    
    cameraNode.rotation = cameraPose.rotationVector
    cameraNode.position = cameraPose.translationVector
}

输出:

结果更好，但与我的预期相去甚远。

我还尝试过一些其他的东西:

1. This question非常相似，尽管我不明白接受的答案在没有内在函数的情况下是如何工作的。
2. decomposeHomographyMat也没有给我预期的结果

我真的被这个问题困住了，所以非常感谢任何帮助。

Answer 1

实际上，我距离使用 OpenCV 的工作解决方案只有一步之遥。

第二种方法的问题是我忘记将 solvePnP 的输出转换回 SpriteKit 的坐标系。

请注意，输入(图像和世界点)实际上已正确转换为 OpenCV 坐标系(convertObjectPoints: 和 convertImagePoints:withSize: 方法)

所以这是一个固定的 findCameraPose 方法，其中打印了一些注释和中间结果:

+ (CameraPose)findCameraPose: (NSArray<NSValue *> *) objectPoints imagePoints: (NSArray<NSValue *> *) imagePoints size: (CGSize) size {

    vector<Point3f> cvObjectPoints = [self convertObjectPoints:objectPoints];
    vector<Point2f> cvImagePoints = [self convertImagePoints:imagePoints withSize: size];

    std::cout << "object points: " << cvObjectPoints << std::endl;
    std::cout << "image points: " << cvImagePoints << std::endl;

    cv::Mat distCoeffs(4,1,cv::DataType<double>::type, 0.0);
    cv::Mat rvec(3,1,cv::DataType<double>::type);
    cv::Mat tvec(3,1,cv::DataType<double>::type);
    cv::Mat cameraMatrix = [self intrinsicMatrixWithImageSize: size];

    cv::solvePnP(cvObjectPoints, cvImagePoints, cameraMatrix, distCoeffs, rvec, tvec);

    std::cout << "rvec: " << rvec << std::endl;
    std::cout << "tvec: " << tvec << std::endl;

    std::vector<cv::Point2f> projectedPoints;
    cvObjectPoints.push_back(Point3f(0.0, 0.0, 0.0));
    cv::projectPoints(cvObjectPoints, rvec, tvec, cameraMatrix, distCoeffs, projectedPoints);

    for(unsigned int i = 0; i < projectedPoints.size(); ++i) {
        std::cout << "Image point: " << cvImagePoints[i] << " Projected to " << projectedPoints[i] << std::endl;
    }


    cv::Mat RotX(3, 3, cv::DataType<double>::type);
    cv::setIdentity(RotX);
    RotX.at<double>(4) = -1; //cos(180) = -1
    RotX.at<double>(8) = -1;

    cv::Mat R;
    cv::Rodrigues(rvec, R);

    R = R.t();  // rotation of inverse
    Mat rvecConverted;
    Rodrigues(R, rvecConverted); //
    std::cout << "rvec in world coords:\n" << rvecConverted << std::endl;
    rvecConverted = RotX * rvecConverted;
    std::cout << "rvec scenekit :\n" << rvecConverted << std::endl;

    Mat tvecConverted = -R * tvec;
    std::cout << "tvec in world coords:\n" << tvecConverted << std::endl;
    tvecConverted = RotX * tvecConverted;
    std::cout << "tvec scenekit :\n" << tvecConverted << std::endl;

    SCNVector4 rotationVector = SCNVector4Make(rvecConverted.at<double>(0), rvecConverted.at<double>(1), rvecConverted.at<double>(2), norm(rvecConverted));
    SCNVector3 translationVector = SCNVector3Make(tvecConverted.at<double>(0), tvecConverted.at<double>(1), tvecConverted.at<double>(2));

    return CameraPose{rotationVector, translationVector};
}

注意事项:

1. RotX 矩阵表示rotation绕 x 轴旋转 180 度，这会将任何向量从 OpenCV 坐标系转换为 SpriteKit 的坐标系

2. Rodrigues方法将旋转向量转换为旋转矩阵 (3x3)，反之亦然