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);