通过单应性或solvePnP()函数估计相机位姿

Posted 2023-03-23

【中文标题】通过单应性或solvePnP()函数估计相机位姿

【英文标题】：Camera pose estimation from homography or with solvePnP() function

【发布时间】：2017-10-15 22:07:03

【问题描述】：

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

这是一个分步流程：

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

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

调整 SCNCamera 的 fov，使其与真实相机的 fov 匹配。 使用世界点 (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 = CameraPoserotationVector, 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

输出：

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

我也尝试过其他一些方法：

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

我真的被这个问题困住了，所以任何帮助都将不胜感激。

【问题讨论】：

快速阅读问题。对于 1)，对我来说需要内在的。我认为它没有出现在方程中，因为它们表示标准化相机帧中的图像点（更多信息here）。从 [u,v] 坐标和内在函数，您可以计算标准化相机帧中的点。如果您以某种方式（由用户）拥有点对：2D 图像点和 3D 对象点，最简单的解决方案是使用 solvePnP()。

调试，建议你验证：1）内在参数，2）每对点（2D / 3D）必须匹配（对应相同的物理角），3）尝试使用与用于校准的图像分辨率相同。按顺序，我将首先使用 3)、2) 和 1)。否则，两种解决方案（单应性估计和solvePnP()）都应该有效。来自单应性的姿势仅适用于平面物体并且更复杂。 solvePnP() 将直接给出相机姿态的旋转和平移向量。

ios 11 中的 AR 功能是否已解决/帮助解决了这个令人惊叹的清晰问题？

@Catree 我的错误在于转换回 SpriteKit 坐标系，如下所述。不管怎样，谢谢你的建议；）

@alexburtnik 魔鬼在细节中。

【参考方案1】：

实际上，我离 OpenCV 的工作解决方案仅一步之遥。

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

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

所以这是一个固定的findCameraPose 方法，打印了一些 cmets 和中间结果：

+ (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 CameraPoserotationVector, translationVector;

注意事项：

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

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

【讨论】：

Homography 计算后如何获取 objectPoints 和 imagePoints ？