首页 > 代码库 > 摄像机标定用于机械臂抓举等(利用标定将图像上的点映射为三维坐标)
摄像机标定用于机械臂抓举等(利用标定将图像上的点映射为三维坐标)
在实验室或者是工程上,我们常常需要将拍摄到的图像的二维图像坐标来计算三维坐标。
如上图所示,我们有上图这样的一副图片。我们需要将物体和机械臂的三维位置算出来,使得机械臂能够精确的抓住物体。
我们采用张正友标定的方法标定出内外参数,利用其内参与外参数算出其对应的三维坐标。具体公式略!可以参考张PAMI的论文。
/*/ //author:YeahPingYE //function: //time:2014/11/25 // // // // // //*/ #include<highgui.h> #include<cv.h> #include<iostream> #include<fstream> using namespace std; int main() { /*initialation //input:the number of images......num_image // prj_board_w,prj_board_h // cam_board_w,cam_board_h */ CvMat*cam_object_points2; CvMat*cam_image_points2; int cam_board_n; int successes = 0; int img_num, cam_board_w, cam_board_h; cout << "输入的图像的组数\n"; cin >> img_num; cout << "输入**真实**棋盘格的##横轴##方向的角点个数\n"; cin >> cam_board_w; cout << "输入**真实**棋盘格的##纵轴##方向的角点个数\n"; cin >> cam_board_h; double cam_Dx = 30;//chessboard's width double cam_Dy = 30; cout << "输入**真实**棋盘格的##横轴##大小dx\n"; cin >> cam_Dx; cout << "输入**真实**棋盘格的##纵轴##大小dy\n"; cin >> cam_Dy; /*int img_num =4; int cam_board_w = 10; int cam_board_h = 8;*/ cam_board_n = cam_board_w*cam_board_h; double x_pixel, y_pixel; cout << "输入要求的位置的像素坐标\n横轴方向的像素坐标#u#\n"; cin >> x_pixel; cout << "纵轴方向的像素坐标#v#\n"; cin >> y_pixel; //x_pixel =0; //y_pixel = 0; //x_pixel = 6.01938965e+002; //y_pixel = 5.65429077e+002; //x_pixel = 7.04663452e+002; //y_pixel = 5.54746582e+002; //x_pixel = 8.88867798e+002; //y_pixel = 9.58109802e+002; /* //init // // */ //camera init CvSize cam_board_sz = cvSize(cam_board_w, cam_board_h); CvMat*cam_image_points = cvCreateMat(cam_board_n*(img_num), 2, CV_32FC1); CvMat*cam_object_points = cvCreateMat(cam_board_n*(img_num), 3, CV_32FC1); CvMat*cam_point_counts = cvCreateMat((img_num), 1, CV_32SC1); CvPoint2D32f*cam_corners = new CvPoint2D32f[cam_board_n]; int cam_corner_count; int cam_step; CvMat*cam_intrinsic_matrix = cvCreateMat(3, 3, CV_32FC1); CvMat*cam_distortion_coeffs = cvCreateMat(4, 1, CV_32FC1); CvSize cam_image_sz; //window intit cvNamedWindow("window", 0); //get image size IplImage *cam_image_temp = cvLoadImage("..\\cam\\cam1.bmp", 0); cam_image_sz = cvGetSize(cam_image_temp); /* //extract cornner // camera image */ for (int ii = 1; ii < img_num + 1; ii++) { char cambuf[20] = { 0 }; sprintf(cambuf, "..\\cam\\cam%d.bmp", ii); IplImage *cam_image = cvLoadImage(cambuf, 0); //extract cam cornner int cam_found = cvFindChessboardCorners(cam_image, cam_board_sz, cam_corners, &cam_corner_count, CV_CALIB_CB_NORMALIZE_IMAGE | CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS); cvFindCornerSubPix(cam_image, cam_corners, cam_corner_count, cvSize(11, 11), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); cvDrawChessboardCorners(cam_image, cam_board_sz, cam_corners, cam_corner_count, cam_found); //when camare success store the result if ((cam_corner_count == cam_board_n)) { //store cam result cam_step = successes*cam_board_n; for (int i = cam_step, j = 0; j < cam_board_n; ++i, ++j) { CV_MAT_ELEM(*cam_image_points, float, i, 0) = cam_corners[j].x; CV_MAT_ELEM(*cam_image_points, float, i, 1) = cam_corners[j].y; CV_MAT_ELEM(*cam_object_points, float, i, 0) = (j%cam_board_w)*cam_Dx; CV_MAT_ELEM(*cam_object_points, float, i, 1) = (j / cam_board_w)*cam_Dy; CV_MAT_ELEM(*cam_object_points, float, i, 2) = 0.0f; } CV_MAT_ELEM(*cam_point_counts, int, successes, 0) = cam_board_n; successes++; cout << "success number" << successes << endl; cvShowImage("window", cam_image); cvWaitKey(500); } } /* //restore the success point */ //cam cam_image_points2 = cvCreateMat(cam_board_n*(successes), 2, CV_32FC1); cam_object_points2 = cvCreateMat(cam_board_n*(successes), 3, CV_32FC1); CvMat*cam_point_counts2 = cvCreateMat((successes), 1, CV_32SC1); for (int i = 0; i < successes*cam_board_n; ++i){ CV_MAT_ELEM(*cam_image_points2, float, i, 0) = CV_MAT_ELEM(*cam_image_points, float, i, 0); CV_MAT_ELEM(*cam_image_points2, float, i, 1) = CV_MAT_ELEM(*cam_image_points, float, i, 1); CV_MAT_ELEM(*cam_object_points2, float, i, 0) = CV_MAT_ELEM(*cam_object_points, float, i, 0); CV_MAT_ELEM(*cam_object_points2, float, i, 1) = CV_MAT_ELEM(*cam_object_points, float, i, 1); CV_MAT_ELEM(*cam_object_points2, float, i, 2) = CV_MAT_ELEM(*cam_object_points, float, i, 2); } for (int i = 0; i < successes; ++i){ CV_MAT_ELEM(*cam_point_counts2, int, i, 0) = CV_MAT_ELEM(*cam_point_counts, int, i, 0); } cvSave("..\\output\\XML\\cam_corners.xml", cam_image_points2); cvReleaseMat(&cam_object_points); cvReleaseMat(&cam_image_points); cvReleaseMat(&cam_point_counts); /* //calibration for camera // */ //calib for cam CV_MAT_ELEM(*cam_intrinsic_matrix, float, 0, 0) = 1.0f; CV_MAT_ELEM(*cam_intrinsic_matrix, float, 1, 1) = 1.0f; CvMat* cam_rotation_all = cvCreateMat( successes, 3, CV_32FC1); CvMat* cam_translation_vector_all = cvCreateMat( successes,3, CV_32FC1); cvCalibrateCamera2( cam_object_points2, cam_image_points2, cam_point_counts2, cam_image_sz, cam_intrinsic_matrix, cam_distortion_coeffs, cam_rotation_all, cam_translation_vector_all, 0//CV_CALIB_FIX_ASPECT_RATIO ); cvSave("..\\output\\XML\\cam_intrinsic_matrix.xml", cam_intrinsic_matrix); cvSave("..\\output\\XML\\cam_distortion_coeffs.xml", cam_distortion_coeffs); cvSave("..\\output\\XML\\cam_rotation_all.xml", cam_rotation_all); cvSave("..\\output\\XML\\cam_translation_vector_all.xml", cam_translation_vector_all); CvMat* cam_dist_image_points = cvCreateMat(cam_board_n, 1, CV_32FC2); CvMat* cam_undist_image_points = cvCreateMat(cam_board_n, 1, CV_32FC2); for (int j = 0; j < cam_board_n; ++j) { cvSet1D(cam_dist_image_points, j, cvScalar(CV_MAT_ELEM(*cam_image_points2, float, j, 0), CV_MAT_ELEM(*cam_image_points2, float, j, 1))); //cout << CV_MAT_ELEM(*cam_dist_image_points, float, j, 0) << "\t" << CV_MAT_ELEM(*cam_dist_image_points, float, j, 1) << endl; } cvUndistortPoints(cam_dist_image_points, cam_undist_image_points, cam_intrinsic_matrix, cam_distortion_coeffs, NULL, NULL); cvReleaseMat(&cam_dist_image_points); CvMat* homography = cvCreateMat(3, 3, CV_32FC1); CvMat* cam_src = http://www.mamicode.com/cvCreateMat(cam_board_n, 3, CV_32FC1);>上述中像素(969.449,635.648)对应的真实第一个棋盘格的坐标是(90,60,0)上图得到的结果误差比较小。
具体程序的操作步骤及相关文件见网址(vs2013配置了opencv可以直接运行):http://download.csdn.net/detail/zhouyelihua/8218649
摄像机标定用于机械臂抓举等(利用标定将图像上的点映射为三维坐标)
声明:以上内容来自用户投稿及互联网公开渠道收集整理发布,本网站不拥有所有权,未作人工编辑处理,也不承担相关法律责任,若内容有误或涉及侵权可进行投诉: 投诉/举报 工作人员会在5个工作日内联系你,一经查实,本站将立刻删除涉嫌侵权内容。