0

我已经使用opencv对我的相机进行了校准，并且已经很好地获得了内在参数。但是，我未能使用 cvFindExtrinsicParams2 和 cv::solvePnP 函数来查找相机的外部参数。这是我的代码：

#include <iostream>
#include<opencv\cv.h>
#include <opencv2/highgui/highgui.hpp>

using namespace cv;
using namespace std;

int n_boards = 0;
const int board_dt = 20;
int board_w;
int board_h;

int main(int argc, char* argv[])
{
board_w = 6; // Board width in squares
board_h = 9; // Board height 
n_boards = 15; // Number of boards
int board_n = board_w * board_h;
CvSize board_sz = cvSize( board_w, board_h );
CvCapture* capture = cvCreateCameraCapture( 0 );
assert( capture );

cvNamedWindow( "Calibration" );
// Allocate Sotrage
CvMat* image_points         = cvCreateMat( n_boards*board_n, 2, CV_32FC1 );
CvMat* object_points        = cvCreateMat( n_boards*board_n, 3, CV_32FC1 );
CvMat* point_counts         = cvCreateMat( n_boards, 1, CV_32SC1 );
CvMat* intrinsic_matrix     = cvCreateMat( 3, 3, CV_32FC1 );
CvMat* extrinsic_matrix     = cvCreateMat( 3, 4, CV_32FC1 );
CvMat* distortion_coeffs    = cvCreateMat( 5, 1, CV_32FC1 );
CvMat* rvec                 =cvCreateMat( 1, 3, CV_32FC1 );
CvMat* tvec                 = cvCreateMat( 3, 1, CV_32FC1 );
CvMat* rotmat               = cvCreateMat( 3, 3, CV_32FC1 );

CvPoint2D32f* corners = new CvPoint2D32f[ board_n ];
int corner_count;
int successes = 0;
int step, frame = 0;

IplImage *image = cvQueryFrame( capture );
IplImage *gray_image = cvCreateImage( cvGetSize( image ), 8, 1 );

// Capture Corner views loop until we've got n_boards
// succesful captures (all corners on the board are found)

while( successes < n_boards ){
    // Skp every board_dt frames to allow user to move chessboard
    if( frame++ % board_dt == 0 ){
        // Find chessboard corners:

    int found = cvFindChessboardCorners( image, board_sz, corners,
&corner_count, CV_CALIB_CB_ADAPTIVE_THRESH |          CV_CALIB_CB_FILTER_QUADS );

        // Get subpixel accuracy on those corners
        cvCvtColor( image, gray_image, CV_BGR2GRAY );
    cvFindCornerSubPix( gray_image, corners, corner_count, cvSize( 11, 11 ), 
cvSize( -1, -1 ), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));

        // Draw it
    cvDrawChessboardCorners( image, board_sz, corners, corner_count, found );
        cvShowImage( "Calibration", image );

        // If we got a good board, add it to our data
        if( corner_count == board_n ){
            step = successes*board_n;
        or( int i=step, j=0; j < board_n; ++i, ++j ){
            CV_MAT_ELEM( *image_points, float, i, 0 ) = corners[j].x;
    CV_MAT_ELEM( *image_points, float, i, 1 ) = corners[j].y;
        CV_MAT_ELEM( *object_points, float, i, 0 ) = j/board_w;
            CV_MAT_ELEM( *object_points, float, i, 1 ) = j%board_w;
                CV_MAT_ELEM( *object_points, float, i, 2 ) = 0.0f;
            }
            CV_MAT_ELEM( *point_counts, int, successes, 0 ) = board_n;
            successes++;
        }
    } 

    // Handle pause/unpause and ESC
    int c = cvWaitKey( 15 );
    if( c == 'p' ){
        c = 0;
        while( c != 'p' && c != 27 ){
            c = cvWaitKey( 250 );
        }
    }
    if( c == 27 )
        return 0;
    image = cvQueryFrame( capture ); // Get next image
} // End collection while loop

// Allocate matrices according to how many chessboards found
CvMat* object_points2 = cvCreateMat( successes*board_n, 3, CV_32FC1 );
CvMat* image_points2 = cvCreateMat( successes*board_n, 2, CV_32FC1 );
CvMat* point_counts2 = cvCreateMat( successes, 1, CV_32SC1 );

// Transfer the points into the correct size matrices
for( int i = 0; i < successes*board_n; ++i ){
CV_MAT_ELEM( *image_points2, float, i, 0) = CV_MAT_ELEM( *image_points, float, i, 0 );
CV_MAT_ELEM( *image_points2, float, i, 1) = CV_MAT_ELEM( *image_points, float, i, 1 );
CV_MAT_ELEM( *object_points2, float, i, 0) = CV_MAT_ELEM( *object_points,float, i, 0 );
CV_MAT_ELEM( *object_points2, float, i, 1) = CV_MAT_ELEM( *object_points, float, i,1 );
CV_MAT_ELEM( *object_points2, float, i, 2) = CV_MAT_ELEM( *object_points, float, i,2);
}

for( int i=0; i < successes; ++i ){
CV_MAT_ELEM( *point_counts2, int, i, 0 ) = CV_MAT_ELEM( *point_counts, int, i, 0 );
}
cvReleaseMat( &object_points );
cvReleaseMat( &image_points );
cvReleaseMat( &point_counts );

// At this point we have all the chessboard corners we need
// Initiliazie the intrinsic matrix such that the two focal lengths
// have a ratio of 1.0

CV_MAT_ELEM( *intrinsic_matrix, float, 0, 0 ) = 1.0;
CV_MAT_ELEM( *intrinsic_matrix, float, 1, 1 ) = 1.0;


cvCalibrateCamera2( object_points2, image_points2,
 point_counts2, cvGetSize( image ), intrinsic_matrix,
 distortion_coeffs, NULL,
  NULL, CV_CALIB_FIX_ASPECT_RATIO ); 

// Save the intrinsics and distortions
cvSave( "Intrinsics.xml", intrinsic_matrix );
cvSave( "Distortion.xml", distortion_coeffs );

// Example of loading these matrices back in
CvMat *intrinsic = (CvMat*)cvLoad( "Intrinsics.xml" );
CvMat *distortion = (CvMat*)cvLoad( "Distortion.xml" );

// Build the undistort map that we will use for all subsequent frames
IplImage* mapx = cvCreateImage( cvGetSize( image ), IPL_DEPTH_32F, 1 );
IplImage* mapy = cvCreateImage( cvGetSize( image ), IPL_DEPTH_32F, 1 );
cvInitUndistortMap( intrinsic, distortion, mapx, mapy );


//Extrinsic parameters

//CV_MAT_ELEM( *extrinsic_matrix, float, 0, 0 ) = 1.0;
//CV_MAT_ELEM( *extrinsic_matrix, float, 1, 1 ) = 1.0;  
cvFindExtrinsicCameraParams2(object_points2,image_points2,
intrinsic_matrix,distortion_coeffs,rvec,tvec);

//convert the rotation matrix into a rotation vector
cvRodrigues2(rvec,rotmat,0);

// Save the intrinsics and distortions
cvSave( "Extrinsics.xml", extrinsic_matrix);

// Example of loading these matrices back in
CvMat *Extrinsic = (CvMat*)cvLoad( "Extrinsics.xml" );


// Run the camera to the screen, now showing the raw and undistorted image
cvNamedWindow( "Undistort" );

while( image ){
    IplImage *t = cvCloneImage( image );
    cvShowImage( "Calibration", image ); // Show raw image
    cvRemap( t, image, mapx, mapy ); // undistort image
    cvReleaseImage( &t );
    cvShowImage( "Undistort", image ); // Show corrected image

    // Handle pause/unpause and esc
    int c = cvWaitKey( 15 );
    if( c == 'p' ){
        c = 0;
        while( c != 'p' && c != 27 )
        {
            c = cvWaitKey( 250 );
        }
    }
    if( c == 27 )
        break;
    image = cvQueryFrame( capture );
}

return 0;
}
4

1 回答 1

0 
extrinsic_matrix = cvFindExtrinsicCameraParams2(object_points2,image_points2, intrinsic_matrix,distortion_coeffs,rvec,tvec);
于 2013-08-30T10:00:40.760 回答