Camera Calibration and 3D Reconstruction¶

The functions in this section use the so-called pinhole camera model. That is, a scene view is formed by projecting 3D points into the image plane using a perspective transformation.

or

Where are the coordinates of a 3D point in the world coordinate space, are the coordinates of the projection point in pixels. is called a camera matrix, or a matrix of intrinsic parameters. is a principal point (that is usually at the image center), and are the focal lengths expressed in pixel-related units. Thus, if an image from camera is scaled by some factor, all of these parameters should be scaled (multiplied/divided, respectively) by the same factor. The matrix of intrinsic parameters does not depend on the scene viewed and, once estimated, can be re-used (as long as the focal length is fixed (in case of zoom lens)). The joint rotation-translation matrix is called a matrix of extrinsic parameters. It is used to describe the camera motion around a static scene, or vice versa, rigid motion of an object in front of still camera. That is, translates coordinates of a point to some coordinate system, fixed with respect to the camera. The transformation above is equivalent to the following (when ):

Real lenses usually have some distortion, mostly radial distortion and slight tangential distortion. So, the above model is extended as:

, , are radial distortion coefficients, , are tangential distortion coefficients. Higher-order coefficients are not considered in OpenCV. In the functions below the coefficients are passed or returned as

vector. That is, if the vector contains 4 elements, it means that . The distortion coefficients do not depend on the scene viewed, thus they also belong to the intrinsic camera parameters. And they remain the same regardless of the captured image resolution. That is, if, for example, a camera has been calibrated on images of resolution, absolutely the same distortion coefficients can be used for images of resolution from the same camera (while , , and need to be scaled appropriately).

The functions below use the above model to

• Project 3D points to the image plane given intrinsic and extrinsic parameters
• Compute extrinsic parameters given intrinsic parameters, a few 3D points and their projections.
• Estimate intrinsic and extrinsic camera parameters from several views of a known calibration pattern (i.e. every view is described by several 3D-2D point correspondences).
• Estimate the relative position and orientation of the stereo camera “heads” and compute the rectification transformation that makes the camera optical axes parallel.

cv::calibrateCamera¶

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)

Finds the camera intrinsic and extrinsic parameters from several views of a calibration pattern.

Parameters: objectPoints – The vector of vectors of points on the calibration pattern in its coordinate system, one vector per view. If the same calibration pattern is shown in each view and it’s fully visible then all the vectors will be the same, although it is possible to use partially occluded patterns, or even different patterns in different views - then the vectors will be different. The points are 3D, but since they are in the pattern coordinate system, then if the rig is planar, it may have sense to put the model to the XY coordinate plane, so that Z-coordinate of each input object point is 0 imagePoints – The vector of vectors of the object point projections on the calibration pattern views, one vector per a view. The projections must be in the same order as the corresponding object points. imageSize – Size of the image, used only to initialize the intrinsic camera matrix cameraMatrix – The output 3x3 floating-point camera matrix . If CV_CALIB_USE_INTRINSIC_GUESS and/or CV_CALIB_FIX_ASPECT_RATIO are specified, some or all of fx, fy, cx, cy must be initialized before calling the function distCoeffs – The output 5x1 or 1x5 vector of distortion coefficients

.

Parameters: rvecs – The output vector of rotation vectors (see Rodrigues2 ), estimated for each pattern view. That is, each k-th rotation vector together with the corresponding k-th translation vector (see the next output parameter description) brings the calibration pattern from the model coordinate space (in which object points are specified) to the world coordinate space, i.e. real position of the calibration pattern in the k-th pattern view (k=0.. M -1) tvecs – The output vector of translation vectors, estimated for each pattern view. flags – Different flags, may be 0 or combination of the following values: CV_CALIB_USE_INTRINSIC_GUESS cameraMatrix contains the valid initial values of fx, fy, cx, cy that are optimized further. Otherwise, (cx, cy) is initially set to the image center ( imageSize is used here), and focal distances are computed in some least-squares fashion. Note, that if intrinsic parameters are known, there is no need to use this function just to estimate the extrinsic parameters. Use FindExtrinsicCameraParams2 instead. CV_CALIB_FIX_PRINCIPAL_POINT The principal point is not changed during the global optimization, it stays at the center or at the other location specified when CV_CALIB_USE_INTRINSIC_GUESS is set too. CV_CALIB_FIX_ASPECT_RATIO The functions considers only fy as a free parameter, the ratio fx/fy stays the same as in the input cameraMatrix . When CV_CALIB_USE_INTRINSIC_GUESS is not set, the actual input values of fx and fy are ignored, only their ratio is computed and used further. CV_CALIB_ZERO_TANGENT_DIST Tangential distortion coefficients will be set to zeros and stay zero.

The function estimates the intrinsic camera parameters and extrinsic parameters for each of the views. The coordinates of 3D object points and their correspondent 2D projections in each view must be specified. That may be achieved by using an object with known geometry and easily detectable feature points. Such an object is called a calibration rig or calibration pattern, and OpenCV has built-in support for a chessboard as a calibration rig (see FindChessboardCorners ). Currently, initialization of intrinsic parameters (when CV_CALIB_USE_INTRINSIC_GUESS is not set) is only implemented for planar calibration patterns (where z-coordinates of the object points must be all 0’s). 3D calibration rigs can also be used as long as initial cameraMatrix is provided.

The algorithm does the following:

1. First, it computes the initial intrinsic parameters (the option only available for planar calibration patterns) or reads them from the input parameters. The distortion coefficients are all set to zeros initially (unless some of CV_CALIB_FIX_K? are specified).
2. The initial camera pose is estimated as if the intrinsic parameters have been already known. This is done using FindExtrinsicCameraParams2
3. After that the global Levenberg-Marquardt optimization algorithm is run to minimize the reprojection error, i.e. the total sum of squared distances between the observed feature points imagePoints and the projected (using the current estimates for camera parameters and the poses) object points objectPoints ; see ProjectPoints2 .

The function returns the final re-projection error. Note: if you’re using a non-square (=non-NxN) grid and findChessboardCorners() for calibration, and calibrateCamera returns bad values (i.e. zero distortion coefficients, an image center very far from , and / or large differences between and (ratios of 10:1 or more)), then you’ve probably used patternSize=cvSize(rows,cols) , but should use patternSize=cvSize(cols,rows) in FindChessboardCorners .

cv::calibrationMatrixValues¶

void calibrationMatrixValues(const Mat& cameraMatrix, Size imageSize, double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength, Point2d& principalPoint, double& aspectRatio)

Computes some useful camera characteristics from the camera matrix

Parameters: cameraMatrix – The input camera matrix that can be estimated by calibrateCamera() or stereoCalibrate() imageSize – The input image size in pixels apertureWidth – Physical width of the sensor apertureHeight – Physical height of the sensor fovx – The output field of view in degrees along the horizontal sensor axis fovy – The output field of view in degrees along the vertical sensor axis focalLength – The focal length of the lens in mm principalPoint – The principal point in pixels aspectRatio –

The function computes various useful camera characteristics from the previously estimated camera matrix.

cv::composeRT¶

void composeRT(const Mat& rvec1, const Mat& tvec1, const Mat& rvec2, const Mat& tvec2, Mat& rvec3, Mat& tvec3)
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)

Combines two rotation-and-shift transformations

Parameters: rvec1 – The first rotation vector tvec1 – The first translation vector rvec2 – The second rotation vector tvec2 – The second translation vector rvec3 – The output rotation vector of the superposition tvec3 – The output translation vector of the superposition d??d?? – The optional output derivatives of rvec3 or tvec3 w.r.t. rvec? or tvec?

The functions compute:

where denotes a rotation vector to rotation matrix transformation, and denotes the inverse transformation, see Rodrigues() .

Also, the functions can compute the derivatives of the output vectors w.r.t the input vectors (see matMulDeriv() ). The functions are used inside stereoCalibrate() but can also be used in your own code where Levenberg-Marquardt or another gradient-based solver is used to optimize a function that contains matrix multiplication.

cv::computeCorrespondEpilines¶

void computeCorrespondEpilines(const Mat& points, int whichImage, const Mat& F, vector<Vec3f>& lines)

For points in one image of a stereo pair, computes the corresponding epilines in the other image.

Parameters: points – The input points. or matrix of type CV_32FC2 or vector whichImage – Index of the image (1 or 2) that contains the points F – The fundamental matrix that can be estimated using FindFundamentalMat or StereoRectify . lines – The output vector of the corresponding to the points epipolar lines in the other image. Each line is encoded by 3 numbers

For every point in one of the two images of a stereo-pair the function finds the equation of the corresponding epipolar line in the other image.

From the fundamental matrix definition (see FindFundamentalMat ), line in the second image for the point in the first image (i.e. when whichImage=1 ) is computed as:

and, vice versa, when whichImage=2 , is computed from as:

Line coefficients are defined up to a scale. They are normalized, such that .

cv::convertPointsHomogeneous¶

void convertPointsHomogeneous(const Mat& src, vector<Point3f>& dst ) void convertPointsHomogeneous( const Mat& src, vector<Point2f>& dst)

Convert points to/from homogeneous coordinates.

Parameters: src – The input array or vector of 2D or 3D points dst – The output vector of 3D or 2D points, respectively

The functions convert 2D or 3D points from/to homogeneous coordinates, or simply copy or transpose the array. If the input array dimensionality is larger than the output, each coordinate is divided by the last coordinate:

If the output array dimensionality is larger, an extra 1 is appended to each point. Otherwise, the input array is simply copied (with optional transposition) to the output.

cv::decomposeProjectionMatrix¶

void decomposeProjectionMatrix(const Mat& projMatrix, Mat& cameraMatrix, Mat& rotMatrix, Mat& transVect)
void decomposeProjectionMatrix(const Mat& projMatrix, Mat& cameraMatrix, Mat& rotMatrix, Mat& transVect, Mat& rotMatrixX, Mat& rotMatrixY, Mat& rotMatrixZ, Vec3d& eulerAngles)

Decomposes the projection matrix into a rotation matrix and a camera matrix.

Parameters: projMatrix – The 3x4 input projection matrix P cameraMatrix – The output 3x3 camera matrix K rotMatrix – The output 3x3 external rotation matrix R transVect – The output 4x1 translation vector T rotMatrX – Optional 3x3 rotation matrix around x-axis rotMatrY – Optional 3x3 rotation matrix around y-axis rotMatrZ – Optional 3x3 rotation matrix around z-axis eulerAngles – Optional 3 points containing the three Euler angles of rotation

The function computes a decomposition of a projection matrix into a calibration and a rotation matrix and the position of the camera.

It optionally returns three rotation matrices, one for each axis, and the three Euler angles that could be used in OpenGL.

The function is based on RQDecomp3x3 .

cv::drawChessboardCorners¶

void drawChessboardCorners(Mat& image, Size patternSize, const Mat& corners, bool patternWasFound)

Renders the detected chessboard corners.

Parameters: image – The destination image; it must be an 8-bit color image patternSize – The number of inner corners per chessboard row and column. (patternSize = cvSize(points _ per _ row,points _ per _ colum) = cvSize(columns,rows) ) corners – The array of corners detected patternWasFound – Indicates whether the complete board was found or not . One may just pass the return value FindChessboardCorners here

The function draws the individual chessboard corners detected as red circles if the board was not found or as colored corners connected with lines if the board was found.

cv::findChessboardCorners¶

bool findChessboardCorners(const Mat& image, Size patternSize, vector<Point2f>& corners, int flags=CV_CALIB_CB_ADAPTIVE_THRESH+ CV_CALIB_CB_NORMALIZE_IMAGE)

Finds the positions of the internal corners of the chessboard.

Parameters: image – Source chessboard view; it must be an 8-bit grayscale or color image patternSize – The number of inner corners per chessboard row and column ( patternSize = cvSize(points _ per _ row,points _ per _ colum) = cvSize(columns,rows) ) corners – The output array of corners detected flags – Various operation flags, can be 0 or a combination of the following values: CV_CALIB_CB_ADAPTIVE_THRESH use adaptive thresholding to convert the image to black and white, rather than a fixed threshold level (computed from the average image brightness). CV_CALIB_CB_NORMALIZE_IMAGE normalize the image gamma with EqualizeHist before applying fixed or adaptive thresholding. CV_CALIB_CB_FILTER_QUADS use additional criteria (like contour area, perimeter, square-like shape) to filter out false quads that are extracted at the contour retrieval stage.

The function attempts to determine whether the input image is a view of the chessboard pattern and locate the internal chessboard corners. The function returns a non-zero value if all of the corners have been found and they have been placed in a certain order (row by row, left to right in every row), otherwise, if the function fails to find all the corners or reorder them, it returns 0. For example, a regular chessboard has 8 x 8 squares and 7 x 7 internal corners, that is, points, where the black squares touch each other. The coordinates detected are approximate, and to determine their position more accurately, the user may use the function FindCornerSubPix .

Note: the function requires some white space (like a square-thick border, the wider the better) around the board to make the detection more robust in various environment (otherwise if there is no border and the background is dark, the outer black squares could not be segmented properly and so the square grouping and ordering algorithm will fail).

cv::solvePnP¶

void solvePnP(const Mat& objectPoints, const Mat& imagePoints, const Mat& cameraMatrix, const Mat& distCoeffs, Mat& rvec, Mat& tvec, bool useExtrinsicGuess=false)

Finds the object pose from the 3D-2D point correspondences

Parameters: objectPoints – The array of object points in the object coordinate space, 3xN or Nx3 1-channel, or 1xN or Nx1 3-channel, where N is the number of points. Can also pass vector here. imagePoints – The array of corresponding image points, 2xN or Nx2 1-channel or 1xN or Nx1 2-channel, where N is the number of points. Can also pass vector here. cameraMatrix – The input camera matrix distCoeffs – The input 4x1, 1x4, 5x1 or 1x5 vector of distortion coefficients . If it is NULL, all of the distortion coefficients are set to 0 rvec – The output rotation vector (see Rodrigues2 ) that (together with tvec ) brings points from the model coordinate system to the camera coordinate system tvec – The output translation vector useExtrinsicGuess – If true (1), the function will use the provided rvec and tvec as the initial approximations of the rotation and translation vectors, respectively, and will further optimize them.

The function estimates the object pose given a set of object points, their corresponding image projections, as well as the camera matrix and the distortion coefficients. This function finds such a pose that minimizes reprojection error, i.e. the sum of squared distances between the observed projections imagePoints and the projected (using ProjectPoints2 ) objectPoints .

cv::findFundamentalMat¶

Mat findFundamentalMat(const Mat& points1, const Mat& points2, vector<uchar>& status, int method=FM_RANSAC, double param1=3., double param2=0.99)
Mat findFundamentalMat(const Mat& points1, const Mat& points2, int method=FM_RANSAC, double param1=3., double param2=0.99)

Calculates the fundamental matrix from the corresponding points in two images.

Parameters: points1 – Array of N points from the first image. . The point coordinates should be floating-point (single or double precision) points2 – Array of the second image points of the same size and format as points1 method – Method for computing the fundamental matrix CV_FM_7POINT for a 7-point algorithm. CV_FM_8POINT for an 8-point algorithm. CV_FM_RANSAC for the RANSAC algorithm. CV_FM_LMEDS for the LMedS algorithm. param1 – The parameter is used for RANSAC. It is the maximum distance from point to epipolar line in pixels, beyond which the point is considered an outlier and is not used for computing the final fundamental matrix. It can be set to something like 1-3, depending on the accuracy of the point localization, image resolution and the image noise param2 – The parameter is used for RANSAC or LMedS methods only. It specifies the desirable level of confidence (probability) that the estimated matrix is correct status – The output array of N elements, every element of which is set to 0 for outliers and to 1 for the other points. The array is computed only in RANSAC and LMedS methods. For other methods it is set to all 1’s

The epipolar geometry is described by the following equation:

where is fundamental matrix, and are corresponding points in the first and the second images, respectively.

The function calculates the fundamental matrix using one of four methods listed above and returns the found fundamental matrix . Normally just 1 matrix is found, but in the case of 7-point algorithm the function may return up to 3 solutions ( matrix that stores all 3 matrices sequentially).

The calculated fundamental matrix may be passed further to ComputeCorrespondEpilines that finds the epipolar lines corresponding to the specified points. It can also be passed to StereoRectifyUncalibrated to compute the rectification transformation.

// Example. Estimation of fundamental matrix using RANSAC algorithm
int point_count = 100;
vector<Point2f> points1(point_count);
vector<Point2f> points2(point_count);

// initialize the points here ... */
for( int i = 0; i < point_count; i++ )
{
points1[i] = ...;
points2[i] = ...;
}

Mat fundamental_matrix =
findFundamentalMat(points1, points2, FM_RANSAC, 3, 0.99);


cv::findHomography¶

Mat findHomography(const Mat& srcPoints, const Mat& dstPoints, Mat& status, int method=0, double ransacReprojThreshold=0)
Mat findHomography(const Mat& srcPoints, const Mat& dstPoints, vector<uchar>& status, int method=0, double ransacReprojThreshold=0)
Mat findHomography(const Mat& srcPoints, const Mat& dstPoints, int method=0, double ransacReprojThreshold=0)

Finds the perspective transformation between two planes.

Parameters: srcPoints – Coordinates of the points in the original plane, a matrix of type CV_32FC2 or a vector . dstPoints – Coordinates of the points in the target plane, a matrix of type CV_32FC2 or a vector . method – The method used to computed homography matrix; one of the following: 0 a regular method using all the points CV_RANSAC RANSAC-based robust method CV_LMEDS Least-Median robust method ransacReprojThreshold – The maximum allowed reprojection error to treat a point pair as an inlier (used in the RANSAC method only). That is, if then the point is considered an outlier. If srcPoints and dstPoints are measured in pixels, it usually makes sense to set this parameter somewhere in the range 1 to 10. status – The optional output mask set by a robust method ( CV_RANSAC or CV_LMEDS ). Note that the input mask values are ignored.

The functions find and return the perspective transformation between the source and the destination planes:

So that the back-projection error

is minimized. If the parameter method is set to the default value 0, the function uses all the point pairs to compute the initial homography estimate with a simple least-squares scheme.

However, if not all of the point pairs ( , ) fit the rigid perspective transformation (i.e. there are some outliers), this initial estimate will be poor. In this case one can use one of the 2 robust methods. Both methods, RANSAC and LMeDS , try many different random subsets of the corresponding point pairs (of 4 pairs each), estimate the homography matrix using this subset and a simple least-square algorithm and then compute the quality/goodness of the computed homography (which is the number of inliers for RANSAC or the median re-projection error for LMeDs). The best subset is then used to produce the initial estimate of the homography matrix and the mask of inliers/outliers.

Regardless of the method, robust or not, the computed homography matrix is refined further (using inliers only in the case of a robust method) with the Levenberg-Marquardt method in order to reduce the re-projection error even more.

The method RANSAC can handle practically any ratio of outliers, but it needs the threshold to distinguish inliers from outliers. The method LMeDS does not need any threshold, but it works correctly only when there are more than 50 % of inliers. Finally, if you are sure in the computed features, where can be only some small noise present, but no outliers, the default method could be the best choice.

The function is used to find initial intrinsic and extrinsic matrices. Homography matrix is determined up to a scale, thus it is normalized so that .

cv::getDefaultNewCameraMatrix¶

Mat getDefaultNewCameraMatrix(const Mat& cameraMatrix, Size imgSize=Size(), bool centerPrincipalPoint=false)

Returns the default new camera matrix

Parameters: cameraMatrix – The input camera matrix imageSize – The camera view image size in pixels centerPrincipalPoint – Indicates whether in the new camera matrix the principal point should be at the image center or not

The function returns the camera matrix that is either an exact copy of the input cameraMatrix (when centerPrinicipalPoint=false ), or the modified one (when centerPrincipalPoint =true).

In the latter case the new camera matrix will be:

where and are and elements of cameraMatrix , respectively.

By default, the undistortion functions in OpenCV (see initUndistortRectifyMap , undistort ) do not move the principal point. However, when you work with stereo, it’s important to move the principal points in both views to the same y-coordinate (which is required by most of stereo correspondence algorithms), and maybe to the same x-coordinate too. So you can form the new camera matrix for each view, where the principal points will be at the center.

cv::getOptimalNewCameraMatrix¶

Mat getOptimalNewCameraMatrix(const Mat& cameraMatrix, const Mat& distCoeffs, Size imageSize, double alpha, Size newImageSize=Size(), Rect* validPixROI=0)

Returns the new camera matrix based on the free scaling parameter

Parameters: cameraMatrix – The input camera matrix distCoeffs – The input 4x1, 1x4, 5x1 or 1x5 vector of distortion coefficients

.

Parameters: imageSize – The original image size alpha – The free scaling parameter between 0 (when all the pixels in the undistorted image will be valid) and 1 (when all the source image pixels will be retained in the undistorted image); see StereoRectify newCameraMatrix – The output new camera matrix. newImageSize – The image size after rectification. By default it will be set to imageSize . validPixROI – The optional output rectangle that will outline all-good-pixels region in the undistorted image. See roi1, roi2 description in StereoRectify

The function computes and returns the optimal new camera matrix based on the free scaling parameter. By varying this parameter the user may retrieve only sensible pixels alpha=0 , keep all the original image pixels if there is valuable information in the corners alpha=1 , or get something in between. When alpha>0 , the undistortion result will likely have some black pixels corresponding to “virtual” pixels outside of the captured distorted image. The original camera matrix, distortion coefficients, the computed new camera matrix and the newImageSize should be passed to InitUndistortRectifyMap to produce the maps for Remap .

cv::initCameraMatrix2D¶

Mat initCameraMatrix2D(const vector<vector<Point3f> >& objectPoints, const vector<vector<Point2f> >& imagePoints, Size imageSize, double aspectRatio=1.)

Finds the initial camera matrix from the 3D-2D point correspondences

Parameters: objectPoints – The vector of vectors of the object points. See calibrateCamera() imagePoints – The vector of vectors of the corresponding image points. See calibrateCamera() imageSize – The image size in pixels; used to initialize the principal point aspectRatio – If it is zero or negative, both and are estimated independently. Otherwise

The function estimates and returns the initial camera matrix for camera calibration process. Currently, the function only supports planar calibration patterns, i.e. patterns where each object point has z-coordinate =0.

cv::initUndistortRectifyMap¶

void initUndistortRectifyMap(const Mat& cameraMatrix, const Mat& distCoeffs, const Mat& R, const Mat& newCameraMatrix, Size size, int m1type, Mat& map1, Mat& map2)

Computes the undistortion and rectification transformation map.

Parameters: cameraMatrix – The input camera matrix distCoeffs – The input 4x1, 1x4, 5x1 or 1x5 vector of distortion coefficients

.

Parameters: R – The optional rectification transformation in object space (3x3 matrix). R1 or R2 , computed by StereoRectify can be passed here. If the matrix is empty , the identity transformation is assumed newCameraMatrix – The new camera matrix size – The undistorted image size m1type – The type of the first output map, can be CV_32FC1 or CV_16SC2 . See convertMaps() map1 – The first output map map2 – The second output map

The function computes the joint undistortion+rectification transformation and represents the result in the form of maps for Remap . The undistorted image will look like the original, as if it was captured with a camera with camera matrix =newCameraMatrix and zero distortion. In the case of monocular camera newCameraMatrix is usually equal to cameraMatrix , or it can be computed by GetOptimalNewCameraMatrix for a better control over scaling. In the case of stereo camera newCameraMatrix is normally set to P1 or P2 computed by StereoRectify .

Also, this new camera will be oriented differently in the coordinate space, according to R . That, for example, helps to align two heads of a stereo camera so that the epipolar lines on both images become horizontal and have the same y- coordinate (in the case of horizontally aligned stereo camera).

The function actually builds the maps for the inverse mapping algorithm that is used by Remap . That is, for each pixel in the destination (corrected and rectified) image the function computes the corresponding coordinates in the source image (i.e. in the original image from camera). The process is the following:

where are the distortion coefficients.

In the case of a stereo camera this function is called twice, once for each camera head, after StereoRectify , which in its turn is called after StereoCalibrate . But if the stereo camera was not calibrated, it is still possible to compute the rectification transformations directly from the fundamental matrix using StereoRectifyUncalibrated . For each camera the function computes homography H as the rectification transformation in pixel domain, not a rotation matrix R in 3D space. The R can be computed from H as

where the cameraMatrix can be chosen arbitrarily.

cv::matMulDeriv¶

void matMulDeriv(const Mat& A, const Mat& B, Mat& dABdA, Mat& dABdB)

Computes partial derivatives of the matrix product w.r.t each multiplied matrix

Parameters: A – The first multiplied matrix B – The second multiplied matrix dABdA – The first output derivative matrix d(A*B)/dA of size dABdA – The second output derivative matrix d(A*B)/dB of size

The function computes the partial derivatives of the elements of the matrix product w.r.t. the elements of each of the two input matrices. The function is used to compute Jacobian matrices in stereoCalibrate() , but can also be used in any other similar optimization function.

cv::projectPoints¶

void projectPoints(const Mat& objectPoints, const Mat& rvec, const Mat& tvec, const Mat& cameraMatrix, const Mat& distCoeffs, vector<Point2f>& imagePoints)
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)

Project 3D points on to an image plane.

Parameters: objectPoints – The array of object points, 3xN or Nx3 1-channel or 1xN or Nx1 3-channel (or vector ) , where N is the number of points in the view rvec – The rotation vector, see Rodrigues2 tvec – The translation vector cameraMatrix – The camera matrix distCoeffs – The input 4x1, 1x4, 5x1 or 1x5 vector of distortion coefficients . If it is empty , all of the distortion coefficients are considered 0’s imagePoints – The output array of image points, 2xN or Nx2 1-channel or 1xN or Nx1 2-channel (or vector ) dpdrot – Optional 2Nx3 matrix of derivatives of image points with respect to components of the rotation vector dpdt – Optional 2Nx3 matrix of derivatives of image points with respect to components of the translation vector dpdf – Optional 2Nx2 matrix of derivatives of image points with respect to and dpdc – Optional 2Nx2 matrix of derivatives of image points with respect to and dpddist – Optional 2Nx4 matrix of derivatives of image points with respect to distortion coefficients

The function computes projections of 3D points to the image plane given intrinsic and extrinsic camera parameters. Optionally, the function computes jacobians - matrices of partial derivatives of image points coordinates (as functions of all the input parameters) with respect to the particular parameters, intrinsic and/or extrinsic. The jacobians are used during the global optimization in CalibrateCamera2 , FindExtrinsicCameraParams2 and StereoCalibrate . The function itself can also used to compute re-projection error given the current intrinsic and extrinsic parameters.

Note, that by setting rvec=tvec=(0,0,0) , or by setting cameraMatrix to 3x3 identity matrix, or by passing zero distortion coefficients, you can get various useful partial cases of the function, i.e. you can compute the distorted coordinates for a sparse set of points, or apply a perspective transformation (and also compute the derivatives) in the ideal zero-distortion setup etc.

cv::reprojectImageTo3D¶

void reprojectImageTo3D(const Mat& disparity, Mat& _3dImage, const Mat& Q, bool handleMissingValues=false)

Reprojects disparity image to 3D space.

Parameters: disparity – The input single-channel 16-bit signed or 32-bit floating-point disparity image _3dImage – The output 3-channel floating-point image of the same size as disparity . Each element of _3dImage(x,y) will contain the 3D coordinates of the point (x,y) , computed from the disparity map. Q – The perspective transformation matrix that can be obtained with StereoRectify handleMissingValues – If true, when the pixels with the minimal disparity (that corresponds to the outliers; see FindStereoCorrespondenceBM ) will be transformed to 3D points with some very large Z value (currently set to 10000)

The function transforms 1-channel disparity map to 3-channel image representing a 3D surface. That is, for each pixel (x,y) and the corresponding disparity d=disparity(x,y) it computes:

The matrix Q can be arbitrary matrix, e.g. the one computed by StereoRectify . To reproject a sparse set of points {(x,y,d),...} to 3D space, use PerspectiveTransform .

cv::RQDecomp3x3¶

void RQDecomp3x3(const Mat& M, Mat& R, Mat& Q)
Vec3d RQDecomp3x3(const Mat& M, Mat& R, Mat& Q, Mat& Qx, Mat& Qy, Mat& Qz)

Computes the ‘RQ’ decomposition of 3x3 matrices.

Parameters: M – The 3x3 input matrix R – The output 3x3 upper-triangular matrix Q – The output 3x3 orthogonal matrix Qx – Optional 3x3 rotation matrix around x-axis Qy – Optional 3x3 rotation matrix around y-axis Qz – Optional 3x3 rotation matrix around z-axis

The function computes a RQ decomposition using the given rotations. This function is used in DecomposeProjectionMatrix to decompose the left 3x3 submatrix of a projection matrix into a camera and a rotation matrix.

It optionally returns three rotation matrices, one for each axis, and the three Euler angles (as the return value) that could be used in OpenGL.

cv::Rodrigues¶

void Rodrigues(const Mat& src, Mat& dst)
void Rodrigues(const Mat& src, Mat& dst, Mat& jacobian)

Converts a rotation matrix to a rotation vector or vice versa.

Parameters: src – The input rotation vector (3x1 or 1x3) or rotation matrix (3x3) dst – The output rotation matrix (3x3) or rotation vector (3x1 or 1x3), respectively jacobian – Optional output Jacobian matrix, 3x9 or 9x3 - partial derivatives of the output array components with respect to the input array components

Inverse transformation can also be done easily, since

A rotation vector is a convenient and most-compact representation of a rotation matrix (since any rotation matrix has just 3 degrees of freedom). The representation is used in the global 3D geometry optimization procedures like CalibrateCamera2 , StereoCalibrate or FindExtrinsicCameraParams2 .

StereoBM¶

StereoBM

The class for computing stereo correspondence using block matching algorithm.

// Block matching stereo correspondence algorithmclass StereoBM
{
enum { NORMALIZED_RESPONSE = CV_STEREO_BM_NORMALIZED_RESPONSE,
BASIC_PRESET=CV_STEREO_BM_BASIC,
FISH_EYE_PRESET=CV_STEREO_BM_FISH_EYE,
NARROW_PRESET=CV_STEREO_BM_NARROW };

StereoBM();
// the preset is one of ..._PRESET above.
// ndisparities is the size of disparity range,
// in which the optimal disparity at each pixel is searched for.
// SADWindowSize is the size of averaging window used to match pixel blocks
//    (larger values mean better robustness to noise, but yield blurry disparity maps)
StereoBM(int preset, int ndisparities=0, int SADWindowSize=21);
// separate initialization function
void init(int preset, int ndisparities=0, int SADWindowSize=21);
// computes the disparity for the two rectified 8-bit single-channel images.
// the disparity will be 16-bit signed (fixed-point) or 32-bit floating-point image of the same size as left.
void operator()( const Mat& left, const Mat& right, Mat& disparity, int disptype=CV_16S );

Ptr<CvStereoBMState> state;
};


The class is a C++ wrapper for and the associated functions. In particular, StereoBM::operator () is the wrapper for FindStereoCorrespondceBM . See the respective descriptions.

StereoSGBM¶

StereoSGBM

The class for computing stereo correspondence using semi-global block matching algorithm.

class StereoSGBM
{
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 preFilterCap;
int uniquenessRatio;
int P1, P2;
int speckleWindowSize;
int speckleRange;
int disp12MaxDiff;
bool fullDP;

...
};


The class implements modified H. Hirschmuller algorithm [HH08] . The main differences between the implemented algorithm and the original one are:

• by default the algorithm is single-pass, i.e. instead of 8 directions we only consider 5. Set fullDP=true to run the full variant of the algorithm (which could consume a lot of memory)
• the algorithm matches blocks, not individual pixels (though, by setting SADWindowSize=1 the blocks are reduced to single pixels)
• mutual information cost function is not implemented. Instead, we use a simpler Birchfield-Tomasi sub-pixel metric from [BT96] , though the color images are supported as well.
• we include some pre- and post- processing steps from K. Konolige algorithm FindStereoCorrespondceBM , such as pre-filtering ( CV_STEREO_BM_XSOBEL type) and post-filtering (uniqueness check, quadratic interpolation and speckle filtering)

cv::StereoSGBM::StereoSGBM¶

StereoSGBM::StereoSGBM()
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)

StereoSGBM constructors

Parameters: minDisparity – The minimum possible disparity value. Normally it is 0, but sometimes rectification algorithms can shift images, so this parameter needs to be adjusted accordingly numDisparities – This is maximum disparity minus minimum disparity. Always greater than 0. In the current implementation this parameter must be divisible by 16. SADWindowSize – The matched block size. Must be an odd number >=1 . Normally, it should be somewhere in 3..11 range

.

Parameters: P1, P2 – Parameters that control disparity smoothness. The larger the values, the smoother the disparity. P1 is the penalty on the disparity change by plus or minus 1 between neighbor pixels. P2 is the penalty on the disparity change by more than 1 between neighbor pixels. The algorithm requires P2 > P1 . See stereo_match.cpp sample where some reasonably good P1 and P2 values are shown (like 8*number_of_image_channels*SADWindowSize*SADWindowSize and 32*number_of_image_channels*SADWindowSize*SADWindowSize , respectively). disp12MaxDiff – Maximum allowed difference (in integer pixel units) in the left-right disparity check. Set it to non-positive value to disable the check. preFilterCap – Truncation value for the prefiltered image pixels. The algorithm first computes x-derivative at each pixel and clips its value by [-preFilterCap, preFilterCap] interval. The result values are passed to the Birchfield-Tomasi pixel cost function. uniquenessRatio – The margin in percents by which the best (minimum) computed cost function value should “win” the second best value to consider the found match correct. Normally, some value within 5-15 range is good enough speckleWindowSize – Maximum size of smooth disparity regions to consider them noise speckles and invdalidate. Set it to 0 to disable speckle filtering. Otherwise, set it somewhere in 50-200 range. speckleRange – Maximum disparity variation within each connected component. If you do speckle filtering, set it to some positive value, multiple of 16. Normally, 16 or 32 is good enough. fullDP – Set it to true to run full-scale 2-pass dynamic programming algorithm. It will consume O(W*H*numDisparities) bytes, which is large for 640x480 stereo and huge for HD-size pictures. By default this is false

The first constructor initializes StereoSGBM with all the default parameters (so actually one will only have to set StereoSGBM::numberOfDisparities at minimum). The second constructor allows you to set each parameter to a custom value.

cv::StereoSGBM::operator ()¶

void SGBM::operator()(const Mat& left, const Mat& right, Mat& disp)

Computes disparity using SGBM algorithm for a rectified stereo pair

Parameters: left – The left image, 8-bit single-channel or 3-channel. right – The right image of the same size and the same type as the left one. disp – The output disparity map. It will be 16-bit signed single-channel image of the same size as the input images. It will contain scaled by 16 disparity values, so that to get the floating-point disparity map, you will need to divide each disp element by 16.

The method executes SGBM algorithm on a rectified stereo pair. See stereo_match.cpp OpenCV sample on how to prepare the images and call the method. Note that the method is not constant, thus you should not use the same StereoSGBM instance from within different threads simultaneously.

cv::stereoCalibrate¶

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 term_crit = TermCriteria(TermCriteria::COUNT+ TermCriteria::EPS, 30, 1e-6), int flags=CALIB_FIX_INTRINSIC)

Calibrates stereo camera.

Parameters: objectPoints – The vector of vectors of points on the calibration pattern in its coordinate system, one vector per view. If the same calibration pattern is shown in each view and it’s fully visible then all the vectors will be the same, although it is possible to use partially occluded patterns, or even different patterns in different views - then the vectors will be different. The points are 3D, but since they are in the pattern coordinate system, then if the rig is planar, it may have sense to put the model to the XY coordinate plane, so that Z-coordinate of each input object point is 0 imagePoints1 – The vector of vectors of the object point projections on the calibration pattern views from the 1st camera, one vector per a view. The projections must be in the same order as the corresponding object points. imagePoints2 – The vector of vectors of the object point projections on the calibration pattern views from the 2nd camera, one vector per a view. The projections must be in the same order as the corresponding object points. cameraMatrix1 – The input/output first camera matrix: , . If any of CV_CALIB_USE_INTRINSIC_GUESS , CV_CALIB_FIX_ASPECT_RATIO , CV_CALIB_FIX_INTRINSIC or CV_CALIB_FIX_FOCAL_LENGTH are specified, some or all of the matrices’ components must be initialized; see the flags description distCoeffs1 – The input/output lens distortion coefficients for the first camera, 4x1, 5x1, 1x4 or 1x5 floating-point vectors , . If any of CV_CALIB_FIX_K1 , CV_CALIB_FIX_K2 or CV_CALIB_FIX_K3 is specified, then the corresponding elements of the distortion coefficients must be initialized. cameraMatrix2 – The input/output second camera matrix, as cameraMatrix1. distCoeffs2 – The input/output lens distortion coefficients for the second camera, as distCoeffs1. imageSize – Size of the image, used only to initialize intrinsic camera matrix. R – The output rotation matrix between the 1st and the 2nd cameras’ coordinate systems. T – The output translation vector between the cameras’ coordinate systems. E – The output essential matrix. F – The output fundamental matrix. term_crit – The termination criteria for the iterative optimization algorithm. flags – Different flags, may be 0 or combination of the following values: CV_CALIB_FIX_INTRINSIC If it is set, cameraMatrix? , as well as distCoeffs? are fixed, so that only R, T, E and F are estimated. CV_CALIB_USE_INTRINSIC_GUESS The flag allows the function to optimize some or all of the intrinsic parameters, depending on the other flags, but the initial values are provided by the user. CV_CALIB_FIX_PRINCIPAL_POINT The principal points are fixed during the optimization. CV_CALIB_FIX_FOCAL_LENGTH and are fixed. CV_CALIB_FIX_ASPECT_RATIO is optimized, but the ratio is fixed. CV_CALIB_SAME_FOCAL_LENGTH Enforces and CV_CALIB_ZERO_TANGENT_DIST Tangential distortion coefficients for each camera are set to zeros and fixed there. CV_CALIB_FIX_K1, CV_CALIB_FIX_K2, CV_CALIB_FIX_K3 Fixes the corresponding radial distortion coefficient (the coefficient must be passed to the function)

The function estimates transformation between the 2 cameras making a stereo pair. If we have a stereo camera, where the relative position and orientation of the 2 cameras is fixed, and if we computed poses of an object relative to the fist camera and to the second camera, (R1, T1) and (R2, T2), respectively (that can be done with FindExtrinsicCameraParams2 ), obviously, those poses will relate to each other, i.e. given ( , ) it should be possible to compute ( , ) - we only need to know the position and orientation of the 2nd camera relative to the 1st camera. That’s what the described function does. It computes ( , ) such that:

Optionally, it computes the essential matrix E:

where are components of the translation vector : . And also the function can compute the fundamental matrix F:

Besides the stereo-related information, the function can also perform full calibration of each of the 2 cameras. However, because of the high dimensionality of the parameter space and noise in the input data the function can diverge from the correct solution. Thus, if intrinsic parameters can be estimated with high accuracy for each of the cameras individually (e.g. using CalibrateCamera2 ), it is recommended to do so and then pass CV_CALIB_FIX_INTRINSIC flag to the function along with the computed intrinsic parameters. Otherwise, if all the parameters are estimated at once, it makes sense to restrict some parameters, e.g. pass CV_CALIB_SAME_FOCAL_LENGTH and CV_CALIB_ZERO_TANGENT_DIST flags, which are usually reasonable assumptions.

Similarly to CalibrateCamera2 , the function minimizes the total re-projection error for all the points in all the available views from both cameras. The function returns the final value of the re-projection error.

cv::stereoRectify¶

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)
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* roi1=0, Rect* roi2=0, int flags=CALIB_ZERO_DISPARITY)

Computes rectification transforms for each head of a calibrated stereo camera.

Parameters: cameraMatrix1, cameraMatrix2 – The camera matrices . distCoeffs1, distCoeffs2 – The input distortion coefficients for each camera, imageSize – Size of the image used for stereo calibration. R – The rotation matrix between the 1st and the 2nd cameras’ coordinate systems. T – The translation vector between the cameras’ coordinate systems. R1, R2 – The output rectification transforms (rotation matrices) for the first and the second cameras, respectively. P1, P2 – The output projection matrices in the new (rectified) coordinate systems. Q – The output disparity-to-depth mapping matrix, see reprojectImageTo3D() . flags – The operation flags; may be 0 or CV_CALIB_ZERO_DISPARITY . If the flag is set, the function makes the principal points of each camera have the same pixel coordinates in the rectified views. And if the flag is not set, the function may still shift the images in horizontal or vertical direction (depending on the orientation of epipolar lines) in order to maximize the useful image area. alpha – The free scaling parameter. If it is -1 or absent , the functions performs some default scaling. Otherwise the parameter should be between 0 and 1. alpha=0 means that the rectified images will be zoomed and shifted so that only valid pixels are visible (i.e. there will be no black areas after rectification). alpha=1 means that the rectified image will be decimated and shifted so that all the pixels from the original images from the cameras are retained in the rectified images, i.e. no source image pixels are lost. Obviously, any intermediate value yields some intermediate result between those two extreme cases. newImageSize – The new image resolution after rectification. The same size should be passed to InitUndistortRectifyMap , see the stereo_calib.cpp sample in OpenCV samples directory. By default, i.e. when (0,0) is passed, it is set to the original imageSize . Setting it to larger value can help you to preserve details in the original image, especially when there is big radial distortion. roi1, roi2 – The optional output rectangles inside the rectified images where all the pixels are valid. If alpha=0 , the ROIs will cover the whole images, otherwise they likely be smaller, see the picture below

The function computes the rotation matrices for each camera that (virtually) make both camera image planes the same plane. Consequently, that makes all the epipolar lines parallel and thus simplifies the dense stereo correspondence problem. On input the function takes the matrices computed by stereoCalibrate() and on output it gives 2 rotation matrices and also 2 projection matrices in the new coordinates. The 2 cases are distinguished by the function are:

1. Horizontal stereo, when 1st and 2nd camera views are shifted relative to each other mainly along the x axis (with possible small vertical shift). Then in the rectified images the corresponding epipolar lines in left and right cameras will be horizontal and have the same y-coordinate. P1 and P2 will look as:

where is horizontal shift between the cameras and if CV_CALIB_ZERO_DISPARITY is set.

2. Vertical stereo, when 1st and 2nd camera views are shifted relative to each other mainly in vertical direction (and probably a bit in the horizontal direction too). Then the epipolar lines in the rectified images will be vertical and have the same x coordinate. P2 and P2 will look as:

where is vertical shift between the cameras and if CALIB_ZERO_DISPARITY is set.

As you can see, the first 3 columns of P1 and P2 will effectively be the new “rectified” camera matrices. The matrices, together with R1 and R2 , can then be passed to InitUndistortRectifyMap to initialize the rectification map for each camera.

Below is the screenshot from stereo_calib.cpp sample. Some red horizontal lines, as you can see, pass through the corresponding image regions, i.e. the images are well rectified (which is what most stereo correspondence algorithms rely on). The green rectangles are roi1 and roi2 - indeed, their interior are all valid pixels.

cv::stereoRectifyUncalibrated¶

bool stereoRectifyUncalibrated(const Mat& points1, const Mat& points2, const Mat& F, Size imgSize, Mat& H1, Mat& H2, double threshold=5)

Computes rectification transform for uncalibrated stereo camera.

Parameters: points1, points2 – The 2 arrays of corresponding 2D points. The same formats as in FindFundamentalMat are supported F – The input fundamental matrix. It can be computed from the same set of point pairs using FindFundamentalMat . imageSize – Size of the image. H1, H2 – The output rectification homography matrices for the first and for the second images. threshold – The optional threshold used to filter out the outliers. If the parameter is greater than zero, then all the point pairs that do not comply the epipolar geometry well enough (that is, the points for which ) are rejected prior to computing the homographies. Otherwise all the points are considered inliers.

The function computes the rectification transformations without knowing intrinsic parameters of the cameras and their relative position in space, hence the suffix “Uncalibrated”. Another related difference from StereoRectify is that the function outputs not the rectification transformations in the object (3D) space, but the planar perspective transformations, encoded by the homography matrices H1 and H2 . The function implements the algorithm [Hartley99] .

Note that while the algorithm does not need to know the intrinsic parameters of the cameras, it heavily depends on the epipolar geometry. Therefore, if the camera lenses have significant distortion, it would better be corrected before computing the fundamental matrix and calling this function. For example, distortion coefficients can be estimated for each head of stereo camera separately by using CalibrateCamera2 and then the images can be corrected using Undistort2 , or just the point coordinates can be corrected with UndistortPoints .

cv::undistort¶

void undistort(const Mat& src, Mat& dst, const Mat& cameraMatrix, const Mat& distCoeffs, const Mat& newCameraMatrix=Mat())

Transforms an image to compensate for lens distortion.

Parameters: src – The input (distorted) image dst – The output (corrected) image; will have the same size and the same type as src cameraMatrix – The input camera matrix distCoeffs – The vector of distortion coefficients, newCameraMatrix – Camera matrix of the distorted image. By default it is the same as cameraMatrix , but you may additionally scale and shift the result by using some different matrix

The function transforms the image to compensate radial and tangential lens distortion.

The function is simply a combination of InitUndistortRectifyMap (with unity R ) and Remap (with bilinear interpolation). See the former function for details of the transformation being performed.

Those pixels in the destination image, for which there is no correspondent pixels in the source image, are filled with 0’s (black color).

The particular subset of the source image that will be visible in the corrected image can be regulated by newCameraMatrix . You can use GetOptimalNewCameraMatrix to compute the appropriate newCameraMatrix , depending on your requirements.

The camera matrix and the distortion parameters can be determined using CalibrateCamera2 . If the resolution of images is different from the used at the calibration stage, and need to be scaled accordingly, while the distortion coefficients remain the same.

cv::undistortPoints¶

void undistortPoints(const Mat& src, vector<Point2f>& dst, const Mat& cameraMatrix, const Mat& distCoeffs, const Mat& R=Mat(), const Mat& P=Mat())
void undistortPoints(const Mat& src, Mat& dst, const Mat& cameraMatrix, const Mat& distCoeffs, const Mat& R=Mat(), const Mat& P=Mat())

Computes the ideal point coordinates from the observed point coordinates.

Parameters: src – The observed point coordinates, same format as imagePoints in ProjectPoints2 dst – The output ideal point coordinates, after undistortion and reverse perspective transformation . cameraMatrix – The camera matrix distCoeffs – The vector of distortion coefficients, R – The rectification transformation in object space (3x3 matrix). R1 or R2 , computed by StereoRectify() can be passed here. If the matrix is empty, the identity transformation is used P – The new camera matrix (3x3) or the new projection matrix (3x4). P1 or P2 , computed by StereoRectify() can be passed here. If the matrix is empty, the identity new camera matrix is used

The function is similar to Undistort2 and InitUndistortRectifyMap , but it operates on a sparse set of points instead of a raster image. Also the function does some kind of reverse transformation to ProjectPoints2 (in the case of 3D object it will not reconstruct its 3D coordinates, of course; but for a planar object it will, up to a translation vector, if the proper R is specified).

// (u,v) is the input point, (u', v') is the output point
// camera_matrix=[fx 0 cx; 0 fy cy; 0 0 1]
// P=[fx' 0 cx' tx; 0 fy' cy' ty; 0 0 1 tz]
x" = (u - cx)/fx
y" = (v - cy)/fy
(x',y') = undistort(x",y",dist_coeffs)
[X,Y,W]T = R*[x' y' 1]T
x = X/W, y = Y/W
u' = x*fx' + cx'
v' = y*fy' + cy',

where undistort() is approximate iterative algorithm that estimates the normalized original point coordinates out of the normalized distorted point coordinates (“normalized” means that the coordinates do not depend on the camera matrix).

The function can be used both for a stereo camera head or for monocular camera (when R is empty ).