From 5ecf22c99c497301a04dbf5e69fe1791e545b49e Mon Sep 17 00:00:00 2001 From: Jakob Engel Date: Thu, 18 Sep 2014 18:54:40 +0200 Subject: [PATCH] changed CV camera calibration undistorter to 4-parameter format. fixed bug when changing image-resolution with callibratino file --- README.md | 2 +- lsd_slam_core/src/main_on_images.cpp | 9 ++++- lsd_slam_core/src/util/Undistorter.cpp | 49 +++++++++++++++----------- lsd_slam_core/src/util/Undistorter.h | 33 +++++++++++++++++ 4 files changed, 71 insertions(+), 22 deletions(-) diff --git a/README.md b/README.md index f309f8f0..8412965d 100644 --- a/README.md +++ b/README.md @@ -165,7 +165,7 @@ This one is with no radial distortion, as a special case of ATAN camera model bu #### Calibration File for OpenCV camera model [untested!]: - fx fy cx cy d1 d2 d3 d4 d5 d6 + fx fy cx cy d1 d2 d3 d4 inputWidth inputHeight "crop" / "full" / "none" / "e1 e2 e3 e4 0" outputWidth outputHeight diff --git a/lsd_slam_core/src/main_on_images.cpp b/lsd_slam_core/src/main_on_images.cpp index c119d798..1aa06861 100644 --- a/lsd_slam_core/src/main_on_images.cpp +++ b/lsd_slam_core/src/main_on_images.cpp @@ -157,6 +157,10 @@ int main( int argc, char** argv ) int w = undistorter->getOutputWidth(); int h = undistorter->getOutputHeight(); + + int w_inp = undistorter->getInputWidth(); + int h_inp = undistorter->getInputHeight(); + float fx = undistorter->getK().at(0, 0); float fy = undistorter->getK().at(1, 1); float cx = undistorter->getK().at(2, 0); @@ -219,7 +223,7 @@ int main( int argc, char** argv ) { cv::Mat imageDist = cv::imread(files[i], CV_LOAD_IMAGE_GRAYSCALE); - if(imageDist.rows != h || imageDist.cols != w) + if(imageDist.rows != h_inp || imageDist.cols != w_inp) { if(imageDist.rows * imageDist.cols == 0) printf("failed to load image %s! skipping.\n", files[i].c_str()); @@ -258,6 +262,9 @@ int main( int argc, char** argv ) } ros::spinOnce(); + + if(!ros::ok()) + break; } diff --git a/lsd_slam_core/src/util/Undistorter.cpp b/lsd_slam_core/src/util/Undistorter.cpp index f22a3138..0a1965ac 100644 --- a/lsd_slam_core/src/util/Undistorter.cpp +++ b/lsd_slam_core/src/util/Undistorter.cpp @@ -68,9 +68,9 @@ Undistorter* Undistorter::getUndistorterForFile(const char* configFilename) float ic[10]; - if(std::sscanf(l1.c_str(), "%f %f %f %f %f %f %f %f %f %f", + if(std::sscanf(l1.c_str(), "%f %f %f %f %f %f %f %f", &ic[0], &ic[1], &ic[2], &ic[3], &ic[4], - &ic[5], &ic[6], &ic[7], &ic[8], &ic[9]) == 10) + &ic[5], &ic[6], &ic[7]) == 8) { printf("found OpenCV camera model, building rectifier.\n"); Undistorter* u = new UndistorterOpenCV(completeFileName.c_str()); @@ -428,6 +428,16 @@ int UndistorterPTAM::getOutputHeight() const { return out_height; } +int UndistorterPTAM::getInputWidth() const +{ + return in_width; +} + +int UndistorterPTAM::getInputHeight() const +{ + return in_height; +} + bool UndistorterPTAM::isValid() const { @@ -451,16 +461,16 @@ UndistorterOpenCV::UndistorterOpenCV(const char* configFileName) std::getline(infile,l4); // l1 & l2 - if(std::sscanf(l1.c_str(), "%f %f %f %f %f %f %f %f %f %f", + if(std::sscanf(l1.c_str(), "%f %f %f %f %f %f %f %f", &inputCalibration[0], &inputCalibration[1], &inputCalibration[2], &inputCalibration[3], &inputCalibration[4], - &inputCalibration[5], &inputCalibration[6], &inputCalibration[7], &inputCalibration[8], &inputCalibration[9] - ) == 10 && + &inputCalibration[5], &inputCalibration[6], &inputCalibration[7] + ) == 8 && std::sscanf(l2.c_str(), "%d %d", &in_width, &in_height) == 2) { printf("Input resolution: %d %d\n",in_width, in_height); - printf("In: %f %f %f %f %f %f %f %f %f %f\n", + printf("In: %f %f %f %f %f %f %f %f\n", inputCalibration[0], inputCalibration[1], inputCalibration[2], inputCalibration[3], inputCalibration[4], - inputCalibration[5], inputCalibration[6], inputCalibration[7], inputCalibration[8], inputCalibration[9]); + inputCalibration[5], inputCalibration[6], inputCalibration[7]); } else { @@ -501,12 +511,10 @@ UndistorterOpenCV::UndistorterOpenCV(const char* configFileName) valid = false; } - cv::Mat distCoeffs = cv::Mat::zeros(8, 1, CV_32F); - for (int i = 0; i < 2; ++ i) - distCoeffs.at(i, 0) = inputCalibration[4 + i]; + cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_32F); for (int i = 0; i < 4; ++ i) - distCoeffs.at(4 + i, 0) = inputCalibration[6 + i]; - + distCoeffs.at(i, 0) = inputCalibration[4 + i]; + originalK_ = cv::Mat(3, 3, CV_64F, cv::Scalar(0)); originalK_.at(0, 0) = inputCalibration[0] * in_width; originalK_.at(1, 1) = inputCalibration[1] * in_height; @@ -521,14 +529,6 @@ UndistorterOpenCV::UndistorterOpenCV(const char* configFileName) cv::initUndistortRectifyMap(originalK_, distCoeffs, cv::Mat(), K_, cv::Size(out_width, out_height), CV_16SC2, map1, map2); -// K_.at(0, 0) /= out_width; -// K_.at(0, 2) /= out_width; -// K_.at(1, 1) /= out_height; -// K_.at(1, 2) /= out_height; - // TODO: PTAM code uses the following, should here also 0.5 be subtracted? -// K_.at(2, 0) = outputCalibration[2] * out_width - 0.5; -// K_.at(2, 1) = outputCalibration[3] * out_height - 0.5; - originalK_.at(0, 0) /= in_width; originalK_.at(0, 2) /= in_width; originalK_.at(1, 1) /= in_height; @@ -567,6 +567,15 @@ int UndistorterOpenCV::getOutputHeight() const { return out_height; } +int UndistorterOpenCV::getInputWidth() const +{ + return in_width; +} + +int UndistorterOpenCV::getInputHeight() const +{ + return in_height; +} bool UndistorterOpenCV::isValid() const { diff --git a/lsd_slam_core/src/util/Undistorter.h b/lsd_slam_core/src/util/Undistorter.h index e9369ae7..8cc47dc9 100644 --- a/lsd_slam_core/src/util/Undistorter.h +++ b/lsd_slam_core/src/util/Undistorter.h @@ -58,6 +58,17 @@ class Undistorter */ virtual int getOutputHeight() const = 0; + /** + * Returns the width of the input images in pixels. + */ + virtual int getInputWidth() const = 0; + + /** + * Returns the height of the input images in pixels. + */ + virtual int getInputHeight() const = 0; + + /** * Returns if the undistorter was initialized successfully. */ @@ -117,6 +128,17 @@ class UndistorterPTAM : public Undistorter */ int getOutputHeight() const; + /** + * Returns the width of the input images in pixels. + */ + int getInputWidth() const; + + /** + * Returns the height of the input images in pixels. + */ + int getInputHeight() const; + + /** * Returns if the undistorter was initialized successfully. */ @@ -186,6 +208,17 @@ class UndistorterOpenCV : public Undistorter */ int getOutputHeight() const; + + /** + * Returns the width of the input images in pixels. + */ + int getInputWidth() const; + + /** + * Returns the height of the input images in pixels. + */ + int getInputHeight() const; + /** * Returns if the undistorter was initialized successfully. */