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 (RIGHT));
slMat2cvMat(zed->retrieveImage(static_cast (RIGHT))).copyTo(frameRight);
sl::zed::Mat leftSide = zed->retrieveImage(static_cast (LEFT));
slMat2cvMat(zed->retrieveImage(static_cast (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(0, 0) = rotMat.at(0, 0);
RTMat.at(0, 1) = rotMat.at(0, 1);
RTMat.at(0, 2) = rotMat.at(0, 2);
RTMat.at(1, 0) = rotMat.at(1, 0);
RTMat.at(1, 1) = rotMat.at(1, 1);
RTMat.at(1, 2) = rotMat.at(1, 2);
RTMat.at(2, 0) = rotMat.at(2, 0);
RTMat.at(2, 1) = rotMat.at(2, 1);
RTMat.at(2, 2) = rotMat.at(2, 2);
RTMat.at(0, 3) = transVec.at(0, 0);
RTMat.at(1, 3) = transVec.at(0, 1);
RTMat.at(2, 3) = transVec.at(0, 2);
// std::cout << RTMat << endl;
//3. Compute projection matrix by multiplying intrinsic parameter
//matrix (A) with 3 x 4 rotation and translation pose matrix (RT).
cv::Mat mProjection1 = camMat * RTMat;
return mProjection1;
}
Where am i going wrong here? Any help greatly appreciated!
Thank you.
↧