Skip to content

Commit

Permalink
Updated Photo corner picker to more user friendly version.
Browse files Browse the repository at this point in the history 
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
  • Loading branch information
@kaanoguzhan
kaanoguzhan committed Sep 10, 2020
1 parent 27f9e6c commit cda9ce6
Show file tree
Hide file tree
Showing 2 changed files with 212 additions and 86 deletions.
6 changes: 3 additions & 3 deletions launch/cornerPhoto.launch
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>

<param name="intrinsic_path" value="$(find camera_lidar_calibration)/../../data/parameters/intrinsic.txt" /> <!-- intrinsic file -->
<param name="input_photo_path" value="$(find camera_lidar_calibration)/../../data/photo/0.bmp" /> <!-- photo to find the corner -->
<param name="ouput_path" value="$(find camera_lidar_calibration)/../../data/corner_photo.txt" /> <!-- file to save the photo corner -->
<param name="intrinsic_path" value="$(find camera_lidar_calibration)/../../data/parameters/intrinsic.txt" /> <!-- intrinsic file -->
<param name="input_photo_folder_path" value="$(find camera_lidar_calibration)/../../data/photo/0.bmp" /> <!-- photo's folder to find the corners -->
<param name="ouput_path" value="$(find camera_lidar_calibration)/../../data/corner_photo.txt" /> <!-- file to save the photo corners -->

<node pkg="camera_lidar_calibration" name="cornerPhoto" type="cornerPhoto" output="screen"></node>

Expand Down
292 changes: 209 additions & 83 deletions src/corner_photo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,26 +8,46 @@
#include <fstream>
#include <opencv2/opencv.hpp>
#include <string>
#include <sys/types.h>
#include <dirent.h>

#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;
exit(0);
}
switch(mode) {
case(0):
outfile << "photo" << endl;
outfile << imagename << endl;
outfile << "1" << endl;
break;
case(1):
Expand All @@ -46,100 +66,206 @@ 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<cv::Point2f> 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;
}
}

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<string> 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<float> intrinsic;
getIntrinsic(intrinsic_path, intrinsic);
vector<float> distortion;
getDistortion(intrinsic_path, distortion);

// set intrinsic parameters of the camera
cv::Mat cameraMatrix = cv::Mat::eye(3, 3, CV_64F);
cameraMatrix.at<double>(0, 0) = intrinsic[0];
cameraMatrix.at<double>(0, 2) = intrinsic[2];
cameraMatrix.at<double>(1, 1) = intrinsic[4];
cameraMatrix.at<double>(1, 2) = intrinsic[5];

// set radial distortion and tangential distortion
cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64F);
distCoeffs.at<double>(0, 0) = distortion[0];
distCoeffs.at<double>(1, 0) = distortion[1];
distCoeffs.at<double>(2, 0) = distortion[2];
distCoeffs.at<double>(3, 0) = distortion[3];
distCoeffs.at<double>(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<cv::Point2f> 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<float> intrinsic;
getIntrinsic(intrinsic_path, intrinsic);
vector<float> distortion;
getDistortion(intrinsic_path, distortion);

// Set intrinsic parameters of the camera
cv::Mat cameraMatrix = cv::Mat::eye(3, 3, CV_64F);
cameraMatrix.at<double>(0, 0) = intrinsic[0];
cameraMatrix.at<double>(0, 2) = intrinsic[2];
cameraMatrix.at<double>(1, 1) = intrinsic[4];
cameraMatrix.at<double>(1, 2) = intrinsic[5];

// Set radial distortion and tangential distortion
cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64F);
distCoeffs.at<double>(0, 0) = distortion[0];
distCoeffs.at<double>(1, 0) = distortion[1];
distCoeffs.at<double>(2, 0) = distortion[2];
distCoeffs.at<double>(3, 0) = distortion[3];
distCoeffs.at<double>(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;
}

Expand Down

0 comments on commit cda9ce6

Please sign in to comment.