stereo camera triangulatepoints returns nan

edit

I have a stereo camera setup (a Zed stereo camera). I have calibrated it using the Matlab stereocalibrate app, which gives what looks like a correct calibration. What I am trying to do, is find a chessboard in the camera view, and return the 3d coordinates of the points. However, what I get is a matrix of 0s and 1s, that returns nan when I try to convert it. My code is:

//setup
    int width = zed->getImageSize().width;
    int height = zed->getImageSize().height;

    cv::Mat frameLeft(height, width, CV_8UC4);  
    cv::Mat frameRight(height, width, CV_8UC4);

//fill Mats with Zed frames
sl::zed::Mat rightSide = zed->retrieveImage(static_cast<sl::zed::SIDE> (RIGHT));
slMat2cvMat(zed->retrieveImage(static_cast<sl::zed::SIDE> (RIGHT))).copyTo(frameRight);

sl::zed::Mat leftSide = zed->retrieveImage(static_cast<sl::zed::SIDE> (LEFT));
slMat2cvMat(zed->retrieveImage(static_cast<sl::zed::SIDE> (LEFT))).copyTo(frameLeft);

//get chessboard in frames
cv::Mat cornersLeft, cornersRight;
cv::Size boardSize(8, 6);

bool foundL = cv::findChessboardCorners(frameLeft, boardSize, cornersLeft, CV_CALIB_CB_ADAPTIVE_THRESH);
bool foundR = cv::findChessboardCorners(frameRight, boardSize, cornersRight, CV_CALIB_CB_ADAPTIVE_THRESH);

//setup Intrinsic matrices (from Matlab calibration)
double k1L = -0.168446;
double k2L = 0.0225543;
double p1L = 0;
double p2L = 0;

double k1R = -0.1685;
double k2R = 0.0223743;
double p1R = 0;
double p2R = 0;

cv::Mat distCoeff1 = (cv::Mat1d(1, 4) << k1L, k2L, p1L, p2L);
cv::Mat distCoeff2 = (cv::Mat1d(1, 4) << k1R, k2R, p1R, p2R);

cv::Mat camMat1 = (cv::Mat1d(3, 3) << 672.208668146945, 0,  0,  0,  677.681105920833,   0,
645.529924473109,   342.811492652937,   1);

cv::Mat camMat2 = (cv::Mat1d(3, 3) << 674.107271093112, 0,  0,  0,  678.998021719537,   0,      643.222825318966,   343.244345701717,   1);

//undistort points

cv::Mat UndistCoords1(1, 4, CV_8UC4);
cv::Mat UndistCoords2(1, 4, CV_8UC4);
cv::undistortPoints(cornersLeft, UndistCoords1, camMat1, distCoeff1);
cv::undistortPoints(cornersRight, UndistCoords2, camMat2, distCoeff2);

// Compute projection matrix for camera 1/2

cv::Mat rvec = (cv::Mat3d(3,3) << 0.999991782928634, -0.000307654498095364, -0.00404220532902106,
                0.000310337436529291,   0.999999731980639,  0.000663120897690960,
                0.00404200023350482, -0.000664369896418950, 0.999991610388184);

cv::Mat tvec = (cv::Mat1d(1, 3) << -119.474115546361,   0.0294666455704870, 0.770660705965257);


cv::Mat projMat1 = computeProjMat(camMat1, rvec, tvec);
cv::Mat projMat2 = computeProjMat(camMat2, rvec, tvec);

                //triangulate points
cv::Mat triangCoords4D;
cv::triangulatePoints(projMat1, projMat2, UndistCoords1, UndistCoords2, triangCoords4D);

std::cout << triangCoords4D << endl;

            //convert to 3d
cv::Vec4d triangCoords1 = triangCoords4D.col(0);
cv::Vec4d trinagCoords2 = triangCoords4D.col(1);
cv::Vec3d Coords13D, Coords23D;
for (unsigned int i = 0; i < 3; i++) {
        Coords13D[i] = triangCoords1[i] / triangCoords1[3];
        Coords23D[i] = trinagCoords2[i] / trinagCoords2[3];
}

My computeProjMat function is:

cv::Mat computeProjMat(cv::Mat camMat, cv::Mat rotMat, cv::Mat transVec)
{
    //pass the camera rotation & translation matrices here instead. 
    //combine to get 3 x 4 transfom matrix
    cv::Mat RTMat(3, 4, CV_64F);

    RTMat.at<double>(0, 0) = rotMat.at<double>(0, 0);
    RTMat.at<double>(0, 1) = rotMat.at<double>(0, 1);
    RTMat.at<double>(0, 2) = rotMat.at<double>(0, 2);
    RTMat.at<double>(1, 0) = rotMat.at<double>(1, 0);
    RTMat.at<double>(1, 1) = rotMat.at<double>(1, 1);
    RTMat.at<double>(1, 2 ...