opencv sgbm 三维重建_[转载]视觉测量与三维重建（二）：OpenCV的源代码(转载自Opencv中文论坛...

#include "cv.h"

#include "highgui.h"

#include

#include

#include

// example command line (for copy-n-paste):

// calibration -w 6 -h 8 -s 2 -n 10 -o camera.yml -op -oe

[]

enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };

double compute_reprojection_error( const CvMat*

object_points,

const CvMat* rot_vects, const CvMat* trans_vects,

const CvMat* camera_matrix, const CvMat* dist_coeffs,

const CvMat* image_points, const CvMat* point_counts,

CvMat* per_view_errors )

{

CvMat* image_points2 = cvCreateMat(

image_points->rows,

image_points->cols,

image_points->type );

int i, image_count = rot_vects->rows, points_so_far

= 0;

double total_err = 0, err;

for( i = 0; i < image_count; i++ )

{

CvMat object_points_i, image_points_i, image_points2_i;

int point_count = point_counts->data.i[i];

CvMat rot_vect, trans_vect;

cvGetCols( object_points, &object_points_i,

points_so_far, points_so_far + point_count );

cvGetCols( image_points, &image_points_i,

points_so_far, points_so_far + point_count );

cvGetCols( image_points2, &image_points2_i,

points_so_far, points_so_far + point_count );

points_so_far += point_count;

cvGetRow( rot_vects, &rot_vect, i );

cvGetRow( trans_vects, &trans_vect, i );

cvProjectPoints2( &object_points_i,

&rot_vect, &trans_vect,

camera_matrix, dist_coeffs, &image_points2_i,

0, 0, 0, 0, 0 );

err = cvNorm( &image_points_i,

&image_points2_i, CV_L1 );

if( per_view_errors )

per_view_errors->data.db[i] = err/point_count;

total_err += err;

}

cvReleaseMat( &image_points2 );

return total_err/points_so_far;

}

int run_calibration( CvSeq* image_points_seq, CvSize img_size,

CvSize board_size,

float square_size, float aspect_ratio, int flags,

CvMat* camera_matrix, CvMat* dist_coeffs, CvMat**

extr_params,

CvMat** reproj_errs, double* avg_reproj_err )

{

int code;

int image_count = image_points_seq->total;

int point_count = board_size.width*board_size.height;

CvMat* image_points = cvCreateMat( 1, image_count*point_count,

CV_32FC2 );

CvMat* object_points = cvCreateMat( 1, image_count*point_count,

CV_32FC3 );

CvMat* point_counts = cvCreateMat( 1, image_count, CV_32SC1

);

CvMat rot_vects, trans_vects;

int i, j, k;

CvSeqReader reader;

cvStartReadSeq( image_points_seq, &reader );

// initialize arrays of points

for( i = 0; i < image_count; i++ )

{

CvPoint2D32f* src_img_pt = (CvPoint2D32f*)reader.ptr;

CvPoint2D32f* dst_img_pt =

((CvPoint2D32f*)image_points->data.fl) +

i*point_count;

CvPoint3D32f* obj_pt =

((CvPoint3D32f*)object_points->data.fl) +

i*point_count;

for( j = 0; j < board_size.height; j++ )

for( k = 0; k < board_size.width; k++ )

{

*obj_pt++ = cvPoint3D32f(j*square_size, k*square_size, 0);

*dst_img_pt++ = *src_img_pt++;

}

CV_NEXT_SEQ_ELEM( image_points_seq->elem_size,

reader );

}

cvSet( point_counts, cvScalar(point_count) );

*extr_params = cvCreateMat( image_count, 6, CV_32FC1 );

cvGetCols( *extr_params, &rot_vects, 0, 3 );

cvGetCols( *extr_params, &trans_vects, 3, 6

);

cvZero( camera_matrix );

cvZero( dist_coe