OpenCV cv:solvePnP to GLM matrix to render image with PCL pointcloud

asked 2014-11-11 20:55:58 -0500

Ropeburn gravatar image

I am having trouble converting the rotation and translation matrices returned by cv:solvePnP into an OpenGL (GLM) modelview matrix. I am trying to render the checkerboard texture over the checkerboard in the point cloud and my conversation is either not right or I am missing a step some where. The GL camera should be at the location of the camera in world space and is looking up the positive Z axis. What am I doing wrong?

  {
  Pnt2DLst point2DLst;
  bool     found = false;

   found = cv::findChessboardCorners(_image,_boardSize,point2DLst,
                                     cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK | cv::CALIB_CB_NORMALIZE_IMAGE);  

    if (found)
    {
    Pnt3DLst  point3DLst;
    cv::Mat   vRc;
    cv::Mat   vTc;

      create3DBoardPoints(point3DLst);

      found = cv::solvePnP(point3DLst,point2DLst,_cM,_dC,vRc,vTc);

      if (found)
      {
      cv::Mat       mRod; 

        cv::Rodrigues(vRc,mRod);

        {
        glm::vec3 vT = glm::vec3(-(float)((double *) vTc.data)[0], 
                                 -(float)((double *) vTc.data)[1], 
                                 -(float)((double *) vTc.data)[2]);
        glm::mat3 mR = glm::mat3( (float)((double *)mRod.data)[0],  
                                  (float)((double *)mRod.data)[3],  
                                  (float)((double *)mRod.data)[6],
                                  (float)((double *)mRod.data)[1], 
                                 -(float)((double *)mRod.data)[4], 
                                  (float)((double *)mRod.data)[7],
                                  (float)((double *)mRod.data)[2],  
                                  (float)((double *)mRod.data)[5], 
                                 -(float)((double *)mRod.data)[8]);
        glm::mat4 mT = glm::translate(glm::mat4(mR),vT / 1000.0f);  // convert to meters

          _pRO->setImageTransform(mT);
        }     
      }
    }
  }

image description image description

edit retag flag offensive close merge delete