Problem z przesunięciem modelu

0

Wykrywam sobie aruco za pomocą OpenCV 3.x
Wszystko fajnie ale model tak jakby przesuwał się lekko (punkt 0.0, 0.0, 0.0 nie jest w pozycji, gdzie rysowane są osie), gdy rotuję kamerę.
A no i właśnie osie rysowane za pomocą OpenCV są dobrze umiejscowione, tylko model rysowane za pomocą OGL źle.

Wyliczanie macierzy widoku:

bool invalid = false;
            for (int i = 0; i < 3 && !invalid; i++) {
                if (translationVec[i] != -999999)
                    invalid |= false;
                if (rotationVec[i] != -999999)
                    invalid |= false;
            }
            if (invalid)
                throw cv::Exception(9003, "extrinsic parameters are not set", "Marker::getModelViewMatrix", __FILE__, __LINE__);
            cv::Mat Rot(3, 3, CV_32F), Jacob;
            cv::Rodrigues(rotationVec, Rot, Jacob);
            
            float para[3][4];
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                {
                    para[i][j] = Rot.at< double >(i, j);

                }
            
            // now, add the translation
            para[0][3] = translationVec[0];
            para[1][3] = translationVec[1];
            para[2][3] = translationVec[2];
            float scale = 1;

            m_matrix = cv::Mat(4, 4, CV_32F);
            
            m_matrix.at<float>(cv::Point(0, 0)) = para[0][0];
            // R1C2
            m_matrix.at<float>(cv::Point(0, 1)) = para[0][1];
            m_matrix.at<float>(cv::Point(0, 2)) = para[0][2];
            m_matrix.at<float>(cv::Point(0, 3)) = para[0][3];
            // R2
            m_matrix.at<float>(cv::Point(1, 0)) = -para[1][0];
            m_matrix.at<float>(cv::Point(1, 1)) = -para[1][1];
            m_matrix.at<float>(cv::Point(1, 2)) = -para[1][2];
            m_matrix.at<float>(cv::Point(1, 3)) = -para[1][3];
            // R3
            m_matrix.at<float>(cv::Point(2, 0)) = -para[2][0];
            m_matrix.at<float>(cv::Point(2, 1)) = -para[2][1];
            m_matrix.at<float>(cv::Point(2, 2)) = -para[2][2];
            m_matrix.at<float>(cv::Point(2, 3)) = -para[2][3];
            m_matrix.at<float>(cv::Point(3, 0)) = 0.0;
            m_matrix.at<float>(cv::Point(3, 1)) = 0.0;
            m_matrix.at<float>(cv::Point(3, 2)) = 0.0;
            m_matrix.at<float>(cv::Point(3, 3)) = 1.0;
            if (scale != 0.0) {
                m_matrix.at<float>(cv::Point(0, 3)) *= scale;
                m_matrix.at<float>(cv::Point(1, 3)) *= scale;
                m_matrix.at<float>(cv::Point(2, 3)) *= scale;
            }

Wyliczanie macierzy projekcji:

        void Projection::InitializePerspectiveProjection(const Camera* camera, double near, double far)
        {
            /*if (cv::countNonZero(camera->GetDistortionCoefficients()) != 0)
                Log::WarningLog::Instance().Write("Wrong distortion coeficients of camera!", "The camera has distortion coefficients");*/
            if (camera->IsValid() == false)
                Log::CriticalLog::Instance().Write("Invalid camera parameters!", "CameraParameters::glGetProjectionMatrix: Invalid camera parameters");
            
            // Deterime the rsized info
            double fx = camera->GetCameraMatrix().at< double >(0, 0);
            double cx = camera->GetCameraMatrix().at< double >(0, 2);
            double fy = camera->GetCameraMatrix().at< double >(1, 1);
            double cy = camera->GetCameraMatrix().at< double >(1, 2);
            double cparam[3][4] = { { fx, 0, cx, 0 },{ 0, fy, cy, 0 },{ 0, 0, 1, 0 } };

            ArgConvGLcpara2(cparam, camera->GetFrame().size().width, camera->GetFrame().size().height, near, far);
        }

        void Projection::ArgConvGLcpara2(double cparam[3][4], int width, int height, double near, double far)
        {
            double icpara[3][4];
            double trans[3][4];
            double p[3][3], q[4][4];
            int i, j;

            cparam[0][2] *= -1.0;
            cparam[1][2] *= -1.0;
            cparam[2][2] *= -1.0;

            if (ArParamDecompMat(cparam, icpara, trans) < 0)
                throw cv::Exception(9002, "parameter error", "MarkerDetector::argConvGLcpara2", __FILE__, __LINE__);

            for (i = 0; i < 3; i++) {
                for (j = 0; j < 3; j++) {
                    p[i][j] = icpara[i][j] / icpara[2][2];
                }
            }
            q[0][0] = (2.0 * p[0][0] / width);
            q[0][1] = (2.0 * p[0][1] / width);
            q[0][2] = ((2.0 * p[0][2] / width) - 1.0);
            q[0][3] = 0.0;

            q[1][0] = 0.0;
            q[1][1] = (2.0 * p[1][1] / height);
            q[1][2] = ((2.0 * p[1][2] / height) - 1.0);
            q[1][3] = 0.0;

            q[2][0] = 0.0;
            q[2][1] = 0.0;
            q[2][2] = (far + near) / (far - near);
            q[2][3] = -2.0 * far * near / (far - near);

            q[3][0] = 0.0;
            q[3][1] = 0.0;
            q[3][2] = 1.0;
            q[3][3] = 0.0;

            for (i = 0; i < 4; i++) {
                for (j = 0; j < 3; j++) {
                    m_matrix[i + j * 4] = q[i][0] * trans[0][j] + q[i][1] * trans[1][j] + q[i][2] * trans[2][j];
                }
                m_matrix[i + 3 * 4] = q[i][0] * trans[0][3] + q[i][1] * trans[1][3] + q[i][2] * trans[2][3] + q[i][3];
            }
        }

        int Projection::ArParamDecompMat(double source[3][4], double cpara[3][4], double trans[3][4])
        {
            int r, c;
            double Cpara[3][4];
            double rem1, rem2, rem3;

            if (source[2][3] >= 0) {
                for (r = 0; r < 3; r++) {
                    for (c = 0; c < 4; c++) {
                        Cpara[r][c] = source[r][c];
                    }
                }
            }
            else {
                for (r = 0; r < 3; r++) {
                    for (c = 0; c < 4; c++) {
                        Cpara[r][c] = -(source[r][c]);
                    }
                }
            }

            for (r = 0; r < 3; r++) {
                for (c = 0; c < 4; c++) {
                    cpara[r][c] = 0.0;
                }
            }
            cpara[2][2] = Norm(Cpara[2][0], Cpara[2][1], Cpara[2][2]);
            trans[2][0] = Cpara[2][0] / cpara[2][2];
            trans[2][1] = Cpara[2][1] / cpara[2][2];
            trans[2][2] = Cpara[2][2] / cpara[2][2];
            trans[2][3] = Cpara[2][3] / cpara[2][2];

            cpara[1][2] = Dot(trans[2][0], trans[2][1], trans[2][2], Cpara[1][0], Cpara[1][1], Cpara[1][2]);
            rem1 = Cpara[1][0] - cpara[1][2] * trans[2][0];
            rem2 = Cpara[1][1] - cpara[1][2] * trans[2][1];
            rem3 = Cpara[1][2] - cpara[1][2] * trans[2][2];
            cpara[1][1] = Norm(rem1, rem2, rem3);
            trans[1][0] = rem1 / cpara[1][1];
            trans[1][1] = rem2 / cpara[1][1];
            trans[1][2] = rem3 / cpara[1][1];

            cpara[0][2] = Dot(trans[2][0], trans[2][1], trans[2][2], Cpara[0][0], Cpara[0][1], Cpara[0][2]);
            cpara[0][1] = Dot(trans[1][0], trans[1][1], trans[1][2], Cpara[0][0], Cpara[0][1], Cpara[0][2]);
            rem1 = Cpara[0][0] - cpara[0][1] * trans[1][0] - cpara[0][2] * trans[2][0];
            rem2 = Cpara[0][1] - cpara[0][1] * trans[1][1] - cpara[0][2] * trans[2][1];
            rem3 = Cpara[0][2] - cpara[0][1] * trans[1][2] - cpara[0][2] * trans[2][2];
            cpara[0][0] = Norm(rem1, rem2, rem3);
            trans[0][0] = rem1 / cpara[0][0];
            trans[0][1] = rem2 / cpara[0][0];
            trans[0][2] = rem3 / cpara[0][0];

            trans[1][3] = (Cpara[1][3] - cpara[1][2] * trans[2][3]) / cpara[1][1];
            trans[0][3] = (Cpara[0][3] - cpara[0][1] * trans[1][3] - cpara[0][2] * trans[2][3]) / cpara[0][0];

            for (r = 0; r < 3; r++) {
                for (c = 0; c < 3; c++) {
                    cpara[r][c] /= cpara[2][2];
                }
            }

            return 0;
        }

        double Projection::Norm(double a, double b, double c) { return (sqrt(a * a + b * b + c * c)); }
        double Projection::Dot(double a1, double a2, double a3, double b1, double b2, double b3) { return (a1 * b1 + a2 * b2 + a3 * b3); }

Wykrywanie tablicy aruco:

            std::vector<GLint> ids;
            std::vector<std::vector<cv::Point2f>> corners;
            cv::aruco::detectMarkers(m_camera->GetFrame(), m_dictionary, corners, ids);

            cv::Vec3d rotationVec, translationVec;
            GLint valid = estimatePoseBoard(corners, ids, m_board, m_camera->GetCameraMatrix(), cv::Mat(), rotationVec, translationVec);

            // If at least one board marker detected
            if (valid > 0)
            {
                m_transform.UpdateMatrix(translationVec, rotationVec);
                result = true;
            }

Jeśli chodzi o czytanie klatki z kamery:

if (m_inputVideo.isOpened())
            {
                while (m_inputVideo.grab())
                {
                    //m_inputVideo.retrieve(m_frame);
                    if (m_inputVideo.read(m_frame))
                    {
                        //cv::undistort(beforeUnd, m_frame, mm, m_distortionCoefficients);
                        break;
                    }
                }
            }

Nie wykonuję operacji cv::undistort - w przykładzie nie wykonywali jej, więc chyba może tak być?
Rada?

Wyliczanie projekcji i model widok wzięte bezpośrednie z aruco (starego).

0

Kod OGL to jak wygląda? Sam go tworzysz? Kod openCV za wiele mi nie mówi (innym chyba też), bo nie znam openCV.

0

Wyliczanie macierzy projekcji:

Dlaczego ten kod jest taki długi?

0

Chyba sam wylicza tą macierz projekcji, nie ma tam funkcji która sama ją wyliczy na podstawie near, far, width, height?

1 użytkowników online, w tym zalogowanych: 0, gości: 1