Changeset 494

Timestamp:

05/20/09 00:15:57 (3 years ago)

Author:

mosttw

Message:

Producing what looks like a reasonable K matrix!

Location:

trunk/software/scripts/calib

Files:

• Makefile (modified) (1 diff)
• calibrate.c (modified) (4 diffs)

trunk/software/scripts/calib/Makefile

@@ -25 +25 @@
         ctags -R
 clean:
-        rm -f $(PROG) *.o *.so *.pyc core out*.jpeg *.out tags
+        rm -f $(PROG) *.o *.so *.pyc core corners.*.jpg *.out tags

trunk/software/scripts/calib/calibrate.c

@@ +1 @@
+
+
+
+
 #include <stdio.h>
+#include <stdint.h>
 
 #include <cv.h>
@@ -12 +17 @@
 #define BOARD_COLS 9
 
+// The size of the squares on the chessboard, in whatever units
+// you want the calibration to be related to.
+#define SQUARE_SIZE 1.0
+
+// The size of the images from which you are calibrating.
+#define IMAGE_WIDTH 2592
+#define IMAGE_HEIGHT 1936
 
 // Value for the flags argument of cvFindChessboardCorners
 // for each image.  This is some combination of 
 // CV_CALIB_CB_{ADAPTIVE_THRESH,NORMALIZE_IMAGE,FILTER_QUADS}
-// -- experiment a bit to find what works for each image.
-int CORNER_FLAGS[] = {
+// -- experiment a bit to find what works for each image.  The
+// length of this array must match NUM_INPUTS.
+int CORNER_FLAGS[NUM_INPUTS] = {
   CV_CALIB_CB_FILTER_QUADS,
   CV_CALIB_CB_ADAPTIVE_THRESH,
@@ -24 +37 @@
 };
 
+
+
+// Print out a floating-point, single channel matrix
+void print_matrix(CvMat *mat)
+{
+  for (int r = 0; r < mat->rows; r++) {
+    for (int c = 0; c < mat->cols; c++) {
+      printf("%11.3f ", cvmGet(mat, r, c));
+    }
+    printf("\n");
+  }
+}
+
+
 int main(int argc, char* argv[])
 {
-  // Initialize the array of 3D points
-  CvMat *corners_3d = cvCreateMat(3, BOARD_ROWS * BOARD_COLS, CV_32FC1);
-  for (int row = 0; row < BOARD_ROWS; row++) {
-    for (int col = 0; col < BOARD_COLS; col++) {
-      int m_col = row * BOARD_ROWS + col;
-      cvmSet(corners_3d, 0, m_col, row);
-      cvmSet(corners_3d, 1, m_col, col);
-      cvmSet(corners_3d, 2, m_col, 1);
-    }
-  }
   
+  ///////// Find chessboard corners ////////
   CvSize pattern_size = cvSize(BOARD_COLS, BOARD_ROWS);
-
+  
+  // Find the chessboard corners
+  int success = 1;
+  int total_corners = BOARD_COLS * BOARD_ROWS * NUM_INPUTS;
+  CvPoint2D32f* corners = calloc(total_corners,  sizeof(CvPoint2D32f));
+  
   for (int i = 0; i < NUM_INPUTS; i++) {
     // Figure out filenames and load the image
@@ -49 +72 @@
     // Attempt to find the chessboard
     int corner_count = 0;
-    CvPoint2D32f *corners = calloc(BOARD_ROWS * BOARD_COLS, sizeof(CvPoint2D32f));
-    
+    CvPoint2D32f* my_corners = corners + (BOARD_COLS * BOARD_ROWS * i);
     int pattern_was_found = cvFindChessboardCorners(img,
                                                     pattern_size,
-                                                    corners,
+                                                    my_corners,
                                                     &corner_count,
                                                     CORNER_FLAGS[i]);
-    cvDrawChessboardCorners(img, pattern_size, corners, corner_count, pattern_was_found);
-
+    cvDrawChessboardCorners(img, pattern_size, my_corners, corner_count, pattern_was_found);
     cvSaveImage(out_fn, img);
     cvReleaseImage(&img);
+    
+    printf("%s for %s: found %d corners\n", (pattern_was_found ? "success" : "failure"),
+                                            in_fn,
+                                            corner_count);
+    if (!pattern_was_found) {
+      success = 0;
+    }
   }
+  
+  if (!success) {
+    printf("Didn't find all the corners. Try tweaking CORNER_FLAGS.\n");
+    exit(1);
+  }
+
+  ///////// Calculate K //////////
+  
+  // Initialize the array of 3D coordinates
+  // We just set it to a grid, with the top left starting at (0, 0).
+  // There is no Z coordinate because we are using a planar calibration
+  // pattern.
+  CvMat *corners_3d = cvCreateMat(total_corners, 2, CV_32FC1);
+  for (int i = 0; i < NUM_INPUTS; i++) {
+    for (int row = 0; row < BOARD_ROWS; row++) {
+      for (int col = 0; col < BOARD_COLS; col++) {
+        int m_row = (i * BOARD_ROWS * BOARD_COLS) + (row * BOARD_ROWS) + col;
+        cvmSet(corners_3d, m_row, 0, row * SQUARE_SIZE);
+        cvmSet(corners_3d, m_row, 1, col * SQUARE_SIZE);
+      }
+    }
+  }
+  
+  // Convert the array of 2D points to a matrix.
+  CvMat *corners_2d = cvCreateMat(total_corners, 2, CV_32FC1);
+  for (int row = 0; row < total_corners; row++) {
+    cvmSet(corners_2d, row, 0, corners[row].x);
+    cvmSet(corners_2d, row, 1, corners[row].y);
+  }
+
+  // point_counts is a vector that contains the number of points
+  // in each image.
+  int32_t *point_count_data = malloc(NUM_INPUTS * sizeof(int32_t));
+  for (int i = 0; i < NUM_INPUTS; i++) {
+    point_count_data[i] = BOARD_ROWS * BOARD_COLS;
+  }
+  CvMat point_counts = cvMat(1, NUM_INPUTS, CV_32S, point_count_data);
+ 
+  // Output parameters
+
+  // K is the camera matrix.  K_1,1 and K_2,2 contain the focal length
+  // and K_1,3 and K_2,3 contain the camera center, which should be half
+  // the image dimensions.
+  float K_data[] = {0.0, 0.0, IMAGE_WIDTH / 2.0,
+                    0.0, 0.0, IMAGE_HEIGHT / 2.0,
+                    0.0, 0.0, 1.0};
+  CvMat K = cvMat(3, 3, CV_32FC1, &K_data);
+  printf("original K:\n");
+  print_matrix(&K);
+
+  CvMat *distortion_coeffs = cvCreateMat(4, 1, CV_32FC1);
+  
+  
+  printf("calibrating...\n");
+  cvCalibrateCamera2(corners_3d,
+                     corners_2d,
+                     &point_counts,
+                     cvSize(IMAGE_WIDTH, IMAGE_HEIGHT),
+                     &K,
+                     distortion_coeffs,
+                     NULL, // rotation_vectors
+                     NULL, // translation_vectors
+                     0);
+  
+  printf("\nK:\n");
+  print_matrix(&K);
+  printf("\ndistortion_coeffs:\n");
+  print_matrix(distortion_coeffs);
+
 }
-