void PointCloudBuilder::extractFrom(Mat& dispMap, Mat& leftColorImage, bool handleMissingValues) { reprojectImageTo3D(dispMap, pointCloud, Q, handleMissingValues); pointCloudMsg->points.clear(); pointCloudMsg->height = pointCloud.rows; pointCloudMsg->width = pointCloud.cols; for(unsigned int i = 0; i < pointCloudMsg->height; i++) { Vec3f *point = pointCloud.ptr<Vec3f>(i); Vec3b *pixel = leftColorImage.ptr<Vec3b>(i); for(unsigned int j = 0; j < pointCloudMsg->width; j++) { PointXYZRGB p; p.data[0] = point[j][0]/2.46; p.data[1] = -point[j][1]/2.46; p.data[2] = point[j][2]/2.46; p.data[3] = 1.0f; if(isinf(p.x) || isnan(p.x) || isinf(p.y) || isnan(p.y) || isinf(p.z) || isnan(p.z)) p.data[0] = p.data[1] = p.data[2] = 0; uint8_t r = pixel[j][0]; uint8_t g = pixel[j][1]; uint8_t b = pixel[j][2]; int32_t rgb = (b << 16) | (g << 8) | r; p.rgb = *(float*) (&rgb); pointCloudMsg->points.push_back(p); } } }
void FrameProcessor::processStereoFrame(const Mat & frameL, const Mat & frameR, Mat & pointCloud){ Mat disparityMap, disparityMapNormalized; Mat frameTransposedL, frameTransposedR, frameRemappedL, frameRemappedR, frameGrayscaleL, frameGrayscaleR; Mat rotMatL = cvCreateMat(2,3,CV_32FC1); Mat rotMatR = cvCreateMat(2,3,CV_32FC1); // Compute rotation matrix CvPoint2D32f centerL = cvPoint2D32f( frameL.cols/2, frameL.rows/2 ); rotMatL = getRotationMatrix2D( centerL, 90, 1 ); CvPoint2D32f centerR = cvPoint2D32f( frameR.cols/2, frameR.rows/2 ); rotMatR = getRotationMatrix2D( centerR, 90, 1 ); warpAffine(frameL, frameTransposedL, rotMatL, frameL.size() ); warpAffine(frameR, frameTransposedR, rotMatR, frameR.size() ); //transpose(frameL, frameTransposedL); //transpose(frameR, frameTransposedR); remap(frameTransposedL, frameRemappedL, rmap[0][0], rmap[0][1], CV_INTER_LINEAR); remap(frameTransposedR, frameRemappedR, rmap[1][0], rmap[1][1], CV_INTER_LINEAR); //imshow("LiveFeedL",frameTransposedL); //imshow("LiveFeedR",frameTransposedR); cvtColor(frameRemappedL, frameGrayscaleL, CV_RGB2GRAY); cvtColor(frameRemappedR, frameGrayscaleR, CV_RGB2GRAY); BlockMatcher( frameGrayscaleL, frameGrayscaleR, disparityMap, CV_32F); normalize(disparityMap, disparityMapNormalized, 0, 255, CV_MINMAX, CV_8U); imshow("Disparity", disparityMapNormalized); reprojectImageTo3D( disparityMap, pointCloud, Q, false); }
void Depth_and_Disparity::convert_disperity_Mat_to_depth(Mat in_disp, Mat & out_depth) { reprojectImageTo3D(in_disp, out_depth, Q, true); //Vec3f point_middle = xyz_again.at<Vec3f>(xyz_again.rows/2, xyz_again.cols/2); //printf("\n\n middle point relative coor. are: %f %f %f \n\n", point_middle.val[0],point_middle.val[1],point_middle.val[2]); }
int Depth_and_Disparity::do_stereo_match(Mat imgR, Mat imgL , Mat& disp8 ) { ready_disparity_result = false; // runover current unused previous result. if any. /* when calibrated and saving matrices - i calibrated Left camera as img1 , Right camera as img2 */ img1 = imgL.clone(); img2 = imgR.clone(); // scale if image sizes are different then calibrated images resultion /* if (scale != 1.f) { Mat temp1, temp2; int method = scale < 1 ? INTER_AREA : INTER_CUBIC; resize(img1, temp1, Size(), scale, scale, method); img1 = temp1; resize(img2, temp2, Size(), scale, scale, method); img2 = temp2; }*/ Size img_size = img1.size(); if( intrinsic_filename ) { /* passed to init function stereoRectify( M1, D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2 ); initUndistortRectifyMap(M1, D1, R1, P1, img_size, CV_16SC2, map11, map12); initUndistortRectifyMap(M2, D2, R2, P2, img_size, CV_16SC2, map21, map22);*/ remap(img1, img1r, map11, map12, INTER_LINEAR); remap(img2, img2r, map21, map22, INTER_LINEAR); img1 = img1r; img2 = img2r; rectification_output_vars.imageSize = img_size; rectification_output_vars.rectR = img2; rectification_output_vars.rectL = img1; rectification_output_vars.validROI1 = roi1; rectification_output_vars.validROI2 = roi2; rectification_output_vars.originalyGivenframeCycle = relevantframeCycleIndex; } Mat disp; int64 t = getTickCount(); if( alg == STEREO_BM ){ // connversion not needed because already gray and UINT8 //img1.convertTo(img1, CV_8UC1+CV_BGR2GRAY); //img2.convertTo(img2, CV_8UC1+CV_BGR2GRAY); bm->compute(img1, img2, disp); } else if( alg == STEREO_SGBM || alg == STEREO_HH ) sgbm->compute(img1, img2, disp); t = getTickCount() - t; //// printf("STEREO_BM/STEREO_SGBM Time elapsed: %fms\n\n", t*1000/getTickFrequency()); // disp = disp.colRange(numberOfDisparities, img1p.cols); if( alg != STEREO_VAR ) disp.convertTo(disp8, CV_8U, 255/(numberOfDisparities*16.)); else disp.convertTo(disp8, CV_8U); if( !no_display ) { namedWindow("disparity", 0); imshow("disparity", disp8); Mat tmpD ; applyColorMap(disp8,tmpD, cv::COLORMAP_JET); namedWindow("disparity Heat", 1); imshow("disparity Heat", tmpD); /// fflush(stdout); printf("\n"); } if(disparity_filename) imwrite(disparity_filename, disp8); { ///http://stackoverflow.com/questions/27374970/q-matrix-for-the-reprojectimageto3d-function-in-opencv ///#ifndef COMPILING_ON_ROBOT if (1==2) { Mat xyz_again; ///Q.at<double>(3,2) = Q.at<double>(3,2) ;////10.0; reprojectImageTo3D(disp, xyz_again, Q, true); Vec3f point_middle = xyz_again.at<Vec3f>(xyz_again.rows/2, xyz_again.cols/2); printf("\n\n middle point relative coor. are: %f %f %f \n\n", point_middle.val[0],point_middle.val[1],point_middle.val[2]); } ///#endif } ready_disparity_result = true; return 0; }