Учитывайте известные значения при оценке позы камеры - PullRequest
Новая
       75

Учитывайте известные значения при оценке позы камеры

0 голосов
/ 03 февраля 2019

Я пытаюсь оценить положение моей камеры относительно 4-х известных мировых координат.Из-за ограничений в моей системе некоторые детали позы камеры известны и исправлены.А именно его вертикальное смещение, тангаж и крен являются известными константами.Мне интересно, как я могу использовать эту информацию для улучшения результата алгоритма OpenCV SolvePNP.

В настоящее время я обнаружил, что без этой информации малейшие изменения в точках изображения могут привести к значительным изменениям в результатах.Например, я помещаю камеру в известную позу: 

X = 2ft
Y = 1ft
Z = 5ft
ROLL = 0 degrees
PITCH = 180 degrees
YAW = 0 degrees

Затем, позволяя камере отслеживать 4 точки изображения и вычисляя позу, я получаю следующее: 

 {48.0, 138.0} 
 {40.0, 136.0} 
 {45.0, 114.0} 
 {54.0, 114.0} 
 X = 2.4235989629072314 
 Y = 1.2370888865388812 
 Z = 4.717115774644273 
 ROLL = -7.555688896466208 
 PITCH = 165.9771402205544 
 YAW = 1.5292313860396367 
 ============================= 
 {48.0, 138.0} 
 {40.0, 136.0} 
 {45.0, 114.0} 
 {53.0, 114.0} 
 X = 2.864381855099463 
 Y = 0.9925235082316144 
 Z = 4.605675917036408 
 ROLL = -7.962130849477691 
 PITCH = 168.14583005865828 
 YAW = 6.697852245666419 
 ============================= 
 {48.0, 137.0} 
 {40.0, 136.0} 
 {46.0, 112.0} 
 {53.0, 114.0} 
 X = -3.3067589122064986 
 Y = -0.2727418953073936 
 Z = 4.393018415532629 
 ROLL = -6.929120013468928 
 PITCH = -168.6014586711855 
 YAW = -59.587627235667476 

public VisionProcessor() {
    // Define bottom right corner of left vision target as origin
    mObjectPoints = new MatOfPoint3f(
        new Point3(0.0, 0.0, 0.0),        // bottom right
        new Point3(-1.9363, 0.5008, 0.0), // bottom left
        new Point3(-0.5593, 5.8258, 0.0), // top-left
        new Point3(1.377, 5.325, 0.0)     // top-right
    );

    mCameraMatrix = Mat.eye(3, 3, CvType.CV_64F);
    mCameraMatrix.put(0, 0, 2.5751292067328632e+02);
    mCameraMatrix.put(0, 2, 1.5971077914723165e+02);
    mCameraMatrix.put(1, 1, 2.5635071715912881e+02);
    mCameraMatrix.put(1, 2, 1.1971433393615548e+02);

    mDistortionCoefficients = new MatOfDouble(
        2.9684613693070039e-01, 
        -1.4380252254747885e+00,
        -2.2098421479494509e-03, 
        -3.3894563533907176e-03, 
        2.5344430354806740e+00
    );
}

public void update(double[] cornX, double[] cornY) {
    MatOfPoint2f imagePoints = new MatOfPoint2f(
        mPointFinder.getBottomRight(), 
        mPointFinder.getBottomLeft(),
        mPointFinder.getTopLeft(), 
        mPointFinder.getTopRight()
    );

    Mat rotationVector = new MatOfDouble(Math.PI, 0, 0);
    Mat translationVector = new MatOfDouble(-24, 0, 60);
    Calib3d.solvePnP(mObjectPoints, imagePoints, mCameraMatrix, mDistortionCoefficients,
                     rotationVector, translationVector);

    Mat rotationMatrix = new Mat();
    Calib3d.Rodrigues(rotationVector, rotationMatrix);

    Mat projectionMatrix = new Mat(3, 4, CvType.CV_64F);
    projectionMatrix.put(0, 0,
        rotationMatrix.get(0, 0)[0], rotationMatrix.get(0, 1)[0], rotationMatrix.get(0, 2)[0], translationVector.get(0, 0)[0],
        rotationMatrix.get(1, 0)[0], rotationMatrix.get(1, 1)[0], rotationMatrix.get(1, 2)[0], translationVector.get(1, 0)[0],
        rotationMatrix.get(2, 0)[0], rotationMatrix.get(2, 1)[0], rotationMatrix.get(2, 2)[0], translationVector.get(2, 0)[0]
    );

    Mat cameraMatrix = new Mat();
    Mat rotMatrix = new Mat();
    Mat transVect = new Mat();
    Mat rotMatrixX = new Mat();
    Mat rotMatrixY = new Mat();
    Mat rotMatrixZ = new Mat(); 
    Mat eulerAngles = new Mat();
    Calib3d.decomposeProjectionMatrix(projectionMatrix, cameraMatrix, rotMatrix, transVect, rotMatrixX, rotMatrixY, rotMatrixZ, eulerAngles);

    System.out.println("X = " + translationVector.get(0,0)[0] / 12.0);
    System.out.println("Y = " + translationVector.get(1,0)[0] / 12.0);
    System.out.println("Z = " + translationVector.get(2,0)[0] / 12.0);
    System.out.println("ROLL = " + eulerAngles.get(2,0)[0]);
    System.out.println("PITCH = " + eulerAngles.get(0,0)[0]);
    System.out.println("YAW = " + eulerAngles.get(1,0)[0]);
    System.out.println("=============================")
}

Я ожидаю, что выходные данные системы будут приблизительно приближены к положению реального мира, но данные показывают, что малейшее изменение точек изображения может существенно повлиять на результирующую позу.

...