Оценка позы камеры по гомографии или с функцией solvePnP()

Я пытаюсь построить статическую сцену дополненной реальности над фотографией с 4 определенными соответствиями между копланарными точками на плоскости и изображением.

вот шаг за шагом поток:

1. пользователь добавляет изображение с помощью камеры устройства. Предположим, что он содержит прямоугольник, захваченный с некоторой перспективой.
2. пользователь определяет физический размер прямоугольника, который лежит в горизонтальной плоскости (YOZ в терминах SceneKit). Предположим, это центр-это начало мира (0, 0, 0), поэтому мы можем легко найти (x,y, z) для каждого угла.
3. пользователь определяет UV-координаты в системе координат изображения для каждого угла прямоугольника.
4. сцена SceneKit создается с прямоугольником того же размера и видимым в той же перспективе.
5. другие узлы могут быть добавлены и перемещены в сцене.

Я также измерил положение камеры iphone относительно центра бумаги формата А4. Таким образом, для этого снимка позиция была (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)

Это даст мне ссылку, чтобы сравнить мой результат С.

для того, чтобы построить AR с SceneKit мне нужно:

1. настройки SCNCamera fov, так что он соответствует реальной камере fov.
2. вычислить положение и вращение для узла камеры, используя 4 correnspondensies между точками мира (x, 0, z) и точками изображения (u, v)

H - фонетическое письмо; K - характеристической матрицы; [R/t] - внешняя матрица

я попробовал два подхода, чтобы найти матрицу преобразования для камеры: использование solvePnP из OpenCV и ручной расчет из гомографии на основе 4 копланарных точек.

подход:

1. Узнайте гомографию

этот шаг выполняется успешно, так как УФ-координаты происхождения мира кажутся правильными.

2. Внутренняя матрица

чтобы получить встроенную матрицу iPhone 6, я использовал этой app, который дал мне следующее результат из 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 для такого рода проблем, поэтому я попытался использовать его вместо изобретения колеса.

OpenCV в Objective-C++:

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

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

выход:

в результат лучше, но далек от моих ожиданий.

некоторые другие вещи, которые я тоже пробовал:

1. этот вопрос очень похоже, хотя я не понимаю, как принятый ответ работает без внутренних элементов.
2. decomposeHomographyMat также не дал мне результат, который я ожидал

Я действительно застрял с этой проблемой, поэтому любая помощь будет много оцененный.