2010-11-28 07:15:16 +08:00
/*
* stereo_match . cpp
* calibration
*
* Created by Victor Eruhimov on 1 / 18 / 10.
* Copyright 2010 Argus Corp . All rights reserved .
*
*/
# include "opencv2/calib3d/calib3d.hpp"
# include "opencv2/imgproc/imgproc.hpp"
# include "opencv2/highgui/highgui.hpp"
2011-06-15 20:10:33 +08:00
# include "opencv2/contrib/contrib.hpp"
2013-03-15 00:10:54 +08:00
# include "opencv2/core/utility.hpp"
2010-11-28 07:15:16 +08:00
# include <stdio.h>
using namespace cv ;
2012-06-08 01:21:29 +08:00
static void print_help ( )
2010-12-04 16:30:43 +08:00
{
2012-06-08 01:21:29 +08:00
printf ( " \n Demo stereo matching converting L and R images into disparity and point clouds \n " ) ;
2011-06-15 20:10:33 +08:00
printf ( " \n Usage: stereo_match <left_image> <right_image> [--algorithm=bm|sgbm|hh|var] [--blocksize=<block_size>] \n "
2011-07-14 23:30:28 +08:00
" [--max-disparity=<max_disparity>] [--scale=scale_factor>] [-i <intrinsic_filename>] [-e <extrinsic_filename>] \n "
2010-12-04 16:30:43 +08:00
" [--no-display] [-o <disparity_image>] [-p <point_cloud_file>] \n " ) ;
}
2012-06-08 01:21:29 +08:00
static void saveXYZ ( const char * filename , const Mat & mat )
2010-11-28 07:15:16 +08:00
{
const double max_z = 1.0e4 ;
FILE * fp = fopen ( filename , " wt " ) ;
for ( int y = 0 ; y < mat . rows ; y + + )
{
for ( int x = 0 ; x < mat . cols ; x + + )
{
Vec3f point = mat . at < Vec3f > ( y , x ) ;
if ( fabs ( point [ 2 ] - max_z ) < FLT_EPSILON | | fabs ( point [ 2 ] ) > max_z ) continue ;
fprintf ( fp , " %f %f %f \n " , point [ 0 ] , point [ 1 ] , point [ 2 ] ) ;
}
}
fclose ( fp ) ;
}
int main ( int argc , char * * argv )
{
const char * algorithm_opt = " --algorithm= " ;
const char * maxdisp_opt = " --max-disparity= " ;
const char * blocksize_opt = " --blocksize= " ;
const char * nodisplay_opt = " --no-display= " ;
2011-07-14 23:30:28 +08:00
const char * scale_opt = " --scale= " ;
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
if ( argc < 3 )
{
print_help ( ) ;
2012-06-08 01:21:29 +08:00
return 0 ;
2010-11-28 07:15:16 +08:00
}
const char * img1_filename = 0 ;
const char * img2_filename = 0 ;
const char * intrinsic_filename = 0 ;
const char * extrinsic_filename = 0 ;
const char * disparity_filename = 0 ;
const char * point_cloud_filename = 0 ;
2012-06-08 01:21:29 +08:00
2011-06-15 20:10:33 +08:00
enum { STEREO_BM = 0 , STEREO_SGBM = 1 , STEREO_HH = 2 , STEREO_VAR = 3 } ;
2010-11-28 07:15:16 +08:00
int alg = STEREO_SGBM ;
int SADWindowSize = 0 , numberOfDisparities = 0 ;
bool no_display = false ;
2011-07-14 23:30:28 +08:00
float scale = 1.f ;
2012-06-08 01:21:29 +08:00
2013-03-22 05:05:30 +08:00
Ptr < StereoBM > bm = createStereoBM ( 16 , 9 ) ;
Ptr < StereoSGBM > sgbm = createStereoSGBM ( 0 , 16 , 3 ) ;
2011-06-15 20:10:33 +08:00
StereoVar var ;
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
for ( int i = 1 ; i < argc ; i + + )
{
if ( argv [ i ] [ 0 ] ! = ' - ' )
{
if ( ! img1_filename )
img1_filename = argv [ i ] ;
else
img2_filename = argv [ i ] ;
}
else if ( strncmp ( argv [ i ] , algorithm_opt , strlen ( algorithm_opt ) ) = = 0 )
{
char * _alg = argv [ i ] + strlen ( algorithm_opt ) ;
alg = strcmp ( _alg , " bm " ) = = 0 ? STEREO_BM :
strcmp ( _alg , " sgbm " ) = = 0 ? STEREO_SGBM :
2011-06-15 20:10:33 +08:00
strcmp ( _alg , " hh " ) = = 0 ? STEREO_HH :
strcmp ( _alg , " var " ) = = 0 ? STEREO_VAR : - 1 ;
2010-11-28 07:15:16 +08:00
if ( alg < 0 )
{
printf ( " Command-line parameter error: Unknown stereo algorithm \n \n " ) ;
print_help ( ) ;
return - 1 ;
}
}
else if ( strncmp ( argv [ i ] , maxdisp_opt , strlen ( maxdisp_opt ) ) = = 0 )
{
if ( sscanf ( argv [ i ] + strlen ( maxdisp_opt ) , " %d " , & numberOfDisparities ) ! = 1 | |
numberOfDisparities < 1 | | numberOfDisparities % 16 ! = 0 )
{
printf ( " Command-line parameter error: The max disparity (--maxdisparity=<...>) must be a positive integer divisible by 16 \n " ) ;
print_help ( ) ;
return - 1 ;
}
}
else if ( strncmp ( argv [ i ] , blocksize_opt , strlen ( blocksize_opt ) ) = = 0 )
{
if ( sscanf ( argv [ i ] + strlen ( blocksize_opt ) , " %d " , & SADWindowSize ) ! = 1 | |
SADWindowSize < 1 | | SADWindowSize % 2 ! = 1 )
{
printf ( " Command-line parameter error: The block size (--blocksize=<...>) must be a positive odd number \n " ) ;
return - 1 ;
}
}
2011-07-14 23:30:28 +08:00
else if ( strncmp ( argv [ i ] , scale_opt , strlen ( scale_opt ) ) = = 0 )
{
if ( sscanf ( argv [ i ] + strlen ( scale_opt ) , " %f " , & scale ) ! = 1 | | scale < 0 )
{
printf ( " Command-line parameter error: The scale factor (--scale=<...>) must be a positive floating-point number \n " ) ;
return - 1 ;
}
}
2010-11-28 07:15:16 +08:00
else if ( strcmp ( argv [ i ] , nodisplay_opt ) = = 0 )
no_display = true ;
else if ( strcmp ( argv [ i ] , " -i " ) = = 0 )
intrinsic_filename = argv [ + + i ] ;
else if ( strcmp ( argv [ i ] , " -e " ) = = 0 )
extrinsic_filename = argv [ + + i ] ;
else if ( strcmp ( argv [ i ] , " -o " ) = = 0 )
disparity_filename = argv [ + + i ] ;
else if ( strcmp ( argv [ i ] , " -p " ) = = 0 )
point_cloud_filename = argv [ + + i ] ;
else
{
printf ( " Command-line parameter error: unknown option %s \n " , argv [ i ] ) ;
return - 1 ;
}
}
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
if ( ! img1_filename | | ! img2_filename )
{
printf ( " Command-line parameter error: both left and right images must be specified \n " ) ;
return - 1 ;
}
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
if ( ( intrinsic_filename ! = 0 ) ^ ( extrinsic_filename ! = 0 ) )
{
printf ( " Command-line parameter error: either both intrinsic and extrinsic parameters must be specified, or none of them (when the stereo pair is already rectified) \n " ) ;
return - 1 ;
}
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
if ( extrinsic_filename = = 0 & & point_cloud_filename )
{
printf ( " Command-line parameter error: extrinsic and intrinsic parameters must be specified to compute the point cloud \n " ) ;
return - 1 ;
}
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
int color_mode = alg = = STEREO_BM ? 0 : - 1 ;
Mat img1 = imread ( img1_filename , color_mode ) ;
Mat img2 = imread ( img2_filename , color_mode ) ;
2012-06-08 01:21:29 +08:00
2011-07-14 23:30:28 +08:00
if ( scale ! = 1.f )
{
Mat temp1 , temp2 ;
int method = scale < 1 ? INTER_AREA : INTER_CUBIC ;
resize ( img1 , temp1 , Size ( ) , scale , scale , method ) ;
img1 = temp1 ;
resize ( img2 , temp2 , Size ( ) , scale , scale , method ) ;
img2 = temp2 ;
}
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
Size img_size = img1 . size ( ) ;
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
Rect roi1 , roi2 ;
Mat Q ;
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
if ( intrinsic_filename )
{
// reading intrinsic parameters
2013-04-12 20:19:48 +08:00
FileStorage fs ( intrinsic_filename , FileStorage : : READ ) ;
2010-11-28 07:15:16 +08:00
if ( ! fs . isOpened ( ) )
{
printf ( " Failed to open file %s \n " , intrinsic_filename ) ;
return - 1 ;
}
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
Mat M1 , D1 , M2 , D2 ;
fs [ " M1 " ] > > M1 ;
fs [ " D1 " ] > > D1 ;
fs [ " M2 " ] > > M2 ;
fs [ " D2 " ] > > D2 ;
2012-10-17 15:12:04 +08:00
2012-10-09 02:39:11 +08:00
M1 * = scale ;
M2 * = scale ;
2012-06-08 01:21:29 +08:00
2013-04-12 20:19:48 +08:00
fs . open ( extrinsic_filename , FileStorage : : READ ) ;
2010-11-28 07:15:16 +08:00
if ( ! fs . isOpened ( ) )
{
printf ( " Failed to open file %s \n " , extrinsic_filename ) ;
return - 1 ;
}
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
Mat R , T , R1 , P1 , R2 , P2 ;
fs [ " R " ] > > R ;
fs [ " T " ] > > T ;
2012-06-08 01:21:29 +08:00
2011-04-18 23:14:32 +08:00
stereoRectify ( M1 , D1 , M2 , D2 , img_size , R , T , R1 , R2 , P1 , P2 , Q , CALIB_ZERO_DISPARITY , - 1 , img_size , & roi1 , & roi2 ) ;
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
Mat map11 , map12 , map21 , map22 ;
initUndistortRectifyMap ( M1 , D1 , R1 , P1 , img_size , CV_16SC2 , map11 , map12 ) ;
initUndistortRectifyMap ( M2 , D2 , R2 , P2 , img_size , CV_16SC2 , map21 , map22 ) ;
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
Mat img1r , img2r ;
remap ( img1 , img1r , map11 , map12 , INTER_LINEAR ) ;
remap ( img2 , img2r , map21 , map22 , INTER_LINEAR ) ;
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
img1 = img1r ;
img2 = img2r ;
}
2012-06-08 01:21:29 +08:00
2011-06-15 20:10:33 +08:00
numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : ( ( img_size . width / 8 ) + 15 ) & - 16 ;
2012-06-08 01:21:29 +08:00
2013-03-22 05:05:30 +08:00
bm - > setROI1 ( roi1 ) ;
bm - > setROI2 ( roi2 ) ;
bm - > setPreFilterCap ( 31 ) ;
bm - > setBlockSize ( SADWindowSize > 0 ? SADWindowSize : 9 ) ;
bm - > setMinDisparity ( 0 ) ;
bm - > setNumDisparities ( numberOfDisparities ) ;
bm - > setTextureThreshold ( 10 ) ;
bm - > setUniquenessRatio ( 15 ) ;
bm - > setSpeckleWindowSize ( 100 ) ;
bm - > setSpeckleRange ( 32 ) ;
bm - > setDisp12MaxDiff ( 1 ) ;
sgbm - > setPreFilterCap ( 63 ) ;
2013-03-02 06:17:49 +08:00
int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3 ;
2013-03-22 05:05:30 +08:00
sgbm - > setBlockSize ( sgbmWinSize ) ;
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
int cn = img1 . channels ( ) ;
2012-06-08 01:21:29 +08:00
2013-03-22 05:05:30 +08:00
sgbm - > setP1 ( 8 * cn * sgbmWinSize * sgbmWinSize ) ;
sgbm - > setP2 ( 32 * cn * sgbmWinSize * sgbmWinSize ) ;
sgbm - > setMinDisparity ( 0 ) ;
sgbm - > setNumDisparities ( numberOfDisparities ) ;
sgbm - > setUniquenessRatio ( 10 ) ;
sgbm - > setSpeckleWindowSize ( 100 ) ;
sgbm - > setSpeckleRange ( 32 ) ;
sgbm - > setDisp12MaxDiff ( 1 ) ;
sgbm - > setMode ( alg = = STEREO_HH ? StereoSGBM : : MODE_HH : StereoSGBM : : MODE_SGBM ) ;
2012-06-08 01:21:29 +08:00
var . levels = 3 ; // ignored with USE_AUTO_PARAMS
var . pyrScale = 0.5 ; // ignored with USE_AUTO_PARAMS
var . nIt = 25 ;
var . minDisp = - numberOfDisparities ;
var . maxDisp = 0 ;
var . poly_n = 3 ;
var . poly_sigma = 0.0 ;
var . fi = 15.0f ;
var . lambda = 0.03f ;
var . penalization = var . PENALIZATION_TICHONOV ; // ignored with USE_AUTO_PARAMS
var . cycle = var . CYCLE_V ; // ignored with USE_AUTO_PARAMS
var . flags = var . USE_SMART_ID | var . USE_AUTO_PARAMS | var . USE_INITIAL_DISPARITY | var . USE_MEDIAN_FILTERING ;
2010-11-28 07:15:16 +08:00
Mat disp , disp8 ;
//Mat img1p, img2p, dispp;
//copyMakeBorder(img1, img1p, 0, 0, numberOfDisparities, 0, IPL_BORDER_REPLICATE);
//copyMakeBorder(img2, img2p, 0, 0, numberOfDisparities, 0, IPL_BORDER_REPLICATE);
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
int64 t = getTickCount ( ) ;
if ( alg = = STEREO_BM )
2013-03-02 06:17:49 +08:00
bm - > compute ( img1 , img2 , disp ) ;
2011-07-14 23:30:28 +08:00
else if ( alg = = STEREO_VAR ) {
2011-06-15 20:10:33 +08:00
var ( img1 , img2 , disp ) ;
2012-06-08 01:21:29 +08:00
}
2011-06-15 20:10:33 +08:00
else if ( alg = = STEREO_SGBM | | alg = = STEREO_HH )
2013-03-02 06:17:49 +08:00
sgbm - > compute ( img1 , img2 , disp ) ;
2010-11-28 07:15:16 +08:00
t = getTickCount ( ) - t ;
printf ( " Time elapsed: %fms \n " , t * 1000 / getTickFrequency ( ) ) ;
//disp = dispp.colRange(numberOfDisparities, img1p.cols);
2011-06-15 20:10:33 +08:00
if ( alg ! = STEREO_VAR )
disp . convertTo ( disp8 , CV_8U , 255 / ( numberOfDisparities * 16. ) ) ;
else
disp . convertTo ( disp8 , CV_8U ) ;
2010-11-28 07:15:16 +08:00
if ( ! no_display )
{
namedWindow ( " left " , 1 ) ;
imshow ( " left " , img1 ) ;
namedWindow ( " right " , 1 ) ;
imshow ( " right " , img2 ) ;
namedWindow ( " disparity " , 0 ) ;
imshow ( " disparity " , disp8 ) ;
printf ( " press any key to continue... " ) ;
fflush ( stdout ) ;
waitKey ( ) ;
printf ( " \n " ) ;
}
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
if ( disparity_filename )
imwrite ( disparity_filename , disp8 ) ;
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
if ( point_cloud_filename )
{
printf ( " storing the point cloud... " ) ;
fflush ( stdout ) ;
Mat xyz ;
reprojectImageTo3D ( disp , xyz , Q , true ) ;
saveXYZ ( point_cloud_filename , xyz ) ;
printf ( " \n " ) ;
}
2012-06-08 01:21:29 +08:00
2010-11-28 07:15:16 +08:00
return 0 ;
}