void desplaza(const Mat& src, Mat &dst, int desp){ Mat tempA; //Mat tempB; int d; if (desp>0){ d=desp; tempA.create(src.rows,src.cols+2*d,src.type()); tempA.setTo(Scalar::all(0)); Mat roiA(tempA, Rect(2*d,0,src.cols,src.rows)); src.copyTo(roiA); tempA(Rect(d, 0, src.cols, src.rows)).copyTo(dst); } else { d=-desp; tempA.create(src.rows,src.cols+2*d,src.type()); tempA.setTo(Scalar::all(0)); Mat roiA(tempA, Rect(0,0,src.cols,src.rows)); src.copyTo(roiA); tempA(Rect(d, 0, src.cols, src.rows)).copyTo(dst); } //cout<<"src=" <<src.rows<<" dst="<<dst.rows<<endl; }
void ajustaFFT(const Mat& src, Mat &dst){ Mat tempA; Mat tempB; //rows=N //cols=1 tempA.create(src.rows,2*src.cols,src.type()); tempA.setTo(Scalar::all(0)); Mat roiA(tempA, Rect(src.cols,0,src.cols,src.rows)); src.copyTo(roiA); Mat roiB(tempA,Rect(0,0,src.cols,src.rows)); src.copyTo(roiB); tempA(Rect(src.cols/2,0, src.cols, src.rows)).copyTo(dst); }
void get_spectrum(const cv::Mat &m, cv::Mat &real, cv::Mat &imag) { cv::Size dftSize; // compute the optimal size for faster DFT transform dftSize.width = cv::getOptimalDFTSize(m.cols); dftSize.height = cv::getOptimalDFTSize(m.rows); // temporary cv::Mat real_temp(dftSize, m.type(), cv::Scalar::all(0)); cv::Mat roiA(real_temp, cv::Rect(0, 0, m.cols, m.rows)); // Shift to get a centered spectrum for (int i = 0; i < m.rows; ++i) { for (int j = 0; j < m.cols; ++j) { if ((i + j) % 2 == 1) { // the sum is an odd number roiA.at<double>(i, j) = (-1) * m.at<double>(i, j); } else { roiA.at<double>(i, j) = m.at<double>(i, j); } } } cv::Mat imag_temp(dftSize, m.type(), cv::Scalar::all(0.0)); // Merge and dft cv::Mat temp, TEMP; cv::Mat imgs[] = {real_temp, imag_temp}; cv::merge(imgs, 2, temp); cv::dft(temp, TEMP, cv::DFT_COMPLEX_OUTPUT, m.rows); // Real and imaginary // recover real part from the first channel // both source and destination matrices must be allocated int rows = temp.rows; int cols = temp.cols; cv::Mat TEMP_1st_channel(rows, cols, m.type()); // real cv::Mat TEMP_2nd_channel(rows, cols, m.type()); // imag cv::Mat output[] = {TEMP_1st_channel, TEMP_2nd_channel}; const int fromTo[] = {0, 0, 1, 1}; // see mixChannels help, continuous numbers cv::mixChannels(&TEMP, 1, output, 2, fromTo, 2); // resize the output arrays if needed real.create(m.rows, m.cols, m.type()); imag.create(m.rows, m.cols, m.type()); TEMP_1st_channel(cv::Rect(0, 0, real.cols, real.rows)).copyTo(real); TEMP_2nd_channel(cv::Rect(0, 0, imag.cols, imag.rows)).copyTo(imag); }
void get_correlation2d(const cv::Mat &A, const cv::Mat &B, cv::Mat &corr) { IMP_LOG_VERBOSE("Computing 2D correlation " <<std::endl); IMP_USAGE_CHECK(((A.rows==B.rows) && (A.cols == B.cols)), "em2d:get_correlation2d: Matrices have different size."); // resize the output array if needed corr.create(A.rows,A.cols, A.type()); cv::Size dftSize; // compute the optimal size for faster DFT transform dftSize.width = cv::getOptimalDFTSize(A.cols); dftSize.height = cv::getOptimalDFTSize(A.rows); // temporary matrices cv::Mat tempA(dftSize, A.type(), cv::Scalar::all(0)); cv::Mat tempB(dftSize, B.type(), cv::Scalar::all(0)); // copy A and B to the top-left corners of tempA and tempB, respectively cv::Mat roiA(tempA, cv::Rect(0,0,A.cols,A.rows)); A.copyTo(roiA); cv::Mat roiB(tempB, cv::Rect(0,0,B.cols,B.rows)); B.copyTo(roiB); // FFT the padded A & B in-place; // use "nonzeroRows" hint for faster processing cv::dft(tempA, tempA, 0, A.rows); cv::dft(tempB, tempB, 0, B.rows); // multiply the spectrums; // the function handles packed spectrum representations well cv::mulSpectrums(tempA, tempB, tempA,0,true); // inverse transform // Even though all the result rows will be non-zero, // we need only the first corr.rows of them, and thus we // pass nonzeroRows == corr.rows cv::dft(tempA, tempA, cv::DFT_INVERSE + cv::DFT_SCALE, corr.rows); // now copy the result back to C. tempA(cv::Rect(0, 0, corr.cols, corr.rows)).copyTo(corr); do_matrix_to_image_flip(corr); }
void get_autocorrelation2d(const cv::Mat &m, cv::Mat &corr) { IMP_LOG_VERBOSE("Computing 2D autocorrelation " << std::endl); // resize the output array if needed cv::Size dftSize; // compute the optimal size for faster DFT transform dftSize.width = cv::getOptimalDFTSize(m.cols); dftSize.height = cv::getOptimalDFTSize(m.rows); // temporary cv::Mat tempA(dftSize, m.type(), cv::Scalar::all(0)); // copy A to the top-left corner of tempA cv::Mat roiA(tempA, cv::Rect(0, 0, m.cols, m.rows)); m.copyTo(roiA); cv::dft(tempA, tempA, 0, m.rows); cv::Mat temp; cv::mulSpectrums(tempA, tempA, temp, 0, true); cv::idft(temp, temp, cv::DFT_SCALE + cv::DFT_REAL_OUTPUT); corr.create(m.rows, m.cols, m.type()); temp(cv::Rect(0, 0, corr.cols, corr.rows)).copyTo(corr); em2d::do_matrix_to_image_flip(corr); }
int main(int argc, char * argv[]) { if(argc < 2) { showUsage(); } bool inv = false; for(int i=1; i<argc-1; ++i) { if(strcmp(argv[i], "-inv") == 0) { inv = true; printf(" Inversing option activated...\n"); continue; } if(argc > 3) { showUsage(); printf(" Not recognized option: \"%s\"\n", argv[i]); } } std::string path, pathRight; if(argc == 3 && !inv) { //two paths path = argv[1]; pathRight = argv[2]; printf(" Path left = %s\n", path.c_str()); printf(" Path right = %s\n", pathRight.c_str()); } else { path = argv[argc-1]; printf(" Path = %s\n", path.c_str()); } UDirectory dir(path, "jpg bmp png tiff jpeg"); UDirectory dirRight(pathRight, "jpg bmp png tiff jpeg"); if(!dir.isValid() || (!pathRight.empty() && !dirRight.isValid())) { printf("Path invalid!\n"); exit(-1); } std::string targetDirectory = path+"_joined"; UDirectory::makeDir(targetDirectory); printf(" Creating directory \"%s\"\n", targetDirectory.c_str()); std::string fileNameA = dir.getNextFilePath(); std::string fileNameB; if(dirRight.isValid()) { fileNameB = dirRight.getNextFilePath(); } else { fileNameB = dir.getNextFilePath(); } int i=1; while(!fileNameA.empty() && !fileNameB.empty()) { if(inv) { std::string tmp = fileNameA; fileNameA = fileNameB; fileNameB = tmp; } std::string ext = UFile::getExtension(fileNameA); std::string targetFilePath = targetDirectory+UDirectory::separator()+uNumber2Str(i++)+"."+ext; cv::Mat imageA = cv::imread(fileNameA.c_str()); cv::Mat imageB = cv::imread(fileNameB.c_str()); fileNameA.clear(); fileNameB.clear(); if(!imageA.empty() && !imageB.empty()) { cv::Size sizeA = imageA.size(); cv::Size sizeB = imageB.size(); cv::Size targetSize(0,0); targetSize.width = sizeA.width + sizeB.width; targetSize.height = sizeA.height > sizeB.height ? sizeA.height : sizeB.height; cv::Mat targetImage(targetSize, imageA.type()); cv::Mat roiA(targetImage, cv::Rect( 0, 0, sizeA.width, sizeA.height )); imageA.copyTo(roiA); cv::Mat roiB( targetImage, cvRect( sizeA.width, 0, sizeB.width, sizeB.height ) ); imageB.copyTo(roiB); if(!cv::imwrite(targetFilePath.c_str(), targetImage)) { printf("Error : saving to \"%s\" goes wrong...\n", targetFilePath.c_str()); } else { printf("Saved \"%s\" \n", targetFilePath.c_str()); } fileNameA = dir.getNextFilePath(); if(dirRight.isValid()) { fileNameB = dirRight.getNextFilePath(); } else { fileNameB = dir.getNextFilePath(); } } else { printf("Error: loading images failed!\n"); } } printf("%d files processed\n", i-1); return 0; }
int main(int argc, char * argv[]) { ULogger::setType(ULogger::kTypeConsole); ULogger::setLevel(ULogger::kInfo); //ULogger::setPrintTime(false); //ULogger::setPrintWhere(false); int driver = 0; std::string stereoSavePath; float rate = 0.0f; std::string fourcc = "MJPG"; if(argc < 2) { showUsage(); } else { for(int i=1; i<argc; ++i) { if(strcmp(argv[i], "-rate") == 0) { ++i; if(i < argc) { rate = uStr2Float(argv[i]); if(rate < 0.0f) { showUsage(); } } else { showUsage(); } continue; } if(strcmp(argv[i], "-save_stereo") == 0) { ++i; if(i < argc) { stereoSavePath = argv[i]; } else { showUsage(); } continue; } if(strcmp(argv[i], "-fourcc") == 0) { ++i; if(i < argc) { fourcc = argv[i]; if(fourcc.size() != 4) { UERROR("fourcc should be 4 characters."); showUsage(); } } else { showUsage(); } continue; } if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0) { showUsage(); } else if(i< argc-1) { printf("Unrecognized option \"%s\"", argv[i]); showUsage(); } // last driver = atoi(argv[i]); if(driver < 0 || driver > 8) { UERROR("driver should be between 0 and 8."); showUsage(); } } } UINFO("Using driver %d", driver); rtabmap::Camera * camera = 0; if(driver < 6) { if(!stereoSavePath.empty()) { UWARN("-save_stereo option cannot be used with RGB-D drivers."); stereoSavePath.clear(); } if(driver == 0) { camera = new rtabmap::CameraOpenni(); } else if(driver == 1) { if(!rtabmap::CameraOpenNI2::available()) { UERROR("Not built with OpenNI2 support..."); exit(-1); } camera = new rtabmap::CameraOpenNI2(); } else if(driver == 2) { if(!rtabmap::CameraFreenect::available()) { UERROR("Not built with Freenect support..."); exit(-1); } camera = new rtabmap::CameraFreenect(); } else if(driver == 3) { if(!rtabmap::CameraOpenNICV::available()) { UERROR("Not built with OpenNI from OpenCV support..."); exit(-1); } camera = new rtabmap::CameraOpenNICV(false); } else if(driver == 4) { if(!rtabmap::CameraOpenNICV::available()) { UERROR("Not built with OpenNI from OpenCV support..."); exit(-1); } camera = new rtabmap::CameraOpenNICV(true); } else if(driver == 5) { if(!rtabmap::CameraFreenect2::available()) { UERROR("Not built with Freenect2 support..."); exit(-1); } camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColor2DepthSD); } } else if(driver == 6) { if(!rtabmap::CameraStereoDC1394::available()) { UERROR("Not built with DC1394 support..."); exit(-1); } camera = new rtabmap::CameraStereoDC1394(); } else if(driver == 7) { if(!rtabmap::CameraStereoFlyCapture2::available()) { UERROR("Not built with FlyCapture2/Triclops support..."); exit(-1); } camera = new rtabmap::CameraStereoFlyCapture2(); } else if(driver == 8) { if(!rtabmap::CameraStereoZed::available()) { UERROR("Not built with ZED sdk support..."); exit(-1); } camera = new rtabmap::CameraStereoZed(0); } else { UFATAL(""); } if(!camera->init()) { printf("Camera init failed! Please select another driver (see \"--help\").\n"); delete camera; exit(1); } rtabmap::SensorData data = camera->takeImage(); if(data.imageRaw().cols != data.depthOrRightRaw().cols || data.imageRaw().rows != data.depthOrRightRaw().rows) { UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.", data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows); } pcl::visualization::CloudViewer * viewer = 0; if(!data.stereoCameraModel().isValidForProjection() && (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValidForProjection())) { UWARN("Camera not calibrated! The registered cloud cannot be shown."); } else { viewer = new pcl::visualization::CloudViewer("cloud"); } rtabmap::Transform t(1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0); cv::VideoWriter videoWriter; UDirectory dir; if(!stereoSavePath.empty() && !data.imageRaw().empty() && !data.rightRaw().empty()) { if(UFile::getExtension(stereoSavePath).compare("avi") == 0) { if(data.imageRaw().size() == data.rightRaw().size()) { if(rate <= 0) { UERROR("You should set the input rate when saving stereo images to a video file."); showUsage(); } cv::Size targetSize = data.imageRaw().size(); targetSize.width *= 2; UASSERT(fourcc.size() == 4); videoWriter.open( stereoSavePath, CV_FOURCC(fourcc.at(0), fourcc.at(1), fourcc.at(2), fourcc.at(3)), rate, targetSize, data.imageRaw().channels() == 3); } else { UERROR("Images not the same size, cannot save stereo images to the video file."); } } else if(UDirectory::exists(stereoSavePath)) { UDirectory::makeDir(stereoSavePath+"/"+"left"); UDirectory::makeDir(stereoSavePath+"/"+"right"); } else { UERROR("Directory \"%s\" doesn't exist.", stereoSavePath.c_str()); stereoSavePath.clear(); } } // to catch the ctrl-c signal(SIGABRT, &sighandler); signal(SIGTERM, &sighandler); signal(SIGINT, &sighandler); int id=1; while(!data.imageRaw().empty() && (viewer==0 || !viewer->wasStopped()) && running) { cv::Mat rgb = data.imageRaw(); if(!data.depthRaw().empty() && (data.depthRaw().type() == CV_16UC1 || data.depthRaw().type() == CV_32FC1)) { // depth cv::Mat depth = data.depthRaw(); if(depth.type() == CV_32FC1) { depth = rtabmap::util2d::cvtDepthFromFloat(depth); } if(rgb.cols == depth.cols && rgb.rows == depth.rows && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB( rgb, depth, data.cameraModels()[0]); cloud = rtabmap::util3d::transformPointCloud(cloud, t); if(viewer) viewer->showCloud(cloud, "cloud"); } else if(!depth.empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth( depth, data.cameraModels()[0]); cloud = rtabmap::util3d::transformPointCloud(cloud, t); viewer->showCloud(cloud, "cloud"); } cv::Mat tmp; unsigned short min=0, max = 2048; uMinMax((unsigned short*)depth.data, depth.rows*depth.cols, min, max); depth.convertTo(tmp, CV_8UC1, 255.0/max); cv::imshow("Video", rgb); // show frame cv::imshow("Depth", tmp); } else if(!data.rightRaw().empty()) { // stereo cv::Mat right = data.rightRaw(); cv::imshow("Left", rgb); // show frame cv::imshow("Right", right); if(rgb.cols == right.cols && rgb.rows == right.rows && data.stereoCameraModel().isValidForProjection()) { if(right.channels() == 3) { cv::cvtColor(right, right, CV_BGR2GRAY); } pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromStereoImages( rgb, right, data.stereoCameraModel()); cloud = rtabmap::util3d::transformPointCloud(cloud, t); if(viewer) viewer->showCloud(cloud, "cloud"); } } int c = cv::waitKey(10); // wait 10 ms or for key stroke if(c == 27) break; // if ESC, break and quit if(videoWriter.isOpened()) { cv::Mat left = data.imageRaw(); cv::Mat right = data.rightRaw(); if(left.size() == right.size()) { cv::Size targetSize = left.size(); targetSize.width *= 2; cv::Mat targetImage(targetSize, left.type()); if(right.type() != left.type()) { cv::Mat tmp; cv::cvtColor(right, tmp, left.channels()==3?CV_GRAY2BGR:CV_BGR2GRAY); right = tmp; } UASSERT(left.type() == right.type()); cv::Mat roiA(targetImage, cv::Rect( 0, 0, left.size().width, left.size().height )); left.copyTo(roiA); cv::Mat roiB( targetImage, cvRect( left.size().width, 0, left.size().width, left.size().height ) ); right.copyTo(roiB); videoWriter.write(targetImage); printf("Saved frame %d to \"%s\"\n", id, stereoSavePath.c_str()); } else { UERROR("Left and right images are not the same size!?"); } } else if(!stereoSavePath.empty()) { cv::imwrite(stereoSavePath+"/"+"left/"+uNumber2Str(id) + ".jpg", data.imageRaw()); cv::imwrite(stereoSavePath+"/"+"right/"+uNumber2Str(id) + ".jpg", data.rightRaw()); printf("Saved frames %d to \"%s/left\" and \"%s/right\" directories\n", id, stereoSavePath.c_str(), stereoSavePath.c_str()); } ++id; data = camera->takeImage(); } printf("Closing...\n"); if(viewer) { delete viewer; } cv::destroyWindow("Video"); cv::destroyWindow("Depth"); delete camera; return 0; }