stereoRectify makes strange images distortion

edit

Hello. I am trying to compute point cloud from two cameras.

1. Find chessboard corners using findChessboardCorners
2. Calibrate cameras using stereoCalibrate
3. Calculate rectify matrix using stereoRectify
4. Calculate remap maps using initUndistortRectifyMap
5. Remap images

And I got a very bad results:

It's not so bad like this:

Is the any way to fix it?

This is my code (also available on GitHub): // left - vector of grayscale images from left camera // right - vector of grayscale images from right camera // patternSize - number of chessboard corners StereoCalibData StereoVision::Calibrate(const std::vector<cv::mat>& left, const std::vector<cv::mat>& right, cv::Size patternSize) { cv::Size imSize;

// Images points
std::vector<cv::Point2f> imagePointsLeftSingle, imagePointsRightSingle;
std::vector<std::vector<cv::Point2f>> imagePointsLeft, imagePointsRight;

// Object points
std::vector<cv::Point3f> objectPointsSingle;
std::vector<std::vector<cv::Point3f>> objectPoints;

//stereoCalibrate output
cv::Mat CM1(3, 3, CV_64FC1), CM2(3, 3, CV_64FC1);   //Camera matrix
cv::Mat D1, D2;         //Distortion
cv::Mat R,              //Rotation matrix
        T,              //Translation matrix
        E,              //Существенная матрица
        F;              //Фундаментальная матрица

//stereoRectify output
cv::Mat R1, R2, P1, P2;
cv::Mat Q;


//1. ---------------------------  Find chessboard corners ---------------------------------------------------------------

for (int i = 0; i < left.size(); i++)
{
    //Finding chessboard corners
    bool isFoundLeft = cv::findChessboardCorners(left[i], patternSize, imagePointsLeftSingle, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
    bool isFoundRight = cv::findChessboardCorners(right[i], patternSize, imagePointsRightSingle, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);

    imSize = left[i].size();

    cornerSubPix(left[i], imagePointsLeftSingle, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
    cornerSubPix(right[i], imagePointsRightSingle, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

    //Sometimes chessboard corners are found in bad direction (vertically lines), so I "rotate" them
    _fixChessboardCorners(imagePointsLeftSingle, patternSize);
    _fixChessboardCorners(imagePointsRightSingle, patternSize);

    //Add to the corners vectors
    imagePointsLeft.push_back(imagePointsLeftSingle);
    imagePointsRight.push_back(imagePointsRightSingle);
}

//2. --------------------------- Compute object points ------------------------------------------------------------------

for (int j = 0; j<patternSize.height*patternSize.width; j++)
    objectPointsSingle.push_back(cv::Point3f(j / patternSize.width, j%patternSize.width, 0.0f));

for (int i = 0; i < left.size(); i++)
    objectPoints.push_back(objectPointsSingle);

//3. --------------------------- Stereo calibration --------------------------------------------------------------------------
stereoCalibrate(objectPoints, imagePointsLeft, imagePointsRight,
    CM1, D1, CM2, D2, imSize, R, T, E, F, CV_CALIB_SAME_FOCAL_LENGTH | CV_CALIB_ZERO_TANGENT_DIST,
    cvTermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 100, 1e-5));

cv::stereoRectify(CM1, D1, CM2, D2, imSize, R, T, R1, R2, P1, P2, Q);

// -------------------------- Saving rsults ------------------------------------------------------------------------


calibData.ImageSize = imSize;
calibData.LeftCameraMatrix = CM1.clone();
calibData.RightCameraMatrix = CM2.clone();
calibData.LeftCameraDistortions = D1.clone();
calibData.RightCameraDistortions = D2.clone();
calibData.LeftCameraRectifiedProjection = P1.clone();
calibData.RightCameraRectifiedProjection = P2.clone();
calibData.LeftCameraRot = R1.clone();
calibData.RightCameraRot = R2.clone();
calibData.Q = Q;

return calibData;

}

IPointCloudStorage* StereoVision::CalculatePointCloud(const cv::Mat& left, const cv::Mat& right) const { cv::Mat leftRemaped, rightRemaped; cv::Mat depth, normalDepth;

cv::initUndistortRectifyMap(data.LeftCameraMatrix, data.LeftCameraDistortions, data.LeftCameraRot, data.LeftCameraRectifiedProjection, data.ImageSize, CV_32FC1, data.LeftMapX, data.LeftMapY);
cv::initUndistortRectifyMap(data.RightCameraMatrix, data.RightCameraDistortions, data.RightCameraRot, data.RightCameraRectifiedProjection, data.ImageSize, CV_32FC1, data.RightMapX, data.RightMapY);

cv::remap(left, leftRemaped, calibData.LeftMapX, calibData.LeftMapY, cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar());
cv::remap(right, rightRemaped, calibData.RightMapX, calibData.RightMapY, cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar());

cv::imshow("w1", leftRemaped);
cv::imshow("w2", rightRemaped);
return NULL;

}

P.S. There are some problems with syntax highlighting