From 75e876bcc8c22f4d615bec31841a24069c4cb77d Mon Sep 17 00:00:00 2001 From: dubrovskiykot Date: Sun, 26 Jan 2020 09:18:19 -1000 Subject: [PATCH 1/2] Needed folders existence and their creation added --- src/script1.cpp | 23 ++++++++------ src/script2.cpp | 44 +++++++++++++++++++++++--- src/script4.cpp | 26 ++++++++++++---- src/script5.cpp | 83 +++++++++++++++++++++++++------------------------ src/script6.cpp | 15 +++------ src/script7.cpp | 56 +++++++-------------------------- 6 files changed, 129 insertions(+), 118 deletions(-) diff --git a/src/script1.cpp b/src/script1.cpp index 7dcc28d..3f76603 100644 --- a/src/script1.cpp +++ b/src/script1.cpp @@ -1,7 +1,7 @@ // Copyright (C) 2019 Eugene a.k.a. Realizator, stereopi.com, virt2real team -// Ported from Python to C++ by Konstantin Ozernov on 10/10/2019. +// Ported from Python to C by Konstantin Ozernov on 10/10/2019. // -// This file is part of StereoPi С++ tutorial scripts, and has been +// This file is part of StereoPi С tutorial scripts, and has been // ported from Pyton version (https://github.com/realizator/stereopi-fisheye-robot) // // StereoPi tutorial is free software: you can redistribute it @@ -34,14 +34,12 @@ long long getTimestamp() return epoch.count(); } -std::string folder_name = "/home/pi/stereopi-cpp-tutorial/"; - int main() { fprintf(stderr, "You can press 'Q' to quit this script.\n"); - int imgWidth = 1280; - int imgHeight = 480; + int imgHeight = 240; + int imgWidth = 640; fprintf(stderr, "Camera resolution: %d x %d\n", imgWidth, imgHeight); @@ -60,6 +58,7 @@ int main() long long totalTime = 0; while (true) { + long long frameStartTime = getTimestamp(); fseek(fp, -bufLen, SEEK_END); count = fread(buf, sizeof(*buf), bufLen, fp); if (count == 0) @@ -67,17 +66,21 @@ int main() cv::Mat frame(imgHeight, imgWidth, CV_8UC1, buf); cv::imshow("video", frame); framesNumber++; + + long long frameTime = getTimestamp() - frameStartTime; + totalTime += frameTime; char k = cv::waitKey(1); if (k == 'q' || k == 'Q') { - cv::imwrite(folder_name + "frame.jpg", frame); + cv::imwrite("frame.jpg", frame); break; } } - totalTime = getTimestamp(); - float avgFPS = (totalTime -startTime)/ 1000 / framesNumber; - fprintf(stderr, "Average FPS: %f\n", 1000 / avgFPS); + //float fps = (float)( 1000000 * framesNumber) / (getTimestamp() - startTime); + float avgTime = totalTime / 1000 / framesNumber; + fprintf(stderr, "Average time between frames: %f ms\n", avgTime); + fprintf(stderr, "Average FPS: %f\n", 1000 / avgTime); return 0; } diff --git a/src/script2.cpp b/src/script2.cpp index fb1e4f8..5af887f 100644 --- a/src/script2.cpp +++ b/src/script2.cpp @@ -1,7 +1,7 @@ // Copyright (C) 2019 Eugene a.k.a. Realizator, stereopi.com, virt2real team -// Ported from Python to C++ by Konstantin Ozernov on 10/10/2019. +// Ported from Python to C by Konstantin Ozernov on 10/10/2019. // -// This file is part of StereoPi С++ tutorial scripts, and has been +// This file is part of StereoPi С tutorial scripts, and has been // ported from Pyton version (https://github.com/realizator/stereopi-fisheye-robot) // // StereoPi tutorial is free software: you can redistribute it @@ -24,7 +24,8 @@ #include //#include #include - +#include +#include long long getTimestamp() { @@ -42,8 +43,7 @@ int main() int total_photos = 50;// # Number of images to take int countdown = 3;// # Interval for count-down timer, seconds int font = cv::FONT_HERSHEY_SIMPLEX;// # Cowntdown timer font - std::string folder_name = "/home/pi/stereopi-cpp-tutorial/"; - + std::string folder_name = "/home/pi/stereopi-cpp-tutorial/"; //Camera settimgs int cam_width = 1280;// # Cam sensor width settings int cam_height = 480;// # Cam sensor height settings @@ -76,6 +76,39 @@ int main() return 1; } + // Check if needed dirs exist. If do not - try to create them. + std::string scenesDir = folder_name + "scenes"; + std::string pairsDir = folder_name + "pairs"; + DIR* dir = opendir(scenesDir.c_str()); + if (dir) + { + closedir(dir); + } + else + { + int res = mkdir(scenesDir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH); + if ( res == -1) + { + fprintf(stderr, "Cannot create Scenes dir!\n"); + return 1; + } + } + dir = opendir(pairsDir.c_str()); + if (dir) + { + closedir(dir); + } + else + { + int res = mkdir(pairsDir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH); + if ( res == -1) + { + fprintf(stderr, "Cannot create Pairs dir!\n"); + return 1; + } + } + + int bufLen = cam_width * cam_height; char *buf = (char *)malloc(bufLen); int count = 0; @@ -93,6 +126,7 @@ int main() count = fread(buf, sizeof(*buf), bufLen, fp); if (count == 0) break; + cv::Mat frame(cam_height, cam_width, CV_8UC1, buf); long long t1 = getTimestamp(); int cntdwn_timer = countdown - (int)((t1-t2) / 1000000); diff --git a/src/script4.cpp b/src/script4.cpp index 66eefc6..2f45fcf 100644 --- a/src/script4.cpp +++ b/src/script4.cpp @@ -1,7 +1,7 @@ // Copyright (C) 2019 Eugene a.k.a. Realizator, stereopi.com, virt2real team -// Ported from Python to C++ by Konstantin Ozernov on 10/10/2019. +// Ported from Python to C by Konstantin Ozernov on 10/10/2019. // -// This file is part of StereoPi С++ tutorial scripts, and has been +// This file is part of StereoPi С tutorial scripts, and has been // ported from Pyton version (https://github.com/realizator/stereopi-fisheye-robot) // // StereoPi tutorial is free software: you can redistribute it @@ -52,12 +52,10 @@ void calibrate_one_camera(std::vector > objpoints, std::v int N_OK = (int)objpoints.size(); cv::Size DIM(img_width, img_height); - cv::Mat K;// = cv::Mat::zeros(3, 3, CV_32FC1); - cv::Mat D;// = cv::Mat::zeros(4, 1, CV_32FC1); + cv::Mat K; + cv::Mat D; cv::Vec3f pt(0, 0, 0); - // std::vector rvecs(N_OK, pt); - // std::vector tvecs(N_OK, pt); cv::Mat rvecs = cv::Mat::zeros(N_OK, 1, CV_32FC3); cv::Mat tvecs = cv::Mat::zeros(N_OK, 1, CV_32FC3); @@ -164,6 +162,22 @@ bool calibrate_stereo_cameras(int res_x = img_width, int res_y = img_height) int main() { + // Check if calibration data folder exists. + DIR *dir = opendir(calibration_data_folder.c_str()); + if (dir) + { + closedir(calibration_data_folder); + } + else + { + int res = mkdir(calibration_data_folder.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH); + if (res == -1) + { + fprintf(stderr, "Cannot create calibration data folder!\n"); + return 1; + } + } + // Global variables preset int total_photos = 50; diff --git a/src/script5.cpp b/src/script5.cpp index 15bd0b7..9e42b2d 100644 --- a/src/script5.cpp +++ b/src/script5.cpp @@ -1,7 +1,7 @@ // Copyright (C) 2019 Eugene a.k.a. Realizator, stereopi.com, virt2real team -// Ported from Python to C++ by Konstantin Ozernov on 10/10/2019. +// Ported from Python to C by Konstantin Ozernov on 10/10/2019. // -// This file is part of StereoPi С++ tutorial scripts, and has been +// This file is part of StereoPi С tutorial scripts, and has been // ported from Pyton version (https://github.com/realizator/stereopi-fisheye-robot) // // StereoPi tutorial is free software: you can redistribute it @@ -46,7 +46,7 @@ int SpcklRng = 15; int SpklWinSze = 100; // Global settings -std::string folder_name = "/home/pi/stereopi-cpp-tutorial/"; +std::string folder_name = "home/pi/stereopi-cpp-tutorial/"; std::string calibration_data_folder = folder_name + "calibration_data/"; cv::Mat left, right; @@ -191,8 +191,8 @@ int main() cv::createTrackbar("PreFiltCap", "Image", &preFiltCap, 63, onTrackbar); cv::createTrackbar("MinDISP", "Image", &minDisp, 100, onMinDisp); cv::createTrackbar("NumOfDisp", "Image", &numOfDisp, 256, onTrackbar); - cv::createTrackbar("TxtrThrshld", "Image", &TxtrThrshld, 100, onTrackbar); - cv::createTrackbar("UnicRatio", "Image", &unicRatio, 100, onTrackbar); + cv::createTrackbar("TxtrThrshld", "Image", &TxtrThrshld, 1000, onTrackbar); + cv::createTrackbar("UnicRatio", "Image", &unicRatio, 20, onTrackbar); cv::createTrackbar("SpcklRng", "Image", &SpcklRng, 40, onTrackbar); cv::createTrackbar("SpklWinSze", "Image", &SpklWinSze, 300, onTrackbar); @@ -202,42 +202,43 @@ int main() int prevFrameNumber = 0; while (true) { - fseek(fp, -bufLen, SEEK_END); - count = fread(buf, sizeof(*buf), bufLen, fp); - if (count == 0) - break; - - cv::Mat img(photo_height, photo_width, CV_8UC1, buf); - left = cv::Mat(img, cv::Rect(0, 0, image_width, image_height)); - right = cv::Mat(img, cv::Rect(image_width, 0, image_width, image_height)); - - // Rectifying left and right images - cv::remap(left, left, leftMapX, leftMapY, cv::INTER_LINEAR); - cv::remap(right, right, rightMapX, rightMapY, cv::INTER_LINEAR); - - stereo_depth_map(left, right); - - cv::imshow("Left", left); - cv::imshow("Right", right); - - char k = cv::waitKey(1); - if (k == 's' || k == 'S') - { - fprintf(stderr, "k = %c\n", k); - saveParams(); - break; - } - else if (k == 'q' || k == 'Q') - break; - - frameNumber++; - long long currTime = getTimestamp(); - if (currTime - prevTime > 1000000) - { - fprintf(stderr, "FPS: %d\n", frameNumber - prevFrameNumber); - prevFrameNumber = frameNumber; - prevTime = currTime; - } + fseek(fp, -bufLen, SEEK_END); + count = fread(buf, sizeof(*buf), bufLen, fp); + if (count == 0) + break; + + cv::Mat img(photo_height, photo_width, CV_8UC1, buf); + + left = cv::Mat(img, cv::Rect(0, 0, image_width, image_height)); + right = cv::Mat(img, cv::Rect(image_width, 0, image_width, image_height)); + + // Rectifying left and right images + cv::remap(left, left, leftMapX, leftMapY, cv::INTER_LINEAR); + cv::remap(right, right, rightMapX, rightMapY, cv::INTER_LINEAR); + + stereo_depth_map(left, right); + + cv::imshow("Left", left); + cv::imshow("Right", right); + + char k = cv::waitKey(1); + if (k == 's' || k == 'S') + { + fprintf(stderr, "k = %c\n", k); + saveParams(); + break; + } + else if (k == 'q' || k == 'Q') + break; + + frameNumber++; + long long currTime = getTimestamp(); + if (currTime - prevTime > 1000000) + { + fprintf(stderr, "FPS: %d\n", frameNumber - prevFrameNumber); + prevFrameNumber = frameNumber; + prevTime = currTime; + } } return 0; diff --git a/src/script6.cpp b/src/script6.cpp index 266d39a..57fdd3b 100644 --- a/src/script6.cpp +++ b/src/script6.cpp @@ -1,7 +1,7 @@ // Copyright (C) 2019 Eugene a.k.a. Realizator, stereopi.com, virt2real team -// Ported from Python to C++ by Konstantin Ozernov on 10/10/2019. +// Ported from Python to C by Konstantin Ozernov on 10/10/2019. // -// This file is part of StereoPi С++ tutorial scripts, and has been +// This file is part of StereoPi С tutorial scripts, and has been // ported from Pyton version (https://github.com/realizator/stereopi-fisheye-robot) // // StereoPi tutorial is free software: you can redistribute it @@ -46,7 +46,7 @@ int SpklWinSze = 100; float actualFPS = 0.0; // Global settings -std::string folder_name = "/home/pi/stereopi-cpp-tutorial/"; +std::string folder_name = "home/pi/stereopi-cpp-tutorial/""; std::string calibration_data_folder = folder_name + "calibration_data/"; long long getTimestamp() { @@ -80,11 +80,6 @@ void loadParams() bool stereo_depth_map(cv::Mat &left, cv::Mat &right, cv::Ptr bm, float &time1, float &time2, float &time3, float &time4) { - - - // cv::cvtColor(left, left, cv::COLOR_BGR2GRAY); - // cv::cvtColor(right, right, cv::COLOR_BGR2GRAY); - cv::Mat disp, disp8, colored; long long startT = getTimestamp(); bm->compute( left, right, disp); @@ -170,8 +165,6 @@ int main() float time1 = 0, time2 = 0, time3 = 0, time4 = 0, time5 = 0, time6 = 0, time7 = 0, time8 = 0, time9 = 0, time10 = 0, time11 = 0; int frameNumber = 0; - // while ((count = fread(buf, sizeof(*buf), bufLen, fp)) != 0) - // { while (true) { fseek(fp, -bufLen, SEEK_END); @@ -206,7 +199,7 @@ int main() time4 += timeShow - timeRectify; - // Taking a strip from our image for lidar-like mode (and saving CPU) + // Taking a strip from our image for lidar-like mode (and saving CPU) // cv::Mat imgLCut = cv::Mat(left, cv::Rect(0, 80, left.cols, 80)); // cv::Mat imgRCut = cv::Mat(right, cv::Rect(0, 80, right.cols, 80)); diff --git a/src/script7.cpp b/src/script7.cpp index 48ab3ec..0779416 100644 --- a/src/script7.cpp +++ b/src/script7.cpp @@ -1,7 +1,7 @@ // Copyright (C) 2019 Eugene a.k.a. Realizator, stereopi.com, virt2real team -// Ported from Python to C++ by Konstantin Ozernov on 10/10/2019. +// Ported from Python to C by Konstantin Ozernov on 10/10/2019. // -// This file is part of StereoPi С++ tutorial scripts, and has been +// This file is part of StereoPi С tutorial scripts, and has been // ported from Pyton version (https://github.com/realizator/stereopi-fisheye-robot) // // StereoPi tutorial is free software: you can redistribute it @@ -32,7 +32,6 @@ #include #include #include -#include int SWS = 7; int PFS = 7; @@ -43,16 +42,7 @@ int TxtrThrshld = 13; int unicRatio = 19; int SpcklRng = 0; int SpklWinSze = 0; -float min_y = 10000.0; -float max_y = -10000.0; -float min_x = 10000.0; -float max_x = -10000.0; -float map_zoom_x = 100; -float map_zoom_y = 100; -float map_zoom = 50; -// Comfort and debug flags -bool auto_zoom = true; //try to fit all points on 2D map display if true -bool show_debug = true; //will show additional info like max and min of X and Y + // Global settings std::string folder_name = "/home/pi/stereopi-cpp-tutorial/"; std::string calibration_data_folder = folder_name + "calibration_data/"; @@ -153,9 +143,6 @@ int main() bm->setSpeckleRange(SpcklRng); bm->setDisp12MaxDiff(1); - // while ((count = fread(buf, sizeof(*buf), bufLen, fp)) != 0) - // { - while (true) { fseek(fp, -bufLen, SEEK_END); @@ -220,41 +207,20 @@ int main() // Put all points to the 2D map // Change map_zoom to adjust visible range! - map_zoom_y = (map_height/(max_y-min_y)); - map_zoom_x = (map_width/(max_x-min_x)); - // prevent manual/autozoom to set extremely small numbers caused by values spike - map_zoom_x = std::max(float(10.0), map_zoom_x); - map_zoom_y = std::max(float(10.0), map_zoom_y); - if (show_debug) - { - fprintf(stderr, "\nX and Y before zoom: \nmax_y = %.2f, min_y = %.2f, max_x = %.2f, min_x = %.2f \n", max_y, min_y, max_x, min_x ); - fprintf(stderr, "map_height = %d, map_width = %d, Autozoom: %s, zoom X = %.2f, zoom Y = %.2f\n", map_height, map_width, auto_zoom ? "On" : "Off", map_zoom_x, map_zoom_y ); - } + float map_zoom = 50.0; + for (int i = 0; i < points.rows; i++) { - float cur_y = -points.at(i, 0)[0]; - float cur_x = points.at(i, 0)[1]; - if (!isinf(cur_y)) - { - min_y = std::min(cur_y, min_y); - max_y = std::max(cur_y, max_y); - - } - if (!isinf(cur_x)) - { - max_x = std::max(cur_x, max_x); - min_x = std::min(cur_x, min_x); - } - if (!auto_zoom) {map_zoom_x = map_zoom; map_zoom_y = map_zoom;} - - int xx = (int)((cur_x) * map_zoom_x) + (int)(map_width/2); // zero point is in the middle of the map - int yy = map_height - (int)((cur_y-min_y) * map_zoom_y); // zero point is at the bottom of the map + float cur_x = points.at(i, 0)[0]; + float cur_y = points.at(i, 0)[1]; + + int xx = (int)(cur_y * map_zoom) + (int)(map_width/2); // zero point is in the middle of the map + int yy = (int)(cur_x * map_zoom) + map_height; // zero point is at the bottom of the map // If the point fits on our 2D map - let's draw it! - if (xx < map_width && xx >= 0 && yy < map_height && yy >= 0) + if (yy < map_width && yy >= 0 && xx < map_height && yy >= 0) xy_projection.at(yy, xx) = maxInColumns.at(i, 0);//maximized_line.at(i, 0); } - cv::Mat xy_projection_color, max_line_color, disparity_color; cv::applyColorMap(native_disparity, disparity_color, cv::COLORMAP_JET); cv::applyColorMap(xy_projection, xy_projection_color, cv::COLORMAP_JET); From fe3d132387c4471a9fc71c7277a0d36238735a8f Mon Sep 17 00:00:00 2001 From: dubrovskiykot Date: Sun, 26 Jan 2020 10:00:02 -1000 Subject: [PATCH 2/2] Minor fixes --- src/script1.cpp | 24 ++++++-------- src/script2.cpp | 4 +-- src/script4.cpp | 4 +-- src/script5.cpp | 83 ++++++++++++++++++++++++------------------------- src/script6.cpp | 6 ++-- src/script7.cpp | 56 ++++++++++++++++++++++++++------- 6 files changed, 103 insertions(+), 74 deletions(-) diff --git a/src/script1.cpp b/src/script1.cpp index 3f76603..c984920 100644 --- a/src/script1.cpp +++ b/src/script1.cpp @@ -1,7 +1,7 @@ // Copyright (C) 2019 Eugene a.k.a. Realizator, stereopi.com, virt2real team -// Ported from Python to C by Konstantin Ozernov on 10/10/2019. +// Ported from Python to C++ by Konstantin Ozernov on 10/10/2019. // -// This file is part of StereoPi С tutorial scripts, and has been +// This file is part of StereoPi С++ tutorial scripts, and has been // ported from Pyton version (https://github.com/realizator/stereopi-fisheye-robot) // // StereoPi tutorial is free software: you can redistribute it @@ -34,12 +34,14 @@ long long getTimestamp() return epoch.count(); } +std::string folder_name = "/home/pi/stereopi-cpp-tutorial/"; + int main() { fprintf(stderr, "You can press 'Q' to quit this script.\n"); - int imgHeight = 240; - int imgWidth = 640; + int imgWidth = 1280; + int imgHeight = 480; fprintf(stderr, "Camera resolution: %d x %d\n", imgWidth, imgHeight); @@ -58,7 +60,6 @@ int main() long long totalTime = 0; while (true) { - long long frameStartTime = getTimestamp(); fseek(fp, -bufLen, SEEK_END); count = fread(buf, sizeof(*buf), bufLen, fp); if (count == 0) @@ -66,21 +67,16 @@ int main() cv::Mat frame(imgHeight, imgWidth, CV_8UC1, buf); cv::imshow("video", frame); framesNumber++; - - long long frameTime = getTimestamp() - frameStartTime; - totalTime += frameTime; char k = cv::waitKey(1); if (k == 'q' || k == 'Q') { - cv::imwrite("frame.jpg", frame); + cv::imwrite(folder_name + "frame.jpg", frame); break; } } - //float fps = (float)( 1000000 * framesNumber) / (getTimestamp() - startTime); - float avgTime = totalTime / 1000 / framesNumber; - fprintf(stderr, "Average time between frames: %f ms\n", avgTime); - fprintf(stderr, "Average FPS: %f\n", 1000 / avgTime); + totalTime = getTimestamp(); + float avgFPS = (totalTime -startTime)/ 1000 / framesNumber; + fprintf(stderr, "Average FPS: %f\n", 1000 / avgFPS); return 0; } - diff --git a/src/script2.cpp b/src/script2.cpp index 5af887f..c3723f7 100644 --- a/src/script2.cpp +++ b/src/script2.cpp @@ -1,7 +1,7 @@ // Copyright (C) 2019 Eugene a.k.a. Realizator, stereopi.com, virt2real team -// Ported from Python to C by Konstantin Ozernov on 10/10/2019. +// Ported from Python to C++ by Konstantin Ozernov on 10/10/2019. // -// This file is part of StereoPi С tutorial scripts, and has been +// This file is part of StereoPi С++ tutorial scripts, and has been // ported from Pyton version (https://github.com/realizator/stereopi-fisheye-robot) // // StereoPi tutorial is free software: you can redistribute it diff --git a/src/script4.cpp b/src/script4.cpp index 2f45fcf..d80e1d7 100644 --- a/src/script4.cpp +++ b/src/script4.cpp @@ -1,7 +1,7 @@ // Copyright (C) 2019 Eugene a.k.a. Realizator, stereopi.com, virt2real team -// Ported from Python to C by Konstantin Ozernov on 10/10/2019. +// Ported from Python to C++ by Konstantin Ozernov on 10/10/2019. // -// This file is part of StereoPi С tutorial scripts, and has been +// This file is part of StereoPi С++ tutorial scripts, and has been // ported from Pyton version (https://github.com/realizator/stereopi-fisheye-robot) // // StereoPi tutorial is free software: you can redistribute it diff --git a/src/script5.cpp b/src/script5.cpp index 9e42b2d..15bd0b7 100644 --- a/src/script5.cpp +++ b/src/script5.cpp @@ -1,7 +1,7 @@ // Copyright (C) 2019 Eugene a.k.a. Realizator, stereopi.com, virt2real team -// Ported from Python to C by Konstantin Ozernov on 10/10/2019. +// Ported from Python to C++ by Konstantin Ozernov on 10/10/2019. // -// This file is part of StereoPi С tutorial scripts, and has been +// This file is part of StereoPi С++ tutorial scripts, and has been // ported from Pyton version (https://github.com/realizator/stereopi-fisheye-robot) // // StereoPi tutorial is free software: you can redistribute it @@ -46,7 +46,7 @@ int SpcklRng = 15; int SpklWinSze = 100; // Global settings -std::string folder_name = "home/pi/stereopi-cpp-tutorial/"; +std::string folder_name = "/home/pi/stereopi-cpp-tutorial/"; std::string calibration_data_folder = folder_name + "calibration_data/"; cv::Mat left, right; @@ -191,8 +191,8 @@ int main() cv::createTrackbar("PreFiltCap", "Image", &preFiltCap, 63, onTrackbar); cv::createTrackbar("MinDISP", "Image", &minDisp, 100, onMinDisp); cv::createTrackbar("NumOfDisp", "Image", &numOfDisp, 256, onTrackbar); - cv::createTrackbar("TxtrThrshld", "Image", &TxtrThrshld, 1000, onTrackbar); - cv::createTrackbar("UnicRatio", "Image", &unicRatio, 20, onTrackbar); + cv::createTrackbar("TxtrThrshld", "Image", &TxtrThrshld, 100, onTrackbar); + cv::createTrackbar("UnicRatio", "Image", &unicRatio, 100, onTrackbar); cv::createTrackbar("SpcklRng", "Image", &SpcklRng, 40, onTrackbar); cv::createTrackbar("SpklWinSze", "Image", &SpklWinSze, 300, onTrackbar); @@ -202,43 +202,42 @@ int main() int prevFrameNumber = 0; while (true) { - fseek(fp, -bufLen, SEEK_END); - count = fread(buf, sizeof(*buf), bufLen, fp); - if (count == 0) - break; - - cv::Mat img(photo_height, photo_width, CV_8UC1, buf); - - left = cv::Mat(img, cv::Rect(0, 0, image_width, image_height)); - right = cv::Mat(img, cv::Rect(image_width, 0, image_width, image_height)); - - // Rectifying left and right images - cv::remap(left, left, leftMapX, leftMapY, cv::INTER_LINEAR); - cv::remap(right, right, rightMapX, rightMapY, cv::INTER_LINEAR); - - stereo_depth_map(left, right); - - cv::imshow("Left", left); - cv::imshow("Right", right); - - char k = cv::waitKey(1); - if (k == 's' || k == 'S') - { - fprintf(stderr, "k = %c\n", k); - saveParams(); - break; - } - else if (k == 'q' || k == 'Q') - break; - - frameNumber++; - long long currTime = getTimestamp(); - if (currTime - prevTime > 1000000) - { - fprintf(stderr, "FPS: %d\n", frameNumber - prevFrameNumber); - prevFrameNumber = frameNumber; - prevTime = currTime; - } + fseek(fp, -bufLen, SEEK_END); + count = fread(buf, sizeof(*buf), bufLen, fp); + if (count == 0) + break; + + cv::Mat img(photo_height, photo_width, CV_8UC1, buf); + left = cv::Mat(img, cv::Rect(0, 0, image_width, image_height)); + right = cv::Mat(img, cv::Rect(image_width, 0, image_width, image_height)); + + // Rectifying left and right images + cv::remap(left, left, leftMapX, leftMapY, cv::INTER_LINEAR); + cv::remap(right, right, rightMapX, rightMapY, cv::INTER_LINEAR); + + stereo_depth_map(left, right); + + cv::imshow("Left", left); + cv::imshow("Right", right); + + char k = cv::waitKey(1); + if (k == 's' || k == 'S') + { + fprintf(stderr, "k = %c\n", k); + saveParams(); + break; + } + else if (k == 'q' || k == 'Q') + break; + + frameNumber++; + long long currTime = getTimestamp(); + if (currTime - prevTime > 1000000) + { + fprintf(stderr, "FPS: %d\n", frameNumber - prevFrameNumber); + prevFrameNumber = frameNumber; + prevTime = currTime; + } } return 0; diff --git a/src/script6.cpp b/src/script6.cpp index 57fdd3b..7747609 100644 --- a/src/script6.cpp +++ b/src/script6.cpp @@ -1,7 +1,7 @@ // Copyright (C) 2019 Eugene a.k.a. Realizator, stereopi.com, virt2real team -// Ported from Python to C by Konstantin Ozernov on 10/10/2019. +// Ported from Python to C++ by Konstantin Ozernov on 10/10/2019. // -// This file is part of StereoPi С tutorial scripts, and has been +// This file is part of StereoPi С++ tutorial scripts, and has been // ported from Pyton version (https://github.com/realizator/stereopi-fisheye-robot) // // StereoPi tutorial is free software: you can redistribute it @@ -46,7 +46,7 @@ int SpklWinSze = 100; float actualFPS = 0.0; // Global settings -std::string folder_name = "home/pi/stereopi-cpp-tutorial/""; +std::string folder_name = "/home/pi/stereopi-cpp-tutorial/"; std::string calibration_data_folder = folder_name + "calibration_data/"; long long getTimestamp() { diff --git a/src/script7.cpp b/src/script7.cpp index 0779416..48ab3ec 100644 --- a/src/script7.cpp +++ b/src/script7.cpp @@ -1,7 +1,7 @@ // Copyright (C) 2019 Eugene a.k.a. Realizator, stereopi.com, virt2real team -// Ported from Python to C by Konstantin Ozernov on 10/10/2019. +// Ported from Python to C++ by Konstantin Ozernov on 10/10/2019. // -// This file is part of StereoPi С tutorial scripts, and has been +// This file is part of StereoPi С++ tutorial scripts, and has been // ported from Pyton version (https://github.com/realizator/stereopi-fisheye-robot) // // StereoPi tutorial is free software: you can redistribute it @@ -32,6 +32,7 @@ #include #include #include +#include int SWS = 7; int PFS = 7; @@ -42,7 +43,16 @@ int TxtrThrshld = 13; int unicRatio = 19; int SpcklRng = 0; int SpklWinSze = 0; - +float min_y = 10000.0; +float max_y = -10000.0; +float min_x = 10000.0; +float max_x = -10000.0; +float map_zoom_x = 100; +float map_zoom_y = 100; +float map_zoom = 50; +// Comfort and debug flags +bool auto_zoom = true; //try to fit all points on 2D map display if true +bool show_debug = true; //will show additional info like max and min of X and Y // Global settings std::string folder_name = "/home/pi/stereopi-cpp-tutorial/"; std::string calibration_data_folder = folder_name + "calibration_data/"; @@ -143,6 +153,9 @@ int main() bm->setSpeckleRange(SpcklRng); bm->setDisp12MaxDiff(1); + // while ((count = fread(buf, sizeof(*buf), bufLen, fp)) != 0) + // { + while (true) { fseek(fp, -bufLen, SEEK_END); @@ -207,20 +220,41 @@ int main() // Put all points to the 2D map // Change map_zoom to adjust visible range! - float map_zoom = 50.0; - + map_zoom_y = (map_height/(max_y-min_y)); + map_zoom_x = (map_width/(max_x-min_x)); + // prevent manual/autozoom to set extremely small numbers caused by values spike + map_zoom_x = std::max(float(10.0), map_zoom_x); + map_zoom_y = std::max(float(10.0), map_zoom_y); + if (show_debug) + { + fprintf(stderr, "\nX and Y before zoom: \nmax_y = %.2f, min_y = %.2f, max_x = %.2f, min_x = %.2f \n", max_y, min_y, max_x, min_x ); + fprintf(stderr, "map_height = %d, map_width = %d, Autozoom: %s, zoom X = %.2f, zoom Y = %.2f\n", map_height, map_width, auto_zoom ? "On" : "Off", map_zoom_x, map_zoom_y ); + } for (int i = 0; i < points.rows; i++) { - float cur_x = points.at(i, 0)[0]; - float cur_y = points.at(i, 0)[1]; - - int xx = (int)(cur_y * map_zoom) + (int)(map_width/2); // zero point is in the middle of the map - int yy = (int)(cur_x * map_zoom) + map_height; // zero point is at the bottom of the map + float cur_y = -points.at(i, 0)[0]; + float cur_x = points.at(i, 0)[1]; + if (!isinf(cur_y)) + { + min_y = std::min(cur_y, min_y); + max_y = std::max(cur_y, max_y); + + } + if (!isinf(cur_x)) + { + max_x = std::max(cur_x, max_x); + min_x = std::min(cur_x, min_x); + } + if (!auto_zoom) {map_zoom_x = map_zoom; map_zoom_y = map_zoom;} + + int xx = (int)((cur_x) * map_zoom_x) + (int)(map_width/2); // zero point is in the middle of the map + int yy = map_height - (int)((cur_y-min_y) * map_zoom_y); // zero point is at the bottom of the map // If the point fits on our 2D map - let's draw it! - if (yy < map_width && yy >= 0 && xx < map_height && yy >= 0) + if (xx < map_width && xx >= 0 && yy < map_height && yy >= 0) xy_projection.at(yy, xx) = maxInColumns.at(i, 0);//maximized_line.at(i, 0); } + cv::Mat xy_projection_color, max_line_color, disparity_color; cv::applyColorMap(native_disparity, disparity_color, cv::COLORMAP_JET); cv::applyColorMap(xy_projection, xy_projection_color, cv::COLORMAP_JET);