Calibration is carried out using a stereo camera.
However, the result value is output as Zoom.
What I want is to measure the actual distance after calibration.
Help me.
// global variables
Size boardSize;
Size2f squareSize;
vector<vector<Point2f>> imagePoints1, imagePoints2;
Size imageSize;
Mat image1, image2;
Mat disparityMap;
// mouse event
void onMouse(int event, int x, int y, int flags, void* param)
{
char text[100];
uchar disparity = disparityMap.at<uchar>(y, x);
if (disparity != 0) {
float baseLength = 60.93;
float flocalLength = 4.3;
//Focal length(Pixel) * Baseline(mm) / x2-x1(Pixel) = Z(mm)
//Z_actual = (b * focal_length) / (x_camL - x_camR)
//focal_pixel = (focal_mm / sensor_width_mm) * image_width_in_pixels
//focal_pixel = (image_width_in_pixels * 0.5) / tan(FOV * 0.5 * PI / 180)
// !!! i want get real distance
double z = (baseLength * flocalLength) / disparity;
char szBuffer[50] = { 0, };
sprintf(szBuffer, "%f", z);
putText(disparityMap, szBuffer, cvPoint(x, y),
FONT_HERSHEY_COMPLEX_SMALL, 1.2, cvScalar(0, 0, 255), 1.5, CV_AA);
printf("%s\n", szBuffer);
}
imshow("disparity", disparityMap);
waitKey(10);
}
// find chessboard corners
int find(Mat view1, Mat view2, bool reg)
{
if (view1.empty()) {
return 0;
}
if (view2.empty()) {
return 0;
}
assert(view1.size() == view2.size());
imageSize = view1.size();
Mat viewGray1, viewGray2;
cvtColor(view1, viewGray1, CV_BGR2GRAY);
cvtColor(view2, viewGray2, CV_BGR2GRAY);
vector<Point2f> pointbuf1, pointbuf2;
bool found1 = findChessboardCorners(view1, boardSize, pointbuf1,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
bool found2 = findChessboardCorners(view2, boardSize, pointbuf2,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
if (found1 && found2) {
// improve the found corners' coordinate accuracy
cornerSubPix(viewGray1, pointbuf1, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1));
cornerSubPix(viewGray2, pointbuf2, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1));
if (reg) {
imagePoints1.push_back(pointbuf1);
imagePoints2.push_back(pointbuf2);
}
drawChessboardCorners(view1, boardSize, Mat(pointbuf1), found1);
drawChessboardCorners(view2, boardSize, Mat(pointbuf2), found2);
}
return imagePoints1.size();
}
// start calibration (find() -> runcalibration())
bool runCalibration()
{
Mat map1, map2, map3, map4;
vector<vector<Point3f> > objectPoints(1);
for (int i = 0; i < boardSize.height; i++) {
for (int j = 0; j < boardSize.width; j++) {
objectPoints[0].push_back(Point3f(j*squareSize.width, i*squareSize.height, 0.f));
}
}
objectPoints.resize(imagePoints1.size(), objectPoints[0]);
Mat cameraMatrix1 = Mat::eye(3, 3, CV_64F);
Mat cameraMatrix2 = Mat::eye(3, 3, CV_64F);
Mat distCoeffs1 = Mat::zeros(8, 1, CV_64F);
Mat distCoeffs2 = Mat::zeros(8, 1, CV_64F);
Mat R, T, E, F;
cameraMatrix1 = initCameraMatrix2D(objectPoints, imagePoints1, imageSize, 0);
cameraMatrix2 = initCameraMatrix2D(objectPoints, imagePoints2, imageSize, 0);
double rms = stereoCalibrate(objectPoints, imagePoints1, imagePoints2,
cameraMatrix1, distCoeffs1,
cameraMatrix2, distCoeffs2,
imageSize, R, T, E, F,
CALIB_FIX_ASPECT_RATIO +
CALIB_ZERO_TANGENT_DIST +
CALIB_USE_INTRINSIC_GUESS +
CALIB_SAME_FOCAL_LENGTH +
CALIB_RATIONAL_MODEL +
CALIB_FIX_K3 + CALIB_FIX_K4 + CALIB_FIX_K5,
TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, 1e-5));
bool success = checkRange(cameraMatrix1) && checkRange(distCoeffs1) && checkRange(cameraMatrix2) && checkRange(distCoeffs2);
if (success) {
FileStorage fs("D:\\Sources\\trunk\\Sources\\Stereo_Scopic\\intrinsics.yml", CV_STORAGE_WRITE);
if (!fs.isOpened()) return false;
fs << "M1" << cameraMatrix1;
fs << "D1" << distCoeffs1;
fs << "M2" << cameraMatrix2;
fs << "D2" << distCoeffs2;
fs.release();
Mat R1, R2, P1, P2, Q;
Rect validRoi1, validRoi2;
stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2,
imageSize, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, 1, imageSize, &validRoi1, &validRoi2);
fs.open("D:\\Sources\\trunk\\Sources\\Stereo_Scopic\\extrinsics.yml", CV_STORAGE_WRITE);
if (!fs.isOpened()) return false;
fs << "R" << R;
fs << "T" << T;
fs << "R1" << R1;
fs << "R2" << R2;
fs << "P1" << P1;
fs << "P2" << P2;
fs << "Q" << Q;
fs << "imageSize" << imageSize;
fs.release();
initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1, imageSize, CV_16SC2, map1, map2);
initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2, P2, imageSize, CV_16SC2, map3, map4);
}
return success;
}
// get disparity
int getDisparity(Mat leftImg, Mat rightImg)
{
int SADWindowSize, numberOfDisparities;
bool no_display;
float scale;
numberOfDisparities = /*16*/ 192;
SADWindowSize = /*3*/64;
Ptr<StereoSGBM> sgbm = StereoSGBM::create(0, numberOfDisparities, SADWindowSize);
scale = 1;
int color_mode = -1;
Mat img1 = leftImg;
Mat img2 = rightImg;
if (scale != 1.f)
{
Mat temp1, temp2;
int method = scale < 1 ? INTER_AREA : INTER_CUBIC;
resize(img1, temp1, Size(), scale, scale, method);
img1 = temp1;
resize(img2, temp2, Size(), scale, scale, method);
img2 = temp2;
}
Size img_size = img1.size();
numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : ((img_size.width / 8) + 15) & -16;
sgbm->setPreFilterCap(63);
int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
sgbm->setBlockSize(sgbmWinSize);
int cn = img1.channels();
sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);
sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);
sgbm->setMinDisparity(0);
sgbm->setNumDisparities(numberOfDisparities);
sgbm->setUniquenessRatio(10);
sgbm->setSpeckleWindowSize(100);
sgbm->setSpeckleRange(32);
sgbm->setDisp12MaxDiff(1);
sgbm->setMode(StereoSGBM::MODE_SGBM);
Mat disp, disp8;
sgbm->compute(img1, img2, disp);
disp.convertTo(disp8, CV_8U, 255 / (numberOfDisparities*16.));
namedWindow("left image", 1);
imshow("left image", img1);
namedWindow("right image", 1);
imshow("right image", img2);
cv::cvtColor(disp8, disparityMap, cv::COLOR_GRAY2BGR);
namedWindow("disparity", 0);
imshow("disparity", disparityMap);
setMouseCallback("disparity", onMouse, NULL);
waitKey(1);
return 0;
}
// stereo match (this get parameter and remaping image)
int stereoMatching(Mat leftImg, Mat rightImg) {
if (leftImg.empty() || rightImg.empty()) {
return -1;
}
float scale = 1.f;
Mat img1 = leftImg.clone();
Mat img2 = rightImg.clone();
Size img_size = img1.size();
string intrinsic_filename = "D:\\Sources\\trunk\\Sources\\Stereo_Scopic\\intrinsics.yml";
string extrinsic_filename = "D:\\Sources\\trunk\\Sources\\Stereo_Scopic\\extrinsics.yml";
Mat Q;
Rect roi1, roi2;
if (!intrinsic_filename.empty())
{
Mat cameraMatrix1 = Mat::eye(3, 3, CV_64F);
Mat cameraMatrix2 = Mat::eye(3, 3, CV_64F);
Mat distCoeffs1 = Mat::zeros(8, 1, CV_64F);
Mat distCoeffs2 = Mat::zeros(8, 1, CV_64F);
FileStorage fs(intrinsic_filename, CV_STORAGE_READ);
if (!fs.isOpened()) return false;
fs["M1"] >> cameraMatrix1;
fs["D1"] >> distCoeffs1;
fs["M2"] >> cameraMatrix2;
fs["D2"] >> distCoeffs2;
Mat R, T;
Mat R1, R2, P1, P2, Q;
Rect validRoi1, validRoi2;
fs.open(extrinsic_filename, CV_STORAGE_READ);
if (!fs.isOpened()) return false;
fs["R"] >> R;
fs["T"] >> T;
fs["R1"] >> R1;
fs["P1"] >> P1;
fs["R2"] >> R2;
fs["P2"] >> P2;
fs["Q"] >> Q;
FileNode is = fs["imageSize"];
img_size.width = is[0];
img_size.height = is[1];
Rect roi1, roi2;
double alpha = 0;
stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, img_size, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, alpha, img_size, &roi1, &roi2);
Mat map11, map12, map21, map22;
Mat rmap[2][2];
initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1, img_size, CV_16SC2, rmap[0][0], rmap[0][1]);
initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2, P2, img_size, CV_16SC2, rmap[1][0], rmap[1][1]);
Mat out1, out2;
remap(img1, out1, rmap[0][0], rmap[0][1], CV_INTER_LINEAR);
remap(img2, out2, rmap[1][0], rmap[1][1], CV_INTER_LINEAR);
Mat result;
hconcat(out1, out2, result);
img1 = out1;
img2 = out2;
namedWindow("remapping", 1);
imshow("remapping", result);
waitKey(1);
//getDisparity(img1, img2);
waitKey(1);
}
}
bool isCalib = true;
// 1 channel ip camera drawing thread
static int camera1() {
const std::string address = "rtsp://192.168.0.163:554/Primary";
CvCapture *camera = cvCreateFileCapture(address.c_str());
while (1) {
IplImage *img = cvQueryFrame(camera);
if (img == NULL) {
fflush(stdout);
camera = cvCreateFileCapture(address.c_str());
continue;
}
image1 = cvarrToMat(img);
cv::Rect rect(3, 1, 640, 428);
image1 = image1(rect);
if (isCalib) {
continue;
}
imshow("left image", image1);
waitKey(1);
}
cvReleaseCapture(&camera);
}
// 2 channel ip camera drawing thread
static void camera2() {
const std::string address = "rtsp://192.168.0.164:554/Primary";
CvCapture *camera = cvCreateFileCapture(address.c_str());
while (1) {
IplImage *img = cvQueryFrame(camera);
if (img == NULL) {
fflush(stdout);
camera = cvCreateFileCapture(address.c_str());
continue;
}
image2 = cvarrToMat(img);
cv::Rect rect(3, 1, 640, 428);
image2 = image2(rect);
if (isCalib) {
continue;
}
imshow("right image", image2);
waitKey(1);
}
cvReleaseCapture(&camera);
}
// this thread is calibration
static void command() {
const int nframes = 10;
Mat img1, img2;
while (cvWaitKey(10) != 27) {
cin.clear();
if (isCalib) {
stereoMatching(image1, image2);
waitKey(1);
continue;
}
if (!img1.empty()) {
imshow("Chessboard left image", img1);
}
if (!img2.empty()) {
imshow("Chessboard right image", img2);
}
if (kbhit()) {
if (getch() == 'i') {
isCalib = runCalibration();
}
if (getch() == 's') {
int iframes = find(image1, image2, true);
img1 = image1.clone();
img2 = image2.clone();
if (iframes >= nframes) {
isCalib = runCalibration();
}
char text[256];
sprintf(text, "Recognized chessboard = %d/%d \n", iframes, nframes);
printf("%s", text);
waitKey(1);
}
}
}
}
// main function
int _tmain(int argc, char** argv)
{
imagePoints1.clear();
imagePoints2.clear();
boardSize = Size(10, 7);
squareSize = Size2f(0.314f / 9, 0.209f / 6);
std::thread th1(&camera1);
std::thread th2(&camera2);
std::thread th3(&command);
th1.join();
th2.join();
th3.join();
return 0;
}
process is
find function
run function (bCalb set true)
stereoMatching function
getDisparity function