@ -2753,10 +2753,11 @@ CV_IMPL int cvStereoRectifyUncalibrated(
}
void cv : : reprojectImageTo3D ( const Mat & disparity ,
Mat & _3dImage , const Mat & Q ,
void cv : : reprojectImageTo3D ( const InputArray & _ disparity,
OutputArray __3dImage , const InputArray & _ Qmat ,
bool handleMissingValues , int dtype )
{
Mat disparity = _disparity . getMat ( ) , Q = _Qmat . getMat ( ) ;
int stype = disparity . type ( ) ;
CV_Assert ( stype = = CV_8UC1 | | stype = = CV_16SC1 | |
@ -2771,7 +2772,8 @@ void cv::reprojectImageTo3D( const Mat& disparity,
CV_Assert ( dtype = = CV_16SC3 | | dtype = = CV_32SC3 | | dtype = = CV_32FC3 ) ;
}
_3dImage . create ( disparity . size ( ) , CV_MAKETYPE ( dtype , 3 ) ) ;
__3dImage . create ( disparity . size ( ) , CV_MAKETYPE ( dtype , 3 ) ) ;
Mat _3dImage = __3dImage . getMat ( ) ;
const double bigZ = 10000. ;
double q [ 4 ] [ 4 ] ;
@ -3092,45 +3094,56 @@ cvDecomposeProjectionMatrix( const CvMat *projMatr, CvMat *calibMatr,
namespace cv
{
static void collectCalibrationData ( const vector < vector < Point3f > > & objectPoints ,
const vector < vector < Point2f > > & imagePoints ,
const vector < vector < Point2f > > & imagePoints2 ,
Mat & objPtMat , Mat & imgPtMat , Mat * imgPtMat2 ,
static void collectCalibrationData ( const InputArrayOfArrays & objectPoints ,
const InputArrayOfArrays & imagePoints1 ,
const InputArrayOfArrays & imagePoints2 ,
Mat & objPtMat , Mat & imgPtMat1 , Mat * imgPtMat2 ,
Mat & npoints )
{
size_t i , j = 0 , ni = 0 , nimages = objectPoints . size ( ) , total = 0 ;
CV_Assert ( nimages > 0 & & nimages = = imagePoints . size ( ) & &
( ! imgPtMat2 | | nimages = = imagePoints2 . size ( ) ) ) ;
int nimages = ( int ) objectPoints . total ( ) ;
int i , j = 0 , ni = 0 , total = 0 ;
CV_Assert ( nimages > 0 & & nimages = = ( int ) imagePoints1 . total ( ) & &
( ! imgPtMat2 | | nimages = = ( int ) imagePoints2 . total ( ) ) ) ;
for ( i = 0 ; i < nimages ; i + + )
{
ni = objectPoints [ i ] . size ( ) ;
CV_Assert ( ni = = imagePoints [ i ] . size ( ) & & ( ! imgPtMat2 | | ni = = imagePoints2 [ i ] . size ( ) ) ) ;
ni = objectPoints . getMat ( i ) . checkVector ( 3 , CV_32F ) ;
CV_Assert ( ni > = 0 ) ;
total + = ni ;
}
npoints . create ( 1 , ( int ) nimages , CV_32S ) ;
objPtMat . create ( 1 , ( int ) total , DataType < Point3f > : : type ) ;
imgPtMat . create ( 1 , ( int ) total , DataType < Point2f > : : type ) ;
objPtMat . create ( 1 , ( int ) total , CV_32FC3 ) ;
imgPtMat1 . create ( 1 , ( int ) total , CV_32FC2 ) ;
Point2f * imgPtData2 = 0 ;
if ( imgPtMat2 )
{
imgPtMat2 - > create ( 1 , ( int ) total , DataType < Point2f > : : type ) ;
imgPtMat2 - > create ( 1 , ( int ) total , CV_32FC2 ) ;
imgPtData2 = imgPtMat2 - > ptr < Point2f > ( ) ;
}
Point3f * objPtData = objPtMat . ptr < Point3f > ( ) ;
Point2f * imgPtData = imgPtMat . ptr < Point2f > ( ) ;
Point2f * imgPtData1 = imgPtMat1 . ptr < Point2f > ( ) ;
for ( i = 0 ; i < nimages ; i + + , j + = ni )
{
ni = objectPoints [ i ] . size ( ) ;
( ( int * ) npoints . data ) [ i ] = ( int ) ni ;
std : : copy ( objectPoints [ i ] . begin ( ) , objectPoints [ i ] . end ( ) , objPtData + j ) ;
std : : copy ( imagePoints [ i ] . begin ( ) , imagePoints [ i ] . end ( ) , imgPtData + j ) ;
if ( imgPtMat2 )
std : : copy ( imagePoints2 [ i ] . begin ( ) , imagePoints2 [ i ] . end ( ) , imgPtData2 + j ) ;
Mat objpt = objectPoints . getMat ( i ) ;
Mat imgpt1 = imagePoints1 . getMat ( i ) ;
ni = objpt . checkVector ( 3 , CV_32F ) ;
int ni1 = imgpt1 . checkVector ( 2 , CV_32F ) ;
CV_Assert ( ni > 0 & & ni = = ni1 ) ;
npoints . at < int > ( i ) = ni ;
memcpy ( objPtData + j , objpt . data , ni * sizeof ( objPtData [ 0 ] ) ) ;
memcpy ( imgPtData1 + j , imgpt1 . data , ni * sizeof ( imgPtData1 [ 0 ] ) ) ;
if ( imgPtData2 )
{
Mat imgpt2 = imagePoints2 . getMat ( i ) ;
int ni2 = imgpt2 . checkVector ( 2 , CV_32F ) ;
CV_Assert ( ni = = ni2 ) ;
memcpy ( imgPtData2 + j , imgpt2 . data , ni * sizeof ( imgPtData2 [ 0 ] ) ) ;
}
}
}
@ -3162,126 +3175,153 @@ static Mat prepareDistCoeffs(Mat& distCoeffs0, int rtype)
}
void cv : : Rodrigues ( const Mat & src , Mat & dst )
void cv : : Rodrigues ( const InputArray & _ src, OutputArray _dst , OutputArray _jacobian )
{
Mat src = _src . getMat ( ) ;
bool v2m = src . cols = = 1 | | src . rows = = 1 ;
dst . create ( 3 , v2m ? 3 : 1 , src . depth ( ) ) ;
CvMat _src = src , _dst = dst ;
bool ok = cvRodrigues2 ( & _src , & _dst , 0 ) > 0 ;
_dst . create ( 3 , v2m ? 3 : 1 , src . depth ( ) ) ;
Mat dst = _dst . getMat ( ) ;
CvMat _csrc = src , _cdst = dst , _cjacobian ;
if ( _jacobian . needed ( ) )
{
_jacobian . create ( v2m ? Size ( 9 , 3 ) : Size ( 3 , 9 ) , src . depth ( ) ) ;
_cjacobian = _jacobian . getMat ( ) ;
}
bool ok = cvRodrigues2 ( & _csrc , & _cdst , _jacobian . needed ( ) ? & _cjacobian : 0 ) > 0 ;
if ( ! ok )
dst = Scalar ( 0 ) ;
}
void cv : : Rodrigues ( const Mat & src , Mat & dst , Mat & jacobian )
void cv : : matMulDeriv ( const InputArray & _Amat , const InputArray & _Bmat ,
OutputArray _dABdA , OutputArray _dABdB )
{
bool v2m = src . cols = = 1 | | src . rows = = 1 ;
dst . create ( 3 , v2m ? 3 : 1 , src . depth ( ) ) ;
jacobian . create ( v2m ? Size ( 9 , 3 ) : Size ( 3 , 9 ) , src . depth ( ) ) ;
CvMat _src = src , _dst = dst , _jacobian = jacobian ;
bool ok = cvRodrigues2 ( & _src , & _dst , & _jacobian ) > 0 ;
if ( ! ok )
dst = Scalar ( 0 ) ;
Mat A = _Amat . getMat ( ) , B = _Bmat . getMat ( ) ;
_dABdA . create ( A . rows * B . cols , A . rows * A . cols , A . type ( ) ) ;
_dABdB . create ( A . rows * B . cols , B . rows * B . cols , A . type ( ) ) ;
CvMat matA = A , matB = B , c_dABdA = _dABdA . getMat ( ) , c_dABdB = _dABdB . getMat ( ) ;
cvCalcMatMulDeriv ( & matA , & matB , & c_dABdA , & c_dABdB ) ;
}
void cv : : composeRT ( const InputArray & _rvec1 , const InputArray & _tvec1 ,
const InputArray & _rvec2 , const InputArray & _tvec2 ,
OutputArray _rvec3 , OutputArray _tvec3 ,
OutputArray _dr3dr1 , OutputArray _dr3dt1 ,
OutputArray _dr3dr2 , OutputArray _dr3dt2 ,
OutputArray _dt3dr1 , OutputArray _dt3dt1 ,
OutputArray _dt3dr2 , OutputArray _dt3dt2 )
{
Mat rvec1 = _rvec1 . getMat ( ) , tvec1 = _tvec1 . getMat ( ) ;
Mat rvec2 = _rvec2 . getMat ( ) , tvec2 = _tvec2 . getMat ( ) ;
int rtype = rvec1 . type ( ) ;
_rvec3 . create ( rvec1 . size ( ) , rtype ) ;
_tvec3 . create ( tvec1 . size ( ) , rtype ) ;
Mat rvec3 = _rvec3 . getMat ( ) , tvec3 = _tvec3 . getMat ( ) ;
CvMat c_rvec1 = rvec1 , c_tvec1 = tvec1 , c_rvec2 = rvec2 ,
c_tvec2 = tvec2 , c_rvec3 = rvec3 , c_tvec3 = tvec3 ;
CvMat c_dr3dr1 , c_dr3dt1 , c_dr3dr2 , c_dr3dt2 , c_dt3dr1 , c_dt3dt1 , c_dt3dr2 , c_dt3dt2 ;
CvMat * p_dr3dr1 = 0 , * p_dr3dt1 = 0 , * p_dr3dr2 = 0 , * p_dr3dt2 = 0 , * p_dt3dr1 = 0 , * p_dt3dt1 = 0 , * p_dt3dr2 = 0 , * p_dt3dt2 = 0 ;
if ( _dr3dr1 . needed ( ) )
{
_dr3dr1 . create ( 3 , 3 , rtype ) ;
p_dr3dr1 = & ( c_dr3dr1 = _dr3dr1 . getMat ( ) ) ;
}
void cv : : matMulDeriv ( const Mat & A , const Mat & B , Mat & dABdA , Mat & dABdB )
if ( _dr3dt1 . needed ( ) )
{
dABdA . create ( A . rows * B . cols , A . rows * A . cols , A . type ( ) ) ;
dABdB . create ( A . rows * B . cols , B . rows * B . cols , A . type ( ) ) ;
CvMat matA = A , matB = B , _dABdA = dABdA , _dABdB = dABdB ;
cvCalcMatMulDeriv ( & matA , & matB , & _dABdA , & _dABdB ) ;
_dr3dt1 . create ( 3 , 3 , rtype ) ;
p_dr3dt1 = & ( c_dr3dt1 = _dr3dt1 . getMat ( ) ) ;
}
void cv : : composeRT ( const Mat & rvec1 , const Mat & tvec1 ,
const Mat & rvec2 , const Mat & tvec2 ,
Mat & rvec3 , Mat & tvec3 )
if ( _dr3dr2 . needed ( ) )
{
rvec3 . create ( rvec1 . size ( ) , rvec1 . type ( ) ) ;
tvec3 . create ( tvec1 . size ( ) , tvec1 . type ( ) ) ;
CvMat _rvec1 = rvec1 , _tvec1 = tvec1 , _rvec2 = rvec2 ,
_tvec2 = tvec2 , _rvec3 = rvec3 , _tvec3 = tvec3 ;
cvComposeRT ( & _rvec1 , & _tvec1 , & _rvec2 , & _tvec2 , & _rvec3 , & _tvec3 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ) ;
_dr3dr2 . create ( 3 , 3 , rtype ) ;
p_dr3dr2 = & ( c_dr3dr2 = _dr3dr2 . getMat ( ) ) ;
}
if ( _dr3dt2 . needed ( ) )
{
_dr3dt2 . create ( 3 , 3 , rtype ) ;
p_dr3dt2 = & ( c_dr3dt2 = _dr3dt2 . getMat ( ) ) ;
}
void cv : : composeRT ( const Mat & rvec1 , const Mat & tvec1 ,
const Mat & rvec2 , const Mat & tvec2 ,
Mat & rvec3 , Mat & tvec3 ,
Mat & dr3dr1 , Mat & dr3dt1 ,
Mat & dr3dr2 , Mat & dr3dt2 ,
Mat & dt3dr1 , Mat & dt3dt1 ,
Mat & dt3dr2 , Mat & dt3dt2 )
if ( _dt3dr1 . needed ( ) )
{
int rtype = rvec1 . type ( ) ;
rvec3 . create ( rvec1 . size ( ) , rtype ) ;
tvec3 . create ( tvec1 . size ( ) , rtype ) ;
dr3dr1 . create ( 3 , 3 , rtype ) ; dr3dt1 . create ( 3 , 3 , rtype ) ;
dr3dr2 . create ( 3 , 3 , rtype ) ; dr3dt2 . create ( 3 , 3 , rtype ) ;
dt3dr1 . create ( 3 , 3 , rtype ) ; dt3dt1 . create ( 3 , 3 , rtype ) ;
dt3dr2 . create ( 3 , 3 , rtype ) ; dt3dt2 . create ( 3 , 3 , rtype ) ;
CvMat _rvec1 = rvec1 , _tvec1 = tvec1 , _rvec2 = rvec2 ,
_tvec2 = tvec2 , _rvec3 = rvec3 , _tvec3 = tvec3 ;
CvMat _dr3dr1 = dr3dr1 , _dr3dt1 = dr3dt1 , _dr3dr2 = dr3dr2 , _dr3dt2 = dr3dt2 ;
CvMat _dt3dr1 = dt3dr1 , _dt3dt1 = dt3dt1 , _dt3dr2 = dt3dr2 , _dt3dt2 = dt3dt2 ;
cvComposeRT ( & _rvec1 , & _tvec1 , & _rvec2 , & _tvec2 , & _rvec3 , & _tvec3 ,
& _dr3dr1 , & _dr3dt1 , & _dr3dr2 , & _dr3dt2 ,
& _dt3dr1 , & _dt3dt1 , & _dt3dr2 , & _dt3dt2 ) ;
}
void cv : : projectPoints ( const Mat & opoints ,
const Mat & rvec , const Mat & tvec ,
const Mat & cameraMatrix ,
const Mat & distCoeffs ,
vector < Point2f > & ipoints )
{
CV_Assert ( opoints . isContinuous ( ) & & opoints . depth ( ) = = CV_32F & &
( ( opoints . rows = = 1 & & opoints . channels ( ) = = 3 ) | |
opoints . cols * opoints . channels ( ) = = 3 ) ) ;
ipoints . resize ( opoints . cols * opoints . rows * opoints . channels ( ) / 3 ) ;
CvMat _objectPoints = opoints , _imagePoints = Mat ( ipoints ) ;
CvMat _rvec = rvec , _tvec = tvec , _cameraMatrix = cameraMatrix , _distCoeffs = distCoeffs ;
cvProjectPoints2 ( & _objectPoints , & _rvec , & _tvec , & _cameraMatrix ,
distCoeffs . data ? & _distCoeffs : 0 ,
& _imagePoints , 0 , 0 , 0 , 0 , 0 , 0 ) ;
}
void cv : : projectPoints ( const Mat & opoints ,
const Mat & rvec , const Mat & tvec ,
const Mat & cameraMatrix ,
const Mat & distCoeffs ,
vector < Point2f > & ipoints ,
Mat & dpdrot , Mat & dpdt , Mat & dpdf ,
Mat & dpdc , Mat & dpddist ,
_dt3dr1 . create ( 3 , 3 , rtype ) ;
p_dt3dr1 = & ( c_dt3dr1 = _dt3dr1 . getMat ( ) ) ;
}
if ( _dt3dt1 . needed ( ) )
{
_dt3dt1 . create ( 3 , 3 , rtype ) ;
p_dt3dt1 = & ( c_dt3dt1 = _dt3dt1 . getMat ( ) ) ;
}
if ( _dt3dr2 . needed ( ) )
{
_dt3dr2 . create ( 3 , 3 , rtype ) ;
p_dt3dr2 = & ( c_dt3dr2 = _dt3dr2 . getMat ( ) ) ;
}
if ( _dt3dt2 . needed ( ) )
{
_dt3dt2 . create ( 3 , 3 , rtype ) ;
p_dt3dt2 = & ( c_dt3dt2 = _dt3dt2 . getMat ( ) ) ;
}
cvComposeRT ( & c_rvec1 , & c_tvec1 , & c_rvec2 , & c_tvec2 , & c_rvec3 , & c_tvec3 ,
p_dr3dr1 , p_dr3dt1 , p_dr3dr2 , p_dr3dt2 ,
p_dt3dr1 , p_dt3dt1 , p_dt3dr2 , p_dt3dt2 ) ;
}
void cv : : projectPoints ( const InputArray & _opoints ,
const InputArray & _rvec ,
const InputArray & _tvec ,
const InputArray & _cameraMatrix ,
const InputArray & _distCoeffs ,
OutputArray _ipoints ,
OutputArray _jacobian ,
double aspectRatio )
{
CV_Assert ( opoints . isContinuous ( ) & & opoints . depth ( ) = = CV_32F & &
( ( opoints . rows = = 1 & & opoints . channels ( ) = = 3 ) | |
opoints . cols * opoints . channels ( ) = = 3 ) ) ;
Mat opoints = _opoints . getMat ( ) ;
int npoints = opoints . checkVector ( 3 ) , depth = opoints . depth ( ) ;
CV_Assert ( npoints > = 0 & & ( depth = = CV_32F | | depth = = CV_64F ) ) ;
CvMat dpdrot , dpdt , dpdf , dpdc , dpddist ;
CvMat * pdpdrot = 0 , * pdpdt = 0 , * pdpdf = 0 , * pdpdc = 0 , * pdpddist = 0 ;
_ipoints . create ( npoints , 1 , CV_MAKETYPE ( depth , 2 ) , - 1 , true ) ;
CvMat imagePoints = _ipoints . getMat ( ) ;
CvMat objectPoints = opoints ;
CvMat cameraMatrix = _cameraMatrix . getMat ( ) ;
CvMat rvec = _rvec . getMat ( ) , tvec = _tvec . getMat ( ) ;
CvMat distCoeffs = _distCoeffs . getMat ( ) ;
int ndistCoeffs = distCoeffs . rows + distCoeffs . cols - 1 ;
int npoints = opoints . cols * opoints . rows * opoints . channels ( ) / 3 ;
ipoints . resize ( npoints ) ;
dpdrot . create ( npoints * 2 , 3 , CV_64F ) ;
dpdt . create ( npoints * 2 , 3 , CV_64F ) ;
dpdf . create ( npoints * 2 , 2 , CV_64F ) ;
dpdc . create ( npoints * 2 , 2 , CV_64F ) ;
dpddist . create ( npoints * 2 , distCoeffs . rows + distCoeffs . cols - 1 , CV_64F ) ;
CvMat _objectPoints = opoints , _imagePoints = Mat ( ipoints ) ;
CvMat _rvec = rvec , _tvec = tvec , _cameraMatrix = cameraMatrix , _distCoeffs = distCoeffs ;
CvMat _dpdrot = dpdrot , _dpdt = dpdt , _dpdf = dpdf , _dpdc = dpdc , _dpddist = dpddist ;
if ( _jacobian . needed ( ) )
{
_jacobian . create ( npoints * 2 , 3 + 3 + 2 + 2 + ndistCoeffs , CV_64F ) ;
Mat jacobian = _jacobian . getMat ( ) ;
pdpdrot = & ( dpdrot = jacobian . colRange ( 0 , 3 ) ) ;
pdpdt = & ( dpdt = jacobian . colRange ( 3 , 6 ) ) ;
pdpdf = & ( dpdf = jacobian . colRange ( 6 , 8 ) ) ;
pdpdc = & ( dpdc = jacobian . colRange ( 8 , 10 ) ) ;
pdpddist = & ( dpddist = jacobian . colRange ( 10 , 10 + ndistCoeffs ) ) ;
}
cvProjectPoints2 ( & _ objectPoints, & _ rvec, & _ tvec, & _ cameraMatrix, & _ distCoeffs,
& _ imagePoints, & _dpdrot , & _dpdt , & _dpdf , & _dpdc , & _ dpddist, aspectRatio ) ;
cvProjectPoints2 ( & objectPoints , & rvec , & tvec , & cameraMatrix , & distCoeffs ,
& imagePoints , pdpdrot , pdpdt , pdpdf , pdpdc , p dpddist, aspectRatio ) ;
}
cv : : Mat cv : : initCameraMatrix2D ( const vector < vector < Point3f > > & objectPoints ,
const vector < vector < Point2f > > & imagePoints ,
cv : : Mat cv : : initCameraMatrix2D ( const InputArrayOfArrays & objectPoints ,
const InputArrayOfArrays & imagePoints ,
Size imageSize , double aspectRatio )
{
Mat objPt , imgPt , npoints , cameraMatrix ( 3 , 3 , CV_64F ) ;
collectCalibrationData ( objectPoints , imagePoints , vector < vector < Point2f > > ( ) ,
collectCalibrationData ( objectPoints , imagePoints , InputArrayOfArrays ( ) ,
objPt , imgPt , 0 , npoints ) ;
CvMat _objPt = objPt , _imgPt = imgPt , _npoints = npoints , _cameraMatrix = cameraMatrix ;
cvInitIntrinsicParams2D ( & _objPt , & _imgPt , & _npoints ,
@ -3290,60 +3330,73 @@ cv::Mat cv::initCameraMatrix2D( const vector<vector<Point3f> >& objectPoints,
}
double cv : : calibrateCamera ( const vector < vector < Point3f > > & objectPoints ,
const vector < vector < Point2f > > & imagePoints ,
Size imageSize , Mat & cameraMatrix , Mat & distCoeffs ,
vector < Mat > & rvecs , vector < Mat > & tvecs , int flags )
double cv : : calibrateCamera ( const InputArrayOfArrays & _ objectPoints,
const InputArrayOfArrays & _ imagePoints,
Size imageSize , InputOutputArray _cameraMatrix , InputOutputArray _ distCoeffs,
OutputArray _rvecs , OutputArray _ tvecs , int flags )
{
int rtype = CV_64F ;
Mat cameraMatrix = _cameraMatrix . getMat ( ) ;
cameraMatrix = prepareCameraMatrix ( cameraMatrix , rtype ) ;
Mat distCoeffs = _distCoeffs . getMat ( ) ;
distCoeffs = prepareDistCoeffs ( distCoeffs , rtype ) ;
if ( ! ( flags & CALIB_RATIONAL_MODEL ) )
distCoeffs = distCoeffs . rows = = 1 ? distCoeffs . colRange ( 0 , 5 ) : distCoeffs . rowRange ( 0 , 5 ) ;
size_t i , nimages = objectPoints . size ( ) ;
size_t i , nimages = _ objectPoints. total ( ) ;
CV_Assert ( nimages > 0 ) ;
Mat objPt , imgPt , npoints , rvecM ( ( int ) nimages , 3 , CV_64FC1 ) , tvecM ( ( int ) nimages , 3 , CV_64FC1 ) ;
collectCalibrationData ( objectPoints , imagePoints , vector < vector < Point2f > > ( ) ,
collectCalibrationData ( _ objectPoints, _ imagePoints, InputArrayOfArrays ( ) ,
objPt , imgPt , 0 , npoints ) ;
CvMat _objPt = objPt , _imgPt = imgPt , _npoints = npoints ;
CvMat _cameraMatrix = cameraMatrix , _distCoeffs = distCoeffs ;
CvMat _rvecM = rvecM , _tvecM = tvecM ;
double reprojErr = cvCalibrateCamera2 ( & _objPt , & _imgPt , & _npoints , imageSize ,
& _cameraMatrix , & _distCoeffs , & _rvecM ,
& _tvecM , flags ) ;
rvecs . resize ( nimages ) ;
tvecs . resize ( nimages ) ;
CvMat c_objPt = objPt , c_imgPt = imgPt , c_npoints = npoints ;
CvMat c_cameraMatrix = cameraMatrix , c_distCoeffs = distCoeffs ;
CvMat c_rvecM = rvecM , c_tvecM = tvecM ;
double reprojErr = cvCalibrateCamera2 ( & c_objPt , & c_imgPt , & c_npoints , imageSize ,
& c_cameraMatrix , & c_distCoeffs , & c_rvecM ,
& c_tvecM , flags ) ;
_rvecs . create ( nimages , 1 , CV_64FC3 ) ;
_tvecs . create ( nimages , 1 , CV_64FC3 ) ;
for ( i = 0 ; i < nimages ; i + + )
{
rvecM . row ( ( int ) i ) . copyTo ( rvecs [ i ] ) ;
tvecM . row ( ( int ) i ) . copyTo ( tvecs [ i ] ) ;
_rvecs . create ( 3 , 1 , CV_64F , i , true ) ;
_tvecs . create ( 3 , 1 , CV_64F , i , true ) ;
Mat rv = _rvecs . getMat ( i ) , tv = _tvecs . getMat ( i ) ;
memcpy ( rv . data , rvecM . ptr < double > ( i ) , 3 * sizeof ( double ) ) ;
memcpy ( tv . data , tvecM . ptr < double > ( i ) , 3 * sizeof ( double ) ) ;
}
cameraMatrix . copyTo ( _cameraMatrix ) ;
distCoeffs . copyTo ( _distCoeffs ) ;
return reprojErr ;
}
void cv : : calibrationMatrixValues ( const Mat & cameraMatrix , Size imageSize ,
void cv : : calibrationMatrixValues ( const InputArray & _ cameraMatrix, Size imageSize ,
double apertureWidth , double apertureHeight ,
double & fovx , double & fovy , double & focalLength ,
Point2d & principalPoint , double & aspectRatio )
{
CvMat _cameraMatrix = cameraMatrix ;
cvCalibrationMatrixValues ( & _cameraMatrix , imageSize , apertureWidth , apertureHeight ,
CvMat c _cameraMatrix = _ cameraMatrix. getMat ( ) ;
cvCalibrationMatrixValues ( & c _cameraMatrix, imageSize , apertureWidth , apertureHeight ,
& fovx , & fovy , & focalLength , ( CvPoint2D64f * ) & principalPoint , & aspectRatio ) ;
}
double cv : : stereoCalibrate ( const vector < vector < Point3f > > & objectPoints ,
const vector < vector < Point2f > > & imagePoints1 ,
const vector < vector < Point2f > > & imagePoints2 ,
Mat & cameraMatrix1 , Mat & distCoeffs1 ,
Mat & cameraMatrix2 , Mat & distCoeffs2 ,
Size imageSize , Mat & R , Mat & T ,
Mat & E , Mat & F , TermCriteria criteria ,
double cv : : stereoCalibrate ( const InputArrayOfArrays & _ objectPoints,
const InputArrayOfArrays & _ imagePoints1,
const InputArrayOfArrays & _ imagePoints2,
InputOutputArray _cameraMatrix1 , InputOutputArray _ distCoeffs1,
InputOutputArray _cameraMatrix2 , InputOutputArray _ distCoeffs2,
Size imageSize , OutputArray _Rmat , OutputArray _Tmat ,
OutputArray _Emat , OutputArray _Fmat , TermCriteria criteria ,
int flags )
{
int rtype = CV_64F ;
Mat cameraMatrix1 = _cameraMatrix1 . getMat ( ) ;
Mat cameraMatrix2 = _cameraMatrix2 . getMat ( ) ;
Mat distCoeffs1 = _distCoeffs1 . getMat ( ) ;
Mat distCoeffs2 = _distCoeffs2 . getMat ( ) ;
cameraMatrix1 = prepareCameraMatrix ( cameraMatrix1 , rtype ) ;
cameraMatrix2 = prepareCameraMatrix ( cameraMatrix2 , rtype ) ;
distCoeffs1 = prepareDistCoeffs ( distCoeffs1 , rtype ) ;
@ -3355,176 +3408,210 @@ double cv::stereoCalibrate( const vector<vector<Point3f> >& objectPoints,
distCoeffs2 = distCoeffs2 . rows = = 1 ? distCoeffs2 . colRange ( 0 , 5 ) : distCoeffs2 . rowRange ( 0 , 5 ) ;
}
R . create ( 3 , 3 , rtype ) ;
T . create ( 3 , 1 , rtype ) ;
E . create ( 3 , 3 , rtype ) ;
F . create ( 3 , 3 , rtype ) ;
_Rmat . create ( 3 , 3 , rtype ) ;
_Tmat . create ( 3 , 1 , rtype ) ;
Mat objPt , imgPt , imgPt2 , npoints ;
collectCalibrationData ( objectPoints , imagePoints1 , imagePoints2 ,
collectCalibrationData ( _ objectPoints, _ imagePoints1, _ imagePoints2,
objPt , imgPt , & imgPt2 , npoints ) ;
CvMat _objPt = objPt , _imgPt = imgPt , _imgPt2 = imgPt2 , _npoints = npoints ;
CvMat _cameraMatrix1 = cameraMatrix1 , _distCoeffs1 = distCoeffs1 ;
CvMat _cameraMatrix2 = cameraMatrix2 , _distCoeffs2 = distCoeffs2 ;
CvMat matR = R , matT = T , matE = E , matF = F ;
CvMat c _objPt = objPt , c _imgPt = imgPt , c _imgPt2 = imgPt2 , c _npoints = npoints ;
CvMat c _cameraMatrix1 = cameraMatrix1 , c _distCoeffs1 = distCoeffs1 ;
CvMat c _cameraMatrix2 = cameraMatrix2 , c _distCoeffs2 = distCoeffs2 ;
CvMat c_ matR = _ Rmat . getMat ( ) , c_ matT = _Tmat . getMat ( ) , c_matE , c_matF , * p_matE = 0 , * p_matF = 0 ;
return cvStereoCalibrate ( & _objPt , & _imgPt , & _imgPt2 , & _npoints , & _cameraMatrix1 ,
& _distCoeffs1 , & _cameraMatrix2 , & _distCoeffs2 , imageSize ,
& matR , & matT , & matE , & matF , criteria , flags ) ;
if ( _Emat . needed ( ) )
{
_Emat . create ( 3 , 3 , rtype ) ;
p_matE = & ( c_matE = _Emat . getMat ( ) ) ;
}
if ( _Fmat . needed ( ) )
{
_Fmat . create ( 3 , 3 , rtype ) ;
p_matF = & ( c_matF = _Fmat . getMat ( ) ) ;
}
double err = cvStereoCalibrate ( & c_objPt , & c_imgPt , & c_imgPt2 , & c_npoints , & c_cameraMatrix1 ,
& c_distCoeffs1 , & c_cameraMatrix2 , & c_distCoeffs2 , imageSize ,
& c_matR , & c_matT , p_matE , p_matF , criteria , flags ) ;
void cv : : stereoRectify ( const Mat & cameraMatrix1 , const Mat & distCoeffs1 ,
const Mat & cameraMatrix2 , const Mat & distCoeffs2 ,
Size imageSize , const Mat & R , const Mat & T ,
Mat & R1 , Mat & R2 , Mat & P1 , Mat & P2 , Mat & Q ,
int flags )
{
int rtype = CV_64F ;
R1 . create ( 3 , 3 , rtype ) ;
R2 . create ( 3 , 3 , rtype ) ;
P1 . create ( 3 , 4 , rtype ) ;
P2 . create ( 3 , 4 , rtype ) ;
Q . create ( 4 , 4 , rtype ) ;
CvMat _cameraMatrix1 = cameraMatrix1 , _distCoeffs1 = distCoeffs1 ;
CvMat _cameraMatrix2 = cameraMatrix2 , _distCoeffs2 = distCoeffs2 ;
CvMat matR = R , matT = T , _R1 = R1 , _R2 = R2 , _P1 = P1 , _P2 = P2 , matQ = Q ;
cvStereoRectify ( & _cameraMatrix1 , & _cameraMatrix2 , & _distCoeffs1 , & _distCoeffs2 ,
imageSize , & matR , & matT , & _R1 , & _R2 , & _P1 , & _P2 , & matQ , flags ) ;
}
void cv : : stereoRectify ( const Mat & cameraMatrix1 , const Mat & distCoeffs1 ,
const Mat & cameraMatrix2 , const Mat & distCoeffs2 ,
Size imageSize , const Mat & R , const Mat & T ,
Mat & R1 , Mat & R2 , Mat & P1 , Mat & P2 , Mat & Q ,
cameraMatrix1 . copyTo ( _cameraMatrix1 ) ;
cameraMatrix2 . copyTo ( _cameraMatrix2 ) ;
distCoeffs1 . copyTo ( _distCoeffs1 ) ;
distCoeffs2 . copyTo ( _distCoeffs2 ) ;
return err ;
}
void cv : : stereoRectify ( const InputArray & _cameraMatrix1 , const InputArray & _distCoeffs1 ,
const InputArray & _cameraMatrix2 , const InputArray & _distCoeffs2 ,
Size imageSize , const InputArray & _Rmat , const InputArray & _Tmat ,
OutputArray _Rmat1 , OutputArray _Rmat2 ,
OutputArray _Pmat1 , OutputArray _Pmat2 ,
OutputArray _Qmat , int flags ,
double alpha , Size newImageSize ,
Rect * validPixROI1 , Rect * validPixROI2 ,
int flags )
Rect * validPixROI1 , Rect * validPixROI2 )
{
CvMat c_cameraMatrix1 = _cameraMatrix1 . getMat ( ) ;
CvMat c_cameraMatrix2 = _cameraMatrix2 . getMat ( ) ;
CvMat c_distCoeffs1 = _distCoeffs1 . getMat ( ) ;
CvMat c_distCoeffs2 = _distCoeffs2 . getMat ( ) ;
CvMat c_R = _Rmat . getMat ( ) , c_T = _Tmat . getMat ( ) ;
int rtype = CV_64F ;
R1 . create ( 3 , 3 , rtype ) ;
R2 . create ( 3 , 3 , rtype ) ;
P1 . create ( 3 , 4 , rtype ) ;
P2 . create ( 3 , 4 , rtype ) ;
Q . create ( 4 , 4 , rtype ) ;
CvMat _cameraMatrix1 = cameraMatrix1 , _distCoeffs1 = distCoeffs1 ;
CvMat _cameraMatrix2 = cameraMatrix2 , _distCoeffs2 = distCoeffs2 ;
CvMat matR = R , matT = T , _R1 = R1 , _R2 = R2 , _P1 = P1 , _P2 = P2 , matQ = Q ;
cvStereoRectify ( & _cameraMatrix1 , & _cameraMatrix2 , & _distCoeffs1 , & _distCoeffs2 ,
imageSize , & matR , & matT , & _R1 , & _R2 , & _P1 , & _P2 , & matQ , flags ,
alpha , newImageSize , ( CvRect * ) validPixROI1 , ( CvRect * ) validPixROI2 ) ;
}
bool cv : : stereoRectifyUncalibrated ( const Mat & points1 , const Mat & points2 ,
const Mat & F , Size imgSize ,
Mat & H1 , Mat & H2 , double threshold )
_Rmat1 . create ( 3 , 3 , rtype ) ;
_Rmat2 . create ( 3 , 3 , rtype ) ;
_Pmat1 . create ( 3 , 4 , rtype ) ;
_Pmat2 . create ( 3 , 4 , rtype ) ;
CvMat c_R1 = _Rmat1 . getMat ( ) , c_R2 = _Rmat2 . getMat ( ) , c_P1 = _Pmat1 . getMat ( ) , c_P2 = _Pmat2 . getMat ( ) ;
CvMat c_Q , * p_Q = 0 ;
if ( _Qmat . needed ( ) )
{
_Qmat . create ( 4 , 4 , rtype ) ;
p_Q = & ( c_Q = _Qmat . getMat ( ) ) ;
}
cvStereoRectify ( & c_cameraMatrix1 , & c_cameraMatrix2 , & c_distCoeffs1 , & c_distCoeffs2 ,
imageSize , & c_R , & c_T , & c_R1 , & c_R2 , & c_P1 , & c_P2 , p_Q , flags , alpha ,
newImageSize , ( CvRect * ) validPixROI1 , ( CvRect * ) validPixROI2 ) ;
}
bool cv : : stereoRectifyUncalibrated ( const InputArray & _points1 , const InputArray & _points2 ,
const InputArray & _Fmat , Size imgSize ,
OutputArray _Hmat1 , OutputArray _Hmat2 , double threshold )
{
int rtype = CV_64F ;
H1 . create ( 3 , 3 , rtype ) ;
H2 . create ( 3 , 3 , rtype ) ;
CvMat _pt1 = points1 , _pt2 = points2 , matF , * pF = 0 , _H1 = H1 , _H2 = H2 ;
_Hmat1 . create ( 3 , 3 , rtype ) ;
_Hmat2 . create ( 3 , 3 , rtype ) ;
Mat F = _Fmat . getMat ( ) ;
CvMat c_pt1 = _points1 . getMat ( ) , c_pt2 = _points2 . getMat ( ) ;
CvMat c_F , * p_F = 0 , c_H1 = _Hmat1 . getMat ( ) , c_H2 = _Hmat2 . getMat ( ) ;
if ( F . size ( ) = = Size ( 3 , 3 ) )
pF = & ( matF = F ) ;
return cvStereoRectifyUncalibrated ( & _pt1 , & _pt2 , pF , imgSize , & _H1 , & _H2 , threshold ) > 0 ;
p_ F = & ( c_ F = F ) ;
return cvStereoRectifyUncalibrated ( & c _pt1, & c _pt2, p_ F , imgSize , & c _H1, & c _H2, threshold ) > 0 ;
}
cv : : Mat cv : : getOptimalNewCameraMatrix ( const Mat & cameraMatrix , const Mat & distCoeffs ,
cv : : Mat cv : : getOptimalNewCameraMatrix ( const InputArray & _cameraMatrix ,
const InputArray & _distCoeffs ,
Size imgSize , double alpha , Size newImgSize ,
Rect * validPixROI )
{
Mat newCameraMatrix ( 3 , 3 , cameraMatrix . type ( ) ) ;
CvMat _cameraMatrix = cameraMatrix ,
_distCoeffs = distCoeffs ,
_newCameraMatrix = newCameraMatrix ;
cvGetOptimalNewCameraMatrix ( & _cameraMatrix , & _distCoeffs , imgSize ,
alpha , & _newCameraMatrix ,
CvMat c_cameraMatrix = _cameraMatrix . getMat ( ) , c_distCoeffs = _distCoeffs . getMat ( ) ;
Mat newCameraMatrix ( 3 , 3 , CV_MAT_TYPE ( c_cameraMatrix . type ) ) ;
CvMat c_newCameraMatrix = newCameraMatrix ;
cvGetOptimalNewCameraMatrix ( & c_cameraMatrix , & c_distCoeffs , imgSize ,
alpha , & c_newCameraMatrix ,
newImgSize , ( CvRect * ) validPixROI ) ;
return newCameraMatrix ;
}
void cv : : RQDecomp3x3 ( const Mat & M , Mat & R , Mat & Q )
cv : : Vec3d cv : : RQDecomp3x3 ( const InputArray & _Mmat ,
OutputArray _Rmat ,
OutputArray _Qmat ,
OutputArray _Qx ,
OutputArray _Qy ,
OutputArray _Qz )
{
R . create ( 3 , 3 , M . type ( ) ) ;
Q . create ( 3 , 3 , M . type ( ) ) ;
Mat M = _Mmat . getMat ( ) ;
_Rmat . create ( 3 , 3 , M . type ( ) ) ;
_Qmat . create ( 3 , 3 , M . type ( ) ) ;
Vec3d eulerAngles ;
CvMat matM = M , matR = R , matQ = Q ;
cvRQDecomp3x3 ( & matM , & matR , & matQ , 0 , 0 , 0 , 0 ) ;
CvMat matM = M , matR = _Rmat . getMat ( ) , matQ = _Qmat . getMat ( ) , Qx , Qy , Qz , * pQx = 0 , * pQy = 0 , * pQz = 0 ;
if ( _Qx . needed ( ) )
{
_Qx . create ( 3 , 3 , M . type ( ) ) ;
pQx = & ( Qx = _Qx . getMat ( ) ) ;
}
cv : : Vec3d cv : : RQDecomp3x3 ( const Mat & M , Mat & R , Mat & Q ,
Mat & Qx , Mat & Qy , Mat & Qz )
if ( _Qy . needed ( ) )
{
R . create ( 3 , 3 , M . type ( ) ) ;
Q . create ( 3 , 3 , M . type ( ) ) ;
Vec3d eulerAngles ;
CvMat matM = M , matR = R , matQ = Q , _Qx = Qx , _Qy = Qy , _Qz = Qz ;
cvRQDecomp3x3 ( & matM , & matR , & matQ , & _Qx , & _Qy , & _Qz , ( CvPoint3D64f * ) & eulerAngles [ 0 ] ) ;
_Qy . create ( 3 , 3 , M . type ( ) ) ;
pQy = & ( Qy = _Qy . getMat ( ) ) ;
}
if ( _Qz . needed ( ) )
{
_Qz . create ( 3 , 3 , M . type ( ) ) ;
pQz = & ( Qz = _Qz . getMat ( ) ) ;
}
cvRQDecomp3x3 ( & matM , & matR , & matQ , pQx , pQy , pQz , ( CvPoint3D64f * ) & eulerAngles [ 0 ] ) ;
return eulerAngles ;
}
void cv : : decomposeProjectionMatrix ( const Mat & projMatrix , Mat & cameraMatrix ,
Mat & rotMatrix , Mat & transVect )
void cv : : decomposeProjectionMatrix ( const InputArray & _projMatrix , OutputArray _cameraMatrix ,
OutputArray _rotMatrix , OutputArray _transVect ,
OutputArray _rotMatrixX , OutputArray _rotMatrixY ,
OutputArray _rotMatrixZ , OutputArray _eulerAngles )
{
Mat projMatrix = _projMatrix . getMat ( ) ;
int type = projMatrix . type ( ) ;
cameraMatrix . create ( 3 , 3 , type ) ;
rotMatrix . create ( 3 , 3 , type ) ;
transVect . create ( 4 , 1 , type ) ;
CvMat _projMatrix = projMatrix , _cameraMatrix = cameraMatrix ;
CvMat _rotMatrix = rotMatrix , _transVect = transVect ;
cvDecomposeProjectionMatrix ( & _projMatrix , & _cameraMatrix , & _rotMatrix ,
& _transVect , 0 , 0 , 0 , 0 ) ;
}
_ cameraMatrix. create ( 3 , 3 , type ) ;
_ rotMatrix. create ( 3 , 3 , type ) ;
_ transVect. create ( 4 , 1 , type ) ;
CvMat c _projMatrix = projMatrix , c _cameraMatrix = _ cameraMatrix. getMat ( ) ;
CvMat c _rotMatrix = _ rotMatrix. getMat ( ) , c _transVect = _ transVect. getMat ( ) ;
CvMat c_rotMatrixX , * p_rotMatrixX = 0 ;
CvMat c_rotMatrixY , * p_rotMatrixY = 0 ;
CvMat c_rotMatrixZ , * p_rotMatrixZ = 0 ;
CvPoint3D64f * p_eulerAngles = 0 ;
void cv : : decomposeProjectionMatrix ( const Mat & projMatrix , Mat & cameraMatrix ,
Mat & rotMatrix , Mat & transVect ,
Mat & rotMatrixX , Mat & rotMatrixY ,
Mat & rotMatrixZ , Vec3d & eulerAngles )
if ( _rotMatrixX . needed ( ) )
{
int type = projMatrix . type ( ) ;
cameraMatrix . create ( 3 , 3 , type ) ;
rotMatrix . create ( 3 , 3 , type ) ;
transVect . create ( 4 , 1 , type ) ;
rotMatrixX . create ( 3 , 3 , type ) ;
rotMatrixY . create ( 3 , 3 , type ) ;
rotMatrixZ . create ( 3 , 3 , type ) ;
CvMat _projMatrix = projMatrix , _cameraMatrix = cameraMatrix ;
CvMat _rotMatrix = rotMatrix , _transVect = transVect ;
CvMat _rotMatrixX = rotMatrixX , _rotMatrixY = rotMatrixY ;
CvMat _rotMatrixZ = rotMatrixZ ;
cvDecomposeProjectionMatrix ( & _projMatrix , & _cameraMatrix , & _rotMatrix ,
& _transVect , & _rotMatrixX , & _rotMatrixY ,
& _rotMatrixZ , ( CvPoint3D64f * ) & eulerAngles [ 0 ] ) ;
_rotMatrixX . create ( 3 , 3 , type ) ;
p_rotMatrixX = & ( c_rotMatrixX = _rotMatrixX . getMat ( ) ) ;
}
if ( _rotMatrixY . needed ( ) )
{
_rotMatrixY . create ( 3 , 3 , type ) ;
p_rotMatrixY = & ( c_rotMatrixY = _rotMatrixY . getMat ( ) ) ;
}
if ( _rotMatrixZ . needed ( ) )
{
_rotMatrixZ . create ( 3 , 3 , type ) ;
p_rotMatrixZ = & ( c_rotMatrixZ = _rotMatrixZ . getMat ( ) ) ;
}
if ( _eulerAngles . needed ( ) )
{
_eulerAngles . create ( 3 , 1 , CV_64F , - 1 , true ) ;
p_eulerAngles = ( CvPoint3D64f * ) _eulerAngles . getMat ( ) . data ;
}
cvDecomposeProjectionMatrix ( & c_projMatrix , & c_cameraMatrix , & c_rotMatrix ,
& c_transVect , p_rotMatrixX , p_rotMatrixY ,
p_rotMatrixZ , p_eulerAngles ) ;
}
namespace cv
{
static void adjust3rdMatrix ( const vector < vector < Point2f > > & imgpt1_0 ,
const vector < vector < Point2f > > & imgpt3_0 ,
static void adjust3rdMatrix ( const InputArrayOfArrays & _ imgpt1_0,
const InputArrayOfArrays & _ imgpt3_0,
const Mat & cameraMatrix1 , const Mat & distCoeffs1 ,
const Mat & cameraMatrix3 , const Mat & distCoeffs3 ,
const Mat & R1 , const Mat & R3 , const Mat & P1 , Mat & P3 )
{
size_t n1 = _imgpt1_0 . total ( ) , n3 = _imgpt3_0 . total ( ) ;
vector < Point2f > imgpt1 , imgpt3 ;
for ( int i = 0 ; i < ( int ) std : : min ( imgpt1_0 . size ( ) , imgpt3_0 . size ( ) ) ; i + + )
{
if ( ! imgpt1_0 [ i ] . empty ( ) & & ! imgpt3_0 [ i ] . empty ( ) )
for ( int i = 0 ; i < ( int ) std : : min ( n1 , n3 ) ; i + + )
{
std : : copy ( imgpt1_0 [ i ] . begin ( ) , imgpt1_0 [ i ] . end ( ) , std : : back_inserter ( imgpt1 ) ) ;
std : : copy ( imgpt3_0 [ i ] . begin ( ) , imgpt3_0 [ i ] . end ( ) , std : : back_inserter ( imgpt3 ) ) ;
}
Mat pt1 = _imgpt1_0 . getMat ( i ) , pt3 = _imgpt3_0 . getMat ( i ) ;
int ni1 = pt1 . checkVector ( 2 , CV_32F ) , ni3 = pt3 . checkVector ( 2 , CV_32F ) ;
CV_Assert ( ni1 > 0 & & ni1 = = ni3 ) ;
const Point2f * pt1data = pt1 . ptr < Point2f > ( ) ;
const Point2f * pt3data = pt3 . ptr < Point2f > ( ) ;
std : : copy ( pt1data , pt1data + ni1 , std : : back_inserter ( imgpt1 ) ) ;
std : : copy ( pt3data , pt3data + ni3 , std : : back_inserter ( imgpt3 ) ) ;
}
undistortPoints ( Mat ( imgpt1 ) , imgpt1 , cameraMatrix1 , distCoeffs1 , R1 , P1 ) ;
undistortPoints ( Mat ( imgpt3 ) , imgpt3 , cameraMatrix3 , distCoeffs3 , R3 , P3 ) ;
undistortPoints ( imgpt1 , imgpt1 , cameraMatrix1 , distCoeffs1 , R1 , P1 ) ;
undistortPoints ( imgpt3 , imgpt3 , cameraMatrix3 , distCoeffs3 , R3 , P3 ) ;
double y1_ = 0 , y2_ = 0 , y1y1_ = 0 , y1y2_ = 0 ;
size_t n = imgpt1 . size ( ) ;
@ -3555,20 +3642,31 @@ static void adjust3rdMatrix(const vector<vector<Point2f> >& imgpt1_0,
}
float cv : : rectify3Collinear ( const Mat & cameraMatrix1 , const Mat & distCoeffs1 ,
const Mat & cameraMatrix2 , const Mat & distCoeffs2 ,
const Mat & cameraMatrix3 , const Mat & distCoeffs3 ,
const vector < vector < Point2f > > & imgpt1 ,
const vector < vector < Point2f > > & imgpt3 ,
Size imageSize , const Mat & R12 , const Mat & T12 , const Mat & R13 , const Mat & T13 ,
Mat & R1 , Mat & R2 , Mat & R3 , Mat & P1 , Mat & P2 , Mat & P3 , Mat & Q ,
float cv : : rectify3Collinear ( const InputArray & _cameraMatrix1 , const InputArray & _distCoeffs1 ,
const InputArray & _cameraMatrix2 , const InputArray & _distCoeffs2 ,
const InputArray & _cameraMatrix3 , const InputArray & _distCoeffs3 ,
const InputArrayOfArrays & _imgpt1 ,
const InputArrayOfArrays & _imgpt3 ,
Size imageSize , const InputArray & _Rmat12 , const InputArray & _Tmat12 ,
const InputArray & _Rmat13 , const InputArray & _Tmat13 ,
OutputArray _Rmat1 , OutputArray _Rmat2 , OutputArray _Rmat3 ,
OutputArray _Pmat1 , OutputArray _Pmat2 , OutputArray _Pmat3 ,
OutputArray _Qmat ,
double alpha , Size /*newImgSize*/ ,
Rect * roi1 , Rect * roi2 , int flags )
{
// first, rectify the 1-2 stereo pair
stereoRectify ( cameraMatrix1 , distCoeffs1 , cameraMatrix2 , distCoeffs2 ,
imageSize , R12 , T12 , R1 , R2 , P1 , P2 , Q ,
alpha , imageSize , roi1 , roi2 , flags ) ;
stereoRectify ( _cameraMatrix1 , _distCoeffs1 , _cameraMatrix2 , _distCoeffs2 ,
imageSize , _Rmat12 , _Tmat12 , _Rmat1 , _Rmat2 , _Pmat1 , _Pmat2 , _Qmat ,
flags , alpha , imageSize , roi1 , roi2 ) ;
Mat R12 = _Rmat12 . getMat ( ) , R13 = _Rmat13 . getMat ( ) , T12 = _Tmat12 . getMat ( ) , T13 = _Tmat13 . getMat ( ) ;
_Rmat3 . create ( 3 , 3 , CV_64F ) ;
_Pmat3 . create ( 3 , 4 , CV_64F ) ;
Mat P1 = _Pmat1 . getMat ( ) , P2 = _Pmat2 . getMat ( ) ;
Mat R3 = _Rmat3 . getMat ( ) , P3 = _Pmat3 . getMat ( ) ;
// recompute rectification transforms for cameras 1 & 2.
Mat om , r_r , r_r13 ;
@ -3608,8 +3706,9 @@ float cv::rectify3Collinear( const Mat& cameraMatrix1, const Mat& distCoeffs1,
P3 . at < double > ( 0 , 3 ) * = P3 . at < double > ( 0 , 0 ) ;
P3 . at < double > ( 1 , 3 ) * = P3 . at < double > ( 1 , 1 ) ;
if ( ! imgpt1 . empty ( ) & & imgpt3 . empty ( ) )
adjust3rdMatrix ( imgpt1 , imgpt3 , cameraMatrix1 , distCoeffs1 , cameraMatrix3 , distCoeffs3 , R1 , R3 , P1 , P3 ) ;
if ( ! _imgpt1 . empty ( ) & & _imgpt3 . empty ( ) )
adjust3rdMatrix ( _imgpt1 , _imgpt3 , _cameraMatrix1 . getMat ( ) , _distCoeffs1 . getMat ( ) ,
_cameraMatrix3 . getMat ( ) , _distCoeffs3 . getMat ( ) , _Rmat1 . getMat ( ) , R3 , P1 , P3 ) ;
return ( float ) ( ( P3 . at < double > ( idx , 3 ) / P3 . at < double > ( idx , idx ) ) /
( P2 . at < double > ( idx , 3 ) / P2 . at < double > ( idx , idx ) ) ) ;