void CVStereo::stereo(
int minDisparity,
int numDisparities,
int width,
int height,
const uint8_t* leftGray,
const uint8_t* rightGray,
int16_t* resultBuf) {
int rows = height;
int cols = width;
const cv::Mat left(rows, cols, CV_8U, (void*) leftGray);
const cv::Mat right(rows, cols, CV_8U, (void*) rightGray);
cv::Mat result(rows, cols, CV_16S, (void*) resultBuf);
int SADWindowSize = 3; // 3 to 11 is recommended
float smoothnessScale = 1.0f;
int P1=8 * 3 * sqr(SADWindowSize) * smoothnessScale;
int P2=32 * 3 * sqr(SADWindowSize) * smoothnessScale;
int disp12MaxDiff=2;
int preFilterCap=0;
int uniquenessRatio=0;
int speckleWindowSize=0;
int speckleRange=0;
bool fullDP=false;
cv::StereoSGBM sgbm(minDisparity, numDisparities, SADWindowSize,
P1, P2, disp12MaxDiff, preFilterCap, uniquenessRatio, speckleWindowSize,
speckleRange, fullDP);
sgbm(left, right, result);
}
void CVStereo::matchStereo(
int minDisparity,
int maxDisparity,
int windowSize,
float smoothnessScale) {
this->minDisparity = minDisparity;
this->numDisparities = maxDisparity - minDisparity;
int SADWindowSize = windowSize; // 3 to 11 is recommended
int P1=8 * 3 * sqr(SADWindowSize) * smoothnessScale;
int P2=32 * 3 * sqr(SADWindowSize) * smoothnessScale;
int disp12MaxDiff=2;
int preFilterCap=0;
int uniquenessRatio=0;
int speckleWindowSize=0;
int speckleRange=0;
bool fullDP=false;
cv::StereoSGBM sgbm(minDisparity, numDisparities, SADWindowSize,
P1, P2, disp12MaxDiff, preFilterCap, uniquenessRatio, speckleWindowSize,
speckleRange, fullDP);
cv::Mat left, right;
rectified[0].convertTo(left, CV_8UC3);
rectified[1].convertTo(right, CV_8UC3);
sgbm(left, right, stereoDisparity);
}
void QtOpenCVZedDemo::doStereoSGBM_CPU( cv::Mat left, cv::Mat right )
{
cv::StereoSGBM sgbm;
sgbm.SADWindowSize = 5;
sgbm.numberOfDisparities = 192;
sgbm.preFilterCap = 4;
sgbm.minDisparity = -64;
sgbm.uniquenessRatio = 1;
sgbm.speckleWindowSize = 150;
sgbm.speckleRange = 2;
sgbm.disp12MaxDiff = 10;
sgbm.fullDP = true;
sgbm.P1 = 600;
sgbm.P2 = 2400;
if( left.channels() > 1 )
cv::cvtColor( left, left, CV_BGR2GRAY );
if( right.channels() > 1 )
cv::cvtColor( right, right, CV_BGR2GRAY );
sgbm(left, right, mDisparity);
normalize(mDisparity, mDisparity, 0, 255, CV_MINMAX, CV_8U);
}
cv::Mat ossimOpenCvDisparityMapGenerator::execute(cv::Mat master_mat, cv::Mat slave_mat)
{
cout << "DISPARITY MAP GENERATION..." << endl;
// Disparity Map generation
int ndisparities = 16; //Maximum disparity minus minimum disparity //con fattore di conversione 1 metti 16*2*2
int SADWindowSize = 11; //Matched block size
cv::StereoSGBM sgbm;
sgbm.preFilterCap = 63;
sgbm.SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 3;
int cn = master_mat.channels();
sgbm.P1 = 8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
sgbm.P2 = 40*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
sgbm.minDisparity = -8; // Minimum possible disparity value //con fattore di conversione 1 metti -16*2
sgbm.numberOfDisparities = ndisparities;
sgbm.uniquenessRatio = 5;
sgbm.speckleWindowSize = 100;
sgbm.speckleRange = 1;
sgbm.disp12MaxDiff = 1; // Maximum allowed difference (in integer pixel units) in the left-right disparity check
//sgbm.fullDP = true;
double minVal, maxVal;
cv::Mat array_disp;
cv::Mat array_disp_8U;
sgbm(master_mat, slave_mat, array_disp);
minMaxLoc( array_disp, &minVal, &maxVal );
array_disp.convertTo( array_disp_8U, CV_8UC1, 255/(maxVal - minVal), -minVal*255/(maxVal - minVal));
cout << "min\t" << minVal << "max\t" << maxVal << endl;
cv::namedWindow( "SGM Disparity", CV_WINDOW_NORMAL );
cv::imshow( "SGM Disparity", array_disp_8U);
cv::imwrite( "SGM Disparity.tif", array_disp_8U);
cv::waitKey(0);
//Create and write the log file
ofstream disparity;
disparity.open ("DSM_parameters_disparity.txt");
disparity <<"DISPARITY RANGE:" << " " << ndisparities << endl;
disparity <<"SAD WINDOW SIZE:" << " " << SADWindowSize<< endl;
disparity << "MINIMUM DISPARITY VALUE:"<< sgbm.minDisparity << endl;
disparity.close();
return array_disp;
}
void SGBM_cpu::Compute(Mat left,Mat right,Mat& disp)
{
UpdateParameters();
imgL = left;
imgR = right;
ScaleImages();
RectifyImages();
sgbm(imgL, imgR, disp);
resize(disp,disp,Size(),SHOW_SCALE,SHOW_SCALE,INTER_AREA);
if(STORE_RESULTS) StoreResults(disp,imgL,imgR);
return;
}
int main(int argc, char** argv)
{
/// load calibration data
cv::Mat leftCameraMatrix; //3x3 floating-point left camera matrix
cv::Mat rightCameraMatrix; //3x3 floating-point right camera matrix
cv::Mat leftDistCoeffs; //8x1 vector of left distortion coefficients
cv::Mat rightDistCoeffs; //8x1 vector of right distortion coefficients
cv::Mat R1; // 3x3 rectification transform (rotation matrix) for the first camera
cv::Mat R2; // 3x3 rectification transform (rotation matrix) for the second camera
cv::Mat P1; // 3x4 projection matrix in the new (rectified) coordinate systems for the first camera
cv::Mat P2; // 3x4 projection matrix in the new (rectified) coordinate systems for the second camera
cv::Mat Q; // 4x4 disparity-to-depth mapping matrix
//load all calibration parameters from outputCalibration.xml
cv::FileStorage fsR("config/outputCalibration.xml", cv::FileStorage::READ);
fsR["leftCameraMatrix"] >> leftCameraMatrix;
fsR["rightCameraMatrix"] >> rightCameraMatrix;
fsR["leftDistCoeffs"] >> leftDistCoeffs;
fsR["rightDistCoeffs"] >> rightDistCoeffs;
fsR["R1"] >> R1;
fsR["R2"] >> R2;
fsR["P1"] >> P1;
fsR["P2"] >> P2;
fsR["Q"] >> Q;
//load test images
std::string left_filename, right_filename;
left_filename="../../dataset/test/left.jpg";
right_filename="../../dataset/test/right.jpg";
cv::Mat left_image = cv::imread(left_filename);
cv::Mat right_image = cv::imread(right_filename);
cv::Size image_size = left_image.size(); //size of test images
//left and right undistort and rectification maps
cv::Mat left_map1;
cv::Mat left_map2;
cv::Mat right_map1;
cv::Mat right_map2;
//timers
clock_t init, timeComplete;
init=clock(); //start timer
/// create rectification maps
cv::initUndistortRectifyMap(leftCameraMatrix, leftDistCoeffs, R1, P1, image_size, CV_32FC1, left_map1, left_map2);
cv::initUndistortRectifyMap(rightCameraMatrix, rightDistCoeffs, R2, P2, image_size, CV_32FC1, right_map1, right_map2);
//remap images
cv::Mat left_image_remap;
cv::Mat right_image_remap;
/// use the maps to rectificate images
cv::remap(left_image, left_image_remap, left_map1, left_map2, cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar());
cv::remap(right_image, right_image_remap, right_map1, right_map2, cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar());
//show undistorted and rectified test images;
/*cv::namedWindow("Original left image");
cv::imshow("Original left image",left_image);
cv::namedWindow("Original right image");
cv::imshow("Original right image",right_image);
cv::namedWindow("Rectified left image");
cv::imshow("Rectified left image",left_image_remap);
cv::namedWindow("Rectified right image");
cv::imshow("Rectified right image",right_image_remap);
cv::waitKey(0);
return 0;*/
/// compute the disparity
cv::StereoSGBM sgbm;
sgbm.preFilterCap = 100;
sgbm.SADWindowSize = 5;
sgbm.P1 = 8 * left_image_remap.channels() * sgbm.SADWindowSize * sgbm.SADWindowSize;
sgbm.P2 = 32 * left_image_remap.channels() * sgbm.SADWindowSize * sgbm.SADWindowSize;
sgbm.minDisparity = 40;
sgbm.numberOfDisparities = 256;
sgbm.uniquenessRatio = 10;
sgbm.speckleWindowSize = 200;
sgbm.speckleRange = 2;
sgbm.disp12MaxDiff = 0;
cv::Mat disparity_image;
sgbm(left_image_remap, right_image_remap, disparity_image);
//show disparity image;
/*
cv::namedWindow("Disparity");
cv::imshow("Disparity",disparity_image);
cv::waitKey(0);
return 0;*/
/// convert to 3D points
cv::Mat cloud_image;
cv::reprojectImageTo3D(disparity_image, cloud_image, Q);
//convert cloud_image into point format of PCL
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
float px, py, pz;
int pr, pg, pb;
//scan cloud_image and convert its information in a Point Cloud
for (int i = 0; i < cloud_image.rows; i++)
{
for (int j = 0; j < cloud_image.cols; j++)
{
px= cloud_image.at<cv::Vec3f>(i,j)[0];
py= cloud_image.at<cv::Vec3f>(i,j)[1];
pz= cloud_image.at<cv::Vec3f>(i,j)[2];
pb= left_image.at<cv::Vec3b>(i,j)[0];
pg= left_image.at<cv::Vec3b>(i,j)[1];
pr= left_image.at<cv::Vec3b>(i,j)[2];
//Insert info into point cloud structure
pcl::PointXYZRGB point;
point.x = px;
point.y = py;
point.z = pz;
point.r = pr;
point.g = pg;
point.b = pb;
//delete unwanted infinites points and outliers
if(point.x>-30 && point.x<30 && point.y>-30 && point.y<30 && point.z>-3 && point.z <3){
point_cloud->points.push_back (point);
}
}
}
/// visualize 3D points
pcl::visualization::PCLVisualizer visualizer("PCL visualizer");
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(point_cloud);
visualizer.addPointCloud<pcl::PointXYZRGB> (point_cloud, rgb, "point_cloud");
timeComplete=clock()-init; //final time
std::cout <<"Reconstruction complete in: "<<timeComplete <<std::endl;
visualizer.spin();
return 0;
}
