calibrated - opencv camera calibration c++

Calibración Extrínseca con cv:: SolvePnP

Question

Actualmente estoy tratando de implementar un método alternativo a la AR basada en webcam utilizando un sistema de seguimiento externo. Tengo todo en mi entorno configurado salvo para la calibración extrínseca. Decidí usar cv::solvePnP() ya que supuestamente hace exactamente lo que quiero, pero después de dos semanas me estoy quitando el pelo tratando de que funcione. Un diagrama a continuación muestra mi configuración. c1 es mi cámara, c2 es el rastreador óptico que estoy usando, M es el marcador rastreado conectado a la cámara, y ch es el tablero de ajedrez.

Tal como está, paso en mi imagen las coordenadas de píxel adquiridas con cv::findChessboardCorners() . Los puntos del mundo se adquieren con referencia al marcador M rastreado fijado a la cámara c1 (lo extrínseco es, por lo tanto, la transformación del marco de este marcador al origen de la cámara). He probado esto con conjuntos de datos de hasta 50 puntos para mitigar la posibilidad de mínimos locales, pero por ahora estoy probando con solo cuatro pares de puntos 2D / 3D. La extrínseca resultante que obtengo de la rvec y la tvec devuelta por cv::solvePnP() es masiva tanto en términos de traducción como de rotación, tanto en términos de una verdad terrestre extrínseca que he creado manualmente como de un análisis visual básico (La traducción implica una Distancia de 1100 mm cuando la cámara está a una distancia máxima de 10 mm).

Inicialmente pensé que el problema era que tenía algunas ambigüedades sobre cómo se determinó la posición de mi tablero, pero ahora estoy bastante seguro de que ese no es el caso. La matemática parece bastante sencilla y, después de todo mi trabajo para configurar el sistema, quedar atrapado en lo que es esencialmente una sola línea es una gran frustración. Honestamente, me estoy quedando sin opciones, así que si alguien puede ayudar, estaría enormemente en deuda con usted. Mi código de prueba se publica a continuación y es igual a mi implementación menos algunas llamadas de representación. La verdad de la tierra extrínseca que tengo que funciona con mi programa es la siguiente (básicamente una rotación pura alrededor de un eje y una pequeña traducción):

1     0     0     29
0   .77  -.64   32.06
0   .64   .77  -39.86
0     0     0     1

¡Gracias!

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>

int main()
{
    int imageSize     = 4;
    int markupsSize   = 4;
    std::vector<cv::Point2d> imagePoints;
    std::vector<cv::Point3d> markupsPoints;
    double tempImage[3], tempMarkups[3]; // Temp variables or iterative data construction  

    cv::Mat CamMat = (cv::Mat_<double>(3,3) <<  (566.07469648019332), 0, 318.20416967732666, 
        0,  (565.68051204299513), -254.95231997403764, 0,  0, 1);  

    cv::Mat DistMat = (cv::Mat_<double>(5,1) <<  -1.1310542849120900e-001, 4.5797249991542077e-001,
    7.8356355644908070e-003, 3.7617039978623504e-003, -1.2734302146228518e+000);

    cv::Mat rvec = cv::Mat::zeros(3,1, cv::DataType<double>::type);
    cv::Mat tvec = cv::Mat::zeros(3,1,cv::DataType<double>::type);
    cv::Mat R;
    cv::Mat extrinsic = cv::Mat::eye(4, 4, CV_64F);

    // Escape if markup lists aren''t equally sized
    if(imageSize != markupsSize)
    {
    //TODO: Replace with try, throw error code, and catch in qSlicerLocalizationModuleWidget
    return 0;
    }

    // Four principal chessboard corners only
    imagePoints.push_back(cv::Point2d(368.906, 248.123));
    imagePoints.push_back(cv::Point2d(156.583, 252.414));
    imagePoints.push_back(cv::Point2d(364.808, 132.384));
    imagePoints.push_back(cv::Point2d(156.692, 128.289));

    markupsPoints.push_back(cv::Point3d(495.115, 39.106, 93.79));  
    markupsPoints.push_back(cv::Point3d(463.143, -86.286, -51.178));  
    markupsPoints.push_back(cv::Point3d(500.236, 121.988, 24.056));  
    markupsPoints.push_back(cv::Point3d(471.276, -3.23, -127.809));    

    // Larger data set
    /*imagePoints.push_back(cv::Point2d(482.066, 233.778));
    imagePoints.push_back(cv::Point2d(448.024, 232.038));
    imagePoints.push_back(cv::Point2d(413.895, 230.785));
    imagePoints.push_back(cv::Point2d(380.653, 229.242 ));
    imagePoints.push_back(cv::Point2d(347.983, 227.785));
    imagePoints.push_back(cv::Point2d(316.103, 225.977));
    imagePoints.push_back(cv::Point2d(284.02, 224.905));
    imagePoints.push_back(cv::Point2d(252.929, 223.611));
    imagePoints.push_back(cv::Point2d(483.41, 200.527));
    imagePoints.push_back(cv::Point2d(449.456, 199.406));
    imagePoints.push_back(cv::Point2d(415.843, 197.849));
    imagePoints.push_back(cv::Point2d(382.59, 196.763));
    imagePoints.push_back(cv::Point2d(350.094, 195.616));
    imagePoints.push_back(cv::Point2d(317.922, 194.027));
    imagePoints.push_back(cv::Point2d(286.922, 192.814));
    imagePoints.push_back(cv::Point2d(256.006, 192.022));
    imagePoints.push_back(cv::Point2d(484.292, 167.816));
    imagePoints.push_back(cv::Point2d(450.678, 166.982));
    imagePoints.push_back(cv::Point2d(417.377, 165.961));

    markupsPoints.push_back(cv::Point3d(457.132, 59.822, 89.247));  
    markupsPoints.push_back(cv::Point3d(451.634, 42.015, 69.719));  
    markupsPoints.push_back(cv::Point3d(447.06, 22.927, 48.635));  
    markupsPoints.push_back(cv::Point3d(442.424, 4.454, 28.659));  
    markupsPoints.push_back(cv::Point3d(437.621, -14.395, 7.495));  
    markupsPoints.push_back(cv::Point3d(433.386, -33.034, -12.009));  
    markupsPoints.push_back(cv::Point3d(429.227, -51.001, -32.269));  
    markupsPoints.push_back(cv::Point3d(424.291, -70.266, -52.667));  
    markupsPoints.push_back(cv::Point3d(460.300, 79.769, 69.948));  
    markupsPoints.push_back(cv::Point3d(455.020, 61.379, 49.306));  
    markupsPoints.push_back(cv::Point3d(450.501, 43.288, 30.250));  
    markupsPoints.push_back(cv::Point3d(446.062, 24.572, 8.713));  
    markupsPoints.push_back(cv::Point3d(441.346, 5.823, -10.997));  
    markupsPoints.push_back(cv::Point3d(436.670, -13.135, -31.428));  
    markupsPoints.push_back(cv::Point3d(432.367, -31.428, -51.785));  
    markupsPoints.push_back(cv::Point3d(427.745, -50.016, -72.519));  
    markupsPoints.push_back(cv::Point3d(464.824, 101.129, 52.251));  
    markupsPoints.push_back(cv::Point3d(458.628, 81.864, 30.810));  
    markupsPoints.push_back(cv::Point3d(454.120, 63.546, 10.458));  */


    // Calculate camera pose
    cv::solvePnP(cv::Mat(markupsPoints), cv::Mat(imagePoints), CamMat, DistMat, rvec, tvec);
    cv::Rodrigues(rvec, R);

    // Invert results of Rodrigues by transposing rotation matrix and calculating inverted tvec
    R = R.t();  
    tvec = -R * tvec; // translation of inverse

    std::cout << "Tvec = " << std::endl << tvec << std::endl;

    // Copy R and tvec into the extrinsic matrix
    extrinsic( cv::Range(0,3), cv::Range(0,3) ) = R * 1; 
    extrinsic( cv::Range(0,3), cv::Range(3,4) ) = tvec * 1; 

    // Fill last row of homogeneous transform (0,0,0,1)
    double *p = extrinsic.ptr<double>(3);
    p[0] = p[1] = p[2] = 0; p[3] = 1;

    std::cout << "Extrinsic = " << std::endl << extrinsic << std::endl << std::endl;
    std::cout << "Extrinsic (inv) = " << std::endl << extrinsic.inv() << std::endl;
    std::cin >> tempImage[0];
    return 0;
}

EDIT 1: Intenté normalizar los valores de píxel usando el método de Chi Xu (xn = (x-cx) / f, yn = (y-cy) / f). Sin suerte :(

EDIT 2: Como parece que casi todos los que usan solvePnP usan un método donde marcan las esquinas del tablero como los vectores de su marco y origen mundial, voy a tratar de registrar mi rastreador en el tablero. Si esto funciona como se espera, la primera coordenada que marque será aproximadamente <0,0>. La transformación resultante de solvePnP se multiplicará por la inversa de la transformación de marcador de la cámara a la cámara, lo que dará como resultado (con suerte) la matriz extrínseca.

EDIT 3: Realicé los pasos descritos en el paso 2. Después de establecer un sistema de coordenadas en el tablero de ajedrez, calculé la transformación cTw del espacio del tablero de damas al espacio mundial y lo verifiqué en mi entorno de representación. Luego calculé el mTc extrínseco que representa la transformación del espacio de la cámara al espacio de tablero de ajedrez. La transformada del marcador de cámara wTr se adquirió del sistema de seguimiento. Finalmente, tomé todas las transformaciones y las multipliqué de la siguiente manera para mover mi transformación desde el origen de la cámara hasta el marcador de la cámara:

mTc*cTw*wTr

Y he aquí, dio exactamente el mismo resultado. Me estoy muriendo aquí por cualquier señal de lo que estoy haciendo mal. Si alguien tiene alguna pista, te ruego que me ayudes.

EDIT 4: Hoy me di cuenta de que había configurado mis ejes de tablero de ajedrez de tal manera que estaban en un sistema de coordenadas para zurdos. Dado que OpenCV opera utilizando la forma estándar de la mano derecha, decidí volver a intentar las pruebas con mis ejes de cuadros de tablero de ajedrez configurados a la derecha. Si bien los resultados fueron aproximadamente los mismos, noté que la transformación del mundo al tablero de ajedrez era ahora un formato de transformación no estándar, es decir, la matriz de rotación de 3x3 ya no era más o menos simétrica alrededor de la diagonal como debería ser. Esto indica que mi sistema de seguimiento no está utilizando un sistema de coordenadas de la derecha (Hubiera sido genial si lo hubieran documentado, o cualquier cosa, para el caso). Aunque no estoy seguro de cómo resolver esto, estoy seguro de que es parte del problema. Continuaré martillando y espero que alguien aquí sepa qué hacer. También agregué un diagrama que Eduardo me proporcionó en los tableros OpenCV. Gracias Eduardo!

Answer 1

Así que descubrí el problema. Como era de esperar, fue en la implementación, no en la teoría. Cuando el proyecto se diseñó inicialmente, utilizamos un rastreador basado en cámara web. Este rastreador tenía el eje Z saliendo del plano marcador. Cuando nos movimos a un rastreador óptico, el código fue portado principalmente como está. Desafortunadamente para nosotros (RIP 2 meses de mi vida), nunca verificamos si el eje z todavía salía del plano marcador (no fue así). Por lo tanto, a la tubería de renderizado se le asignó una vista incorrecta y ver vectores a la cámara de escena. Actualmente funciona principalmente, excepto que las traducciones están desactivadas por algún motivo. ¡Un problema completamente diferente!