stereo

void stereo_rectify(double*_K, CvSize image_size, double* _rot, double* _tran, double* _R1, double* _R2, double* _P1, double* _P2)

{

CvMat K_matrix = cvMat(3,3, CV_64F, _K);

CvMat rot_mat = cvMat(3,3, CV_64F, _rot);

CvMat tran_mat = cvMat(3, 1, CV_64F, _tran);

CvMat R1_mat = cvMat(3,3, CV_64F, _R1);

CvMat R2_mat = cvMat(3,3, CV_64F, _R2);

CvMat P1_mat = cvMat(3,4, CV_64F, _P1);

CvMat P2_mat = cvMat(3,4, CV_64F, _P2);

cvStereoRectify(&K_matrix, &K_matrix, NULL, NULL, image_size, &rot_mat, &tran_mat, &R1_mat, &R2_mat, &P1_mat, &P2_mat);

}

// 5. warp images

double _KBig[] = {400, 0, image_size.width, 0, 400, image_size.height, 0, 0, 1};

CvSize bigsize = cvSize(image_size.width*2, image_size.height*2);

CvMat KBig = cvMat(3,3, CV_64F, _KBig);

CvMat* mx1 = cvCreateMat( image_size.height*2,

image_size.width*2, CV_32F );

CvMat* my1 = cvCreateMat( image_size.height*2,

image_size.width*2, CV_32F );

CvMat* mx2 = cvCreateMat( image_size.height*2,

image_size.width*2, CV_32F );

CvMat* my2 = cvCreateMat( image_size.height*2,

image_size.width*2, CV_32F );

cvInitUndistortRectifyMap(&K,NULL,&R1,&KBig,mx1,my1);

cvInitUndistortRectifyMap(&K,NULL,&R2,&KBig,mx2,my2);

IplImage* stereo_left = cvCreateImage(bigsize, 8, 3);

IplImage* stereo_right = cvCreateImage(bigsize, 8, 3);

cvRemap(image_left, stereo_left, mx1, my1);

cvRemap(image_right, stereo_right, mx2, my2);

cvSaveImage("stereo_left.jpg", stereo_left);

cvSaveImage("stereo_right.jpg", stereo_right);