generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

"atomic bomb" commit. Reorganized OpenCV directory structure

Browse Source
This commit is contained in:
Vadim Pisarevsky
2010-05-11 17:44:00 +00:00
commit 127d6649a1
1761 changed files with 1766340 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
modules/calib3d/CMakeLists.txt Normal file
View File

@@ -0,0 +1 @@
define_opencv_module(calib3d opencv_core opencv_imgproc)

697
modules/calib3d/include/opencv2/calib3d/calib3d.hpp Normal file
View File

@@ -0,0 +1,697 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#ifndef __OPENCV_CALIB3D_HPP__
#define __OPENCV_CALIB3D_HPP__
#include "opencv2/core/core.hpp"
#ifdef __cplusplus
extern "C" {
#endif
/****************************************************************************************\
* Camera Calibration, Pose Estimation and Stereo *
\****************************************************************************************/
typedef struct CvPOSITObject CvPOSITObject;
/* Allocates and initializes CvPOSITObject structure before doing cvPOSIT */
CVAPI(CvPOSITObject*) cvCreatePOSITObject( CvPoint3D32f* points, int point_count );
/* Runs POSIT (POSe from ITeration) algorithm for determining 3d position of
an object given its model and projection in a weak-perspective case */
CVAPI(void) cvPOSIT( CvPOSITObject* posit_object, CvPoint2D32f* image_points,
double focal_length, CvTermCriteria criteria,
float* rotation_matrix, float* translation_vector);
/* Releases CvPOSITObject structure */
CVAPI(void) cvReleasePOSITObject( CvPOSITObject** posit_object );
/* updates the number of RANSAC iterations */
CVAPI(int) cvRANSACUpdateNumIters( double p, double err_prob,
int model_points, int max_iters );
CVAPI(void) cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst );
/* Calculates fundamental matrix given a set of corresponding points */
#define CV_FM_7POINT 1
#define CV_FM_8POINT 2
#define CV_FM_LMEDS_ONLY 8
#define CV_FM_RANSAC_ONLY 4
#define CV_FM_LMEDS 8
#define CV_FM_RANSAC 4
CVAPI(int) cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
CvMat* fundamental_matrix,
int method CV_DEFAULT(CV_FM_RANSAC),
double param1 CV_DEFAULT(3.), double param2 CV_DEFAULT(0.99),
CvMat* status CV_DEFAULT(NULL) );
/* For each input point on one of images
computes parameters of the corresponding
epipolar line on the other image */
CVAPI(void) cvComputeCorrespondEpilines( const CvMat* points,
int which_image,
const CvMat* fundamental_matrix,
CvMat* correspondent_lines );
/* Triangulation functions */
CVAPI(void) cvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2,
CvMat* projPoints1, CvMat* projPoints2,
CvMat* points4D);
CVAPI(void) cvCorrectMatches(CvMat* F, CvMat* points1, CvMat* points2,
CvMat* new_points1, CvMat* new_points2);
/* Transforms the input image to compensate lens distortion */
CVAPI(void) cvUndistort2( const CvArr* src, CvArr* dst,
const CvMat* camera_matrix,
const CvMat* distortion_coeffs,
const CvMat* new_camera_matrix CV_DEFAULT(0) );
/* Computes transformation map from intrinsic camera parameters
that can used by cvRemap */
CVAPI(void) cvInitUndistortMap( const CvMat* camera_matrix,
const CvMat* distortion_coeffs,
CvArr* mapx, CvArr* mapy );
/* Computes undistortion+rectification map for a head of stereo camera */
CVAPI(void) cvInitUndistortRectifyMap( const CvMat* camera_matrix,
const CvMat* dist_coeffs,
const CvMat *R, const CvMat* new_camera_matrix,
CvArr* mapx, CvArr* mapy );
/* Computes the original (undistorted) feature coordinates
from the observed (distorted) coordinates */
CVAPI(void) cvUndistortPoints( const CvMat* src, CvMat* dst,
const CvMat* camera_matrix,
const CvMat* dist_coeffs,
const CvMat* R CV_DEFAULT(0),
const CvMat* P CV_DEFAULT(0));
/* Computes the optimal new camera matrix according to the free scaling parameter alpha:
alpha=0 - only valid pixels will be retained in the undistorted image
alpha=1 - all the source image pixels will be retained in the undistorted image
*/
CVAPI(void) cvGetOptimalNewCameraMatrix( const CvMat* camera_matrix,
const CvMat* dist_coeffs,
CvSize image_size, double alpha,
CvMat* new_camera_matrix,
CvSize new_imag_size CV_DEFAULT(cvSize(0,0)),
CvRect* valid_pixel_ROI CV_DEFAULT(0) );
/* Converts rotation vector to rotation matrix or vice versa */
CVAPI(int) cvRodrigues2( const CvMat* src, CvMat* dst,
CvMat* jacobian CV_DEFAULT(0) );
#define CV_LMEDS 4
#define CV_RANSAC 8
/* Finds perspective transformation between the object plane and image (view) plane */
CVAPI(int) cvFindHomography( const CvMat* src_points,
const CvMat* dst_points,
CvMat* homography,
int method CV_DEFAULT(0),
double ransacReprojThreshold CV_DEFAULT(3),
CvMat* mask CV_DEFAULT(0));
/* Computes RQ decomposition for 3x3 matrices */
CVAPI(void) cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ,
CvMat *matrixQx CV_DEFAULT(NULL),
CvMat *matrixQy CV_DEFAULT(NULL),
CvMat *matrixQz CV_DEFAULT(NULL),
CvPoint3D64f *eulerAngles CV_DEFAULT(NULL));
/* Computes projection matrix decomposition */
CVAPI(void) cvDecomposeProjectionMatrix( const CvMat *projMatr, CvMat *calibMatr,
CvMat *rotMatr, CvMat *posVect,
CvMat *rotMatrX CV_DEFAULT(NULL),
CvMat *rotMatrY CV_DEFAULT(NULL),
CvMat *rotMatrZ CV_DEFAULT(NULL),
CvPoint3D64f *eulerAngles CV_DEFAULT(NULL));
/* Computes d(AB)/dA and d(AB)/dB */
CVAPI(void) cvCalcMatMulDeriv( const CvMat* A, const CvMat* B, CvMat* dABdA, CvMat* dABdB );
/* Computes r3 = rodrigues(rodrigues(r2)*rodrigues(r1)),
t3 = rodrigues(r2)*t1 + t2 and the respective derivatives */
CVAPI(void) cvComposeRT( const CvMat* _rvec1, const CvMat* _tvec1,
const CvMat* _rvec2, const CvMat* _tvec2,
CvMat* _rvec3, CvMat* _tvec3,
CvMat* dr3dr1 CV_DEFAULT(0), CvMat* dr3dt1 CV_DEFAULT(0),
CvMat* dr3dr2 CV_DEFAULT(0), CvMat* dr3dt2 CV_DEFAULT(0),
CvMat* dt3dr1 CV_DEFAULT(0), CvMat* dt3dt1 CV_DEFAULT(0),
CvMat* dt3dr2 CV_DEFAULT(0), CvMat* dt3dt2 CV_DEFAULT(0) );
/* Projects object points to the view plane using
the specified extrinsic and intrinsic camera parameters */
CVAPI(void) cvProjectPoints2( const CvMat* object_points, const CvMat* rotation_vector,
const CvMat* translation_vector, const CvMat* camera_matrix,
const CvMat* distortion_coeffs, CvMat* image_points,
CvMat* dpdrot CV_DEFAULT(NULL), CvMat* dpdt CV_DEFAULT(NULL),
CvMat* dpdf CV_DEFAULT(NULL), CvMat* dpdc CV_DEFAULT(NULL),
CvMat* dpddist CV_DEFAULT(NULL),
double aspect_ratio CV_DEFAULT(0));
/* Finds extrinsic camera parameters from
a few known corresponding point pairs and intrinsic parameters */
CVAPI(void) cvFindExtrinsicCameraParams2( const CvMat* object_points,
const CvMat* image_points,
const CvMat* camera_matrix,
const CvMat* distortion_coeffs,
CvMat* rotation_vector,
CvMat* translation_vector,
int use_extrinsic_guess CV_DEFAULT(0) );
/* Computes initial estimate of the intrinsic camera parameters
in case of planar calibration target (e.g. chessboard) */
CVAPI(void) cvInitIntrinsicParams2D( const CvMat* object_points,
const CvMat* image_points,
const CvMat* npoints, CvSize image_size,
CvMat* camera_matrix,
double aspect_ratio CV_DEFAULT(1.) );
#define CV_CALIB_CB_ADAPTIVE_THRESH 1
#define CV_CALIB_CB_NORMALIZE_IMAGE 2
#define CV_CALIB_CB_FILTER_QUADS 4
#define CV_CALIB_CB_FAST_CHECK 8
// Performs a fast check if a chessboard is in the input image. This is a workaround to
// a problem of cvFindChessboardCorners being slow on images with no chessboard
// - src: input image
// - size: chessboard size
// Returns 1 if a chessboard can be in this image and findChessboardCorners should be called,
// 0 if there is no chessboard, -1 in case of error
CVAPI(int) cvCheckChessboard(IplImage* src, CvSize size);
/* Detects corners on a chessboard calibration pattern */
CVAPI(int) cvFindChessboardCorners( const void* image, CvSize pattern_size,
CvPoint2D32f* corners,
int* corner_count CV_DEFAULT(NULL),
int flags CV_DEFAULT(CV_CALIB_CB_ADAPTIVE_THRESH+
CV_CALIB_CB_NORMALIZE_IMAGE) );
/* Draws individual chessboard corners or the whole chessboard detected */
CVAPI(void) cvDrawChessboardCorners( CvArr* image, CvSize pattern_size,
CvPoint2D32f* corners,
int count, int pattern_was_found );
#define CV_CALIB_USE_INTRINSIC_GUESS 1
#define CV_CALIB_FIX_ASPECT_RATIO 2
#define CV_CALIB_FIX_PRINCIPAL_POINT 4
#define CV_CALIB_ZERO_TANGENT_DIST 8
#define CV_CALIB_FIX_FOCAL_LENGTH 16
#define CV_CALIB_FIX_K1 32
#define CV_CALIB_FIX_K2 64
#define CV_CALIB_FIX_K3 128
/* Finds intrinsic and extrinsic camera parameters
from a few views of known calibration pattern */
CVAPI(double) cvCalibrateCamera2( const CvMat* object_points,
const CvMat* image_points,
const CvMat* point_counts,
CvSize image_size,
CvMat* camera_matrix,
CvMat* distortion_coeffs,
CvMat* rotation_vectors CV_DEFAULT(NULL),
CvMat* translation_vectors CV_DEFAULT(NULL),
int flags CV_DEFAULT(0) );
/* Computes various useful characteristics of the camera from the data computed by
cvCalibrateCamera2 */
CVAPI(void) cvCalibrationMatrixValues( const CvMat *camera_matrix,
CvSize image_size,
double aperture_width CV_DEFAULT(0),
double aperture_height CV_DEFAULT(0),
double *fovx CV_DEFAULT(NULL),
double *fovy CV_DEFAULT(NULL),
double *focal_length CV_DEFAULT(NULL),
CvPoint2D64f *principal_point CV_DEFAULT(NULL),
double *pixel_aspect_ratio CV_DEFAULT(NULL));
#define CV_CALIB_FIX_INTRINSIC 256
#define CV_CALIB_SAME_FOCAL_LENGTH 512
/* Computes the transformation from one camera coordinate system to another one
from a few correspondent views of the same calibration target. Optionally, calibrates
both cameras */
CVAPI(double) cvStereoCalibrate( const CvMat* object_points, const CvMat* image_points1,
const CvMat* image_points2, const CvMat* npoints,
CvMat* camera_matrix1, CvMat* dist_coeffs1,
CvMat* camera_matrix2, CvMat* dist_coeffs2,
CvSize image_size, CvMat* R, CvMat* T,
CvMat* E CV_DEFAULT(0), CvMat* F CV_DEFAULT(0),
CvTermCriteria term_crit CV_DEFAULT(cvTermCriteria(
CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,1e-6)),
int flags CV_DEFAULT(CV_CALIB_FIX_INTRINSIC));
#define CV_CALIB_ZERO_DISPARITY 1024
/* Computes 3D rotations (+ optional shift) for each camera coordinate system to make both
views parallel (=> to make all the epipolar lines horizontal or vertical) */
CVAPI(void) cvStereoRectify( const CvMat* camera_matrix1, const CvMat* camera_matrix2,
const CvMat* dist_coeffs1, const CvMat* dist_coeffs2,
CvSize image_size, const CvMat* R, const CvMat* T,
CvMat* R1, CvMat* R2, CvMat* P1, CvMat* P2,
CvMat* Q CV_DEFAULT(0),
int flags CV_DEFAULT(CV_CALIB_ZERO_DISPARITY),
double alpha CV_DEFAULT(-1),
CvSize new_image_size CV_DEFAULT(cvSize(0,0)),
CvRect* valid_pix_ROI1 CV_DEFAULT(0),
CvRect* valid_pix_ROI2 CV_DEFAULT(0));
/* Computes rectification transformations for uncalibrated pair of images using a set
of point correspondences */
CVAPI(int) cvStereoRectifyUncalibrated( const CvMat* points1, const CvMat* points2,
const CvMat* F, CvSize img_size,
CvMat* H1, CvMat* H2,
double threshold CV_DEFAULT(5));
/* stereo correspondence parameters and functions */
#define CV_STEREO_BM_NORMALIZED_RESPONSE 0
#define CV_STEREO_BM_XSOBEL 1
/* Block matching algorithm structure */
typedef struct CvStereoBMState
{
// pre-filtering (normalization of input images)
int preFilterType; // =CV_STEREO_BM_NORMALIZED_RESPONSE now
int preFilterSize; // averaging window size: ~5x5..21x21
int preFilterCap; // the output of pre-filtering is clipped by [-preFilterCap,preFilterCap]
// correspondence using Sum of Absolute Difference (SAD)
int SADWindowSize; // ~5x5..21x21
int minDisparity; // minimum disparity (can be negative)
int numberOfDisparities; // maximum disparity - minimum disparity (> 0)
// post-filtering
int textureThreshold; // the disparity is only computed for pixels
// with textured enough neighborhood
int uniquenessRatio; // accept the computed disparity d* only if
// SAD(d) >= SAD(d*)*(1 + uniquenessRatio/100.)
// for any d != d*+/-1 within the search range.
int speckleWindowSize; // disparity variation window
int speckleRange; // acceptable range of variation in window
int trySmallerWindows; // if 1, the results may be more accurate,
// at the expense of slower processing
CvRect roi1, roi2;
int disp12MaxDiff;
// temporary buffers
CvMat* preFilteredImg0;
CvMat* preFilteredImg1;
CvMat* slidingSumBuf;
CvMat* cost;
CvMat* disp;
} CvStereoBMState;
#define CV_STEREO_BM_BASIC 0
#define CV_STEREO_BM_FISH_EYE 1
#define CV_STEREO_BM_NARROW 2
CVAPI(CvStereoBMState*) cvCreateStereoBMState(int preset CV_DEFAULT(CV_STEREO_BM_BASIC),
int numberOfDisparities CV_DEFAULT(0));
CVAPI(void) cvReleaseStereoBMState( CvStereoBMState** state );
CVAPI(void) cvFindStereoCorrespondenceBM( const CvArr* left, const CvArr* right,
CvArr* disparity, CvStereoBMState* state );
CVAPI(CvRect) cvGetValidDisparityROI( CvRect roi1, CvRect roi2, int minDisparity,
int numberOfDisparities, int SADWindowSize );
CVAPI(void) cvValidateDisparity( CvArr* disparity, const CvArr* cost,
int minDisparity, int numberOfDisparities,
int disp12MaxDiff CV_DEFAULT(1) );
/* Kolmogorov-Zabin stereo-correspondence algorithm (a.k.a. KZ1) */
#define CV_STEREO_GC_OCCLUDED SHRT_MAX
typedef struct CvStereoGCState
{
int Ithreshold;
int interactionRadius;
float K, lambda, lambda1, lambda2;
int occlusionCost;
int minDisparity;
int numberOfDisparities;
int maxIters;
CvMat* left;
CvMat* right;
CvMat* dispLeft;
CvMat* dispRight;
CvMat* ptrLeft;
CvMat* ptrRight;
CvMat* vtxBuf;
CvMat* edgeBuf;
} CvStereoGCState;
CVAPI(CvStereoGCState*) cvCreateStereoGCState( int numberOfDisparities, int maxIters );
CVAPI(void) cvReleaseStereoGCState( CvStereoGCState** state );
CVAPI(void) cvFindStereoCorrespondenceGC( const CvArr* left, const CvArr* right,
CvArr* disparityLeft, CvArr* disparityRight,
CvStereoGCState* state,
int useDisparityGuess CV_DEFAULT(0) );
/* Reprojects the computed disparity image to the 3D space using the specified 4x4 matrix */
CVAPI(void) cvReprojectImageTo3D( const CvArr* disparityImage,
CvArr* _3dImage, const CvMat* Q,
int handleMissingValues CV_DEFAULT(0) );
#ifdef __cplusplus
}
//////////////////////////////////////////////////////////////////////////////////////////
class CV_EXPORTS CvLevMarq
{
public:
CvLevMarq();
CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria=
cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON),
bool completeSymmFlag=false );
~CvLevMarq();
void init( int nparams, int nerrs, CvTermCriteria criteria=
cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON),
bool completeSymmFlag=false );
bool update( const CvMat*& param, CvMat*& J, CvMat*& err );
bool updateAlt( const CvMat*& param, CvMat*& JtJ, CvMat*& JtErr, double*& errNorm );
void clear();
void step();
enum { DONE=0, STARTED=1, CALC_J=2, CHECK_ERR=3 };
cv::Ptr<CvMat> mask;
cv::Ptr<CvMat> prevParam;
cv::Ptr<CvMat> param;
cv::Ptr<CvMat> J;
cv::Ptr<CvMat> err;
cv::Ptr<CvMat> JtJ;
cv::Ptr<CvMat> JtJN;
cv::Ptr<CvMat> JtErr;
cv::Ptr<CvMat> JtJV;
cv::Ptr<CvMat> JtJW;
double prevErrNorm, errNorm;
int lambdaLg10;
CvTermCriteria criteria;
int state;
int iters;
bool completeSymmFlag;
};
namespace cv
{
CV_EXPORTS void undistortPoints( const Mat& src, vector<Point2f>& dst,
const Mat& cameraMatrix, const Mat& distCoeffs,
const Mat& R=Mat(), const Mat& P=Mat());
CV_EXPORTS void undistortPoints( const Mat& src, Mat& dst,
const Mat& cameraMatrix, const Mat& distCoeffs,
const Mat& R=Mat(), const Mat& P=Mat());
CV_EXPORTS void Rodrigues(const Mat& src, Mat& dst);
CV_EXPORTS void Rodrigues(const Mat& src, Mat& dst, Mat& jacobian);
enum { LMEDS=4, RANSAC=8 };
CV_EXPORTS Mat findHomography( const Mat& srcPoints,
const Mat& dstPoints,
Mat& mask, int method=0,
double ransacReprojThreshold=3 );
CV_EXPORTS Mat findHomography( const Mat& srcPoints,
const Mat& dstPoints,
vector<uchar>& mask, int method=0,
double ransacReprojThreshold=3 );
CV_EXPORTS Mat findHomography( const Mat& srcPoints,
const Mat& dstPoints,
int method=0, double ransacReprojThreshold=3 );
/* Computes RQ decomposition for 3x3 matrices */
CV_EXPORTS void RQDecomp3x3( const Mat& M, Mat& R, Mat& Q );
CV_EXPORTS Vec3d RQDecomp3x3( const Mat& M, Mat& R, Mat& Q,
Mat& Qx, Mat& Qy, Mat& Qz );
CV_EXPORTS void decomposeProjectionMatrix( const Mat& projMatrix, Mat& cameraMatrix,
Mat& rotMatrix, Mat& transVect );
CV_EXPORTS void decomposeProjectionMatrix( const Mat& projMatrix, Mat& cameraMatrix,
Mat& rotMatrix, Mat& transVect,
Mat& rotMatrixX, Mat& rotMatrixY,
Mat& rotMatrixZ, Vec3d& eulerAngles );
CV_EXPORTS void matMulDeriv( const Mat& A, const Mat& B, Mat& dABdA, Mat& dABdB );
CV_EXPORTS void composeRT( const Mat& rvec1, const Mat& tvec1,
const Mat& rvec2, const Mat& tvec2,
Mat& rvec3, Mat& tvec3 );
CV_EXPORTS void 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 );
CV_EXPORTS void projectPoints( const Mat& objectPoints,
const Mat& rvec, const Mat& tvec,
const Mat& cameraMatrix,
const Mat& distCoeffs,
vector<Point2f>& imagePoints );
CV_EXPORTS void projectPoints( const Mat& objectPoints,
const Mat& rvec, const Mat& tvec,
const Mat& cameraMatrix,
const Mat& distCoeffs,
vector<Point2f>& imagePoints,
Mat& dpdrot, Mat& dpdt, Mat& dpdf,
Mat& dpdc, Mat& dpddist,
double aspectRatio=0 );
CV_EXPORTS void solvePnP( const Mat& objectPoints,
const Mat& imagePoints,
const Mat& cameraMatrix,
const Mat& distCoeffs,
Mat& rvec, Mat& tvec,
bool useExtrinsicGuess=false );
CV_EXPORTS Mat initCameraMatrix2D( const vector<vector<Point3f> >& objectPoints,
const vector<vector<Point2f> >& imagePoints,
Size imageSize, double aspectRatio=1. );
enum { CALIB_CB_ADAPTIVE_THRESH = 1, CALIB_CB_NORMALIZE_IMAGE = 2,
CALIB_CB_FILTER_QUADS = 4, CALIB_CB_FAST_CHECK = 8 };
CV_EXPORTS bool findChessboardCorners( const Mat& image, Size patternSize,
vector<Point2f>& corners,
int flags=CALIB_CB_ADAPTIVE_THRESH+
CALIB_CB_NORMALIZE_IMAGE );
CV_EXPORTS void drawChessboardCorners( Mat& image, Size patternSize,
const Mat& corners,
bool patternWasFound );
enum
{
CALIB_USE_INTRINSIC_GUESS = 1,
CALIB_FIX_ASPECT_RATIO = 2,
CALIB_FIX_PRINCIPAL_POINT = 4,
CALIB_ZERO_TANGENT_DIST = 8,
CALIB_FIX_FOCAL_LENGTH = 16,
CALIB_FIX_K1 = 32,
CALIB_FIX_K2 = 64,
CALIB_FIX_K3 = 128,
// only for stereo
CALIB_FIX_INTRINSIC = 256,
CALIB_SAME_FOCAL_LENGTH = 512,
// for stereo rectification
CALIB_ZERO_DISPARITY = 1024
};
CV_EXPORTS double 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=0 );
CV_EXPORTS void calibrationMatrixValues( const Mat& cameraMatrix,
Size imageSize,
double apertureWidth,
double apertureHeight,
double& fovx,
double& fovy,
double& focalLength,
Point2d& principalPoint,
double& aspectRatio );
CV_EXPORTS double 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 = TermCriteria(TermCriteria::COUNT+
TermCriteria::EPS, 30, 1e-6),
int flags=CALIB_FIX_INTRINSIC );
CV_EXPORTS void 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=CALIB_ZERO_DISPARITY );
CV_EXPORTS void 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,
double alpha, Size newImageSize=Size(),
Rect* validPixROI1=0, Rect* validPixROI2=0,
int flags=CALIB_ZERO_DISPARITY );
CV_EXPORTS bool stereoRectifyUncalibrated( const Mat& points1,
const Mat& points2,
const Mat& F, Size imgSize,
Mat& H1, Mat& H2,
double threshold=5 );
CV_EXPORTS void convertPointsHomogeneous( const Mat& src, vector<Point3f>& dst );
CV_EXPORTS void convertPointsHomogeneous( const Mat& src, vector<Point2f>& dst );
enum
{
FM_7POINT = 1, FM_8POINT = 2, FM_LMEDS = 4, FM_RANSAC = 8
};
CV_EXPORTS Mat findFundamentalMat( const Mat& points1, const Mat& points2,
vector<uchar>& mask, int method=FM_RANSAC,
double param1=3., double param2=0.99 );
CV_EXPORTS Mat findFundamentalMat( const Mat& points1, const Mat& points2,
int method=FM_RANSAC,
double param1=3., double param2=0.99 );
CV_EXPORTS void computeCorrespondEpilines( const Mat& points1,
int whichImage, const Mat& F,
vector<Vec3f>& lines );
template<> CV_EXPORTS void Ptr<CvStereoBMState>::delete_obj();
// Block matching stereo correspondence algorithm
class CV_EXPORTS StereoBM
{
public:
enum { PREFILTER_NORMALIZED_RESPONSE = 0, PREFILTER_XSOBEL = 1,
BASIC_PRESET=0, FISH_EYE_PRESET=1, NARROW_PRESET=2 };
StereoBM();
StereoBM(int preset, int ndisparities=0, int SADWindowSize=21);
void init(int preset, int ndisparities=0, int SADWindowSize=21);
void operator()( const Mat& left, const Mat& right, Mat& disparity, int disptype=CV_16S );
Ptr<CvStereoBMState> state;
};
class CV_EXPORTS StereoSGBM
{
public:
enum { DISP_SHIFT=4, DISP_SCALE = (1<<DISP_SHIFT) };
StereoSGBM();
StereoSGBM(int minDisparity, int numDisparities, int SADWindowSize,
int P1=0, int P2=0, int disp12MaxDiff=0,
int preFilterCap=0, int uniquenessRatio=0,
int speckleWindowSize=0, int speckleRange=0,
bool fullDP=false);
virtual ~StereoSGBM();
virtual void operator()(const Mat& left, const Mat& right, Mat& disp);
int minDisparity;
int numberOfDisparities;
int SADWindowSize;
int preFilterCap;
int uniquenessRatio;
int P1, P2;
int speckleWindowSize;
int speckleRange;
int disp12MaxDiff;
bool fullDP;
protected:
Mat buffer;
};
CV_EXPORTS void filterSpeckles( Mat& img, double newVal, int maxSpeckleSize, double maxDiff, Mat& buf );
CV_EXPORTS Rect getValidDisparityROI( Rect roi1, Rect roi2,
int minDisparity, int numberOfDisparities,
int SADWindowSize );
CV_EXPORTS void validateDisparity( Mat& disparity, const Mat& cost,
int minDisparity, int numberOfDisparities,
int disp12MaxDisp=1 );
CV_EXPORTS void reprojectImageTo3D( const Mat& disparity,
Mat& _3dImage, const Mat& Q,
bool handleMissingValues=false );
}
#endif
#include "opencv2/calib3d/compat_c.h"
#endif

321
modules/calib3d/include/opencv2/calib3d/compat_c.h Normal file
View File

@@ -0,0 +1,321 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#ifndef __OPENCV_CALIB3D_COMPAT_C_H__
#define __OPENCV_CALIB3D_COMPAT_C_H__
#include "opencv2/imgproc/imgproc_c.h"
#ifdef __cplusplus
extern "C" {
#endif
/* Find fundamental matrix */
CV_INLINE void cvFindFundamentalMatrix( int* points1, int* points2,
int numpoints, int CV_UNREFERENCED(method), float* matrix )
{
CvMat* pointsMat1;
CvMat* pointsMat2;
CvMat fundMatr = cvMat(3,3,CV_32F,matrix);
int i, curr = 0;
pointsMat1 = cvCreateMat(3,numpoints,CV_64F);
pointsMat2 = cvCreateMat(3,numpoints,CV_64F);
for( i = 0; i < numpoints; i++ )
{
cvmSet(pointsMat1,0,i,points1[curr]);//x
cvmSet(pointsMat1,1,i,points1[curr+1]);//y
cvmSet(pointsMat1,2,i,1.0);
cvmSet(pointsMat2,0,i,points2[curr]);//x
cvmSet(pointsMat2,1,i,points2[curr+1]);//y
cvmSet(pointsMat2,2,i,1.0);
curr += 2;
}
cvFindFundamentalMat(pointsMat1,pointsMat2,&fundMatr,CV_FM_RANSAC,1,0.99,0);
cvReleaseMat(&pointsMat1);
cvReleaseMat(&pointsMat2);
}
CV_INLINE int
cvFindChessBoardCornerGuesses( const void* arr, void* CV_UNREFERENCED(thresharr),
CvMemStorage * CV_UNREFERENCED(storage),
CvSize pattern_size, CvPoint2D32f * corners,
int *corner_count )
{
return cvFindChessboardCorners( arr, pattern_size, corners,
corner_count, CV_CALIB_CB_ADAPTIVE_THRESH );
}
/* Calibrates camera using multiple views of calibration pattern */
CV_INLINE void cvCalibrateCamera( int image_count, int* _point_counts,
CvSize image_size, CvPoint2D32f* _image_points, CvPoint3D32f* _object_points,
float* _distortion_coeffs, float* _camera_matrix, float* _translation_vectors,
float* _rotation_matrices, int flags )
{
int i, total = 0;
CvMat point_counts = cvMat( image_count, 1, CV_32SC1, _point_counts );
CvMat image_points, object_points;
CvMat dist_coeffs = cvMat( 4, 1, CV_32FC1, _distortion_coeffs );
CvMat camera_matrix = cvMat( 3, 3, CV_32FC1, _camera_matrix );
CvMat rotation_matrices = cvMat( image_count, 9, CV_32FC1, _rotation_matrices );
CvMat translation_vectors = cvMat( image_count, 3, CV_32FC1, _translation_vectors );
for( i = 0; i < image_count; i++ )
total += _point_counts[i];
image_points = cvMat( total, 1, CV_32FC2, _image_points );
object_points = cvMat( total, 1, CV_32FC3, _object_points );
cvCalibrateCamera2( &object_points, &image_points, &point_counts, image_size,
&camera_matrix, &dist_coeffs, &rotation_matrices, &translation_vectors,
flags );
}
CV_INLINE void cvCalibrateCamera_64d( int image_count, int* _point_counts,
CvSize image_size, CvPoint2D64f* _image_points, CvPoint3D64f* _object_points,
double* _distortion_coeffs, double* _camera_matrix, double* _translation_vectors,
double* _rotation_matrices, int flags )
{
int i, total = 0;
CvMat point_counts = cvMat( image_count, 1, CV_32SC1, _point_counts );
CvMat image_points, object_points;
CvMat dist_coeffs = cvMat( 4, 1, CV_64FC1, _distortion_coeffs );
CvMat camera_matrix = cvMat( 3, 3, CV_64FC1, _camera_matrix );
CvMat rotation_matrices = cvMat( image_count, 9, CV_64FC1, _rotation_matrices );
CvMat translation_vectors = cvMat( image_count, 3, CV_64FC1, _translation_vectors );
for( i = 0; i < image_count; i++ )
total += _point_counts[i];
image_points = cvMat( total, 1, CV_64FC2, _image_points );
object_points = cvMat( total, 1, CV_64FC3, _object_points );
cvCalibrateCamera2( &object_points, &image_points, &point_counts, image_size,
&camera_matrix, &dist_coeffs, &rotation_matrices, &translation_vectors,
flags );
}
/* Find 3d position of object given intrinsic camera parameters,
3d model of the object and projection of the object into view plane */
CV_INLINE void cvFindExtrinsicCameraParams( int point_count,
CvSize CV_UNREFERENCED(image_size), CvPoint2D32f* _image_points,
CvPoint3D32f* _object_points, float* focal_length,
CvPoint2D32f principal_point, float* _distortion_coeffs,
float* _rotation_vector, float* _translation_vector )
{
CvMat image_points = cvMat( point_count, 1, CV_32FC2, _image_points );
CvMat object_points = cvMat( point_count, 1, CV_32FC3, _object_points );
CvMat dist_coeffs = cvMat( 4, 1, CV_32FC1, _distortion_coeffs );
float a[9];
CvMat camera_matrix = cvMat( 3, 3, CV_32FC1, a );
CvMat rotation_vector = cvMat( 1, 1, CV_32FC3, _rotation_vector );
CvMat translation_vector = cvMat( 1, 1, CV_32FC3, _translation_vector );
a[0] = focal_length[0]; a[4] = focal_length[1];
a[2] = principal_point.x; a[5] = principal_point.y;
a[1] = a[3] = a[6] = a[7] = 0.f;
a[8] = 1.f;
cvFindExtrinsicCameraParams2( &object_points, &image_points, &camera_matrix,
&dist_coeffs, &rotation_vector, &translation_vector, 0 );
}
/* Variant of the previous function that takes double-precision parameters */
CV_INLINE void cvFindExtrinsicCameraParams_64d( int point_count,
CvSize CV_UNREFERENCED(image_size), CvPoint2D64f* _image_points,
CvPoint3D64f* _object_points, double* focal_length,
CvPoint2D64f principal_point, double* _distortion_coeffs,
double* _rotation_vector, double* _translation_vector )
{
CvMat image_points = cvMat( point_count, 1, CV_64FC2, _image_points );
CvMat object_points = cvMat( point_count, 1, CV_64FC3, _object_points );
CvMat dist_coeffs = cvMat( 4, 1, CV_64FC1, _distortion_coeffs );
double a[9];
CvMat camera_matrix = cvMat( 3, 3, CV_64FC1, a );
CvMat rotation_vector = cvMat( 1, 1, CV_64FC3, _rotation_vector );
CvMat translation_vector = cvMat( 1, 1, CV_64FC3, _translation_vector );
a[0] = focal_length[0]; a[4] = focal_length[1];
a[2] = principal_point.x; a[5] = principal_point.y;
a[1] = a[3] = a[6] = a[7] = 0.;
a[8] = 1.;
cvFindExtrinsicCameraParams2( &object_points, &image_points, &camera_matrix,
&dist_coeffs, &rotation_vector, &translation_vector, 0 );
}
/* Rodrigues transform */
#define CV_RODRIGUES_M2V 0
#define CV_RODRIGUES_V2M 1
/* Converts rotation_matrix matrix to rotation_matrix vector or vice versa */
CV_INLINE void cvRodrigues( CvMat* rotation_matrix, CvMat* rotation_vector,
CvMat* jacobian, int conv_type )
{
if( conv_type == CV_RODRIGUES_V2M )
cvRodrigues2( rotation_vector, rotation_matrix, jacobian );
else
cvRodrigues2( rotation_matrix, rotation_vector, jacobian );
}
/* Does reprojection of 3d object points to the view plane */
CV_INLINE void cvProjectPoints( int point_count, CvPoint3D64f* _object_points,
double* _rotation_vector, double* _translation_vector,
double* focal_length, CvPoint2D64f principal_point,
double* _distortion, CvPoint2D64f* _image_points,
double* _deriv_points_rotation_matrix,
double* _deriv_points_translation_vect,
double* _deriv_points_focal,
double* _deriv_points_principal_point,
double* _deriv_points_distortion_coeffs )
{
CvMat object_points = cvMat( point_count, 1, CV_64FC3, _object_points );
CvMat image_points = cvMat( point_count, 1, CV_64FC2, _image_points );
CvMat rotation_vector = cvMat( 3, 1, CV_64FC1, _rotation_vector );
CvMat translation_vector = cvMat( 3, 1, CV_64FC1, _translation_vector );
double a[9];
CvMat camera_matrix = cvMat( 3, 3, CV_64FC1, a );
CvMat dist_coeffs = cvMat( 4, 1, CV_64FC1, _distortion );
CvMat dpdr = cvMat( 2*point_count, 3, CV_64FC1, _deriv_points_rotation_matrix );
CvMat dpdt = cvMat( 2*point_count, 3, CV_64FC1, _deriv_points_translation_vect );
CvMat dpdf = cvMat( 2*point_count, 2, CV_64FC1, _deriv_points_focal );
CvMat dpdc = cvMat( 2*point_count, 2, CV_64FC1, _deriv_points_principal_point );
CvMat dpdk = cvMat( 2*point_count, 4, CV_64FC1, _deriv_points_distortion_coeffs );
a[0] = focal_length[0]; a[4] = focal_length[1];
a[2] = principal_point.x; a[5] = principal_point.y;
a[1] = a[3] = a[6] = a[7] = 0.;
a[8] = 1.;
cvProjectPoints2( &object_points, &rotation_vector, &translation_vector,
&camera_matrix, &dist_coeffs, &image_points,
&dpdr, &dpdt, &dpdf, &dpdc, &dpdk, 0 );
}
/* Simpler version of the previous function */
CV_INLINE void cvProjectPointsSimple( int point_count, CvPoint3D64f* _object_points,
double* _rotation_matrix, double* _translation_vector,
double* _camera_matrix, double* _distortion, CvPoint2D64f* _image_points )
{
CvMat object_points = cvMat( point_count, 1, CV_64FC3, _object_points );
CvMat image_points = cvMat( point_count, 1, CV_64FC2, _image_points );
CvMat rotation_matrix = cvMat( 3, 3, CV_64FC1, _rotation_matrix );
CvMat translation_vector = cvMat( 3, 1, CV_64FC1, _translation_vector );
CvMat camera_matrix = cvMat( 3, 3, CV_64FC1, _camera_matrix );
CvMat dist_coeffs = cvMat( 4, 1, CV_64FC1, _distortion );
cvProjectPoints2( &object_points, &rotation_matrix, &translation_vector,
&camera_matrix, &dist_coeffs, &image_points,
0, 0, 0, 0, 0, 0 );
}
CV_INLINE void cvUnDistortOnce( const CvArr* src, CvArr* dst,
const float* intrinsic_matrix,
const float* distortion_coeffs,
int CV_UNREFERENCED(interpolate) )
{
CvMat _a = cvMat( 3, 3, CV_32F, (void*)intrinsic_matrix );
CvMat _k = cvMat( 4, 1, CV_32F, (void*)distortion_coeffs );
cvUndistort2( src, dst, &_a, &_k, 0 );
}
/* the two functions below have quite hackerish implementations, use with care
(or, which is better, switch to cvUndistortInitMap and cvRemap instead */
CV_INLINE void cvUnDistortInit( const CvArr* CV_UNREFERENCED(src),
CvArr* undistortion_map,
const float* A, const float* k,
int CV_UNREFERENCED(interpolate) )
{
union { uchar* ptr; float* fl; } data;
CvSize sz;
cvGetRawData( undistortion_map, &data.ptr, 0, &sz );
assert( sz.width >= 8 );
/* just save the intrinsic parameters to the map */
data.fl[0] = A[0]; data.fl[1] = A[4];
data.fl[2] = A[2]; data.fl[3] = A[5];
data.fl[4] = k[0]; data.fl[5] = k[1];
data.fl[6] = k[2]; data.fl[7] = k[3];
}
CV_INLINE void cvUnDistort( const CvArr* src, CvArr* dst,
const CvArr* undistortion_map,
int CV_UNREFERENCED(interpolate) )
{
union { uchar* ptr; float* fl; } data;
float a[] = {0,0,0,0,0,0,0,0,1};
CvSize sz;
cvGetRawData( undistortion_map, &data.ptr, 0, &sz );
assert( sz.width >= 8 );
a[0] = data.fl[0]; a[4] = data.fl[1];
a[2] = data.fl[2]; a[5] = data.fl[3];
cvUnDistortOnce( src, dst, a, data.fl + 4, 1 );
}
#define cvMake2DPoints cvConvertPointsHomogeneous
#define cvMake3DPoints cvConvertPointsHomogeneous
#define cvWarpPerspectiveQMatrix cvGetPerspectiveTransform
#define cvConvertPointsHomogenious cvConvertPointsHomogeneous
#ifdef __cplusplus
}
#endif
#endif

81
modules/calib3d/src/_modelest.h Normal file
View File

@@ -0,0 +1,81 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of Intel Corporation may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#ifndef _CV_MODEL_EST_H_
#define _CV_MODEL_EST_H_
#include "precomp.hpp"
class CvModelEstimator2
{
public:
CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions);
virtual ~CvModelEstimator2();
virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )=0;
virtual bool runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model,
CvMat* mask, double confidence=0.99, int maxIters=2000 );
virtual bool runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model,
CvMat* mask, double threshold,
double confidence=0.99, int maxIters=2000 );
virtual bool refine( const CvMat*, const CvMat*, CvMat*, int ) { return true; }
virtual void setSeed( int64 seed );
protected:
virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
const CvMat* model, CvMat* error ) = 0;
virtual int findInliers( const CvMat* m1, const CvMat* m2,
const CvMat* model, CvMat* error,
CvMat* mask, double threshold );
virtual bool getSubset( const CvMat* m1, const CvMat* m2,
CvMat* ms1, CvMat* ms2, int maxAttempts=1000 );
virtual bool checkSubset( const CvMat* ms1, int count );
CvRNG rng;
int modelPoints;
CvSize modelSize;
int maxBasicSolutions;
bool checkPartialSubsets;
};
#endif // _CV_MODEL_EST_H_

1921
modules/calib3d/src/calibinit.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

3452
modules/calib3d/src/calibration.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

200
modules/calib3d/src/checkchessboard.cpp Normal file
View File

@@ -0,0 +1,200 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of Intel Corporation may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
#include <vector>
#include <algorithm>
//#define DEBUG_WINDOWS
#if defined(DEBUG_WINDOWS)
#include "highgui.h"
#endif
void icvGetQuadrangleHypotheses(CvSeq* contours, std::vector<std::pair<float, int> >& quads, int class_id)
{
const float min_aspect_ratio = 0.3f;
const float max_aspect_ratio = 3.0f;
const float min_box_size = 10.0f;
for(CvSeq* seq = contours; seq != NULL; seq = seq->h_next)
{
CvBox2D box = cvMinAreaRect2(seq);
float box_size = MAX(box.size.width, box.size.height);
if(box_size < min_box_size)
{
continue;
}
float aspect_ratio = box.size.width/MAX(box.size.height, 1);
if(aspect_ratio < min_aspect_ratio || aspect_ratio > max_aspect_ratio)
{
continue;
}
quads.push_back(std::pair<float, int>(box_size, class_id));
}
}
void countClasses(const std::vector<std::pair<float, int> >& pairs, size_t idx1, size_t idx2, std::vector<int>& counts)
{
counts.assign(2, 0);
for(size_t i = idx1; i != idx2; i++)
{
counts[pairs[i].second]++;
}
}
bool less_pred(const std::pair<float, int>& p1, const std::pair<float, int>& p2)
{
return p1.first < p2.first;
}
// does a fast check if a chessboard is in the input image. This is a workaround to
// a problem of cvFindChessboardCorners being slow on images with no chessboard
// - src: input image
// - size: chessboard size
// Returns 1 if a chessboard can be in this image and findChessboardCorners should be called,
// 0 if there is no chessboard, -1 in case of error
int cvCheckChessboard(IplImage* src, CvSize size)
{
if(src->nChannels > 1)
{
cvError(CV_BadNumChannels, "cvCheckChessboard", "supports single-channel images only",
__FILE__, __LINE__);
}
if(src->depth != 8)
{
cvError(CV_BadDepth, "cvCheckChessboard", "supports depth=8 images only",
__FILE__, __LINE__);
}
const int erosion_count = 1;
const float black_level = 20.f;
const float white_level = 130.f;
const float black_white_gap = 70.f;
#if defined(DEBUG_WINDOWS)
cvNamedWindow("1", 1);
cvShowImage("1", src);
cvWaitKey(0);
#endif //DEBUG_WINDOWS
CvMemStorage* storage = cvCreateMemStorage();
IplImage* white = cvCloneImage(src);
IplImage* black = cvCloneImage(src);
cvErode(white, white, NULL, erosion_count);
cvDilate(black, black, NULL, erosion_count);
IplImage* thresh = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);
int result = 0;
for(float thresh_level = black_level; thresh_level < white_level && !result; thresh_level += 20.0f)
{
cvThreshold(white, thresh, thresh_level + black_white_gap, 255, CV_THRESH_BINARY);
#if defined(DEBUG_WINDOWS)
cvShowImage("1", thresh);
cvWaitKey(0);
#endif //DEBUG_WINDOWS
CvSeq* first = 0;
std::vector<std::pair<float, int> > quads;
cvFindContours(thresh, storage, &first, sizeof(CvContour), CV_RETR_CCOMP);
icvGetQuadrangleHypotheses(first, quads, 1);
cvThreshold(black, thresh, thresh_level, 255, CV_THRESH_BINARY_INV);
#if defined(DEBUG_WINDOWS)
cvShowImage("1", thresh);
cvWaitKey(0);
#endif //DEBUG_WINDOWS
cvFindContours(thresh, storage, &first, sizeof(CvContour), CV_RETR_CCOMP);
icvGetQuadrangleHypotheses(first, quads, 0);
const size_t min_quads_count = size.width*size.height/2;
std::sort(quads.begin(), quads.end(), less_pred);
// now check if there are many hypotheses with similar sizes
// do this by floodfill-style algorithm
const float size_rel_dev = 0.4f;
for(size_t i = 0; i < quads.size(); i++)
{
size_t j = i + 1;
for(; j < quads.size(); j++)
{
if(quads[j].first/quads[i].first > 1.0f + size_rel_dev)
{
break;
}
}
if(j + 1 > min_quads_count + i)
{
// check the number of black and white squares
std::vector<int> counts;
countClasses(quads, i, j, counts);
const int black_count = cvRound(ceil(size.width/2.0)*ceil(size.height/2.0));
const int white_count = cvRound(floor(size.width/2.0)*floor(size.height/2.0));
if(counts[0] < black_count*0.75 ||
counts[1] < white_count*0.75)
{
continue;
}
result = 1;
break;
}
}
}
cvReleaseImage(&thresh);
cvReleaseImage(&white);
cvReleaseImage(&black);
cvReleaseMemStorage(&storage);
return result;
}

1163
modules/calib3d/src/fundam.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

484
modules/calib3d/src/modelest.cpp Normal file
View File

@@ -0,0 +1,484 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of Intel Corporation may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
#include "_modelest.h"
#include <algorithm>
#include <iterator>
#include <limits>
using namespace std;
CvModelEstimator2::CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions)
{
modelPoints = _modelPoints;
modelSize = _modelSize;
maxBasicSolutions = _maxBasicSolutions;
checkPartialSubsets = true;
rng = cvRNG(-1);
}
CvModelEstimator2::~CvModelEstimator2()
{
}
void CvModelEstimator2::setSeed( int64 seed )
{
rng = cvRNG(seed);
}
int CvModelEstimator2::findInliers( const CvMat* m1, const CvMat* m2,
const CvMat* model, CvMat* _err,
CvMat* _mask, double threshold )
{
int i, count = _err->rows*_err->cols, goodCount = 0;
const float* err = _err->data.fl;
uchar* mask = _mask->data.ptr;
computeReprojError( m1, m2, model, _err );
threshold *= threshold;
for( i = 0; i < count; i++ )
goodCount += mask[i] = err[i] <= threshold;
return goodCount;
}
CV_IMPL int
cvRANSACUpdateNumIters( double p, double ep,
int model_points, int max_iters )
{
if( model_points <= 0 )
CV_Error( CV_StsOutOfRange, "the number of model points should be positive" );
p = MAX(p, 0.);
p = MIN(p, 1.);
ep = MAX(ep, 0.);
ep = MIN(ep, 1.);
// avoid inf's & nan's
double num = MAX(1. - p, DBL_MIN);
double denom = 1. - pow(1. - ep,model_points);
if( denom < DBL_MIN )
return 0;
num = log(num);
denom = log(denom);
return denom >= 0 || -num >= max_iters*(-denom) ?
max_iters : cvRound(num/denom);
}
bool CvModelEstimator2::runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model,
CvMat* mask0, double reprojThreshold,
double confidence, int maxIters )
{
bool result = false;
cv::Ptr<CvMat> mask = cvCloneMat(mask0);
cv::Ptr<CvMat> models, err, tmask;
cv::Ptr<CvMat> ms1, ms2;
int iter, niters = maxIters;
int count = m1->rows*m1->cols, maxGoodCount = 0;
CV_Assert( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) );
if( count < modelPoints )
return false;
models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 );
err = cvCreateMat( 1, count, CV_32FC1 );
tmask = cvCreateMat( 1, count, CV_8UC1 );
if( count > modelPoints )
{
ms1 = cvCreateMat( 1, modelPoints, m1->type );
ms2 = cvCreateMat( 1, modelPoints, m2->type );
}
else
{
niters = 1;
ms1 = cvCloneMat(m1);
ms2 = cvCloneMat(m2);
}
for( iter = 0; iter < niters; iter++ )
{
int i, goodCount, nmodels;
if( count > modelPoints )
{
bool found = getSubset( m1, m2, ms1, ms2, 300 );
if( !found )
{
if( iter == 0 )
return false;
break;
}
}
nmodels = runKernel( ms1, ms2, models );
if( nmodels <= 0 )
continue;
for( i = 0; i < nmodels; i++ )
{
CvMat model_i;
cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );
goodCount = findInliers( m1, m2, &model_i, err, tmask, reprojThreshold );
if( goodCount > MAX(maxGoodCount, modelPoints-1) )
{
std::swap(tmask, mask);
cvCopy( &model_i, model );
maxGoodCount = goodCount;
niters = cvRANSACUpdateNumIters( confidence,
(double)(count - goodCount)/count, modelPoints, niters );
}
}
}
if( maxGoodCount > 0 )
{
if( mask != mask0 )
cvCopy( mask, mask0 );
result = true;
}
return result;
}
static CV_IMPLEMENT_QSORT( icvSortDistances, int, CV_LT )
bool CvModelEstimator2::runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model,
CvMat* mask, double confidence, int maxIters )
{
const double outlierRatio = 0.45;
bool result = false;
cv::Ptr<CvMat> models;
cv::Ptr<CvMat> ms1, ms2;
cv::Ptr<CvMat> err;
int iter, niters = maxIters;
int count = m1->rows*m1->cols;
double minMedian = DBL_MAX, sigma;
CV_Assert( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) );
if( count < modelPoints )
return false;
models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 );
err = cvCreateMat( 1, count, CV_32FC1 );
if( count > modelPoints )
{
ms1 = cvCreateMat( 1, modelPoints, m1->type );
ms2 = cvCreateMat( 1, modelPoints, m2->type );
}
else
{
niters = 1;
ms1 = cvCloneMat(m1);
ms2 = cvCloneMat(m2);
}
niters = cvRound(log(1-confidence)/log(1-pow(1-outlierRatio,(double)modelPoints)));
niters = MIN( MAX(niters, 3), maxIters );
for( iter = 0; iter < niters; iter++ )
{
int i, nmodels;
if( count > modelPoints )
{
bool found = getSubset( m1, m2, ms1, ms2, 300 );
if( !found )
{
if( iter == 0 )
return false;
break;
}
}
nmodels = runKernel( ms1, ms2, models );
if( nmodels <= 0 )
continue;
for( i = 0; i < nmodels; i++ )
{
CvMat model_i;
cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );
computeReprojError( m1, m2, &model_i, err );
icvSortDistances( err->data.i, count, 0 );
double median = count % 2 != 0 ?
err->data.fl[count/2] : (err->data.fl[count/2-1] + err->data.fl[count/2])*0.5;
if( median < minMedian )
{
minMedian = median;
cvCopy( &model_i, model );
}
}
}
if( minMedian < DBL_MAX )
{
sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*sqrt(minMedian);
sigma = MAX( sigma, FLT_EPSILON*100 );
count = findInliers( m1, m2, model, err, mask, sigma );
result = count >= modelPoints;
}
return result;
}
bool CvModelEstimator2::getSubset( const CvMat* m1, const CvMat* m2,
CvMat* ms1, CvMat* ms2, int maxAttempts )
{
int* idx = (int*)cvStackAlloc( modelPoints*sizeof(idx[0]) );
int i = 0, j, k, idx_i, iters = 0;
int type = CV_MAT_TYPE(m1->type), elemSize = CV_ELEM_SIZE(type);
const int *m1ptr = m1->data.i, *m2ptr = m2->data.i;
int *ms1ptr = ms1->data.i, *ms2ptr = ms2->data.i;
int count = m1->cols*m1->rows;
assert( CV_IS_MAT_CONT(m1->type & m2->type) && (elemSize % sizeof(int) == 0) );
elemSize /= sizeof(int);
for(; iters < maxAttempts; iters++)
{
for( i = 0; i < modelPoints && iters < maxAttempts; )
{
idx[i] = idx_i = cvRandInt(&rng) % count;
for( j = 0; j < i; j++ )
if( idx_i == idx[j] )
break;
if( j < i )
continue;
for( k = 0; k < elemSize; k++ )
{
ms1ptr[i*elemSize + k] = m1ptr[idx_i*elemSize + k];
ms2ptr[i*elemSize + k] = m2ptr[idx_i*elemSize + k];
}
if( checkPartialSubsets && (!checkSubset( ms1, i+1 ) || !checkSubset( ms2, i+1 )))
{
iters++;
continue;
}
i++;
}
if( !checkPartialSubsets && i == modelPoints &&
(!checkSubset( ms1, i ) || !checkSubset( ms2, i )))
continue;
break;
}
return i == modelPoints && iters < maxAttempts;
}
bool CvModelEstimator2::checkSubset( const CvMat* m, int count )
{
int j, k, i, i0, i1;
CvPoint2D64f* ptr = (CvPoint2D64f*)m->data.ptr;
assert( CV_MAT_TYPE(m->type) == CV_64FC2 );
if( checkPartialSubsets )
i0 = i1 = count - 1;
else
i0 = 0, i1 = count - 1;
for( i = i0; i <= i1; i++ )
{
// check that the i-th selected point does not belong
// to a line connecting some previously selected points
for( j = 0; j < i; j++ )
{
double dx1 = ptr[j].x - ptr[i].x;
double dy1 = ptr[j].y - ptr[i].y;
for( k = 0; k < j; k++ )
{
double dx2 = ptr[k].x - ptr[i].x;
double dy2 = ptr[k].y - ptr[i].y;
if( fabs(dx2*dy1 - dy2*dx1) <= FLT_EPSILON*(fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2)))
break;
}
if( k < j )
break;
}
if( j < i )
break;
}
return i >= i1;
}
namespace cv
{
class Affine3DEstimator : public CvModelEstimator2
{
public:
Affine3DEstimator() : CvModelEstimator2(4, cvSize(4, 3), 1) {}
virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
protected:
virtual void computeReprojError( const CvMat* m1, const CvMat* m2, const CvMat* model, CvMat* error );
virtual bool checkSubset( const CvMat* ms1, int count );
};
}
int cv::Affine3DEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )
{
const Point3d* from = reinterpret_cast<const Point3d*>(m1->data.ptr);
const Point3d* to = reinterpret_cast<const Point3d*>(m2->data.ptr);
Mat A(12, 12, CV_64F);
Mat B(12, 1, CV_64F);
A = Scalar(0.0);
for(int i = 0; i < modelPoints; ++i)
{
*B.ptr<Point3d>(3*i) = to[i];
double *aptr = A.ptr<double>(3*i);
for(int k = 0; k < 3; ++k)
{
aptr[3] = 1.0;
*reinterpret_cast<Point3d*>(aptr) = from[i];
aptr += 16;
}
}
CvMat cvA = A;
CvMat cvB = B;
CvMat cvX;
cvReshape(model, &cvX, 1, 12);
cvSolve(&cvA, &cvB, &cvX, CV_SVD );
return 1;
}
void cv::Affine3DEstimator::computeReprojError( const CvMat* m1, const CvMat* m2, const CvMat* model, CvMat* error )
{
int count = m1->rows * m1->cols;
const Point3d* from = reinterpret_cast<const Point3d*>(m1->data.ptr);
const Point3d* to = reinterpret_cast<const Point3d*>(m2->data.ptr);
const double* F = model->data.db;
float* err = error->data.fl;
for(int i = 0; i < count; i++ )
{
const Point3d& f = from[i];
const Point3d& t = to[i];
double a = F[0]*f.x + F[1]*f.y + F[ 2]*f.z + F[ 3] - t.x;
double b = F[4]*f.x + F[5]*f.y + F[ 6]*f.z + F[ 7] - t.y;
double c = F[8]*f.x + F[9]*f.y + F[10]*f.z + F[11] - t.z;
err[i] = (float)sqrt(a*a + b*b + c*c);
}
}
bool cv::Affine3DEstimator::checkSubset( const CvMat* ms1, int count )
{
CV_Assert( CV_MAT_TYPE(ms1->type) == CV_64FC3 );
int j, k, i = count - 1;
const Point3d* ptr = reinterpret_cast<const Point3d*>(ms1->data.ptr);
// check that the i-th selected point does not belong
// to a line connecting some previously selected points
for(j = 0; j < i; ++j)
{
Point3d d1 = ptr[j] - ptr[i];
double n1 = norm(d1);
for(k = 0; k < j; ++k)
{
Point3d d2 = ptr[k] - ptr[i];
double n = norm(d2) * n1;
if (fabs(d1.dot(d2) / n) > 0.996)
break;
}
if( k < j )
break;
}
return j == i;
}
int cv::estimateAffine3D(const Mat& from, const Mat& to, Mat& out, vector<uchar>& outliers, double param1, double param2)
{
size_t count = from.cols*from.rows*from.channels()/3;
CV_Assert( count >= 4 && from.isContinuous() && to.isContinuous() &&
from.depth() == CV_32F && to.depth() == CV_32F &&
((from.rows == 1 && from.channels() == 3) || from.cols*from.channels() == 3) &&
((to.rows == 1 && to.channels() == 3) || to.cols*to.channels() == 3) &&
count == (size_t)to.cols*to.rows*to.channels()/3);
out.create(3, 4, CV_64F);
outliers.resize(count);
fill(outliers.begin(), outliers.end(), (uchar)1);
vector<Point3d> dFrom;
vector<Point3d> dTo;
copy(from.ptr<Point3f>(), from.ptr<Point3f>() + count, back_inserter(dFrom));
copy(to.ptr<Point3f>(), to.ptr<Point3f>() + count, back_inserter(dTo));
CvMat F3x4 = out;
CvMat mask = cvMat( 1, count, CV_8U, &outliers[0] );
CvMat m1 = cvMat( 1, count, CV_64FC3, &dFrom[0] );
CvMat m2 = cvMat( 1, count, CV_64FC3, &dTo[0] );
const double epsilon = numeric_limits<double>::epsilon();
param1 = param1 <= 0 ? 3 : param1;
param2 = (param2 < epsilon) ? 0.99 : (param2 > 1 - epsilon) ? 0.99 : param2;
return Affine3DEstimator().runRANSAC(&m1,& m2, &F3x4, &mask, param1, param2 );
}

358
modules/calib3d/src/posit.cpp Normal file
View File

@@ -0,0 +1,358 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of Intel Corporation may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
/* POSIT structure */
struct CvPOSITObject
{
int N;
float* inv_matr;
float* obj_vecs;
float* img_vecs;
};
static void icvPseudoInverse3D( float *a, float *b, int n, int method );
static CvStatus icvCreatePOSITObject( CvPoint3D32f *points,
int numPoints,
CvPOSITObject **ppObject )
{
int i;
/* Compute size of required memory */
/* buffer for inverse matrix = N*3*float */
/* buffer for storing weakImagePoints = numPoints * 2 * float */
/* buffer for storing object vectors = N*3*float */
/* buffer for storing image vectors = N*2*float */
int N = numPoints - 1;
int inv_matr_size = N * 3 * sizeof( float );
int obj_vec_size = inv_matr_size;
int img_vec_size = N * 2 * sizeof( float );
CvPOSITObject *pObject;
/* check bad arguments */
if( points == NULL )
return CV_NULLPTR_ERR;
if( numPoints < 4 )
return CV_BADSIZE_ERR;
if( ppObject == NULL )
return CV_NULLPTR_ERR;
/* memory allocation */
pObject = (CvPOSITObject *) cvAlloc( sizeof( CvPOSITObject ) +
inv_matr_size + obj_vec_size + img_vec_size );
if( !pObject )
return CV_OUTOFMEM_ERR;
/* part the memory between all structures */
pObject->N = N;
pObject->inv_matr = (float *) ((char *) pObject + sizeof( CvPOSITObject ));
pObject->obj_vecs = (float *) ((char *) (pObject->inv_matr) + inv_matr_size);
pObject->img_vecs = (float *) ((char *) (pObject->obj_vecs) + obj_vec_size);
/****************************************************************************************\
* Construct object vectors from object points *
\****************************************************************************************/
for( i = 0; i < numPoints - 1; i++ )
{
pObject->obj_vecs[i] = points[i + 1].x - points[0].x;
pObject->obj_vecs[N + i] = points[i + 1].y - points[0].y;
pObject->obj_vecs[2 * N + i] = points[i + 1].z - points[0].z;
}
/****************************************************************************************\
* Compute pseudoinverse matrix *
\****************************************************************************************/
icvPseudoInverse3D( pObject->obj_vecs, pObject->inv_matr, N, 0 );
*ppObject = pObject;
return CV_NO_ERR;
}
static CvStatus icvPOSIT( CvPOSITObject *pObject, CvPoint2D32f *imagePoints,
float focalLength, CvTermCriteria criteria,
float* rotation, float* translation )
{
int i, j, k;
int count = 0, converged = 0;
float inorm, jnorm, invInorm, invJnorm, invScale, scale = 0, inv_Z = 0;
float diff = (float)criteria.epsilon;
float inv_focalLength = 1 / focalLength;
/* init variables */
int N = pObject->N;
float *objectVectors = pObject->obj_vecs;
float *invMatrix = pObject->inv_matr;
float *imgVectors = pObject->img_vecs;
/* Check bad arguments */
if( imagePoints == NULL )
return CV_NULLPTR_ERR;
if( pObject == NULL )
return CV_NULLPTR_ERR;
if( focalLength <= 0 )
return CV_BADFACTOR_ERR;
if( !rotation )
return CV_NULLPTR_ERR;
if( !translation )
return CV_NULLPTR_ERR;
if( (criteria.type == 0) || (criteria.type > (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS)))
return CV_BADFLAG_ERR;
if( (criteria.type & CV_TERMCRIT_EPS) && criteria.epsilon < 0 )
return CV_BADFACTOR_ERR;
if( (criteria.type & CV_TERMCRIT_ITER) && criteria.max_iter <= 0 )
return CV_BADFACTOR_ERR;
while( !converged )
{
if( count == 0 )
{
/* subtract out origin to get image vectors */
for( i = 0; i < N; i++ )
{
imgVectors[i] = imagePoints[i + 1].x - imagePoints[0].x;
imgVectors[N + i] = imagePoints[i + 1].y - imagePoints[0].y;
}
}
else
{
diff = 0;
/* Compute new SOP (scaled orthograthic projection) image from pose */
for( i = 0; i < N; i++ )
{
/* objectVector * k */
float old;
float tmp = objectVectors[i] * rotation[6] /*[2][0]*/ +
objectVectors[N + i] * rotation[7] /*[2][1]*/ +
objectVectors[2 * N + i] * rotation[8] /*[2][2]*/;
tmp *= inv_Z;
tmp += 1;
old = imgVectors[i];
imgVectors[i] = imagePoints[i + 1].x * tmp - imagePoints[0].x;
diff = MAX( diff, (float) fabs( imgVectors[i] - old ));
old = imgVectors[N + i];
imgVectors[N + i] = imagePoints[i + 1].y * tmp - imagePoints[0].y;
diff = MAX( diff, (float) fabs( imgVectors[N + i] - old ));
}
}
/* calculate I and J vectors */
for( i = 0; i < 2; i++ )
{
for( j = 0; j < 3; j++ )
{
rotation[3*i+j] /*[i][j]*/ = 0;
for( k = 0; k < N; k++ )
{
rotation[3*i+j] /*[i][j]*/ += invMatrix[j * N + k] * imgVectors[i * N + k];
}
}
}
inorm = rotation[0] /*[0][0]*/ * rotation[0] /*[0][0]*/ +
rotation[1] /*[0][1]*/ * rotation[1] /*[0][1]*/ +
rotation[2] /*[0][2]*/ * rotation[2] /*[0][2]*/;
jnorm = rotation[3] /*[1][0]*/ * rotation[3] /*[1][0]*/ +
rotation[4] /*[1][1]*/ * rotation[4] /*[1][1]*/ +
rotation[5] /*[1][2]*/ * rotation[5] /*[1][2]*/;
invInorm = cvInvSqrt( inorm );
invJnorm = cvInvSqrt( jnorm );
inorm *= invInorm;
jnorm *= invJnorm;
rotation[0] /*[0][0]*/ *= invInorm;
rotation[1] /*[0][1]*/ *= invInorm;
rotation[2] /*[0][2]*/ *= invInorm;
rotation[3] /*[1][0]*/ *= invJnorm;
rotation[4] /*[1][1]*/ *= invJnorm;
rotation[5] /*[1][2]*/ *= invJnorm;
/* row2 = row0 x row1 (cross product) */
rotation[6] /*->m[2][0]*/ = rotation[1] /*->m[0][1]*/ * rotation[5] /*->m[1][2]*/ -
rotation[2] /*->m[0][2]*/ * rotation[4] /*->m[1][1]*/;
rotation[7] /*->m[2][1]*/ = rotation[2] /*->m[0][2]*/ * rotation[3] /*->m[1][0]*/ -
rotation[0] /*->m[0][0]*/ * rotation[5] /*->m[1][2]*/;
rotation[8] /*->m[2][2]*/ = rotation[0] /*->m[0][0]*/ * rotation[4] /*->m[1][1]*/ -
rotation[1] /*->m[0][1]*/ * rotation[3] /*->m[1][0]*/;
scale = (inorm + jnorm) / 2.0f;
inv_Z = scale * inv_focalLength;
count++;
converged = ((criteria.type & CV_TERMCRIT_EPS) && (diff < criteria.epsilon));
converged |= ((criteria.type & CV_TERMCRIT_ITER) && (count == criteria.max_iter));
}
invScale = 1 / scale;
translation[0] = imagePoints[0].x * invScale;
translation[1] = imagePoints[0].y * invScale;
translation[2] = 1 / inv_Z;
return CV_NO_ERR;
}
static CvStatus icvReleasePOSITObject( CvPOSITObject ** ppObject )
{
cvFree( ppObject );
return CV_NO_ERR;
}
/*F///////////////////////////////////////////////////////////////////////////////////////
// Name: icvPseudoInverse3D
// Purpose: Pseudoinverse N x 3 matrix N >= 3
// Context:
// Parameters:
// a - input matrix
// b - pseudoinversed a
// n - number of rows in a
// method - if 0, then b = inv(transpose(a)*a) * transpose(a)
// if 1, then SVD used.
// Returns:
// Notes: Both matrix are stored by n-dimensional vectors.
// Now only method == 0 supported.
//F*/
void
icvPseudoInverse3D( float *a, float *b, int n, int method )
{
int k;
if( method == 0 )
{
float ata00 = 0;
float ata11 = 0;
float ata22 = 0;
float ata01 = 0;
float ata02 = 0;
float ata12 = 0;
float det = 0;
/* compute matrix ata = transpose(a) * a */
for( k = 0; k < n; k++ )
{
float a0 = a[k];
float a1 = a[n + k];
float a2 = a[2 * n + k];
ata00 += a0 * a0;
ata11 += a1 * a1;
ata22 += a2 * a2;
ata01 += a0 * a1;
ata02 += a0 * a2;
ata12 += a1 * a2;
}
/* inverse matrix ata */
{
float inv_det;
float p00 = ata11 * ata22 - ata12 * ata12;
float p01 = -(ata01 * ata22 - ata12 * ata02);
float p02 = ata12 * ata01 - ata11 * ata02;
float p11 = ata00 * ata22 - ata02 * ata02;
float p12 = -(ata00 * ata12 - ata01 * ata02);
float p22 = ata00 * ata11 - ata01 * ata01;
det += ata00 * p00;
det += ata01 * p01;
det += ata02 * p02;
inv_det = 1 / det;
/* compute resultant matrix */
for( k = 0; k < n; k++ )
{
float a0 = a[k];
float a1 = a[n + k];
float a2 = a[2 * n + k];
b[k] = (p00 * a0 + p01 * a1 + p02 * a2) * inv_det;
b[n + k] = (p01 * a0 + p11 * a1 + p12 * a2) * inv_det;
b[2 * n + k] = (p02 * a0 + p12 * a1 + p22 * a2) * inv_det;
}
}
}
/*if ( method == 1 )
{
}
*/
return;
}
CV_IMPL CvPOSITObject *
cvCreatePOSITObject( CvPoint3D32f * points, int numPoints )
{
CvPOSITObject *pObject = 0;
IPPI_CALL( icvCreatePOSITObject( points, numPoints, &pObject ));
return pObject;
}
CV_IMPL void
cvPOSIT( CvPOSITObject * pObject, CvPoint2D32f * imagePoints,
double focalLength, CvTermCriteria criteria,
float* rotation, float* translation )
{
IPPI_CALL( icvPOSIT( pObject, imagePoints,(float) focalLength, criteria,
rotation, translation ));
}
CV_IMPL void
cvReleasePOSITObject( CvPOSITObject ** ppObject )
{
IPPI_CALL( icvReleasePOSITObject( ppObject ));
}
/* End of file. */

44
modules/calib3d/src/precomp.cpp Normal file
View File

@@ -0,0 +1,44 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of Intel Corporation may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
/* End of file. */

54
modules/calib3d/src/precomp.hpp Normal file
View File

@@ -0,0 +1,54 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#ifndef __OPENCV_PRECOMP_H__
#define __OPENCV_PRECOMP_H__
#if _MSC_VER >= 1200
#pragma warning( disable: 4251 4710 4711 4514 4996 ) /* function AAA selected for automatic inline expansion */
#endif
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/imgproc/imgproc_c.h"
#include "opencv2/core/internal.hpp"
#endif

945
modules/calib3d/src/stereobm.cpp Normal file
View File

@@ -0,0 +1,945 @@
//M*//////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of Intel Corporation may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
/****************************************************************************************\
* Very fast SAD-based (Sum-of-Absolute-Diffrences) stereo correspondence algorithm. *
* Contributed by Kurt Konolige *
\****************************************************************************************/
#include "precomp.hpp"
//#undef CV_SSE2
//#define CV_SSE2 0
//#include "emmintrin.h"
#include <limits>
CV_IMPL CvStereoBMState* cvCreateStereoBMState( int /*preset*/, int numberOfDisparities )
{
CvStereoBMState* state = (CvStereoBMState*)cvAlloc( sizeof(*state) );
if( !state )
return 0;
state->preFilterType = CV_STEREO_BM_XSOBEL; //CV_STEREO_BM_NORMALIZED_RESPONSE;
state->preFilterSize = 9;
state->preFilterCap = 31;
state->SADWindowSize = 15;
state->minDisparity = 0;
state->numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : 64;
state->textureThreshold = 10;
state->uniquenessRatio = 15;
state->speckleRange = state->speckleWindowSize = 0;
state->trySmallerWindows = 0;
state->roi1 = state->roi2 = cvRect(0,0,0,0);
state->disp12MaxDiff = -1;
state->preFilteredImg0 = state->preFilteredImg1 = state->slidingSumBuf =
state->disp = state->cost = 0;
return state;
}
CV_IMPL void cvReleaseStereoBMState( CvStereoBMState** state )
{
if( !state )
CV_Error( CV_StsNullPtr, "" );
if( !*state )
return;
cvReleaseMat( &(*state)->preFilteredImg0 );
cvReleaseMat( &(*state)->preFilteredImg1 );
cvReleaseMat( &(*state)->slidingSumBuf );
cvReleaseMat( &(*state)->disp );
cvReleaseMat( &(*state)->cost );
cvFree( state );
}
namespace cv
{
static void prefilterNorm( const Mat& src, Mat& dst, int winsize, int ftzero, uchar* buf )
{
int x, y, wsz2 = winsize/2;
int* vsum = (int*)alignPtr(buf + (wsz2 + 1)*sizeof(vsum[0]), 32);
int scale_g = winsize*winsize/8, scale_s = (1024 + scale_g)/(scale_g*2);
const int OFS = 256*5, TABSZ = OFS*2 + 256;
uchar tab[TABSZ];
const uchar* sptr = src.data;
int srcstep = src.step;
Size size = src.size();
scale_g *= scale_s;
for( x = 0; x < TABSZ; x++ )
tab[x] = (uchar)(x - OFS < -ftzero ? 0 : x - OFS > ftzero ? ftzero*2 : x - OFS + ftzero);
for( x = 0; x < size.width; x++ )
vsum[x] = (ushort)(sptr[x]*(wsz2 + 2));
for( y = 1; y < wsz2; y++ )
{
for( x = 0; x < size.width; x++ )
vsum[x] = (ushort)(vsum[x] + sptr[srcstep*y + x]);
}
for( y = 0; y < size.height; y++ )
{
const uchar* top = sptr + srcstep*MAX(y-wsz2-1,0);
const uchar* bottom = sptr + srcstep*MIN(y+wsz2,size.height-1);
const uchar* prev = sptr + srcstep*MAX(y-1,0);
const uchar* curr = sptr + srcstep*y;
const uchar* next = sptr + srcstep*MIN(y+1,size.height-1);
uchar* dptr = dst.ptr<uchar>(y);
x = 0;
for( ; x < size.width; x++ )
vsum[x] = (ushort)(vsum[x] + bottom[x] - top[x]);
for( x = 0; x <= wsz2; x++ )
{
vsum[-x-1] = vsum[0];
vsum[size.width+x] = vsum[size.width-1];
}
int sum = vsum[0]*(wsz2 + 1);
for( x = 1; x <= wsz2; x++ )
sum += vsum[x];
int val = ((curr[0]*5 + curr[1] + prev[0] + next[0])*scale_g - sum*scale_s) >> 10;
dptr[0] = tab[val + OFS];
for( x = 1; x < size.width-1; x++ )
{
sum += vsum[x+wsz2] - vsum[x-wsz2-1];
val = ((curr[x]*4 + curr[x-1] + curr[x+1] + prev[x] + next[x])*scale_g - sum*scale_s) >> 10;
dptr[x] = tab[val + OFS];
}
sum += vsum[x+wsz2] - vsum[x-wsz2-1];
val = ((curr[x]*5 + curr[x-1] + prev[x] + next[x])*scale_g - sum*scale_s) >> 10;
dptr[x] = tab[val + OFS];
}
}
static void
prefilterXSobel( const Mat& src, Mat& dst, int ftzero )
{
int x, y;
const int OFS = 256*4, TABSZ = OFS*2 + 256;
uchar tab[TABSZ];
Size size = src.size();
for( x = 0; x < TABSZ; x++ )
tab[x] = (uchar)(x - OFS < -ftzero ? 0 : x - OFS > ftzero ? ftzero*2 : x - OFS + ftzero);
uchar val0 = tab[0 + OFS];
#if CV_SSE2
volatile bool useSIMD = checkHardwareSupport(CV_CPU_SSE2);
#endif
for( y = 0; y < size.height-1; y += 2 )
{
const uchar* srow1 = src.ptr<uchar>(y);
const uchar* srow0 = y > 0 ? srow1 - src.step : size.height > 1 ? srow1 + src.step : srow1;
const uchar* srow2 = y < size.height-1 ? srow1 + src.step : size.height > 1 ? srow1 - src.step : srow1;
const uchar* srow3 = y < size.height-2 ? srow1 + src.step*2 : srow1;
uchar* dptr0 = dst.ptr<uchar>(y);
uchar* dptr1 = dptr0 + dst.step;
dptr0[0] = dptr0[size.width-1] = dptr1[0] = dptr1[size.width-1] = val0;
x = 1;
#if CV_SSE2
if( useSIMD )
{
__m128i z = _mm_setzero_si128(), ftz = _mm_set1_epi16((short)ftzero),
ftz2 = _mm_set1_epi8(CV_CAST_8U(ftzero*2));
for( ; x <= size.width-9; x += 8 )
{
__m128i c0 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow0 + x - 1)), z);
__m128i c1 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow1 + x - 1)), z);
__m128i d0 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow0 + x + 1)), z);
__m128i d1 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow1 + x + 1)), z);
d0 = _mm_sub_epi16(d0, c0);
d1 = _mm_sub_epi16(d1, c1);
__m128i c2 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow2 + x - 1)), z);
__m128i c3 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow2 + x - 1)), z);
__m128i d2 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow2 + x + 1)), z);
__m128i d3 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow2 + x + 1)), z);
d2 = _mm_sub_epi16(d2, c2);
d3 = _mm_sub_epi16(d3, c3);
__m128i v0 = _mm_add_epi16(d0, _mm_add_epi16(d2, _mm_add_epi16(d1, d1)));
__m128i v1 = _mm_add_epi16(d1, _mm_add_epi16(d3, _mm_add_epi16(d2, d2)));
v0 = _mm_packus_epi16(_mm_add_epi16(v0, ftz), _mm_add_epi16(v1, ftz));
v0 = _mm_min_epu8(v0, ftz2);
_mm_storel_epi64((__m128i*)(dptr0 + x), v0);
_mm_storel_epi64((__m128i*)(dptr1 + x), _mm_unpackhi_epi64(v0, v0));
}
}
#endif
for( ; x < size.width-1; x++ )
{
int d0 = srow0[x+1] - srow0[x-1], d1 = srow1[x+1] - srow1[x-1],
d2 = srow2[x+1] - srow2[x-1], d3 = srow3[x+1] - srow3[x-1];
int v0 = tab[d0 + d1*2 + d2 + OFS];
int v1 = tab[d1 + d2*2 + d3 + OFS];
dptr0[x] = (uchar)v0;
dptr1[x] = (uchar)v1;
}
}
for( ; y < size.height; y++ )
{
uchar* dptr = dst.ptr<uchar>(y);
for( x = 0; x < size.width; x++ )
dptr[x] = val0;
}
}
static const int DISPARITY_SHIFT = 4;
#if CV_SSE2
static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
Mat& disp, Mat& cost, CvStereoBMState& state,
uchar* buf, int _dy0, int _dy1 )
{
const int ALIGN = 16;
int x, y, d;
int wsz = state.SADWindowSize, wsz2 = wsz/2;
int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1);
int ndisp = state.numberOfDisparities;
int mindisp = state.minDisparity;
int lofs = MAX(ndisp - 1 + mindisp, 0);
int rofs = -MIN(ndisp - 1 + mindisp, 0);
int width = left.cols, height = left.rows;
int width1 = width - rofs - ndisp + 1;
int ftzero = state.preFilterCap;
int textureThreshold = state.textureThreshold;
int uniquenessRatio = state.uniquenessRatio*256/100;
short FILTERED = (short)((mindisp - 1) << DISPARITY_SHIFT);
ushort *sad, *hsad0, *hsad, *hsad_sub;
int *htext;
uchar *cbuf0, *cbuf;
const uchar* lptr0 = left.data + lofs;
const uchar* rptr0 = right.data + rofs;
const uchar *lptr, *lptr_sub, *rptr;
short* dptr = (short*)disp.data;
int sstep = left.step;
int dstep = disp.step/sizeof(dptr[0]);
int cstep = (height + dy0 + dy1)*ndisp;
short costbuf = 0;
int coststep = cost.data ? cost.step/sizeof(costbuf) : 0;
const int TABSZ = 256;
uchar tab[TABSZ];
const __m128i d0_8 = _mm_setr_epi16(0,1,2,3,4,5,6,7), dd_8 = _mm_set1_epi16(8);
sad = (ushort*)alignPtr(buf + sizeof(sad[0]), ALIGN);
hsad0 = (ushort*)alignPtr(sad + ndisp + 1 + dy0*ndisp, ALIGN);
htext = (int*)alignPtr((int*)(hsad0 + (height+dy1)*ndisp) + wsz2 + 2, ALIGN);
cbuf0 = (uchar*)alignPtr(htext + height + wsz2 + 2 + dy0*ndisp, ALIGN);
for( x = 0; x < TABSZ; x++ )
tab[x] = (uchar)std::abs(x - ftzero);
// initialize buffers
memset( hsad0 - dy0*ndisp, 0, (height + dy0 + dy1)*ndisp*sizeof(hsad0[0]) );
memset( htext - wsz2 - 1, 0, (height + wsz + 1)*sizeof(htext[0]) );
for( x = -wsz2-1; x < wsz2; x++ )
{
hsad = hsad0 - dy0*ndisp; cbuf = cbuf0 + (x + wsz2 + 1)*cstep - dy0*ndisp;
lptr = lptr0 + MIN(MAX(x, -lofs), width-lofs-1) - dy0*sstep;
rptr = rptr0 + MIN(MAX(x, -rofs), width-rofs-1) - dy0*sstep;
for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep )
{
int lval = lptr[0];
__m128i lv = _mm_set1_epi8((char)lval), z = _mm_setzero_si128();
for( d = 0; d < ndisp; d += 16 )
{
__m128i rv = _mm_loadu_si128((const __m128i*)(rptr + d));
__m128i hsad_l = _mm_load_si128((__m128i*)(hsad + d));
__m128i hsad_h = _mm_load_si128((__m128i*)(hsad + d + 8));
__m128i diff = _mm_adds_epu8(_mm_subs_epu8(lv, rv), _mm_subs_epu8(rv, lv));
_mm_store_si128((__m128i*)(cbuf + d), diff);
hsad_l = _mm_add_epi16(hsad_l, _mm_unpacklo_epi8(diff,z));
hsad_h = _mm_add_epi16(hsad_h, _mm_unpackhi_epi8(diff,z));
_mm_store_si128((__m128i*)(hsad + d), hsad_l);
_mm_store_si128((__m128i*)(hsad + d + 8), hsad_h);
}
htext[y] += tab[lval];
}
}
// initialize the left and right borders of the disparity map
for( y = 0; y < height; y++ )
{
for( x = 0; x < lofs; x++ )
dptr[y*dstep + x] = FILTERED;
for( x = lofs + width1; x < width; x++ )
dptr[y*dstep + x] = FILTERED;
}
dptr += lofs;
for( x = 0; x < width1; x++, dptr++ )
{
short* costptr = cost.data ? (short*)cost.data + lofs + x : &costbuf;
int x0 = x - wsz2 - 1, x1 = x + wsz2;
const uchar* cbuf_sub = cbuf0 + ((x0 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
uchar* cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
hsad = hsad0 - dy0*ndisp;
lptr_sub = lptr0 + MIN(MAX(x0, -lofs), width-1-lofs) - dy0*sstep;
lptr = lptr0 + MIN(MAX(x1, -lofs), width-1-lofs) - dy0*sstep;
rptr = rptr0 + MIN(MAX(x1, -rofs), width-1-rofs) - dy0*sstep;
for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp,
hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep )
{
int lval = lptr[0];
__m128i lv = _mm_set1_epi8((char)lval), z = _mm_setzero_si128();
for( d = 0; d < ndisp; d += 16 )
{
__m128i rv = _mm_loadu_si128((const __m128i*)(rptr + d));
__m128i hsad_l = _mm_load_si128((__m128i*)(hsad + d));
__m128i hsad_h = _mm_load_si128((__m128i*)(hsad + d + 8));
__m128i cbs = _mm_load_si128((const __m128i*)(cbuf_sub + d));
__m128i diff = _mm_adds_epu8(_mm_subs_epu8(lv, rv), _mm_subs_epu8(rv, lv));
__m128i diff_h = _mm_sub_epi16(_mm_unpackhi_epi8(diff, z), _mm_unpackhi_epi8(cbs, z));
_mm_store_si128((__m128i*)(cbuf + d), diff);
diff = _mm_sub_epi16(_mm_unpacklo_epi8(diff, z), _mm_unpacklo_epi8(cbs, z));
hsad_h = _mm_add_epi16(hsad_h, diff_h);
hsad_l = _mm_add_epi16(hsad_l, diff);
_mm_store_si128((__m128i*)(hsad + d), hsad_l);
_mm_store_si128((__m128i*)(hsad + d + 8), hsad_h);
}
htext[y] += tab[lval] - tab[lptr_sub[0]];
}
// fill borders
for( y = dy1; y <= wsz2; y++ )
htext[height+y] = htext[height+dy1-1];
for( y = -wsz2-1; y < -dy0; y++ )
htext[y] = htext[-dy0];
// initialize sums
for( d = 0; d < ndisp; d++ )
sad[d] = (ushort)(hsad0[d-ndisp*dy0]*(wsz2 + 2 - dy0));
hsad = hsad0 + (1 - dy0)*ndisp;
for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp )
for( d = 0; d < ndisp; d += 16 )
{
__m128i s0 = _mm_load_si128((__m128i*)(sad + d));
__m128i s1 = _mm_load_si128((__m128i*)(sad + d + 8));
__m128i t0 = _mm_load_si128((__m128i*)(hsad + d));
__m128i t1 = _mm_load_si128((__m128i*)(hsad + d + 8));
s0 = _mm_add_epi16(s0, t0);
s1 = _mm_add_epi16(s1, t1);
_mm_store_si128((__m128i*)(sad + d), s0);
_mm_store_si128((__m128i*)(sad + d + 8), s1);
}
int tsum = 0;
for( y = -wsz2-1; y < wsz2; y++ )
tsum += htext[y];
// finally, start the real processing
for( y = 0; y < height; y++ )
{
int minsad = INT_MAX, mind = -1;
hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp;
hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp;
__m128i minsad8 = _mm_set1_epi16(SHRT_MAX);
__m128i mind8 = _mm_set1_epi16(0), d8 = d0_8, mask;
for( d = 0; d < ndisp; d += 16 )
{
__m128i u0 = _mm_load_si128((__m128i*)(hsad_sub + d));
__m128i u1 = _mm_load_si128((__m128i*)(hsad + d));
__m128i v0 = _mm_load_si128((__m128i*)(hsad_sub + d + 8));
__m128i v1 = _mm_load_si128((__m128i*)(hsad + d + 8));
__m128i usad8 = _mm_load_si128((__m128i*)(sad + d));
__m128i vsad8 = _mm_load_si128((__m128i*)(sad + d + 8));
u1 = _mm_sub_epi16(u1, u0);
v1 = _mm_sub_epi16(v1, v0);
usad8 = _mm_add_epi16(usad8, u1);
vsad8 = _mm_add_epi16(vsad8, v1);
mask = _mm_cmpgt_epi16(minsad8, usad8);
minsad8 = _mm_min_epi16(minsad8, usad8);
mind8 = _mm_max_epi16(mind8, _mm_and_si128(mask, d8));
_mm_store_si128((__m128i*)(sad + d), usad8);
_mm_store_si128((__m128i*)(sad + d + 8), vsad8);
mask = _mm_cmpgt_epi16(minsad8, vsad8);
minsad8 = _mm_min_epi16(minsad8, vsad8);
d8 = _mm_add_epi16(d8, dd_8);
mind8 = _mm_max_epi16(mind8, _mm_and_si128(mask, d8));
d8 = _mm_add_epi16(d8, dd_8);
}
tsum += htext[y + wsz2] - htext[y - wsz2 - 1];
if( tsum < textureThreshold )
{
dptr[y*dstep] = FILTERED;
continue;
}
__m128i minsad82 = _mm_unpackhi_epi64(minsad8, minsad8);
__m128i mind82 = _mm_unpackhi_epi64(mind8, mind8);
mask = _mm_cmpgt_epi16(minsad8, minsad82);
mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask));
minsad8 = _mm_min_epi16(minsad8, minsad82);
minsad82 = _mm_shufflelo_epi16(minsad8, _MM_SHUFFLE(3,2,3,2));
mind82 = _mm_shufflelo_epi16(mind8, _MM_SHUFFLE(3,2,3,2));
mask = _mm_cmpgt_epi16(minsad8, minsad82);
mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask));
minsad8 = _mm_min_epi16(minsad8, minsad82);
minsad82 = _mm_shufflelo_epi16(minsad8, 1);
mind82 = _mm_shufflelo_epi16(mind8, 1);
mask = _mm_cmpgt_epi16(minsad8, minsad82);
mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask));
mind = (short)_mm_cvtsi128_si32(mind8);
minsad = sad[mind];
if( uniquenessRatio > 0 )
{
int thresh = minsad + ((minsad * uniquenessRatio) >> 8);
__m128i thresh8 = _mm_set1_epi16((short)(thresh + 1));
__m128i d1 = _mm_set1_epi16((short)(mind-1)), d2 = _mm_set1_epi16((short)(mind+1));
__m128i dd_16 = _mm_add_epi16(dd_8, dd_8), d8 = _mm_sub_epi16(d0_8, dd_16);
for( d = 0; d < ndisp; d += 16 )
{
__m128i usad8 = _mm_load_si128((__m128i*)(sad + d));
__m128i vsad8 = _mm_load_si128((__m128i*)(sad + d + 8));
mask = _mm_cmpgt_epi16( thresh8, _mm_min_epi16(usad8,vsad8));
d8 = _mm_add_epi16(d8, dd_16);
if( !_mm_movemask_epi8(mask) )
continue;
mask = _mm_cmpgt_epi16( thresh8, usad8);
mask = _mm_and_si128(mask, _mm_or_si128(_mm_cmpgt_epi16(d1,d8), _mm_cmpgt_epi16(d8,d2)));
if( _mm_movemask_epi8(mask) )
break;
__m128i t8 = _mm_add_epi16(d8, dd_8);
mask = _mm_cmpgt_epi16( thresh8, vsad8);
mask = _mm_and_si128(mask, _mm_or_si128(_mm_cmpgt_epi16(d1,t8), _mm_cmpgt_epi16(t8,d2)));
if( _mm_movemask_epi8(mask) )
break;
}
if( d < ndisp )
{
dptr[y*dstep] = FILTERED;
continue;
}
}
if( 0 < mind && mind < ndisp - 1 )
{
int p = sad[mind+1], n = sad[mind-1], d = p + n - 2*sad[mind];
dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4);
}
else
dptr[y*dstep] = (short)((ndisp - mind - 1)*16);
costptr[y*coststep] = sad[mind];
}
}
}
#endif
static void
findStereoCorrespondenceBM( const Mat& left, const Mat& right,
Mat& disp, Mat& cost, const CvStereoBMState& state,
uchar* buf, int _dy0, int _dy1 )
{
const int ALIGN = 16;
int x, y, d;
int wsz = state.SADWindowSize, wsz2 = wsz/2;
int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1);
int ndisp = state.numberOfDisparities;
int mindisp = state.minDisparity;
int lofs = MAX(ndisp - 1 + mindisp, 0);
int rofs = -MIN(ndisp - 1 + mindisp, 0);
int width = left.cols, height = left.rows;
int width1 = width - rofs - ndisp + 1;
int ftzero = state.preFilterCap;
int textureThreshold = state.textureThreshold;
int uniquenessRatio = state.uniquenessRatio;
short FILTERED = (short)((mindisp - 1) << DISPARITY_SHIFT);
int *sad, *hsad0, *hsad, *hsad_sub, *htext;
uchar *cbuf0, *cbuf;
const uchar* lptr0 = left.data + lofs;
const uchar* rptr0 = right.data + rofs;
const uchar *lptr, *lptr_sub, *rptr;
short* dptr = (short*)disp.data;
int sstep = left.step;
int dstep = disp.step/sizeof(dptr[0]);
int cstep = (height+dy0+dy1)*ndisp;
int costbuf = 0;
int coststep = cost.data ? cost.step/sizeof(costbuf) : 0;
const int TABSZ = 256;
uchar tab[TABSZ];
sad = (int*)alignPtr(buf + sizeof(sad[0]), ALIGN);
hsad0 = (int*)alignPtr(sad + ndisp + 1 + dy0*ndisp, ALIGN);
htext = (int*)alignPtr((int*)(hsad0 + (height+dy1)*ndisp) + wsz2 + 2, ALIGN);
cbuf0 = (uchar*)alignPtr(htext + height + wsz2 + 2 + dy0*ndisp, ALIGN);
for( x = 0; x < TABSZ; x++ )
tab[x] = (uchar)std::abs(x - ftzero);
// initialize buffers
memset( hsad0 - dy0*ndisp, 0, (height + dy0 + dy1)*ndisp*sizeof(hsad0[0]) );
memset( htext - wsz2 - 1, 0, (height + wsz + 1)*sizeof(htext[0]) );
for( x = -wsz2-1; x < wsz2; x++ )
{
hsad = hsad0 - dy0*ndisp; cbuf = cbuf0 + (x + wsz2 + 1)*cstep - dy0*ndisp;
lptr = lptr0 + MIN(MAX(x, -lofs), width-lofs-1) - dy0*sstep;
rptr = rptr0 + MIN(MAX(x, -rofs), width-rofs-1) - dy0*sstep;
for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep )
{
int lval = lptr[0];
for( d = 0; d < ndisp; d++ )
{
int diff = std::abs(lval - rptr[d]);
cbuf[d] = (uchar)diff;
hsad[d] = (int)(hsad[d] + diff);
}
htext[y] += tab[lval];
}
}
// initialize the left and right borders of the disparity map
for( y = 0; y < height; y++ )
{
for( x = 0; x < lofs; x++ )
dptr[y*dstep + x] = FILTERED;
for( x = lofs + width1; x < width; x++ )
dptr[y*dstep + x] = FILTERED;
}
dptr += lofs;
for( x = 0; x < width1; x++, dptr++ )
{
int* costptr = cost.data ? (int*)cost.data + lofs + x : &costbuf;
int x0 = x - wsz2 - 1, x1 = x + wsz2;
const uchar* cbuf_sub = cbuf0 + ((x0 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
uchar* cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
hsad = hsad0 - dy0*ndisp;
lptr_sub = lptr0 + MIN(MAX(x0, -lofs), width-1-lofs) - dy0*sstep;
lptr = lptr0 + MIN(MAX(x1, -lofs), width-1-lofs) - dy0*sstep;
rptr = rptr0 + MIN(MAX(x1, -rofs), width-1-rofs) - dy0*sstep;
for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp,
hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep )
{
int lval = lptr[0];
for( d = 0; d < ndisp; d++ )
{
int diff = std::abs(lval - rptr[d]);
cbuf[d] = (uchar)diff;
hsad[d] = hsad[d] + diff - cbuf_sub[d];
}
htext[y] += tab[lval] - tab[lptr_sub[0]];
}
// fill borders
for( y = dy1; y <= wsz2; y++ )
htext[height+y] = htext[height+dy1-1];
for( y = -wsz2-1; y < -dy0; y++ )
htext[y] = htext[-dy0];
// initialize sums
for( d = 0; d < ndisp; d++ )
sad[d] = (int)(hsad0[d-ndisp*dy0]*(wsz2 + 2 - dy0));
hsad = hsad0 + (1 - dy0)*ndisp;
for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp )
for( d = 0; d < ndisp; d++ )
sad[d] = (int)(sad[d] + hsad[d]);
int tsum = 0;
for( y = -wsz2-1; y < wsz2; y++ )
tsum += htext[y];
// finally, start the real processing
for( y = 0; y < height; y++ )
{
int minsad = INT_MAX, mind = -1;
hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp;
hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp;
for( d = 0; d < ndisp; d++ )
{
int currsad = sad[d] + hsad[d] - hsad_sub[d];
sad[d] = currsad;
if( currsad < minsad )
{
minsad = currsad;
mind = d;
}
}
tsum += htext[y + wsz2] - htext[y - wsz2 - 1];
if( tsum < textureThreshold )
{
dptr[y*dstep] = FILTERED;
continue;
}
if( uniquenessRatio > 0 )
{
int thresh = minsad + (minsad * uniquenessRatio/100);
for( d = 0; d < ndisp; d++ )
{
if( sad[d] <= thresh && (d < mind-1 || d > mind+1))
break;
}
if( d < ndisp )
{
dptr[y*dstep] = FILTERED;
continue;
}
}
{
sad[-1] = sad[1];
sad[ndisp] = sad[ndisp-2];
int p = sad[mind+1], n = sad[mind-1], d = p + n - 2*sad[mind];
dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4);
costptr[y*coststep] = sad[mind];
}
}
}
}
struct PrefilterInvoker
{
PrefilterInvoker(const Mat& left0, const Mat& right0, Mat& left, Mat& right,
uchar* buf0, uchar* buf1, CvStereoBMState* _state )
{
imgs0[0] = &left0; imgs0[1] = &right0;
imgs[0] = &left; imgs[1] = &right;
buf[0] = buf0; buf[1] = buf1;
state = _state;
}
void operator()( int ind ) const
{
if( state->preFilterType == CV_STEREO_BM_NORMALIZED_RESPONSE )
prefilterNorm( *imgs0[ind], *imgs[ind], state->preFilterSize, state->preFilterCap, buf[ind] );
else
prefilterXSobel( *imgs0[ind], *imgs[ind], state->preFilterCap );
}
const Mat* imgs0[2];
Mat* imgs[2];
uchar* buf[2];
CvStereoBMState *state;
};
struct FindStereoCorrespInvoker
{
FindStereoCorrespInvoker( const Mat& _left, const Mat& _right,
Mat& _disp, CvStereoBMState* _state,
int _nstripes, int _stripeBufSize,
bool _useShorts, Rect _validDisparityRect )
{
left = &_left; right = &_right;
disp = &_disp; state = _state;
nstripes = _nstripes; stripeBufSize = _stripeBufSize;
useShorts = _useShorts;
validDisparityRect = _validDisparityRect;
}
void operator()( const BlockedRange& range ) const
{
int cols = left->cols, rows = left->rows;
int _row0 = min(cvRound(range.begin() * rows / nstripes), rows);
int _row1 = min(cvRound(range.end() * rows / nstripes), rows);
uchar *ptr = state->slidingSumBuf->data.ptr + range.begin() * stripeBufSize;
int FILTERED = (state->minDisparity - 1)*16;
Rect roi = validDisparityRect & Rect(0, _row0, cols, _row1);
if( roi.height == 0 )
return;
int row0 = roi.y;
int row1 = roi.y + roi.height;
Mat part;
if( row0 > _row0 )
{
part = disp->rowRange(_row0, row0);
part = Scalar::all(FILTERED);
}
if( _row1 > row1 )
{
part = disp->rowRange(row1, _row1);
part = Scalar::all(FILTERED);
}
Mat left_i = left->rowRange(row0, row1);
Mat right_i = right->rowRange(row0, row1);
Mat disp_i = disp->rowRange(row0, row1);
Mat cost_i = state->disp12MaxDiff >= 0 ? Mat(state->cost).rowRange(row0, row1) : Mat();
#if CV_SSE2
if( useShorts )
findStereoCorrespondenceBM_SSE2( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 );
else
#endif
findStereoCorrespondenceBM( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 );
if( state->disp12MaxDiff >= 0 )
validateDisparity( disp_i, cost_i, state->minDisparity, state->numberOfDisparities, state->disp12MaxDiff );
if( roi.x > 0 )
{
part = disp_i.colRange(0, roi.x);
part = Scalar::all(FILTERED);
}
if( roi.x + roi.width < cols )
{
part = disp_i.colRange(roi.x + roi.width, cols);
part = Scalar::all(FILTERED);
}
}
protected:
const Mat *left, *right;
Mat* disp;
CvStereoBMState *state;
int nstripes;
int stripeBufSize;
bool useShorts;
Rect validDisparityRect;
};
static void findStereoCorrespondenceBM( const Mat& left0, const Mat& right0, Mat& disp0, CvStereoBMState* state)
{
if (left0.size() != right0.size() || disp0.size() != left0.size())
CV_Error( CV_StsUnmatchedSizes, "All the images must have the same size" );
if (left0.type() != CV_8UC1 || right0.type() != CV_8UC1)
CV_Error( CV_StsUnsupportedFormat, "Both input images must have CV_8UC1" );
if (disp0.type() != CV_16SC1 && disp0.type() != CV_32FC1)
CV_Error( CV_StsUnsupportedFormat, "Disparity image must have CV_16SC1 or CV_32FC1 format" );
if( !state )
CV_Error( CV_StsNullPtr, "Stereo BM state is NULL." );
if( state->preFilterType != CV_STEREO_BM_NORMALIZED_RESPONSE && state->preFilterType != CV_STEREO_BM_XSOBEL )
CV_Error( CV_StsOutOfRange, "preFilterType must be = CV_STEREO_BM_NORMALIZED_RESPONSE" );
if( state->preFilterSize < 5 || state->preFilterSize > 255 || state->preFilterSize % 2 == 0 )
CV_Error( CV_StsOutOfRange, "preFilterSize must be odd and be within 5..255" );
if( state->preFilterCap < 1 || state->preFilterCap > 63 )
CV_Error( CV_StsOutOfRange, "preFilterCap must be within 1..63" );
if( state->SADWindowSize < 5 || state->SADWindowSize > 255 || state->SADWindowSize % 2 == 0 ||
state->SADWindowSize >= min(left0.cols, left0.rows) )
CV_Error( CV_StsOutOfRange, "SADWindowSize must be odd, be within 5..255 and be not larger than image width or height" );
if( state->numberOfDisparities <= 0 || state->numberOfDisparities % 16 != 0 )
CV_Error( CV_StsOutOfRange, "numberOfDisparities must be positive and divisble by 16" );
if( state->textureThreshold < 0 )
CV_Error( CV_StsOutOfRange, "texture threshold must be non-negative" );
if( state->uniquenessRatio < 0 )
CV_Error( CV_StsOutOfRange, "uniqueness ratio must be non-negative" );
if( !state->preFilteredImg0 || state->preFilteredImg0->cols * state->preFilteredImg0->rows < left0.cols * left0.rows )
{
cvReleaseMat( &state->preFilteredImg0 );
cvReleaseMat( &state->preFilteredImg1 );
cvReleaseMat( &state->cost );
state->preFilteredImg0 = cvCreateMat( left0.rows, left0.cols, CV_8U );
state->preFilteredImg1 = cvCreateMat( left0.rows, left0.cols, CV_8U );
state->cost = cvCreateMat( left0.rows, left0.cols, CV_16S );
}
Mat left(left0.size(), CV_8U, state->preFilteredImg0->data.ptr);
Mat right(right0.size(), CV_8U, state->preFilteredImg1->data.ptr);
int mindisp = state->minDisparity;
int ndisp = state->numberOfDisparities;
int width = left0.cols;
int height = left0.rows;
int lofs = max(ndisp - 1 + mindisp, 0);
int rofs = -min(ndisp - 1 + mindisp, 0);
int width1 = width - rofs - ndisp + 1;
int FILTERED = (state->minDisparity - 1) << DISPARITY_SHIFT;
if( lofs >= width || rofs >= width || width1 < 1 )
{
disp0 = Scalar::all( FILTERED * ( disp0.type() < CV_32F ? 1 : 1./(1 << DISPARITY_SHIFT) ) );
return;
}
Mat disp = disp0;
if( disp0.type() == CV_32F)
{
if( !state->disp || state->disp->rows != disp0.rows || state->disp->cols != disp0.cols )
{
cvReleaseMat( &state->disp );
state->disp = cvCreateMat(disp0.rows, disp0.cols, CV_16S);
}
disp = cv::cvarrToMat(state->disp);
}
int wsz = state->SADWindowSize;
int bufSize0 = (ndisp + 2)*sizeof(int) + (height+wsz+2)*ndisp*sizeof(int) +
(height + wsz + 2)*sizeof(int) +
(height+wsz+2)*ndisp*(wsz+1)*sizeof(uchar) + 256;
int bufSize1 = (width + state->preFilterSize + 2) * sizeof(int) + 256;
int bufSize2 = 0;
if( state->speckleRange >= 0 && state->speckleWindowSize > 0 )
bufSize2 = width*height*(sizeof(cv::Point_<short>) + sizeof(int) + sizeof(uchar));
#if CV_SSE2
bool useShorts = state->preFilterCap <= 31 && state->SADWindowSize <= 21 && checkHardwareSupport(CV_CPU_SSE2);
#else
const bool useShorts = false;
#endif
#ifdef HAVE_TBB
const double SAD_overhead_coeff = 10.0;
double N0 = 8000000 / (useShorts ? 1 : 4); // approx tbb's min number instructions reasonable for one thread
double maxStripeSize = min(max(N0 / (width * ndisp), (wsz-1) * SAD_overhead_coeff), (double)height);
int nstripes = cvCeil(height / maxStripeSize);
printf("nstripes=%d\n", nstripes);
#else
const int nstripes = 1;
#endif
int bufSize = max(bufSize0 * nstripes, max(bufSize1 * 2, bufSize2));
if( !state->slidingSumBuf || state->slidingSumBuf->cols < bufSize )
{
cvReleaseMat( &state->slidingSumBuf );
state->slidingSumBuf = cvCreateMat( 1, bufSize, CV_8U );
}
uchar *_buf = state->slidingSumBuf->data.ptr;
int idx[] = {0,1};
parallel_do(idx, idx+2, PrefilterInvoker(left0, right0, left, right, _buf, _buf + bufSize1, state));
Rect validDisparityRect(0, 0, width, height), R1 = state->roi1, R2 = state->roi2;
validDisparityRect = getValidDisparityROI(R1.area() > 0 ? Rect(0, 0, width, height) : validDisparityRect,
R2.area() > 0 ? Rect(0, 0, width, height) : validDisparityRect,
state->minDisparity, state->numberOfDisparities,
state->SADWindowSize);
parallel_for(BlockedRange(0, nstripes),
FindStereoCorrespInvoker(left, right, disp, state, nstripes,
bufSize0, useShorts, validDisparityRect));
if( state->speckleRange >= 0 && state->speckleWindowSize > 0 )
{
Mat buf(state->slidingSumBuf);
filterSpeckles(disp, FILTERED, state->speckleRange, state->speckleWindowSize, buf);
}
if (disp0.data != disp.data)
disp.convertTo(disp0, disp0.type(), 1./(1 << DISPARITY_SHIFT), 0);
}
StereoBM::StereoBM()
{ state = cvCreateStereoBMState(); }
StereoBM::StereoBM(int _preset, int _ndisparities, int _SADWindowSize)
{ init(_preset, _ndisparities, _SADWindowSize); }
void StereoBM::init(int _preset, int _ndisparities, int _SADWindowSize)
{
state = cvCreateStereoBMState(_preset, _ndisparities);
state->SADWindowSize = _SADWindowSize;
}
void StereoBM::operator()( const Mat& left, const Mat& right, Mat& disparity, int disptype )
{
CV_Assert( disptype == CV_16S || disptype == CV_32F );
disparity.create(left.size(), disptype);
findStereoCorrespondenceBM(left, right, disparity, state);
}
template<> void Ptr<CvStereoBMState>::delete_obj()
{ cvReleaseStereoBMState(&obj); }
}
CV_IMPL void cvFindStereoCorrespondenceBM( const CvArr* leftarr, const CvArr* rightarr,
CvArr* disparr, CvStereoBMState* state )
{
cv::Mat left = cv::cvarrToMat(leftarr),
right = cv::cvarrToMat(rightarr),
disp = cv::cvarrToMat(disparr);
cv::findStereoCorrespondenceBM(left, right, disp, state);
}
/* End of file. */

948
modules/calib3d/src/stereogc.cpp Normal file
View File

@@ -0,0 +1,948 @@
//M*//////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of Intel Corporation may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
#undef INFINITY
#define INFINITY 10000
#define OCCLUSION_PENALTY 10000
#define OCCLUSION_PENALTY2 1000
#define DENOMINATOR 16
#undef OCCLUDED
#define OCCLUDED CV_STEREO_GC_OCCLUDED
#define CUTOFF 1000
#define IS_BLOCKED(d1, d2) ((d1) > (d2))
typedef struct GCVtx
{
GCVtx *next;
int parent;
int first;
int ts;
int dist;
short weight;
uchar t;
}
GCVtx;
typedef struct GCEdge
{
GCVtx* dst;
int next;
int weight;
}
GCEdge;
typedef struct CvStereoGCState2
{
int Ithreshold, interactionRadius;
int lambda, lambda1, lambda2, K;
int dataCostFuncTab[CUTOFF+1];
int smoothnessR[CUTOFF*2+1];
int smoothnessGrayDiff[512];
GCVtx** orphans;
int maxOrphans;
}
CvStereoGCState2;
// truncTab[x+255] = MAX(x-255,0)
static uchar icvTruncTab[512];
// cutoffSqrTab[x] = MIN(x*x, CUTOFF)
static int icvCutoffSqrTab[256];
static void icvInitStereoConstTabs()
{
static volatile int initialized = 0;
if( !initialized )
{
int i;
for( i = 0; i < 512; i++ )
icvTruncTab[i] = (uchar)MIN(MAX(i-255,0),255);
for( i = 0; i < 256; i++ )
icvCutoffSqrTab[i] = MIN(i*i, CUTOFF);
initialized = 1;
}
}
static void icvInitStereoTabs( CvStereoGCState2* state2 )
{
int i, K = state2->K;
for( i = 0; i <= CUTOFF; i++ )
state2->dataCostFuncTab[i] = MIN(i*DENOMINATOR - K, 0);
for( i = 0; i < CUTOFF*2 + 1; i++ )
state2->smoothnessR[i] = MIN(abs(i-CUTOFF), state2->interactionRadius);
for( i = 0; i < 512; i++ )
{
int diff = abs(i - 255);
state2->smoothnessGrayDiff[i] = diff < state2->Ithreshold ? state2->lambda1 : state2->lambda2;
}
}
static int icvGCResizeOrphansBuf( GCVtx**& orphans, int norphans )
{
int i, newNOrphans = MAX(norphans*3/2, 256);
GCVtx** newOrphans = (GCVtx**)cvAlloc( newNOrphans*sizeof(orphans[0]) );
for( i = 0; i < norphans; i++ )
newOrphans[i] = orphans[i];
cvFree( &orphans );
orphans = newOrphans;
return newNOrphans;
}
static int64 icvGCMaxFlow( GCVtx* vtx, int nvtx, GCEdge* edges, GCVtx**& _orphans, int& _maxOrphans )
{
const int TERMINAL = -1, ORPHAN = -2;
GCVtx stub, *nilNode = &stub, *first = nilNode, *last = nilNode;
int i, k;
int curr_ts = 0;
int64 flow = 0;
int norphans = 0, maxOrphans = _maxOrphans;
GCVtx** orphans = _orphans;
stub.next = nilNode;
// initialize the active queue and the graph vertices
for( i = 0; i < nvtx; i++ )
{
GCVtx* v = vtx + i;
v->ts = 0;
if( v->weight != 0 )
{
last = last->next = v;
v->dist = 1;
v->parent = TERMINAL;
v->t = v->weight < 0;
}
else
v->parent = 0;
}
first = first->next;
last->next = nilNode;
nilNode->next = 0;
// run the search-path -> augment-graph -> restore-trees loop
for(;;)
{
GCVtx* v, *u;
int e0 = -1, ei = 0, ej = 0, min_weight, weight;
uchar vt;
// grow S & T search trees, find an edge connecting them
while( first != nilNode )
{
v = first;
if( v->parent )
{
vt = v->t;
for( ei = v->first; ei != 0; ei = edges[ei].next )
{
if( edges[ei^vt].weight == 0 )
continue;
u = edges[ei].dst;
if( !u->parent )
{
u->t = vt;
u->parent = ei ^ 1;
u->ts = v->ts;
u->dist = v->dist + 1;
if( !u->next )
{
u->next = nilNode;
last = last->next = u;
}
continue;
}
if( u->t != vt )
{
e0 = ei ^ vt;
break;
}
if( u->dist > v->dist+1 && u->ts <= v->ts )
{
// reassign the parent
u->parent = ei ^ 1;
u->ts = v->ts;
u->dist = v->dist + 1;
}
}
if( e0 > 0 )
break;
}
// exclude the vertex from the active list
first = first->next;
v->next = 0;
}
if( e0 <= 0 )
break;
// find the minimum edge weight along the path
min_weight = edges[e0].weight;
assert( min_weight > 0 );
// k = 1: source tree, k = 0: destination tree
for( k = 1; k >= 0; k-- )
{
for( v = edges[e0^k].dst;; v = edges[ei].dst )
{
if( (ei = v->parent) < 0 )
break;
weight = edges[ei^k].weight;
min_weight = MIN(min_weight, weight);
assert( min_weight > 0 );
}
weight = abs(v->weight);
min_weight = MIN(min_weight, weight);
assert( min_weight > 0 );
}
// modify weights of the edges along the path and collect orphans
edges[e0].weight -= min_weight;
edges[e0^1].weight += min_weight;
flow += min_weight;
// k = 1: source tree, k = 0: destination tree
for( k = 1; k >= 0; k-- )
{
for( v = edges[e0^k].dst;; v = edges[ei].dst )
{
if( (ei = v->parent) < 0 )
break;
edges[ei^(k^1)].weight += min_weight;
if( (edges[ei^k].weight -= min_weight) == 0 )
{
if( norphans >= maxOrphans )
maxOrphans = icvGCResizeOrphansBuf( orphans, norphans );
orphans[norphans++] = v;
v->parent = ORPHAN;
}
}
v->weight = (short)(v->weight + min_weight*(1-k*2));
if( v->weight == 0 )
{
if( norphans >= maxOrphans )
maxOrphans = icvGCResizeOrphansBuf( orphans, norphans );
orphans[norphans++] = v;
v->parent = ORPHAN;
}
}
// restore the search trees by finding new parents for the orphans
curr_ts++;
while( norphans > 0 )
{
GCVtx* v = orphans[--norphans];
int d, min_dist = INT_MAX;
e0 = 0;
vt = v->t;
for( ei = v->first; ei != 0; ei = edges[ei].next )
{
if( edges[ei^(vt^1)].weight == 0 )
continue;
u = edges[ei].dst;
if( u->t != vt || u->parent == 0 )
continue;
// compute the distance to the tree root
for( d = 0;; )
{
if( u->ts == curr_ts )
{
d += u->dist;
break;
}
ej = u->parent;
d++;
if( ej < 0 )
{
if( ej == ORPHAN )
d = INT_MAX-1;
else
{
u->ts = curr_ts;
u->dist = 1;
}
break;
}
u = edges[ej].dst;
}
// update the distance
if( ++d < INT_MAX )
{
if( d < min_dist )
{
min_dist = d;
e0 = ei;
}
for( u = edges[ei].dst; u->ts != curr_ts; u = edges[u->parent].dst )
{
u->ts = curr_ts;
u->dist = --d;
}
}
}
if( (v->parent = e0) > 0 )
{
v->ts = curr_ts;
v->dist = min_dist;
continue;
}
/* no parent is found */
v->ts = 0;
for( ei = v->first; ei != 0; ei = edges[ei].next )
{
u = edges[ei].dst;
ej = u->parent;
if( u->t != vt || !ej )
continue;
if( edges[ei^(vt^1)].weight && !u->next )
{
u->next = nilNode;
last = last->next = u;
}
if( ej > 0 && edges[ej].dst == v )
{
if( norphans >= maxOrphans )
maxOrphans = icvGCResizeOrphansBuf( orphans, norphans );
orphans[norphans++] = u;
u->parent = ORPHAN;
}
}
}
}
_orphans = orphans;
_maxOrphans = maxOrphans;
return flow;
}
CvStereoGCState* cvCreateStereoGCState( int numberOfDisparities, int maxIters )
{
CvStereoGCState* state = 0;
state = (CvStereoGCState*)cvAlloc( sizeof(*state) );
memset( state, 0, sizeof(*state) );
state->minDisparity = 0;
state->numberOfDisparities = numberOfDisparities;
state->maxIters = maxIters <= 0 ? 3 : maxIters;
state->Ithreshold = 5;
state->interactionRadius = 1;
state->K = state->lambda = state->lambda1 = state->lambda2 = -1.f;
state->occlusionCost = OCCLUSION_PENALTY;
return state;
}
void cvReleaseStereoGCState( CvStereoGCState** _state )
{
CvStereoGCState* state;
if( !_state && !*_state )
return;
state = *_state;
cvReleaseMat( &state->left );
cvReleaseMat( &state->right );
cvReleaseMat( &state->ptrLeft );
cvReleaseMat( &state->ptrRight );
cvReleaseMat( &state->vtxBuf );
cvReleaseMat( &state->edgeBuf );
cvFree( _state );
}
// ||I(x) - J(x')|| =
// min(CUTOFF,
// min(
// max(
// max(minJ(x') - I(x), 0),
// max(I(x) - maxJ(x'), 0)),
// max(
// max(minI(x) - J(x'), 0),
// max(J(x') - maxI(x), 0)))**2) ==
// min(CUTOFF,
// min(
// max(minJ(x') - I(x), 0) +
// max(I(x) - maxJ(x'), 0),
//
// max(minI(x) - J(x'), 0) +
// max(J(x') - maxI(x), 0)))**2)
// where (I, minI, maxI) and
// (J, minJ, maxJ) are stored as interleaved 3-channel images.
// minI, maxI are computed from I,
// minJ, maxJ are computed from J - see icvInitGraySubPix.
static inline int icvDataCostFuncGraySubpix( const uchar* a, const uchar* b )
{
int va = a[0], vb = b[0];
int da = icvTruncTab[b[1] - va + 255] + icvTruncTab[va - b[2] + 255];
int db = icvTruncTab[a[1] - vb + 255] + icvTruncTab[vb - a[2] + 255];
return icvCutoffSqrTab[MIN(da,db)];
}
static inline int icvSmoothnessCostFunc( int da, int db, int maxR, const int* stabR, int scale )
{
return da == db ? 0 : (da == OCCLUDED || db == OCCLUDED ? maxR : stabR[da - db])*scale;
}
static void icvInitGraySubpix( const CvMat* left, const CvMat* right,
CvMat* left3, CvMat* right3 )
{
int k, x, y, rows = left->rows, cols = left->cols;
for( k = 0; k < 2; k++ )
{
const CvMat* src = k == 0 ? left : right;
CvMat* dst = k == 0 ? left3 : right3;
int sstep = src->step;
for( y = 0; y < rows; y++ )
{
const uchar* sptr = src->data.ptr + sstep*y;
const uchar* sptr_prev = y > 0 ? sptr - sstep : sptr;
const uchar* sptr_next = y < rows-1 ? sptr + sstep : sptr;
uchar* dptr = dst->data.ptr + dst->step*y;
int v_prev = sptr[0];
for( x = 0; x < cols; x++, dptr += 3 )
{
int v = sptr[x], v1, minv = v, maxv = v;
v1 = (v + v_prev)/2;
minv = MIN(minv, v1); maxv = MAX(maxv, v1);
v1 = (v + sptr_prev[x])/2;
minv = MIN(minv, v1); maxv = MAX(maxv, v1);
v1 = (v + sptr_next[x])/2;
minv = MIN(minv, v1); maxv = MAX(maxv, v1);
if( x < cols-1 )
{
v1 = (v + sptr[x+1])/2;
minv = MIN(minv, v1); maxv = MAX(maxv, v1);
}
v_prev = v;
dptr[0] = (uchar)v;
dptr[1] = (uchar)minv;
dptr[2] = (uchar)maxv;
}
}
}
}
// Optimal K is computed as avg_x(k-th-smallest_d(||I(x)-J(x+d)||)),
// where k = number_of_disparities*0.25.
static float
icvComputeK( CvStereoGCState* state )
{
int x, y, x1, d, i, j, rows = state->left->rows, cols = state->left->cols, n = 0;
int mind = state->minDisparity, nd = state->numberOfDisparities, maxd = mind + nd;
int k = MIN(MAX((nd + 2)/4, 3), nd);
int *arr = (int*)cvStackAlloc(k*sizeof(arr[0])), delta, t, sum = 0;
for( y = 0; y < rows; y++ )
{
const uchar* lptr = state->left->data.ptr + state->left->step*y;
const uchar* rptr = state->right->data.ptr + state->right->step*y;
for( x = 0; x < cols; x++ )
{
for( d = maxd-1, i = 0; d >= mind; d-- )
{
x1 = x - d;
if( (unsigned)x1 >= (unsigned)cols )
continue;
delta = icvDataCostFuncGraySubpix( lptr + x*3, rptr + x1*3 );
if( i < k )
arr[i++] = delta;
else
for( i = 0; i < k; i++ )
if( delta < arr[i] )
CV_SWAP( arr[i], delta, t );
}
delta = arr[0];
for( j = 1; j < i; j++ )
delta = MAX(delta, arr[j]);
sum += delta;
n++;
}
}
return (float)sum/n;
}
static int64 icvComputeEnergy( const CvStereoGCState* state, const CvStereoGCState2* state2,
bool allOccluded )
{
int x, y, rows = state->left->rows, cols = state->left->cols;
int64 E = 0;
const int* dtab = state2->dataCostFuncTab;
int maxR = state2->interactionRadius;
const int* stabR = state2->smoothnessR + CUTOFF;
const int* stabI = state2->smoothnessGrayDiff + 255;
const uchar* left = state->left->data.ptr;
const uchar* right = state->right->data.ptr;
short* dleft = state->dispLeft->data.s;
short* dright = state->dispRight->data.s;
int step = state->left->step;
int dstep = (int)(state->dispLeft->step/sizeof(short));
assert( state->left->step == state->right->step &&
state->dispLeft->step == state->dispRight->step );
if( allOccluded )
return (int64)OCCLUSION_PENALTY*rows*cols*2;
for( y = 0; y < rows; y++, left += step, right += step, dleft += dstep, dright += dstep )
{
for( x = 0; x < cols; x++ )
{
int d = dleft[x], x1, d1;
if( d == OCCLUDED )
E += OCCLUSION_PENALTY;
else
{
x1 = x + d;
if( (unsigned)x1 >= (unsigned)cols )
continue;
d1 = dright[x1];
if( d == -d1 )
E += dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )];
}
if( x < cols-1 )
{
d1 = dleft[x+1];
E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[left[x*3] - left[x*3+3]] );
}
if( y < rows-1 )
{
d1 = dleft[x+dstep];
E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[left[x*3] - left[x*3+step]] );
}
d = dright[x];
if( d == OCCLUDED )
E += OCCLUSION_PENALTY;
if( x < cols-1 )
{
d1 = dright[x+1];
E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[right[x*3] - right[x*3+3]] );
}
if( y < rows-1 )
{
d1 = dright[x+dstep];
E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[right[x*3] - right[x*3+step]] );
}
assert( E >= 0 );
}
}
return E;
}
static inline void icvAddEdge( GCVtx *x, GCVtx* y, GCEdge* edgeBuf, int nedges, int w, int rw )
{
GCEdge *xy = edgeBuf + nedges, *yx = xy + 1;
assert( x != 0 && y != 0 );
xy->dst = y;
xy->next = x->first;
xy->weight = (short)w;
x->first = nedges;
yx->dst = x;
yx->next = y->first;
yx->weight = (short)rw;
y->first = nedges+1;
}
static inline int icvAddTWeights( GCVtx* vtx, int sourceWeight, int sinkWeight )
{
int w = vtx->weight;
if( w > 0 )
sourceWeight += w;
else
sinkWeight -= w;
vtx->weight = (short)(sourceWeight - sinkWeight);
return MIN(sourceWeight, sinkWeight);
}
static inline int icvAddTerm( GCVtx* x, GCVtx* y, int A, int B, int C, int D,
GCEdge* edgeBuf, int& nedges )
{
int dE = 0, w;
assert(B - A + C - D >= 0);
if( B < A )
{
dE += icvAddTWeights(x, D, B);
dE += icvAddTWeights(y, 0, A - B);
if( (w = B - A + C - D) != 0 )
{
icvAddEdge( x, y, edgeBuf, nedges, 0, w );
nedges += 2;
}
}
else if( C < D )
{
dE += icvAddTWeights(x, D, A + D - C);
dE += icvAddTWeights(y, 0, C - D);
if( (w = B - A + C - D) != 0 )
{
icvAddEdge( x, y, edgeBuf, nedges, w, 0 );
nedges += 2;
}
}
else
{
dE += icvAddTWeights(x, D, A);
if( B != A || C != D )
{
icvAddEdge( x, y, edgeBuf, nedges, B - A, C - D );
nedges += 2;
}
}
return dE;
}
static int64 icvAlphaExpand( int64 Eprev, int alpha, CvStereoGCState* state, CvStereoGCState2* state2 )
{
GCVtx *var, *var1;
int64 E = 0;
int delta, E00=0, E0a=0, Ea0=0, Eaa=0;
int k, a, d, d1, x, y, x1, y1, rows = state->left->rows, cols = state->left->cols;
int nvtx = 0, nedges = 2;
GCVtx* vbuf = (GCVtx*)state->vtxBuf->data.ptr;
GCEdge* ebuf = (GCEdge*)state->edgeBuf->data.ptr;
int maxR = state2->interactionRadius;
const int* dtab = state2->dataCostFuncTab;
const int* stabR = state2->smoothnessR + CUTOFF;
const int* stabI = state2->smoothnessGrayDiff + 255;
const uchar* left0 = state->left->data.ptr;
const uchar* right0 = state->right->data.ptr;
short* dleft0 = state->dispLeft->data.s;
short* dright0 = state->dispRight->data.s;
GCVtx** pleft0 = (GCVtx**)state->ptrLeft->data.ptr;
GCVtx** pright0 = (GCVtx**)state->ptrRight->data.ptr;
int step = state->left->step;
int dstep = (int)(state->dispLeft->step/sizeof(short));
int pstep = (int)(state->ptrLeft->step/sizeof(GCVtx*));
int aa[] = { alpha, -alpha };
//double t = (double)cvGetTickCount();
assert( state->left->step == state->right->step &&
state->dispLeft->step == state->dispRight->step &&
state->ptrLeft->step == state->ptrRight->step );
for( k = 0; k < 2; k++ )
{
ebuf[k].dst = 0;
ebuf[k].next = 0;
ebuf[k].weight = 0;
}
for( y = 0; y < rows; y++ )
{
const uchar* left = left0 + step*y;
const uchar* right = right0 + step*y;
const short* dleft = dleft0 + dstep*y;
const short* dright = dright0 + dstep*y;
GCVtx** pleft = pleft0 + pstep*y;
GCVtx** pright = pright0 + pstep*y;
const uchar* lr[] = { left, right };
const short* dlr[] = { dleft, dright };
GCVtx** plr[] = { pleft, pright };
for( k = 0; k < 2; k++ )
{
a = aa[k];
for( y1 = y+(y>0); y1 <= y+(y<rows-1); y1++ )
{
const short* disp = (k == 0 ? dleft0 : dright0) + y1*dstep;
GCVtx** ptr = (k == 0 ? pleft0 : pright0) + y1*pstep;
for( x = 0; x < cols; x++ )
{
GCVtx* v = ptr[x] = &vbuf[nvtx++];
v->first = 0;
v->weight = disp[x] == (short)(OCCLUDED ? -OCCLUSION_PENALTY2 : 0);
}
}
}
for( x = 0; x < cols; x++ )
{
d = dleft[x];
x1 = x + d;
var = pleft[x];
// (left + x, right + x + d)
if( d != alpha && d != OCCLUDED && (unsigned)x1 < (unsigned)cols )
{
var1 = pright[x1];
d1 = dright[x1];
if( d == -d1 )
{
assert( var1 != 0 );
delta = IS_BLOCKED(alpha, d) ? INFINITY : 0;
//add inter edge
E += icvAddTerm( var, var1,
dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )],
delta, delta, 0, ebuf, nedges );
}
else if( IS_BLOCKED(alpha, d) )
E += icvAddTerm( var, var1, 0, INFINITY, 0, 0, ebuf, nedges );
}
// (left + x, right + x + alpha)
x1 = x + alpha;
if( (unsigned)x1 < (unsigned)cols )
{
var1 = pright[x1];
d1 = dright[x1];
E0a = IS_BLOCKED(d, alpha) ? INFINITY : 0;
Ea0 = IS_BLOCKED(-d1, alpha) ? INFINITY : 0;
Eaa = dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )];
E += icvAddTerm( var, var1, 0, E0a, Ea0, Eaa, ebuf, nedges );
}
// smoothness
for( k = 0; k < 2; k++ )
{
GCVtx** p = plr[k];
const short* disp = dlr[k];
const uchar* img = lr[k] + x*3;
int scale;
var = p[x];
d = disp[x];
a = aa[k];
if( x < cols - 1 )
{
var1 = p[x+1];
d1 = disp[x+1];
scale = stabI[img[0] - img[3]];
E0a = icvSmoothnessCostFunc( d, a, maxR, stabR, scale );
Ea0 = icvSmoothnessCostFunc( a, d1, maxR, stabR, scale );
E00 = icvSmoothnessCostFunc( d, d1, maxR, stabR, scale );
E += icvAddTerm( var, var1, E00, E0a, Ea0, 0, ebuf, nedges );
}
if( y < rows - 1 )
{
var1 = p[x+pstep];
d1 = disp[x+dstep];
scale = stabI[img[0] - img[step]];
E0a = icvSmoothnessCostFunc( d, a, maxR, stabR, scale );
Ea0 = icvSmoothnessCostFunc( a, d1, maxR, stabR, scale );
E00 = icvSmoothnessCostFunc( d, d1, maxR, stabR, scale );
E += icvAddTerm( var, var1, E00, E0a, Ea0, 0, ebuf, nedges );
}
}
// visibility term
if( d != OCCLUDED && IS_BLOCKED(alpha, -d))
{
x1 = x + d;
if( (unsigned)x1 < (unsigned)cols )
{
if( d != -dleft[x1] )
{
var1 = pleft[x1];
E += icvAddTerm( var, var1, 0, INFINITY, 0, 0, ebuf, nedges );
}
}
}
}
}
//t = (double)cvGetTickCount() - t;
ebuf[0].weight = ebuf[1].weight = 0;
E += icvGCMaxFlow( vbuf, nvtx, ebuf, state2->orphans, state2->maxOrphans );
if( E < Eprev )
{
for( y = 0; y < rows; y++ )
{
short* dleft = dleft0 + dstep*y;
short* dright = dright0 + dstep*y;
GCVtx** pleft = pleft0 + pstep*y;
GCVtx** pright = pright0 + pstep*y;
for( x = 0; x < cols; x++ )
{
GCVtx* var = pleft[x];
if( var && var->parent && var->t )
dleft[x] = (short)alpha;
var = pright[x];
if( var && var->parent && var->t )
dright[x] = (short)-alpha;
}
}
}
return MIN(E, Eprev);
}
CV_IMPL void cvFindStereoCorrespondenceGC( const CvArr* _left, const CvArr* _right,
CvArr* _dispLeft, CvArr* _dispRight, CvStereoGCState* state, int useDisparityGuess )
{
CvStereoGCState2 state2;
state2.orphans = 0;
state2.maxOrphans = 0;
CvMat lstub, *left = cvGetMat( _left, &lstub );
CvMat rstub, *right = cvGetMat( _right, &rstub );
CvMat dlstub, *dispLeft = cvGetMat( _dispLeft, &dlstub );
CvMat drstub, *dispRight = cvGetMat( _dispRight, &drstub );
CvSize size;
int iter, i, nZeroExpansions = 0;
CvRNG rng = cvRNG(-1);
int* disp;
CvMat _disp;
int64 E;
CV_Assert( state != 0 );
CV_Assert( CV_ARE_SIZES_EQ(left, right) && CV_ARE_TYPES_EQ(left, right) &&
CV_MAT_TYPE(left->type) == CV_8UC1 );
CV_Assert( !dispLeft ||
(CV_ARE_SIZES_EQ(dispLeft, left) && CV_MAT_CN(dispLeft->type) == 1) );
CV_Assert( !dispRight ||
(CV_ARE_SIZES_EQ(dispRight, left) && CV_MAT_CN(dispRight->type) == 1) );
size = cvGetSize(left);
if( !state->left || state->left->width != size.width || state->left->height != size.height )
{
int pcn = (int)(sizeof(GCVtx*)/sizeof(int));
int vcn = (int)(sizeof(GCVtx)/sizeof(int));
int ecn = (int)(sizeof(GCEdge)/sizeof(int));
cvReleaseMat( &state->left );
cvReleaseMat( &state->right );
cvReleaseMat( &state->ptrLeft );
cvReleaseMat( &state->ptrRight );
cvReleaseMat( &state->dispLeft );
cvReleaseMat( &state->dispRight );
state->left = cvCreateMat( size.height, size.width, CV_8UC3 );
state->right = cvCreateMat( size.height, size.width, CV_8UC3 );
state->dispLeft = cvCreateMat( size.height, size.width, CV_16SC1 );
state->dispRight = cvCreateMat( size.height, size.width, CV_16SC1 );
state->ptrLeft = cvCreateMat( size.height, size.width, CV_32SC(pcn) );
state->ptrRight = cvCreateMat( size.height, size.width, CV_32SC(pcn) );
state->vtxBuf = cvCreateMat( 1, size.height*size.width*2, CV_32SC(vcn) );
state->edgeBuf = cvCreateMat( 1, size.height*size.width*12 + 16, CV_32SC(ecn) );
}
if( !useDisparityGuess )
{
cvSet( state->dispLeft, cvScalarAll(OCCLUDED));
cvSet( state->dispRight, cvScalarAll(OCCLUDED));
}
else
{
CV_Assert( dispLeft && dispRight );
cvConvert( dispLeft, state->dispLeft );
cvConvert( dispRight, state->dispRight );
}
state2.Ithreshold = state->Ithreshold;
state2.interactionRadius = state->interactionRadius;
state2.lambda = cvRound(state->lambda*DENOMINATOR);
state2.lambda1 = cvRound(state->lambda1*DENOMINATOR);
state2.lambda2 = cvRound(state->lambda2*DENOMINATOR);
state2.K = cvRound(state->K*DENOMINATOR);
icvInitStereoConstTabs();
icvInitGraySubpix( left, right, state->left, state->right );
disp = (int*)cvStackAlloc( state->numberOfDisparities*sizeof(disp[0]) );
_disp = cvMat( 1, state->numberOfDisparities, CV_32S, disp );
cvRange( &_disp, state->minDisparity, state->minDisparity + state->numberOfDisparities );
cvRandShuffle( &_disp, &rng );
if( state2.lambda < 0 && (state2.K < 0 || state2.lambda1 < 0 || state2.lambda2 < 0) )
{
float L = icvComputeK(state)*0.2f;
state2.lambda = cvRound(L*DENOMINATOR);
}
if( state2.K < 0 )
state2.K = state2.lambda*5;
if( state2.lambda1 < 0 )
state2.lambda1 = state2.lambda*3;
if( state2.lambda2 < 0 )
state2.lambda2 = state2.lambda;
icvInitStereoTabs( &state2 );
E = icvComputeEnergy( state, &state2, !useDisparityGuess );
for( iter = 0; iter < state->maxIters; iter++ )
{
for( i = 0; i < state->numberOfDisparities; i++ )
{
int alpha = disp[i];
int64 Enew = icvAlphaExpand( E, -alpha, state, &state2 );
if( Enew < E )
{
nZeroExpansions = 0;
E = Enew;
}
else if( ++nZeroExpansions >= state->numberOfDisparities )
break;
}
}
if( dispLeft )
cvConvert( state->dispLeft, dispLeft );
if( dispRight )
cvConvert( state->dispRight, dispRight );
cvFree( &state2.orphans );
}

1048
modules/calib3d/src/stereosgbm.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

405
modules/calib3d/src/triangulate.cpp Normal file
View File

@@ -0,0 +1,405 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2009, Intel Corporation and others, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of Intel Corporation may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
// cvCorrectMatches function is Copyright (C) 2009, Jostein Austvik Jacobsen.
// cvTriangulatePoints function is derived from icvReconstructPointsFor3View, originally by Valery Mosyagin.
// HZ, R. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision, Cambridge Univ. Press, 2003.
// This method is the same as icvReconstructPointsFor3View, with only a few numbers adjusted for two-view geometry
CV_IMPL void
cvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2, CvMat* projPoints1, CvMat* projPoints2, CvMat* points4D)
{
if( projMatr1 == 0 || projMatr2 == 0 ||
projPoints1 == 0 || projPoints2 == 0 ||
points4D == 0)
CV_Error( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
if( !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) ||
!CV_IS_MAT(projPoints1) || !CV_IS_MAT(projPoints2) ||
!CV_IS_MAT(points4D) )
CV_Error( CV_StsUnsupportedFormat, "Input parameters must be matrices" );
int numPoints;
numPoints = projPoints1->cols;
if( numPoints < 1 )
CV_Error( CV_StsOutOfRange, "Number of points must be more than zero" );
if( projPoints2->cols != numPoints || points4D->cols != numPoints )
CV_Error( CV_StsUnmatchedSizes, "Number of points must be the same" );
if( projPoints1->rows != 2 || projPoints2->rows != 2)
CV_Error( CV_StsUnmatchedSizes, "Number of proj points coordinates must be == 2" );
if( points4D->rows != 4 )
CV_Error( CV_StsUnmatchedSizes, "Number of world points coordinates must be == 4" );
if( projMatr1->cols != 4 || projMatr1->rows != 3 ||
projMatr2->cols != 4 || projMatr2->rows != 3)
CV_Error( CV_StsUnmatchedSizes, "Size of projection matrices must be 3x4" );
CvMat matrA;
double matrA_dat[24];
matrA = cvMat(6,4,CV_64F,matrA_dat);
//CvMat matrU;
CvMat matrW;
CvMat matrV;
//double matrU_dat[9*9];
double matrW_dat[6*4];
double matrV_dat[4*4];
//matrU = cvMat(6,6,CV_64F,matrU_dat);
matrW = cvMat(6,4,CV_64F,matrW_dat);
matrV = cvMat(4,4,CV_64F,matrV_dat);
CvMat* projPoints[2];
CvMat* projMatrs[2];
projPoints[0] = projPoints1;
projPoints[1] = projPoints2;
projMatrs[0] = projMatr1;
projMatrs[1] = projMatr2;
/* Solve system for each point */
int i,j;
for( i = 0; i < numPoints; i++ )/* For each point */
{
/* Fill matrix for current point */
for( j = 0; j < 2; j++ )/* For each view */
{
double x,y;
x = cvmGet(projPoints[j],0,i);
y = cvmGet(projPoints[j],1,i);
for( int k = 0; k < 4; k++ )
{
cvmSet(&matrA, j*3+0, k, x * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],0,k) );
cvmSet(&matrA, j*3+1, k, y * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],1,k) );
cvmSet(&matrA, j*3+2, k, x * cvmGet(projMatrs[j],1,k) - y * cvmGet(projMatrs[j],0,k) );
}
}
/* Solve system for current point */
{
cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T);
/* Copy computed point */
cvmSet(points4D,0,i,cvmGet(&matrV,3,0));/* X */
cvmSet(points4D,1,i,cvmGet(&matrV,3,1));/* Y */
cvmSet(points4D,2,i,cvmGet(&matrV,3,2));/* Z */
cvmSet(points4D,3,i,cvmGet(&matrV,3,3));/* W */
}
}
/* Points was reconstructed. Try to reproject points */
/* We can compute reprojection error if need */
{
int i;
CvMat point3D;
double point3D_dat[4];
point3D = cvMat(4,1,CV_64F,point3D_dat);
CvMat point2D;
double point2D_dat[3];
point2D = cvMat(3,1,CV_64F,point2D_dat);
for( i = 0; i < numPoints; i++ )
{
double W = cvmGet(points4D,3,i);
point3D_dat[0] = cvmGet(points4D,0,i)/W;
point3D_dat[1] = cvmGet(points4D,1,i)/W;
point3D_dat[2] = cvmGet(points4D,2,i)/W;
point3D_dat[3] = 1;
/* !!! Project this point for each camera */
for( int currCamera = 0; currCamera < 2; currCamera++ )
{
cvMatMul(projMatrs[currCamera], &point3D, &point2D);
float x,y;
float xr,yr,wr;
x = (float)cvmGet(projPoints[currCamera],0,i);
y = (float)cvmGet(projPoints[currCamera],1,i);
wr = (float)point2D_dat[2];
xr = (float)(point2D_dat[0]/wr);
yr = (float)(point2D_dat[1]/wr);
float deltaX,deltaY;
deltaX = (float)fabs(x-xr);
deltaY = (float)fabs(y-yr);
}
}
}
}
/*
* The Optimal Triangulation Method (see HZ for details)
* For each given point correspondence points1[i] <-> points2[i], and a fundamental matrix F,
* computes the corrected correspondences new_points1[i] <-> new_points2[i] that minimize the
* geometric error d(points1[i],new_points1[i])^2 + d(points2[i],new_points2[i])^2 (where d(a,b)
* is the geometric distance between points a and b) subject to the epipolar constraint
* new_points2' * F * new_points1 = 0.
*
* F_ : 3x3 fundamental matrix
* points1_ : 2xN matrix containing the first set of points
* points2_ : 2xN matrix containing the second set of points
* new_points1 : the optimized points1_. if this is NULL, the corrected points are placed back in points1_
* new_points2 : the optimized points2_. if this is NULL, the corrected points are placed back in points2_
*/
CV_IMPL void
cvCorrectMatches(CvMat *F_, CvMat *points1_, CvMat *points2_, CvMat *new_points1, CvMat *new_points2)
{
cv::Ptr<CvMat> tmp33;
cv::Ptr<CvMat> tmp31, tmp31_2;
cv::Ptr<CvMat> T1i, T2i;
cv::Ptr<CvMat> R1, R2;
cv::Ptr<CvMat> TFT, TFTt, RTFTR;
cv::Ptr<CvMat> U, S, V;
cv::Ptr<CvMat> e1, e2;
cv::Ptr<CvMat> polynomial;
cv::Ptr<CvMat> result;
cv::Ptr<CvMat> points1, points2;
cv::Ptr<CvMat> F;
if (!CV_IS_MAT(F_) || !CV_IS_MAT(points1_) || !CV_IS_MAT(points2_) )
CV_Error( CV_StsUnsupportedFormat, "Input parameters must be matrices" );
if (!( F_->cols == 3 && F_->rows == 3))
CV_Error( CV_StsUnmatchedSizes, "The fundamental matrix must be a 3x3 matrix");
if (!(((F_->type & CV_MAT_TYPE_MASK) >> 3) == 0 ))
CV_Error( CV_StsUnsupportedFormat, "The fundamental matrix must be a single-channel matrix" );
if (!(points1_->rows == 1 && points2_->rows == 1 && points1_->cols == points2_->cols))
CV_Error( CV_StsUnmatchedSizes, "The point-matrices must have two rows, and an equal number of columns" );
if (((points1_->type & CV_MAT_TYPE_MASK) >> 3) != 1 )
CV_Error( CV_StsUnmatchedSizes, "The first set of points must contain two channels; one for x and one for y" );
if (((points2_->type & CV_MAT_TYPE_MASK) >> 3) != 1 )
CV_Error( CV_StsUnmatchedSizes, "The second set of points must contain two channels; one for x and one for y" );
if (new_points1 != NULL) {
CV_Assert(CV_IS_MAT(new_points1));
if (new_points1->cols != points1_->cols || new_points1->rows != 1)
CV_Error( CV_StsUnmatchedSizes, "The first output matrix must have the same dimensions as the input matrices" );
if (CV_MAT_CN(new_points1->type) != 2)
CV_Error( CV_StsUnsupportedFormat, "The first output matrix must have two channels; one for x and one for y" );
}
if (new_points2 != NULL) {
CV_Assert(CV_IS_MAT(new_points2));
if (new_points2->cols != points2_->cols || new_points2->rows != 1)
CV_Error( CV_StsUnmatchedSizes, "The second output matrix must have the same dimensions as the input matrices" );
if (CV_MAT_CN(new_points2->type) != 2)
CV_Error( CV_StsUnsupportedFormat, "The second output matrix must have two channels; one for x and one for y" );
}
// Make sure F uses double precision
F = cvCreateMat(3,3,CV_64FC1);
cvConvert(F_, F);
// Make sure points1 uses double precision
points1 = cvCreateMat(points1_->rows,points1_->cols,CV_64FC2);
cvConvert(points1_, points1);
// Make sure points2 uses double precision
points2 = cvCreateMat(points2_->rows,points2_->cols,CV_64FC2);
cvConvert(points2_, points2);
tmp33 = cvCreateMat(3,3,CV_64FC1);
tmp31 = cvCreateMat(3,1,CV_64FC1), tmp31_2 = cvCreateMat(3,1,CV_64FC1);
T1i = cvCreateMat(3,3,CV_64FC1), T2i = cvCreateMat(3,3,CV_64FC1);
R1 = cvCreateMat(3,3,CV_64FC1), R2 = cvCreateMat(3,3,CV_64FC1);
TFT = cvCreateMat(3,3,CV_64FC1), TFTt = cvCreateMat(3,3,CV_64FC1), RTFTR = cvCreateMat(3,3,CV_64FC1);
U = cvCreateMat(3,3,CV_64FC1);
S = cvCreateMat(3,3,CV_64FC1);
V = cvCreateMat(3,3,CV_64FC1);
e1 = cvCreateMat(3,1,CV_64FC1), e2 = cvCreateMat(3,1,CV_64FC1);
double x1, y1, x2, y2;
double scale;
double f1, f2, a, b, c, d;
polynomial = cvCreateMat(1,7,CV_64FC1);
result = cvCreateMat(1,6,CV_64FC2);
double t_min, s_val, t, s;
for (int p = 0; p < points1->cols; ++p) {
// Replace F by T2-t * F * T1-t
x1 = points1->data.db[p*2];
y1 = points1->data.db[p*2+1];
x2 = points2->data.db[p*2];
y2 = points2->data.db[p*2+1];
cvSetZero(T1i);
cvSetReal2D(T1i,0,0,1);
cvSetReal2D(T1i,1,1,1);
cvSetReal2D(T1i,2,2,1);
cvSetReal2D(T1i,0,2,x1);
cvSetReal2D(T1i,1,2,y1);
cvSetZero(T2i);
cvSetReal2D(T2i,0,0,1);
cvSetReal2D(T2i,1,1,1);
cvSetReal2D(T2i,2,2,1);
cvSetReal2D(T2i,0,2,x2);
cvSetReal2D(T2i,1,2,y2);
cvGEMM(T2i,F,1,0,0,tmp33,CV_GEMM_A_T);
cvSetZero(TFT);
cvGEMM(tmp33,T1i,1,0,0,TFT);
// Compute the right epipole e1 from F * e1 = 0
cvSetZero(U);
cvSetZero(S);
cvSetZero(V);
cvSVD(TFT,S,U,V);
scale = sqrt(cvGetReal2D(V,0,2)*cvGetReal2D(V,0,2) + cvGetReal2D(V,1,2)*cvGetReal2D(V,1,2));
cvSetReal2D(e1,0,0,cvGetReal2D(V,0,2)/scale);
cvSetReal2D(e1,1,0,cvGetReal2D(V,1,2)/scale);
cvSetReal2D(e1,2,0,cvGetReal2D(V,2,2)/scale);
if (cvGetReal2D(e1,2,0) < 0) {
cvSetReal2D(e1,0,0,-cvGetReal2D(e1,0,0));
cvSetReal2D(e1,1,0,-cvGetReal2D(e1,1,0));
cvSetReal2D(e1,2,0,-cvGetReal2D(e1,2,0));
}
// Compute the left epipole e2 from e2' * F = 0 => F' * e2 = 0
cvSetZero(TFTt);
cvTranspose(TFT, TFTt);
cvSetZero(U);
cvSetZero(S);
cvSetZero(V);
cvSVD(TFTt,S,U,V);
cvSetZero(e2);
scale = sqrt(cvGetReal2D(V,0,2)*cvGetReal2D(V,0,2) + cvGetReal2D(V,1,2)*cvGetReal2D(V,1,2));
cvSetReal2D(e2,0,0,cvGetReal2D(V,0,2)/scale);
cvSetReal2D(e2,1,0,cvGetReal2D(V,1,2)/scale);
cvSetReal2D(e2,2,0,cvGetReal2D(V,2,2)/scale);
if (cvGetReal2D(e2,2,0) < 0) {
cvSetReal2D(e2,0,0,-cvGetReal2D(e2,0,0));
cvSetReal2D(e2,1,0,-cvGetReal2D(e2,1,0));
cvSetReal2D(e2,2,0,-cvGetReal2D(e2,2,0));
}
// Replace F by R2 * F * R1'
cvSetZero(R1);
cvSetReal2D(R1,0,0,cvGetReal2D(e1,0,0));
cvSetReal2D(R1,0,1,cvGetReal2D(e1,1,0));
cvSetReal2D(R1,1,0,-cvGetReal2D(e1,1,0));
cvSetReal2D(R1,1,1,cvGetReal2D(e1,0,0));
cvSetReal2D(R1,2,2,1);
cvSetZero(R2);
cvSetReal2D(R2,0,0,cvGetReal2D(e2,0,0));
cvSetReal2D(R2,0,1,cvGetReal2D(e2,1,0));
cvSetReal2D(R2,1,0,-cvGetReal2D(e2,1,0));
cvSetReal2D(R2,1,1,cvGetReal2D(e2,0,0));
cvSetReal2D(R2,2,2,1);
cvGEMM(R2,TFT,1,0,0,tmp33);
cvGEMM(tmp33,R1,1,0,0,RTFTR,CV_GEMM_B_T);
// Set f1 = e1(3), f2 = e2(3), a = F22, b = F23, c = F32, d = F33
f1 = cvGetReal2D(e1,2,0);
f2 = cvGetReal2D(e2,2,0);
a = cvGetReal2D(RTFTR,1,1);
b = cvGetReal2D(RTFTR,1,2);
c = cvGetReal2D(RTFTR,2,1);
d = cvGetReal2D(RTFTR,2,2);
// Form the polynomial g(t) = k6*t⁶ + k5*t⁵ + k4*t⁴ + k3*t³ + k2*t² + k1*t + k0
// from f1, f2, a, b, c and d
cvSetReal2D(polynomial,0,6,( +b*c*c*f1*f1*f1*f1*a-a*a*d*f1*f1*f1*f1*c ));
cvSetReal2D(polynomial,0,5,( +f2*f2*f2*f2*c*c*c*c+2*a*a*f2*f2*c*c-a*a*d*d*f1*f1*f1*f1+b*b*c*c*f1*f1*f1*f1+a*a*a*a ));
cvSetReal2D(polynomial,0,4,( +4*a*a*a*b+2*b*c*c*f1*f1*a+4*f2*f2*f2*f2*c*c*c*d+4*a*b*f2*f2*c*c+4*a*a*f2*f2*c*d-2*a*a*d*f1*f1*c-a*d*d*f1*f1*f1*f1*b+b*b*c*f1*f1*f1*f1*d ));
cvSetReal2D(polynomial,0,3,( +6*a*a*b*b+6*f2*f2*f2*f2*c*c*d*d+2*b*b*f2*f2*c*c+2*a*a*f2*f2*d*d-2*a*a*d*d*f1*f1+2*b*b*c*c*f1*f1+8*a*b*f2*f2*c*d ));
cvSetReal2D(polynomial,0,2,( +4*a*b*b*b+4*b*b*f2*f2*c*d+4*f2*f2*f2*f2*c*d*d*d-a*a*d*c+b*c*c*a+4*a*b*f2*f2*d*d-2*a*d*d*f1*f1*b+2*b*b*c*f1*f1*d ));
cvSetReal2D(polynomial,0,1,( +f2*f2*f2*f2*d*d*d*d+b*b*b*b+2*b*b*f2*f2*d*d-a*a*d*d+b*b*c*c ));
cvSetReal2D(polynomial,0,0,( -a*d*d*b+b*b*c*d ));
// Solve g(t) for t to get 6 roots
cvSetZero(result);
cvSolvePoly(polynomial, result, 100, 20);
// Evaluate the cost function s(t) at the real part of the 6 roots
t_min = DBL_MAX;
s_val = 1./(f1*f1) + (c*c)/(a*a+f2*f2*c*c);
for (int ti = 0; ti < 6; ++ti) {
t = result->data.db[2*ti];
s = (t*t)/(1 + f1*f1*t*t) + ((c*t + d)*(c*t + d))/((a*t + b)*(a*t + b) + f2*f2*(c*t + d)*(c*t + d));
if (s < s_val) {
s_val = s;
t_min = t;
}
}
// find the optimal x1 and y1 as the points on l1 and l2 closest to the origin
tmp31->data.db[0] = t_min*t_min*f1;
tmp31->data.db[1] = t_min;
tmp31->data.db[2] = t_min*t_min*f1*f1+1;
tmp31->data.db[0] /= tmp31->data.db[2];
tmp31->data.db[1] /= tmp31->data.db[2];
tmp31->data.db[2] /= tmp31->data.db[2];
cvGEMM(T1i,R1,1,0,0,tmp33,CV_GEMM_B_T);
cvGEMM(tmp33,tmp31,1,0,0,tmp31_2);
x1 = tmp31_2->data.db[0];
y1 = tmp31_2->data.db[1];
tmp31->data.db[0] = f2*pow(c*t_min+d,2);
tmp31->data.db[1] = -(a*t_min+b)*(c*t_min+d);
tmp31->data.db[2] = f2*f2*pow(c*t_min+d,2) + pow(a*t_min+b,2);
tmp31->data.db[0] /= tmp31->data.db[2];
tmp31->data.db[1] /= tmp31->data.db[2];
tmp31->data.db[2] /= tmp31->data.db[2];
cvGEMM(T2i,R2,1,0,0,tmp33,CV_GEMM_B_T);
cvGEMM(tmp33,tmp31,1,0,0,tmp31_2);
x2 = tmp31_2->data.db[0];
y2 = tmp31_2->data.db[1];
// Return the points in the matrix format that the user wants
points1->data.db[p*2] = x1;
points1->data.db[p*2+1] = y1;
points2->data.db[p*2] = x2;
points2->data.db[p*2+1] = y2;
}
if( new_points1 )
cvConvert( points1, new_points1 );
if( new_points2 )
cvConvert( points2, new_points2 );
}

410
modules/calib3d/src/undistort.cpp Normal file
View File

@@ -0,0 +1,410 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
namespace cv
{
Mat getDefaultNewCameraMatrix( const Mat& cameraMatrix, Size imgsize,
bool centerPrincipalPoint )
{
if( !centerPrincipalPoint && cameraMatrix.type() == CV_64F )
return cameraMatrix;
Mat newCameraMatrix;
cameraMatrix.convertTo(newCameraMatrix, CV_64F);
if( centerPrincipalPoint )
{
((double*)newCameraMatrix.data)[2] = (imgsize.width-1)*0.5;
((double*)newCameraMatrix.data)[5] = (imgsize.height-1)*0.5;
}
return newCameraMatrix;
}
void initUndistortRectifyMap( const Mat& _cameraMatrix, const Mat& _distCoeffs,
const Mat& matR, const Mat& _newCameraMatrix,
Size size, int m1type, Mat& map1, Mat& map2 )
{
if( m1type <= 0 )
m1type = CV_16SC2;
CV_Assert( m1type == CV_16SC2 || m1type == CV_32FC1 || m1type == CV_32FC2 );
map1.create( size, m1type );
if( m1type != CV_32FC2 )
map2.create( size, m1type == CV_16SC2 ? CV_16UC1 : CV_32FC1 );
else
map2.release();
Mat_<double> R = Mat_<double>::eye(3, 3), distCoeffs;
Mat_<double> A = Mat_<double>(_cameraMatrix), Ar;
if( _newCameraMatrix.data )
Ar = Mat_<double>(_newCameraMatrix);
else
Ar = getDefaultNewCameraMatrix( A, size, true );
if( matR.data )
R = Mat_<double>(matR);
if( _distCoeffs.data )
distCoeffs = Mat_<double>(_distCoeffs);
else
{
distCoeffs.create(5, 1);
distCoeffs = 0.;
}
CV_Assert( A.size() == Size(3,3) && A.size() == R.size() );
CV_Assert( Ar.size() == Size(3,3) || Ar.size() == Size(4, 3));
Mat_<double> iR = (Ar.colRange(0,3)*R).inv(DECOMP_LU);
const double* ir = &iR(0,0);
double u0 = A(0, 2), v0 = A(1, 2);
double fx = A(0, 0), fy = A(1, 1);
CV_Assert( distCoeffs.size() == Size(1, 4) || distCoeffs.size() == Size(1, 5) ||
distCoeffs.size() == Size(4, 1) || distCoeffs.size() == Size(5, 1));
if( distCoeffs.rows != 1 && !distCoeffs.isContinuous() )
distCoeffs = distCoeffs.t();
double k1 = ((double*)distCoeffs.data)[0];
double k2 = ((double*)distCoeffs.data)[1];
double p1 = ((double*)distCoeffs.data)[2];
double p2 = ((double*)distCoeffs.data)[3];
double k3 = distCoeffs.cols + distCoeffs.rows - 1 == 5 ? ((double*)distCoeffs.data)[4] : 0.;
for( int i = 0; i < size.height; i++ )
{
float* m1f = (float*)(map1.data + map1.step*i);
float* m2f = (float*)(map2.data + map2.step*i);
short* m1 = (short*)m1f;
ushort* m2 = (ushort*)m2f;
double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _w = i*ir[7] + ir[8];
for( int j = 0; j < size.width; j++, _x += ir[0], _y += ir[3], _w += ir[6] )
{
double w = 1./_w, x = _x*w, y = _y*w;
double x2 = x*x, y2 = y*y;
double r2 = x2 + y2, _2xy = 2*x*y;
double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2;
double u = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + u0;
double v = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + v0;
if( m1type == CV_16SC2 )
{
int iu = saturate_cast<int>(u*INTER_TAB_SIZE);
int iv = saturate_cast<int>(v*INTER_TAB_SIZE);
m1[j*2] = (short)(iu >> INTER_BITS);
m1[j*2+1] = (short)(iv >> INTER_BITS);
m2[j] = (ushort)((iv & (INTER_TAB_SIZE-1))*INTER_TAB_SIZE + (iu & (INTER_TAB_SIZE-1)));
}
else if( m1type == CV_32FC1 )
{
m1f[j] = (float)u;
m2f[j] = (float)v;
}
else
{
m1f[j*2] = (float)u;
m1f[j*2+1] = (float)v;
}
}
}
}
void undistort( const Mat& src, Mat& dst, const Mat& _cameraMatrix,
const Mat& _distCoeffs, const Mat& _newCameraMatrix )
{
dst.create( src.size(), src.type() );
CV_Assert( dst.data != src.data );
int stripe_size0 = std::min(std::max(1, (1 << 12) / std::max(src.cols, 1)), src.rows);
Mat map1(stripe_size0, src.cols, CV_16SC2), map2(stripe_size0, src.cols, CV_16UC1);
Mat_<double> A, distCoeffs, Ar, I = Mat_<double>::eye(3,3);
_cameraMatrix.convertTo(A, CV_64F);
if( _distCoeffs.data )
distCoeffs = Mat_<double>(_distCoeffs);
else
{
distCoeffs.create(5, 1);
distCoeffs = 0.;
}
if( _newCameraMatrix.data )
_newCameraMatrix.convertTo(Ar, CV_64F);
else
A.copyTo(Ar);
double v0 = Ar(1, 2);
for( int y = 0; y < src.rows; y += stripe_size0 )
{
int stripe_size = std::min( stripe_size0, src.rows - y );
Ar(1, 2) = v0 - y;
Mat map1_part = map1.rowRange(0, stripe_size),
map2_part = map2.rowRange(0, stripe_size),
dst_part = dst.rowRange(y, y + stripe_size);
initUndistortRectifyMap( A, distCoeffs, I, Ar, Size(src.cols, stripe_size),
map1_part.type(), map1_part, map2_part );
remap( src, dst_part, map1_part, map2_part, INTER_LINEAR, BORDER_CONSTANT );
}
}
}
CV_IMPL void
cvUndistort2( const CvArr* srcarr, CvArr* dstarr, const CvMat* Aarr, const CvMat* dist_coeffs, const CvMat* newAarr )
{
cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr), dst0 = dst;
cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs), newA;
if( newAarr )
newA = cv::cvarrToMat(newAarr);
CV_Assert( src.size() == dst.size() && src.type() == dst.type() );
cv::undistort( src, dst, A, distCoeffs, newA );
}
CV_IMPL void cvInitUndistortMap( const CvMat* Aarr, const CvMat* dist_coeffs,
CvArr* mapxarr, CvArr* mapyarr )
{
cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs);
cv::Mat mapx = cv::cvarrToMat(mapxarr), mapy, mapx0 = mapx, mapy0;
if( mapyarr )
mapy0 = mapy = cv::cvarrToMat(mapyarr);
cv::initUndistortRectifyMap( A, distCoeffs, cv::Mat(), A,
mapx.size(), mapx.type(), mapx, mapy );
CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data );
}
void
cvInitUndistortRectifyMap( const CvMat* Aarr, const CvMat* dist_coeffs,
const CvMat *Rarr, const CvMat* ArArr, CvArr* mapxarr, CvArr* mapyarr )
{
cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs, R, Ar;
cv::Mat mapx = cv::cvarrToMat(mapxarr), mapy, mapx0 = mapx, mapy0;
if( mapyarr )
mapy0 = mapy = cv::cvarrToMat(mapyarr);
if( dist_coeffs )
distCoeffs = cv::cvarrToMat(dist_coeffs);
if( Rarr )
R = cv::cvarrToMat(Rarr);
if( ArArr )
Ar = cv::cvarrToMat(ArArr);
cv::initUndistortRectifyMap( A, distCoeffs, R, Ar, mapx.size(), mapx.type(), mapx, mapy );
CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data );
}
void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
const CvMat* _distCoeffs,
const CvMat* matR, const CvMat* matP )
{
double A[3][3], RR[3][3], k[5]={0,0,0,0,0}, fx, fy, ifx, ify, cx, cy;
CvMat matA=cvMat(3, 3, CV_64F, A), _Dk;
CvMat _RR=cvMat(3, 3, CV_64F, RR);
const CvPoint2D32f* srcf;
const CvPoint2D64f* srcd;
CvPoint2D32f* dstf;
CvPoint2D64f* dstd;
int stype, dtype;
int sstep, dstep;
int i, j, n, iters = 1;
CV_Assert( CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&
(_src->rows == 1 || _src->cols == 1) &&
(_dst->rows == 1 || _dst->cols == 1) &&
_src->cols + _src->rows - 1 == _dst->rows + _dst->cols - 1 &&
(CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) &&
(CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2));
CV_Assert( CV_IS_MAT(_cameraMatrix) &&
_cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 );
cvConvert( _cameraMatrix, &matA );
if( _distCoeffs )
{
CV_Assert( CV_IS_MAT(_distCoeffs) &&
(_distCoeffs->rows == 1 || _distCoeffs->cols == 1) &&
(_distCoeffs->rows*_distCoeffs->cols == 4 ||
_distCoeffs->rows*_distCoeffs->cols == 5) );
_Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k);
cvConvert( _distCoeffs, &_Dk );
iters = 5;
}
if( matR )
{
CV_Assert( CV_IS_MAT(matR) && matR->rows == 3 && matR->cols == 3 );
cvConvert( matR, &_RR );
}
else
cvSetIdentity(&_RR);
if( matP )
{
double PP[3][3];
CvMat _P3x3, _PP=cvMat(3, 3, CV_64F, PP);
CV_Assert( CV_IS_MAT(matP) && matP->rows == 3 && (matP->cols == 3 || matP->cols == 4));
cvConvert( cvGetCols(matP, &_P3x3, 0, 3), &_PP );
cvMatMul( &_PP, &_RR, &_RR );
}
srcf = (const CvPoint2D32f*)_src->data.ptr;
srcd = (const CvPoint2D64f*)_src->data.ptr;
dstf = (CvPoint2D32f*)_dst->data.ptr;
dstd = (CvPoint2D64f*)_dst->data.ptr;
stype = CV_MAT_TYPE(_src->type);
dtype = CV_MAT_TYPE(_dst->type);
sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype);
dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype);
n = _src->rows + _src->cols - 1;
fx = A[0][0];
fy = A[1][1];
ifx = 1./fx;
ify = 1./fy;
cx = A[0][2];
cy = A[1][2];
for( i = 0; i < n; i++ )
{
double x, y, x0, y0;
if( stype == CV_32FC2 )
{
x = srcf[i*sstep].x;
y = srcf[i*sstep].y;
}
else
{
x = srcd[i*sstep].x;
y = srcd[i*sstep].y;
}
x0 = x = (x - cx)*ifx;
y0 = y = (y - cy)*ify;
// compensate distortion iteratively
for( j = 0; j < iters; j++ )
{
double r2 = x*x + y*y;
double icdist = 1./(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
x = (x0 - deltaX)*icdist;
y = (y0 - deltaY)*icdist;
}
double xx = RR[0][0]*x + RR[0][1]*y + RR[0][2];
double yy = RR[1][0]*x + RR[1][1]*y + RR[1][2];
double ww = 1./(RR[2][0]*x + RR[2][1]*y + RR[2][2]);
x = xx*ww;
y = yy*ww;
if( dtype == CV_32FC2 )
{
dstf[i*dstep].x = (float)x;
dstf[i*dstep].y = (float)y;
}
else
{
dstd[i*dstep].x = x;
dstd[i*dstep].y = y;
}
}
}
void cv::undistortPoints( const Mat& src, Mat& dst,
const Mat& cameraMatrix, const Mat& distCoeffs,
const Mat& R, const Mat& P )
{
CV_Assert( src.isContinuous() && src.depth() == CV_32F &&
((src.rows == 1 && src.channels() == 2) || src.cols*src.channels() == 2));
dst.create(src.size(), src.type());
CvMat _src = src, _dst = dst, _cameraMatrix = cameraMatrix;
CvMat matR, matP, _distCoeffs, *pR=0, *pP=0, *pD=0;
if( R.data )
pR = &(matR = R);
if( P.data )
pP = &(matP = P);
if( distCoeffs.data )
pD = &(_distCoeffs = distCoeffs);
cvUndistortPoints(&_src, &_dst, &_cameraMatrix, pD, pR, pP);
}
void cv::undistortPoints( const Mat& src, std::vector<Point2f>& dst,
const Mat& cameraMatrix, const Mat& distCoeffs,
const Mat& R, const Mat& P )
{
size_t sz = src.cols*src.rows*src.channels()/2;
CV_Assert( src.isContinuous() && src.depth() == CV_32F &&
((src.rows == 1 && src.channels() == 2) || src.cols*src.channels() == 2));
dst.resize(sz);
CvMat _src = src, _dst = Mat(dst), _cameraMatrix = cameraMatrix;
CvMat matR, matP, _distCoeffs, *pR=0, *pP=0, *pD=0;
if( R.data )
pR = &(matR = R);
if( P.data )
pP = &(matP = P);
if( distCoeffs.data )
pD = &(_distCoeffs = distCoeffs);
cvUndistortPoints(&_src, &_dst, &_cameraMatrix, pD, pR, pP);
}
/* End of file */