OpenCV cv:solvePnP to GLM matrix to render image with PCL pointcloud
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);
}
}
}
}