从单应性矩阵或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略微向桌子倾斜（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. 使用4个世界点（x,0,z）和图像点（u，v）之间的对应关系，计算相机节点的位置和旋转。

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: $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. 这个问题非常相似，但我不明白为什么被接受的答案没有使用内部函数。
2. decomposeHomographyMat也没有给我期望中的结果。

我真的卡在这个问题上了，所以任何帮助都将不胜感激。

- alexburtnik

---

快速阅读问题。对于1），我需要内在的东西。我认为它没有出现在方程中，因为它们在规范化相机框架中表达了图像点（更多信息在此处）。从[u，v]坐标和内部参数，您可以计算规范化相机框架中的点。如果您以某种方式（由用户）拥有点对：2D图像点和3D对象点，则最简单的解决方案是使用solvePnP()。 - Catree

为了进行调试，我建议您验证以下内容：1）固有参数，2）每对点（2D / 3D）必须匹配（对应于相同的物理角落），3）尝试使用与校准所用分辨率相同的图像分辨率。按顺序，我会先从3）开始，接着是2）和1）。否则，两种解决方案（单应估计和solvePnP()）都应该有效。单应法估计的姿态仅适用于平面物体，并且更加复杂。solvePnP()将直接提供相机姿态的旋转和平移向量。 - Catree

iOS 11中的AR功能是否解决或有所帮助这个非常清晰明了的问题？ - Confused

@Catree我的错误在于将坐标系转换回SpriteKit坐标系，如下所述。无论如何，感谢您的建议;) - alexburtnik

@Confused 是的，我认为ARKit会大大简化我的问题。但是我需要一个适用于iOS 9-10的解决方案。此外，ARKit仅支持从6s / 6s plus开始的新设备:( - alexburtnik

@alexburtnik 细节决定成败。 - Catree

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 矩阵表示绕x轴旋转180度的旋转，这将把OpenCV坐标系中的任何向量转换为SpriteKit的向量

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

- alexburtnik

---

在计算单应性矩阵后，我该如何获取objectPoints和imagePoints？ - Nuibb