Advertisement
Guest User

Untitled

a guest
Jul 6th, 2015
210
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 3.92 KB | None | 0 0
  1. //Title: coordinate_system.cpp
  2. //Author: Nicholas Ballard
  3.  
  4. #include <stdio.h>
  5. #include <string>
  6. #include <cv.h>
  7. #include <highgui.h>
  8.  
  9. using namespace cv;
  10. using namespace std;
  11.  
  12.  
  13.  
  14. // Globals ----------------------------------------------------------------------------------------
  15.  
  16. int boardHeight = 6;
  17. int boardWidth = 9;
  18. Size cbSize = Size(boardHeight, boardWidth);
  19.  
  20. string filename = "out_camera_data.yml";
  21.  
  22.  
  23. //default image size
  24. const int FRAME_WIDTH = 640;
  25. const int FRAME_HEIGHT = 480;
  26.  
  27. //function prototypes
  28. //void generate_boardPoints();
  29.  
  30.  
  31.  
  32.  
  33. // Main -------------------------------------------------------------------------------------------
  34. int main()
  35. {
  36.  
  37. //set up a FileStorage object to read camera params from file
  38. FileStorage fs;
  39. fs.open(filename, FileStorage::READ);
  40. // read camera matrix and distortion coefficients from file
  41. Mat intrinsics, distortion;
  42. fs["Camera_Matrix"] >> intrinsics;
  43. fs["Distortion_Coefficients"] >> distortion;
  44. // close the input file
  45. fs.release();
  46.  
  47.  
  48.  
  49.  
  50. //set up matrices for storage
  51. Mat webcamImage, gray, one;
  52. Mat rvec = Mat(Size(3, 1), CV_64F);
  53. Mat tvec = Mat(Size(3, 1), CV_64F);
  54.  
  55. //setup vectors to hold the chessboard corners in the chessboard coordinate system and in the image
  56. vector<Point2d> imagePoints, imageFramePoints, imageOrigin;
  57. vector<Point3d> boardPoints, framePoints;
  58.  
  59.  
  60. //generate vectors for the points on the chessboard
  61. for (int i = 0; i<boardWidth; i++)
  62. {
  63. for (int j = 0; j<boardHeight; j++)
  64. {
  65. boardPoints.push_back(Point3d(double(i), double(j), 0.0));
  66. }
  67. }
  68. //generate points in the reference frame
  69. framePoints.push_back(Point3d(0.0, 0.0, 0.0));
  70. framePoints.push_back(Point3d(5.0, 0.0, 0.0));
  71. framePoints.push_back(Point3d(0.0, 5.0, 0.0));
  72. framePoints.push_back(Point3d(0.0, 0.0, 5.0));
  73.  
  74.  
  75. //set up VideoCapture object to acquire the webcam feed from location 0 (default webcam location)
  76. VideoCapture capture;
  77. capture.open(0);
  78. //set the capture frame size
  79. capture.set(CV_CAP_PROP_FRAME_WIDTH, FRAME_WIDTH);
  80. capture.set(CV_CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT);
  81.  
  82. while (true)
  83. {
  84. //store image to matrix
  85. capture.read(webcamImage);
  86.  
  87. //make a gray copy of the webcam image
  88. cvtColor(webcamImage, gray, COLOR_BGR2GRAY);
  89.  
  90.  
  91. //detect chessboard corners
  92. bool found = findChessboardCorners(gray, cbSize, imagePoints, CALIB_CB_FAST_CHECK);
  93. //drawChessboardCorners(gray, cbSize, Mat(imagePoints), found);
  94.  
  95.  
  96.  
  97. //find camera orientation if the chessboard corners have been found
  98. if (found)
  99. {
  100. //find the camera extrinsic parameters
  101. solvePnP(Mat(boardPoints), Mat(imagePoints), intrinsics, distortion, rvec, tvec, false);
  102.  
  103. //project the reference frame onto the image
  104. projectPoints(framePoints, rvec, tvec, intrinsics, distortion, imageFramePoints);
  105.  
  106.  
  107. //DRAWING
  108. //draw the reference frame on the image
  109. circle(webcamImage, (Point)imagePoints[0], 4, CV_RGB(255, 0, 0));
  110.  
  111. Point one, two, three;
  112. one.x = 10; one.y = 10;
  113. two.x = 60; two.y = 10;
  114. three.x = 10; three.y = 60;
  115.  
  116.  
  117.  
  118. line(webcamImage, imageFramePoints[0], imageFramePoints[1], CV_RGB(255, 0, 0), 2);
  119. line(webcamImage, imageFramePoints[0], imageFramePoints[2], CV_RGB(0, 255, 0), 2);
  120. line(webcamImage, imageFramePoints[0], imageFramePoints[3], CV_RGB(0, 0, 255), 2);
  121.  
  122.  
  123.  
  124. //show the pose estimation data
  125. cout << fixed << setprecision(2) << "rvec = ["
  126. << rvec.at<double>(0, 0) << ", "
  127. << rvec.at<double>(1, 0) << ", "
  128. << rvec.at<double>(2, 0) << "] \t" << "tvec = ["
  129. << tvec.at<double>(0, 0) << ", "
  130. << tvec.at<double>(1, 0) << ", "
  131. << tvec.at<double>(2, 0) << "]" << endl;
  132.  
  133. }
  134.  
  135. //show the image on screen
  136. namedWindow("OpenCV Webcam", CV_WINDOW_AUTOSIZE);
  137. imshow("OpenCV Webcam", webcamImage);
  138.  
  139.  
  140. //show the gray image
  141. namedWindow("Gray Image", CV_WINDOW_AUTOSIZE);
  142. imshow("Gray Image", gray);
  143.  
  144.  
  145. waitKey(10);
  146. }
  147.  
  148. return 0;
  149. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement