OpenGL InterOpt Rotation Vector and Translation Vector

edit

Here is an implementation for the OpenGL interop stuff. I am having difficulty with the rotation matrix created for OpenGL and applying it to my model. Does anyone know the correct format for the GLDouble and GLFlost structs that are required to do the rotations required.

#include "opencv2/highgui/highgui.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/contrib/contrib.hpp"
#include "opencv2/ml/ml.hpp"
#include <vector>
#include <string>
#include <opencv2/core/opengl_interop.hpp>
#include <windows.h>
#if defined(__APPLE__)
    #include <OpenGL/gl.h>
    #include <OpenGL/glu.h>
#else
    #include <gl\GL.h>
    #include <gl\GLU.h>
#endif

void draw(void* userdata);
struct DrawData
{
    cv::ogl::Arrays arr;
    cv::ogl::Texture2D tex;
    cv::ogl::Buffer indices;
    cv::Mat cameraMatrix;
    cv::Mat rvec;
    cv::Mat tvec;
};

void draw(void* userdata)
{
    DrawData* data = static_cast<DrawData*>(userdata);

    cv::Mat poseMat = cv::Mat(4,4,CV_64FC1),tvec,rvec;
    for (int f=0; f<3; f++)
        for (int c=0; c<3; c++)
                poseMat.at<double>(c,f) = rvec.at<double>(f,c);      //transposed
    poseMat.at<double>(3,0) = 0.0;
    poseMat.at<double>(7,0) = 0.0;
    poseMat.at<double>(11,0) = 0.0;
    poseMat.at<double>(12,0) = tvec.at<double>(0,1);
    poseMat.at<double>(13,0) = tvec.at<double>(0,2);
    poseMat.at<double>(14,0) = tvec.at<double>(0,3);
    poseMat.at<double>(15,0) = 1.0;

    const GLdouble* projectionMatrix;
    const GLfloat* poseMatGL;

    glMatrixMode( GL_PROJECTION );
    glLoadMatrixd( projectionMatrix );
    glMatrixMode( GL_MODELVIEW );
    glScalef( 1.0f, 1.0f, -1.0f);
    glMultMatrixf(poseMatGL);

    cv::ogl::render(data->arr, cv::ogl::POINTS,cv::Scalar(255));
}

int main(int argc, char **argv) 
{
    cv::namedWindow("OpenGL Window",cv::WINDOW_OPENGL);
    cv::resizeWindow("OpenGL Window", 640, 480);
    std::vector<cv::Point3d> model; //Get model points.

    DrawData data;
    data.cameraMatrix=cv::Mat(4,4,CV_64FC1);
    data.rvec=cv::Mat(3,3,CV_64FC1);
    data.tvec=cv::Mat(1,3,CV_64FC1);

    data.arr.setVertexArray(model);
    cv::setOpenGlDrawCallback("OpenGL", draw, &data);
    cv::waitKey(5000);
    return 0;
}