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

Question

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

这是一步一步的流程：

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

我还测量了 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，我需要：

调整SCNCamera的视野，使其与真实相机的视野相匹配。
使用世界点 (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 方法：

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

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: [=13=]) }
    let destination = perspectivePicker.currentPerspective.map { NSValue(cgPoint: [=13=])}

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

输出：

结果比我预期的要好一些。

我还尝试了一些其他的东西：

This question 非常相似，但我不明白接受的答案在没有内在函数的情况下是如何工作的。
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};
}

备注：

RotX 矩阵意味着 rotation 绕 x 轴 180 度，这会将任何向量从 OpenCV 坐标系转换到 SpriteKit 的
Rodrigues 方法将旋转向量转换为旋转矩阵 (3x3)，反之亦然

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

Camera pose estimation from homography or with solvePnP() function

opencv

homography

augmented-reality

ios

scenekit

手动方法：

OpenCV 方法：