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 .
*
*/
2020-12-02 04:42:15 +08:00
# include "opencv2/3d.hpp"
# include "opencv2/stereo.hpp"
2016-02-15 21:37:29 +08:00
# include "opencv2/imgproc.hpp"
2014-07-04 22:48:15 +08:00
# include "opencv2/imgcodecs.hpp"
2016-02-15 21:37:29 +08:00
# include "opencv2/highgui.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>
2020-08-18 22:10:56 +08:00
# include <sstream>
2010-11-28 07:15:16 +08:00
using namespace cv ;
2020-02-23 21:38:04 +08:00
static void print_help ( char * * argv )
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 " ) ;
2020-08-18 22:10:56 +08:00
printf ( " \n Usage: %s <left_image> <right_image> [--algorithm=bm|sgbm|hh|hh4|sgbm3way] [--blocksize=<block_size>] \n "
2015-08-01 23:24:23 +08:00
" [--max-disparity=<max_disparity>] [--scale=scale_factor>] [-i=<intrinsic_filename>] [-e=<extrinsic_filename>] \n "
2020-08-18 22:10:56 +08:00
" [--no-display] [--color] [-o=<disparity_image>] [-p=<point_cloud_file>] \n " , argv [ 0 ] ) ;
2010-12-04 16:30:43 +08:00
}
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 )
{
2015-08-01 23:24:23 +08:00
std : : string img1_filename = " " ;
std : : string img2_filename = " " ;
std : : string intrinsic_filename = " " ;
std : : string extrinsic_filename = " " ;
std : : string disparity_filename = " " ;
std : : string point_cloud_filename = " " ;
2012-06-08 01:21:29 +08:00
2020-08-18 22:10:56 +08:00
enum { STEREO_BM = 0 , STEREO_SGBM = 1 , STEREO_HH = 2 , STEREO_VAR = 3 , STEREO_3WAY = 4 , STEREO_HH4 = 5 } ;
2010-11-28 07:15:16 +08:00
int alg = STEREO_SGBM ;
2015-08-01 23:24:23 +08:00
int SADWindowSize , numberOfDisparities ;
bool no_display ;
2020-08-18 22:10:56 +08:00
bool color_display ;
2015-08-01 23:24:23 +08:00
float scale ;
2012-06-08 01:21:29 +08:00
2014-10-19 00:44:26 +08:00
Ptr < StereoBM > bm = StereoBM : : create ( 16 , 9 ) ;
Ptr < StereoSGBM > sgbm = StereoSGBM : : create ( 0 , 16 , 3 ) ;
2015-08-01 23:24:23 +08:00
cv : : CommandLineParser parser ( argc , argv ,
2020-08-18 22:10:56 +08:00
" {@arg1||}{@arg2||}{help h||}{algorithm||}{max-disparity|0|}{blocksize|0|}{no-display||}{color||}{scale|1|}{i||}{e||}{o||}{p||} " ) ;
2015-08-01 23:24:23 +08:00
if ( parser . has ( " help " ) )
2010-11-28 07:15:16 +08:00
{
2020-02-23 21:38:04 +08:00
print_help ( argv ) ;
2015-08-01 23:24:23 +08:00
return 0 ;
2010-11-28 07:15:16 +08:00
}
2018-10-31 20:48:56 +08:00
img1_filename = samples : : findFile ( parser . get < std : : string > ( 0 ) ) ;
img2_filename = samples : : findFile ( parser . get < std : : string > ( 1 ) ) ;
2015-08-01 23:24:23 +08:00
if ( parser . has ( " algorithm " ) )
{
std : : string _alg = parser . get < std : : string > ( " algorithm " ) ;
alg = _alg = = " bm " ? STEREO_BM :
_alg = = " sgbm " ? STEREO_SGBM :
_alg = = " hh " ? STEREO_HH :
_alg = = " var " ? STEREO_VAR :
2020-08-18 22:10:56 +08:00
_alg = = " hh4 " ? STEREO_HH4 :
2015-08-01 23:24:23 +08:00
_alg = = " sgbm3way " ? STEREO_3WAY : - 1 ;
}
numberOfDisparities = parser . get < int > ( " max-disparity " ) ;
SADWindowSize = parser . get < int > ( " blocksize " ) ;
scale = parser . get < float > ( " scale " ) ;
no_display = parser . has ( " no-display " ) ;
2020-08-18 22:10:56 +08:00
color_display = parser . has ( " color " ) ;
2015-08-01 23:24:23 +08:00
if ( parser . has ( " i " ) )
intrinsic_filename = parser . get < std : : string > ( " i " ) ;
if ( parser . has ( " e " ) )
extrinsic_filename = parser . get < std : : string > ( " e " ) ;
if ( parser . has ( " o " ) )
disparity_filename = parser . get < std : : string > ( " o " ) ;
if ( parser . has ( " p " ) )
point_cloud_filename = parser . get < std : : string > ( " p " ) ;
if ( ! parser . check ( ) )
{
parser . printErrors ( ) ;
return 1 ;
}
if ( alg < 0 )
{
printf ( " Command-line parameter error: Unknown stereo algorithm \n \n " ) ;
2020-02-23 21:38:04 +08:00
print_help ( argv ) ;
2015-08-01 23:24:23 +08:00
return - 1 ;
}
if ( numberOfDisparities < 1 | | numberOfDisparities % 16 ! = 0 )
{
printf ( " Command-line parameter error: The max disparity (--maxdisparity=<...>) must be a positive integer divisible by 16 \n " ) ;
2020-02-23 21:38:04 +08:00
print_help ( argv ) ;
2015-08-01 23:24:23 +08:00
return - 1 ;
}
if ( scale < 0 )
{
printf ( " Command-line parameter error: The scale factor (--scale=<...>) must be a positive floating-point number \n " ) ;
return - 1 ;
}
if ( SADWindowSize < 1 | | SADWindowSize % 2 ! = 1 )
{
printf ( " Command-line parameter error: The block size (--blocksize=<...>) must be a positive odd number \n " ) ;
return - 1 ;
}
if ( img1_filename . empty ( ) | | img2_filename . empty ( ) )
2010-11-28 07:15:16 +08:00
{
printf ( " Command-line parameter error: both left and right images must be specified \n " ) ;
return - 1 ;
}
2015-08-01 23:24:23 +08:00
if ( ( ! intrinsic_filename . empty ( ) ) ^ ( ! extrinsic_filename . empty ( ) ) )
2010-11-28 07:15:16 +08:00
{
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
2015-08-01 23:24:23 +08:00
if ( extrinsic_filename . empty ( ) & & ! point_cloud_filename . empty ( ) )
2010-11-28 07:15:16 +08:00
{
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
2014-11-04 13:38:15 +08:00
if ( img1 . empty ( ) )
{
printf ( " Command-line parameter error: could not load the first input image file \n " ) ;
return - 1 ;
}
if ( img2 . empty ( ) )
{
printf ( " Command-line parameter error: could not load the second input image file \n " ) ;
return - 1 ;
}
if ( scale ! = 1.f )
2011-07-14 23:30:28 +08:00
{
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
2015-08-01 23:24:23 +08:00
if ( ! intrinsic_filename . empty ( ) )
2010-11-28 07:15:16 +08:00
{
// 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 ( ) )
{
2015-08-01 23:24:23 +08:00
printf ( " Failed to open file %s \n " , intrinsic_filename . c_str ( ) ) ;
2010-11-28 07:15:16 +08:00
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 ( ) )
{
2015-08-01 23:24:23 +08:00
printf ( " Failed to open file %s \n " , extrinsic_filename . c_str ( ) ) ;
2010-11-28 07:15:16 +08:00
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
2020-12-02 04:42:15 +08:00
stereoRectify ( M1 , D1 , M2 , D2 , img_size , R , T , R1 , R2 , P1 , P2 , Q , STEREO_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 ) ;
2015-07-08 04:23:51 +08:00
if ( alg = = STEREO_HH )
sgbm - > setMode ( StereoSGBM : : MODE_HH ) ;
else if ( alg = = STEREO_SGBM )
sgbm - > setMode ( StereoSGBM : : MODE_SGBM ) ;
2020-08-18 22:10:56 +08:00
else if ( alg = = STEREO_HH4 )
sgbm - > setMode ( StereoSGBM : : MODE_HH4 ) ;
2015-07-08 04:23:51 +08:00
else if ( alg = = STEREO_3WAY )
sgbm - > setMode ( StereoSGBM : : MODE_SGBM_3WAY ) ;
2012-06-08 01:21:29 +08:00
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 ( ) ;
2019-11-01 03:29:04 +08:00
float disparity_multiplier = 1.0f ;
2010-11-28 07:15:16 +08:00
if ( alg = = STEREO_BM )
2019-11-01 03:29:04 +08:00
{
2013-03-02 06:17:49 +08:00
bm - > compute ( img1 , img2 , disp ) ;
2019-11-01 03:29:04 +08:00
if ( disp . type ( ) = = CV_16S )
disparity_multiplier = 16.0f ;
}
2020-08-18 22:10:56 +08:00
else if ( alg = = STEREO_SGBM | | alg = = STEREO_HH | | alg = = STEREO_HH4 | | alg = = STEREO_3WAY )
2019-11-01 03:29:04 +08:00
{
2013-03-02 06:17:49 +08:00
sgbm - > compute ( img1 , img2 , disp ) ;
2019-11-01 03:29:04 +08:00
if ( disp . type ( ) = = CV_16S )
disparity_multiplier = 16.0f ;
}
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 ) ;
2020-08-18 22:10:56 +08:00
Mat disp8_3c ;
if ( color_display )
cv : : applyColorMap ( disp8 , disp8_3c , COLORMAP_TURBO ) ;
2012-06-08 01:21:29 +08:00
2015-08-01 23:24:23 +08:00
if ( ! disparity_filename . empty ( ) )
2020-08-18 22:10:56 +08:00
imwrite ( disparity_filename , color_display ? disp8_3c : disp8 ) ;
2012-06-08 01:21:29 +08:00
2015-08-01 23:24:23 +08:00
if ( ! point_cloud_filename . empty ( ) )
2010-11-28 07:15:16 +08:00
{
printf ( " storing the point cloud... " ) ;
fflush ( stdout ) ;
Mat xyz ;
2019-11-01 03:29:04 +08:00
Mat floatDisp ;
disp . convertTo ( floatDisp , CV_32F , 1.0f / disparity_multiplier ) ;
reprojectImageTo3D ( floatDisp , xyz , Q , true ) ;
2015-08-01 23:24:23 +08:00
saveXYZ ( point_cloud_filename . c_str ( ) , xyz ) ;
2010-11-28 07:15:16 +08:00
printf ( " \n " ) ;
}
2012-06-08 01:21:29 +08:00
2020-08-18 22:10:56 +08:00
if ( ! no_display )
{
std : : ostringstream oss ;
oss < < " disparity " < < ( alg = = STEREO_BM ? " bm " :
alg = = STEREO_SGBM ? " sgbm " :
alg = = STEREO_HH ? " hh " :
alg = = STEREO_VAR ? " var " :
alg = = STEREO_HH4 ? " hh4 " :
alg = = STEREO_3WAY ? " sgbm3way " : " " ) ;
oss < < " blocksize: " < < ( alg = = STEREO_BM ? SADWindowSize : sgbmWinSize ) ;
oss < < " max-disparity: " < < numberOfDisparities ;
std : : string disp_name = oss . str ( ) ;
namedWindow ( " left " , cv : : WINDOW_NORMAL ) ;
imshow ( " left " , img1 ) ;
namedWindow ( " right " , cv : : WINDOW_NORMAL ) ;
imshow ( " right " , img2 ) ;
namedWindow ( disp_name , cv : : WINDOW_AUTOSIZE ) ;
imshow ( disp_name , color_display ? disp8_3c : disp8 ) ;
printf ( " press ESC key or CTRL+C to close... " ) ;
fflush ( stdout ) ;
printf ( " \n " ) ;
while ( 1 )
{
if ( waitKey ( ) = = 27 ) //ESC (prevents closing on actions like taking screenshots)
break ;
}
}
2010-11-28 07:15:16 +08:00
return 0 ;
}