Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Updated Photo corner picker to more user friendly version. #8

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Updated Photo corner picker to more user friendly version. #8

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions launch/cornerPhoto.launch
View file
Open in desktop
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" /> <!-- 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
Open in desktop
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