From cda9ce6fbde4c0af251db4aab2a557f07227db42 Mon Sep 17 00:00:00 2001 From: KaanOguzhan Date: Thu, 10 Sep 2020 17:43:51 +0200 Subject: [PATCH] Updated Photo corner picker to more user friendly version. Now points are selected directly by clicking on the image Upon selecting a coordinate, undistorted point is immediately displayed on the image A point pick can be versed by keyboard buttons (r) and redone Instead of single image now images folder path is given with .launch file and all images are looped over in sorted order --- launch/cornerPhoto.launch | 6 +- src/corner_photo.cpp | 292 +++++++++++++++++++++++++++----------- 2 files changed, 212 insertions(+), 86 deletions(-) diff --git a/launch/cornerPhoto.launch b/launch/cornerPhoto.launch index ebd7ab6..674f6e4 100644 --- a/launch/cornerPhoto.launch +++ b/launch/cornerPhoto.launch @@ -1,9 +1,9 @@ - - - + + + diff --git a/src/corner_photo.cpp b/src/corner_photo.cpp index 53f1d59..99edcdb 100644 --- a/src/corner_photo.cpp +++ b/src/corner_photo.cpp @@ -8,18 +8,38 @@ #include #include #include +#include +#include #include "common.h" using namespace std; +using namespace cv; -void writeData(const string filename, const float x, const float y, uint mode); +void writeData(const string imagename, const string filename, const float x, const float y, uint mode); cv::Mat gray_img, src_img; cv::RNG random_number_generator; string photo_path, output_name, intrinsic_path; -void writeData(const string filename, const float x, const float y, uint mode) { +void getParameters() { + cout << "Get the parameters from the launch file" << endl; + + if (!ros::param::get("input_photo_folder_path", photo_path)) { + cout << "Can not get the value of input_photo_folder_path" << endl; + exit(1); + } + if (!ros::param::get("ouput_path", output_name)) { + cout << "Can not get the value of ouput_path" << endl; + exit(1); + } + if (!ros::param::get("intrinsic_path", intrinsic_path)) { + cout << "Can not get the value of intrinsic_path" << endl; + exit(1); + } +} + +void writeData(const string imagename, const string filename, const float x, const float y, uint mode) { ofstream outfile(filename.c_str(), ios_base::app); if (!outfile) { cout << "Can not open the file: " << filename << endl; @@ -27,7 +47,7 @@ void writeData(const string filename, const float x, const float y, uint mode) { } switch(mode) { case(0): - outfile << "photo" << endl; + outfile << imagename << endl; outfile << "1" << endl; break; case(1): @@ -46,20 +66,34 @@ void writeData(const string filename, const float x, const float y, uint mode) { outfile << float2str(x) << " " << float2str(y) << endl; } -void getParameters() { - cout << "Get the parameters from the launch file" << endl; +vector corners; - if (!ros::param::get("input_photo_path", photo_path)) { - cout << "Can not get the value of input_photo_path" << endl; - exit(1); +void InputCallBackFunc(int event, int x, int y, int flags, void* userdata) +{ + if (event == EVENT_LBUTTONDOWN){ + cout << "x:" << x << " y:" << y << endl; + cv::Point2f p; + p.x = x; + p.y = y; + corners.push_back(p); } - if (!ros::param::get("ouput_path", output_name)) { - cout << "Can not get the value of ouput_path" << endl; - exit(1); +} + + +bool naturalSortCompare(string i, string j) { + string str_i = i.substr(0,i.find(".")); + string str_j = j.substr(0,j.find(".")); + //cout << i << " " << str_i<< " " << str_i.length()<< endl; + if (str_i.length() == 0 || str_j.length()== 0){ + return false; } - if (!ros::param::get("intrinsic_path", intrinsic_path)) { - cout << "Can not get the value of intrinsic_path" << endl; - exit(1); + try + { + return stoi(str_i) < stoi(str_j); + } + catch (invalid_argument const &e) + { + return false; } } @@ -67,79 +101,171 @@ int main(int argc, char **argv) { ros::init(argc, argv, "cornerPhoto"); getParameters(); - src_img = cv::imread(photo_path); - - if(src_img.empty()) { // use the file name to search the photo - cout << "No Picture found by filename: " << photo_path << endl; - return 0; + vector files; + DIR* dirp = opendir(photo_path.c_str()); + struct dirent * dp; + while ((dp = readdir(dirp)) != NULL) { + files.push_back(dp->d_name); + } + // Sort files list by natural counting order + for(unsigned i=0; i < files.size(); i++){ + sort(files.begin(), files.end(), naturalSortCompare); } + closedir(dirp); - vector intrinsic; - getIntrinsic(intrinsic_path, intrinsic); - vector distortion; - getDistortion(intrinsic_path, distortion); - - // set intrinsic parameters of the camera - cv::Mat cameraMatrix = cv::Mat::eye(3, 3, CV_64F); - cameraMatrix.at(0, 0) = intrinsic[0]; - cameraMatrix.at(0, 2) = intrinsic[2]; - cameraMatrix.at(1, 1) = intrinsic[4]; - cameraMatrix.at(1, 2) = intrinsic[5]; - - // set radial distortion and tangential distortion - cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64F); - distCoeffs.at(0, 0) = distortion[0]; - distCoeffs.at(1, 0) = distortion[1]; - distCoeffs.at(2, 0) = distortion[2]; - distCoeffs.at(3, 0) = distortion[3]; - distCoeffs.at(4, 0) = distortion[4]; - - cv::Mat view, rview, map1, map2; - cv::Size imageSize = src_img.size(); - cv::initUndistortRectifyMap(cameraMatrix, distCoeffs, cv::Mat(),cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), imageSize, CV_16SC2, map1, map2); - cv::remap(src_img, src_img, map1, map2, cv::INTER_LINEAR); // correct the distortion - - cout << "Please note the four corners, and then tap a random key to give the coordinate" << endl; - // cv::namedWindow("source", CV_WINDOW_KEEPRATIO); - cv::namedWindow("source"); - cv::imshow("source", src_img); - cv::waitKey(0); - - cv::destroyWindow("source"); - vector corners; - cout << "Give the corner coordinate, finish by 0 0" << endl; - while(1) { - cv::Point2f p; - cin >> p.x >> p.y; - if(p.x < 0.1 && p.y < 0.1) { // finish by typing "0 0" - break; + cout << "\n\nImage path is set to:\n" << photo_path << endl; + cout << "...Ready for corner picking..." << endl; + for(unsigned i=0; i < files.size(); i++){ + if(files.at(i).compare("..") == 0 || files.at(i).compare(".") == 0){ + continue; } - corners.push_back(p); - } - if (!corners.size()) { - cout << "No input corners, end process" << endl; - return 0; - } - cv::Size winSize = cv::Size(5, 5); - cv::Size zerozone = cv::Size(-1, -1); - cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 40, 0.001); - - // cv::namedWindow("output", CV_WINDOW_KEEPRATIO); - cv::namedWindow("output"); - cv::cvtColor(src_img, gray_img, cv::COLOR_BGR2GRAY); - cv::cornerSubPix(gray_img, corners, winSize, zerozone, criteria); - - cv::Mat result_img = src_img.clone(); - for(uint t = 0; t < corners.size(); ++t) { - cv::circle(result_img, corners[t], 3, cv::Scalar(random_number_generator.uniform(0, 255), random_number_generator.uniform(0, 255), random_number_generator.uniform(0, 255)), 1, 8, 0); - printf("(%.3f %.3f)", corners[t].x, corners[t].y); - writeData(output_name, corners[t].x, corners[t].y, t); + + string image_path = photo_path + "/" + files.at(i); + src_img = cv::imread(image_path); + + if(src_img.empty()) { + cout << image_path <<"is NOT a valid image ! Skipping" << endl; + continue; + } + + cout << "\n\nOpening undistorted Image of: " << files.at(i) << endl; + + // Read intrinsic and distortion + vector intrinsic; + getIntrinsic(intrinsic_path, intrinsic); + vector distortion; + getDistortion(intrinsic_path, distortion); + + // Set intrinsic parameters of the camera + cv::Mat cameraMatrix = cv::Mat::eye(3, 3, CV_64F); + cameraMatrix.at(0, 0) = intrinsic[0]; + cameraMatrix.at(0, 2) = intrinsic[2]; + cameraMatrix.at(1, 1) = intrinsic[4]; + cameraMatrix.at(1, 2) = intrinsic[5]; + + // Set radial distortion and tangential distortion + cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64F); + distCoeffs.at(0, 0) = distortion[0]; + distCoeffs.at(1, 0) = distortion[1]; + distCoeffs.at(2, 0) = distortion[2]; + distCoeffs.at(3, 0) = distortion[3]; + distCoeffs.at(4, 0) = distortion[4]; + + cv::Mat view, rview, map1, map2; + cv::Size imageSize = src_img.size(); + cv::initUndistortRectifyMap(cameraMatrix, distCoeffs, cv::Mat(),cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), imageSize, CV_16SC2, map1, map2); + cv::remap(src_img, src_img, map1, map2, cv::INTER_LINEAR); // correct the distortion + + cout << "Starting from top-left in clockwise direction, click 4 corners of the board on the image" << endl; + cout << "5th click to any location finalizes corner picking" << endl; + cout << "While Image has focus:" << endl; + cout << "Press \"s\" to skip this image (No point must be selected to skip)" << endl; + cout << "Press \"r\" to remove the last point" << endl; + // cv::namedWindow("source", CV_WINDOW_KEEPRATIO); + cv::namedWindow("Source: " + files.at(i)); + //set the callback function for any mouse event + setMouseCallback("Source: " + files.at(i), InputCallBackFunc, NULL); + cv::imshow("Source: " + files.at(i), src_img); + + // Loop while not all 4 points are clicked + bool skip_image = false; + while (corners.size() == 0 || corners.size() % 5 != 0){ + int keypress = cv::waitKey(100); + if (keypress == 115 ){ // "s" is pressed + if (corners.size() == 0){ + skip_image = true; + break; + }else{ + cout << "Cannot skip image while there are selected points, remove them first" << endl; + } + } + if (keypress == 114){ // "r" is pressed + if (corners.size() > 0){ + corners.pop_back(); + cout << "Removed last point" << endl; + }else{ + cout << "No point is left to remove !!" << endl; + } + } + + // Displaying points + if (corners.size() == 0 ){ + cv::imshow("Source: " + files.at(i), src_img); + }else{ + cv::Size winSize = cv::Size(5, 5); + cv::Size zerozone = cv::Size(-1, -1); + cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 40, 0.001); + + cv::namedWindow("Source: " + files.at(i)); + cv::cvtColor(src_img, gray_img, cv::COLOR_BGR2GRAY); + cv::cornerSubPix(gray_img, corners, winSize, zerozone, criteria); + + // Display resulting image for confirmation + cv::Mat result_img = src_img.clone(); + for(uint t = 0; t < corners.size(); ++t) { + cv::circle(result_img, corners[t], 3, cv::Scalar(random_number_generator.uniform(0, 255), random_number_generator.uniform(0, 255), random_number_generator.uniform(0, 255)), 1, 8, 0); + } + cv::namedWindow("Source: " + files.at(i)); + cv::imshow("Source: " + files.at(i), result_img); + } + } + // End corner picking + cv::destroyWindow("Source: " + files.at(i)); + // Skip current image + if (skip_image){ + continue; + }; + + // Remove last click as its just for confirmation + corners.pop_back(); + + // Generating results + cout << "While Image has focus: \nPress \"r\" for repeat same image \nPress \"n\" to confirm and move to next image" << endl; + cv::Size winSize = cv::Size(5, 5); + cv::Size zerozone = cv::Size(-1, -1); + cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 40, 0.001); + + cv::namedWindow("Output: " + files.at(i)); + cv::cvtColor(src_img, gray_img, cv::COLOR_BGR2GRAY); + cv::cornerSubPix(gray_img, corners, winSize, zerozone, criteria); + + // Display resulting image for confirmation + cv::Mat result_img = src_img.clone(); + for(uint t = 0; t < corners.size(); ++t) { + cv::circle(result_img, corners[t], 3, cv::Scalar(random_number_generator.uniform(0, 255), random_number_generator.uniform(0, 255), random_number_generator.uniform(0, 255)), 1, 8, 0); + } + cv::namedWindow("Output: " + files.at(i)); + cv::imshow("Output: " + files.at(i), result_img); + bool repeatImage = false; + while(true){ + int keypress = cv::waitKey(0); + if (keypress == 114){ // "r" is pressed + repeatImage = true; + break; + } + else if (keypress == 110){ // "n" is pressed + break; + }else{ + cout << "Press \"r\" for repeat the image or \"n\" for next image" << endl; + } + } + cv::destroyWindow("Output: " + files.at(i)); + if (repeatImage){ + cout << "Repeating image..." << endl; + i--; + corners.clear(); + continue; + } + + // Confirming complete writing results to file + for(uint t = 0; t < corners.size(); ++t) { + printf("(%.3f %.3f)", corners[t].x, corners[t].y); + writeData(files.at(i), output_name, corners[t].x, corners[t].y, t); + } + + cout << endl << "Result saved for \"" << files.at(i) << "\"!" << endl; + corners.clear(); } - - cout << endl << "Result saved, tap a random key to finish the process" << endl; - cv::namedWindow("output"); - imshow("output", result_img); - cv::waitKey(0); return 0; }