From da2b5abd0aab6c8b22beda441ab02ea0c5438944 Mon Sep 17 00:00:00 2001 From: sebdi Date: Mon, 25 May 2015 19:27:45 -0400 Subject: [PATCH 1/2] Fix: Calibration File for OpenCV camera model can now used with 'e1 e2 e3 e4 0'. --- lsd_slam_core/src/util/Undistorter.cpp | 961 +++++++++++++------------ lsd_slam_core/src/util/Undistorter.h | 388 +++++----- 2 files changed, 677 insertions(+), 672 deletions(-) diff --git a/lsd_slam_core/src/util/Undistorter.cpp b/lsd_slam_core/src/util/Undistorter.cpp index d0b54a91..fc3f78c6 100644 --- a/lsd_slam_core/src/util/Undistorter.cpp +++ b/lsd_slam_core/src/util/Undistorter.cpp @@ -37,524 +37,529 @@ Undistorter::~Undistorter() Undistorter* Undistorter::getUndistorterForFile(const char* configFilename) { - std::string completeFileName = configFilename; - - printf("Reading Calibration from file %s",completeFileName.c_str()); - - - std::ifstream f(completeFileName.c_str()); - if (!f.good()) - { - f.close(); - - completeFileName = packagePath+"calib/"+configFilename; - printf(" ... not found!\n Trying %s", completeFileName.c_str()); - - f.open(completeFileName.c_str()); - - if (!f.good()) - { - printf(" ... not found. Cannot operate without calibration, shutting down.\n"); - f.close(); - return 0; - } - } - - printf(" ... found!\n"); - - std::string l1; - std::getline(f,l1); - f.close(); - - - - float ic[10]; - 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]) == 8) - { - printf("found OpenCV camera model, building rectifier.\n"); - Undistorter* u = new UndistorterOpenCV(completeFileName.c_str()); - if(!u->isValid()) return 0; - return u; - } - else - { - printf("found ATAN camera model, building rectifier.\n"); - Undistorter* u = new UndistorterPTAM(completeFileName.c_str()); - if(!u->isValid()) return 0; - return u; - } + std::string completeFileName = configFilename; + + printf("Reading Calibration from file %s",completeFileName.c_str()); + + + std::ifstream f(completeFileName.c_str()); + if (!f.good()) + { + f.close(); + + completeFileName = packagePath+"calib/"+configFilename; + printf(" ... not found!\n Trying %s", completeFileName.c_str()); + + f.open(completeFileName.c_str()); + + if (!f.good()) + { + printf(" ... not found. Cannot operate without calibration, shutting down.\n"); + f.close(); + return 0; + } + } + + printf(" ... found!\n"); + + std::string l1; + std::getline(f,l1); + f.close(); + + + + float ic[10]; + 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]) == 8) + { + printf("found OpenCV camera model, building rectifier.\n"); + Undistorter* u = new UndistorterOpenCV(completeFileName.c_str()); + if(!u->isValid()) return 0; + return u; + } + else + { + printf("found ATAN camera model, building rectifier.\n"); + Undistorter* u = new UndistorterPTAM(completeFileName.c_str()); + if(!u->isValid()) return 0; + return u; + } } UndistorterPTAM::UndistorterPTAM(const char* configFileName) { - valid = true; - - remapX = nullptr; - remapY = nullptr; - - - - // read parameters - std::ifstream infile(configFileName); - assert(infile.good()); - - - std::string l1,l2,l3,l4; - - std::getline(infile,l1); - std::getline(infile,l2); - std::getline(infile,l3); - std::getline(infile,l4); - - - - - // l1 & l2 - if(std::sscanf(l1.c_str(), "%f %f %f %f %f", &inputCalibration[0], &inputCalibration[1], &inputCalibration[2], &inputCalibration[3], &inputCalibration[4]) == 5 && - 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\n", - inputCalibration[0], inputCalibration[1], inputCalibration[2], inputCalibration[3], inputCalibration[4]); - } - else - { - printf("Failed to read camera calibration (invalid format?)\nCalibration file: %s\n", configFileName); - valid = false; - } - - // l3 - if(l3 == "crop") - { - outputCalibration[0] = -1; - printf("Out: Crop\n"); - } - else if(l3 == "full") - { - outputCalibration[0] = -2; - printf("Out: Full\n"); - } - else if(l3 == "none") - { - printf("NO RECTIFICATION\n"); - } - else if(std::sscanf(l3.c_str(), "%f %f %f %f %f", &outputCalibration[0], &outputCalibration[1], &outputCalibration[2], &outputCalibration[3], &outputCalibration[4]) == 5) - { - printf("Out: %f %f %f %f %f\n", - outputCalibration[0], outputCalibration[1], outputCalibration[2], outputCalibration[3], outputCalibration[4]); - } - else - { - printf("Out: Failed to Read Output pars... not rectifying.\n"); - valid = false; - } - - - // l4 - if(std::sscanf(l4.c_str(), "%d %d", &out_width, &out_height) == 2) - { - printf("Output resolution: %d %d\n",out_width, out_height); - } - else - { - printf("Out: Failed to Read Output resolution... not rectifying.\n"); - valid = false; - } - - - - - // prep warp matrices - if(valid) - { - float dist = inputCalibration[4]; - float d2t = 2.0f * tan(dist / 2.0f); - - // current camera parameters - float fx = inputCalibration[0] * in_width; - float fy = inputCalibration[1] * in_height; - float cx = inputCalibration[2] * in_width - 0.5; - float cy = inputCalibration[3] * in_height - 0.5; - - // scale calibration parameters to input size - double xfactor = in_width / (1.0 * in_width); - double yfactor = in_height / (1.0 * in_height); - fx = fx * xfactor; - fy = fy * yfactor; - cx = (cx + 0.5) * xfactor - 0.5; - cy = (cy + 0.5) * yfactor - 0.5; - - // output camera parameters - float ofx, ofy, ocx, ocy; - - // find new camera matrix for "crop" and "full" - if (inputCalibration[4] == 0) - { - ofx = inputCalibration[0] * out_width; - ofy = inputCalibration[1] * out_height; - ocx = (inputCalibration[2] * out_width) - 0.5; - ocy = (inputCalibration[3] * out_height) - 0.5; - } - else if(outputCalibration[0] == -1) // "crop" - { - // find left-most and right-most radius - float left_radius = (cx)/fx; - float right_radius = (in_width-1 - cx)/fx; - float top_radius = (cy)/fy; - float bottom_radius = (in_height-1 - cy)/fy; - - float trans_left_radius = tan(left_radius * dist)/d2t; - float trans_right_radius = tan(right_radius * dist)/d2t; - float trans_top_radius = tan(top_radius * dist)/d2t; - float trans_bottom_radius = tan(bottom_radius * dist)/d2t; - - //printf("left_radius: %f -> %f\n", left_radius, trans_left_radius); - //printf("right_radius: %f -> %f\n", right_radius, trans_right_radius); - //printf("top_radius: %f -> %f\n", top_radius, trans_top_radius); - //printf("bottom_radius: %f -> %f\n", bottom_radius, trans_bottom_radius); - - - ofy = fy * ((top_radius + bottom_radius) / (trans_top_radius + trans_bottom_radius)) * ((float)out_height / (float)in_height); - ocy = (trans_top_radius/top_radius) * ofy*cy/fy; - - ofx = fx * ((left_radius + right_radius) / (trans_left_radius + trans_right_radius)) * ((float)out_width / (float)in_width); - ocx = (trans_left_radius/left_radius) * ofx*cx/fx; - - printf("new K: %f %f %f %f\n",ofx,ofy,ocx,ocy); - printf("old K: %f %f %f %f\n",fx,fy,cx,cy); - } - else if(outputCalibration[0] == -2) // "full" - { - float left_radius = cx/fx; - float right_radius = (in_width-1 - cx)/fx; - float top_radius = cy/fy; - float bottom_radius = (in_height-1 - cy)/fy; - - // find left-most and right-most radius - float tl_radius = sqrt(left_radius*left_radius + top_radius*top_radius); - float tr_radius = sqrt(right_radius*right_radius + top_radius*top_radius); - float bl_radius = sqrt(left_radius*left_radius + bottom_radius*bottom_radius); - float br_radius = sqrt(right_radius*right_radius + bottom_radius*bottom_radius); - - float trans_tl_radius = tan(tl_radius * dist)/d2t; - float trans_tr_radius = tan(tr_radius * dist)/d2t; - float trans_bl_radius = tan(bl_radius * dist)/d2t; - float trans_br_radius = tan(br_radius * dist)/d2t; - - //printf("trans_tl_radius: %f -> %f\n", tl_radius, trans_tl_radius); - //printf("trans_tr_radius: %f -> %f\n", tr_radius, trans_tr_radius); - //printf("trans_bl_radius: %f -> %f\n", bl_radius, trans_bl_radius); - //printf("trans_br_radius: %f -> %f\n", br_radius, trans_br_radius); - - - float hor = std::max(br_radius,tr_radius) + std::max(bl_radius,tl_radius); - float vert = std::max(tr_radius,tl_radius) + std::max(bl_radius,br_radius); - - float trans_hor = std::max(trans_br_radius,trans_tr_radius) + std::max(trans_bl_radius,trans_tl_radius); - float trans_vert = std::max(trans_tr_radius,trans_tl_radius) + std::max(trans_bl_radius,trans_br_radius); - - ofy = fy * ((vert) / (trans_vert)) * ((float)out_height / (float)in_height); - ocy = std::max(trans_tl_radius/tl_radius,trans_tr_radius/tr_radius) * ofy*cy/fy; - - ofx = fx * ((hor) / (trans_hor)) * ((float)out_width / (float)in_width); - ocx = std::max(trans_bl_radius/bl_radius,trans_tl_radius/tl_radius) * ofx*cx/fx; - - printf("new K: %f %f %f %f\n",ofx,ofy,ocx,ocy); - printf("old K: %f %f %f %f\n",fx,fy,cx,cy); - } - else - { - ofx = outputCalibration[0] * out_width; - ofy = outputCalibration[1] * out_height; - ocx = outputCalibration[2] * out_width-0.5; // TODO: -0.5 here or not? - ocy = outputCalibration[3] * out_height-0.5; - } - - outputCalibration[0] = ofx / out_width; - outputCalibration[1] = ofy / out_height; - outputCalibration[2] = (ocx+0.5) / out_width; - outputCalibration[3] = (ocy+0.5) / out_height; - outputCalibration[4] = 0; - - remapX = (float*)Eigen::internal::aligned_malloc(out_width * out_height *sizeof(float)); - remapY = (float*)Eigen::internal::aligned_malloc(out_width * out_height *sizeof(float)); - - for(int y=0;y 0 && iy > 0 && ix < in_width-1 && iy < in_height-1) - { - remapX[x+y*out_width] = ix; - remapY[x+y*out_width] = iy; - } - else - { - remapX[x+y*out_width] = -1; - remapY[x+y*out_width] = -1; - } - } - } - - printf("Prepped Warp matrices\n"); - } - else - { - printf("Not Rectifying\n"); - outputCalibration[0] = inputCalibration[0]; - outputCalibration[1] = inputCalibration[1]; - outputCalibration[2] = inputCalibration[2]; - outputCalibration[3] = inputCalibration[3]; - outputCalibration[4] = inputCalibration[4]; - out_width = in_width; - out_height = in_height; - } - - - originalK_ = cv::Mat(3, 3, CV_64F, cv::Scalar(0)); - originalK_.at(0, 0) = inputCalibration[0]; - originalK_.at(1, 1) = inputCalibration[1]; - originalK_.at(2, 2) = 1; - originalK_.at(2, 0) = inputCalibration[2]; - originalK_.at(2, 1) = inputCalibration[3]; - - K_ = cv::Mat(3, 3, CV_64F, cv::Scalar(0)); - K_.at(0, 0) = outputCalibration[0] * out_width; - K_.at(1, 1) = outputCalibration[1] * out_height; - K_.at(2, 2) = 1; - K_.at(2, 0) = outputCalibration[2] * out_width - 0.5; - K_.at(2, 1) = outputCalibration[3] * out_height - 0.5; + valid = true; + + remapX = nullptr; + remapY = nullptr; + + + + // read parameters + std::ifstream infile(configFileName); + assert(infile.good()); + + + std::string l1,l2,l3,l4; + + std::getline(infile,l1); + std::getline(infile,l2); + std::getline(infile,l3); + std::getline(infile,l4); + + + + + // l1 & l2 + if(std::sscanf(l1.c_str(), "%f %f %f %f %f", &inputCalibration[0], &inputCalibration[1], &inputCalibration[2], &inputCalibration[3], &inputCalibration[4]) == 5 && + 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\n", + inputCalibration[0], inputCalibration[1], inputCalibration[2], inputCalibration[3], inputCalibration[4]); + } + else + { + printf("Failed to read camera calibration (invalid format?)\nCalibration file: %s\n", configFileName); + valid = false; + } + + // l3 + if(l3 == "crop") + { + outputCalibration[0] = -1; + printf("Out: Crop\n"); + } + else if(l3 == "full") + { + outputCalibration[0] = -2; + printf("Out: Full\n"); + } + else if(l3 == "none") + { + printf("NO RECTIFICATION\n"); + } + else if(std::sscanf(l3.c_str(), "%f %f %f %f %f", &outputCalibration[0], &outputCalibration[1], &outputCalibration[2], &outputCalibration[3], &outputCalibration[4]) == 5) + { + printf("Out: %f %f %f %f %f\n", + outputCalibration[0], outputCalibration[1], outputCalibration[2], outputCalibration[3], outputCalibration[4]); + } + else + { + printf("Out: Failed to Read Output pars... not rectifying.\n"); + valid = false; + } + + + // l4 + if(std::sscanf(l4.c_str(), "%d %d", &out_width, &out_height) == 2) + { + printf("Output resolution: %d %d\n",out_width, out_height); + } + else + { + printf("Out: Failed to Read Output resolution... not rectifying.\n"); + valid = false; + } + + + + + // prep warp matrices + if(valid) + { + float dist = inputCalibration[4]; + float d2t = 2.0f * tan(dist / 2.0f); + + // current camera parameters + float fx = inputCalibration[0] * in_width; + float fy = inputCalibration[1] * in_height; + float cx = inputCalibration[2] * in_width - 0.5; + float cy = inputCalibration[3] * in_height - 0.5; + + // scale calibration parameters to input size + double xfactor = in_width / (1.0 * in_width); + double yfactor = in_height / (1.0 * in_height); + fx = fx * xfactor; + fy = fy * yfactor; + cx = (cx + 0.5) * xfactor - 0.5; + cy = (cy + 0.5) * yfactor - 0.5; + + // output camera parameters + float ofx, ofy, ocx, ocy; + + // find new camera matrix for "crop" and "full" + if (inputCalibration[4] == 0) + { + ofx = inputCalibration[0] * out_width; + ofy = inputCalibration[1] * out_height; + ocx = (inputCalibration[2] * out_width) - 0.5; + ocy = (inputCalibration[3] * out_height) - 0.5; + } + else if(outputCalibration[0] == -1) // "crop" + { + // find left-most and right-most radius + float left_radius = (cx)/fx; + float right_radius = (in_width-1 - cx)/fx; + float top_radius = (cy)/fy; + float bottom_radius = (in_height-1 - cy)/fy; + + float trans_left_radius = tan(left_radius * dist)/d2t; + float trans_right_radius = tan(right_radius * dist)/d2t; + float trans_top_radius = tan(top_radius * dist)/d2t; + float trans_bottom_radius = tan(bottom_radius * dist)/d2t; + + //printf("left_radius: %f -> %f\n", left_radius, trans_left_radius); + //printf("right_radius: %f -> %f\n", right_radius, trans_right_radius); + //printf("top_radius: %f -> %f\n", top_radius, trans_top_radius); + //printf("bottom_radius: %f -> %f\n", bottom_radius, trans_bottom_radius); + + + ofy = fy * ((top_radius + bottom_radius) / (trans_top_radius + trans_bottom_radius)) * ((float)out_height / (float)in_height); + ocy = (trans_top_radius/top_radius) * ofy*cy/fy; + + ofx = fx * ((left_radius + right_radius) / (trans_left_radius + trans_right_radius)) * ((float)out_width / (float)in_width); + ocx = (trans_left_radius/left_radius) * ofx*cx/fx; + + printf("new K: %f %f %f %f\n",ofx,ofy,ocx,ocy); + printf("old K: %f %f %f %f\n",fx,fy,cx,cy); + } + else if(outputCalibration[0] == -2) // "full" + { + float left_radius = cx/fx; + float right_radius = (in_width-1 - cx)/fx; + float top_radius = cy/fy; + float bottom_radius = (in_height-1 - cy)/fy; + + // find left-most and right-most radius + float tl_radius = sqrt(left_radius*left_radius + top_radius*top_radius); + float tr_radius = sqrt(right_radius*right_radius + top_radius*top_radius); + float bl_radius = sqrt(left_radius*left_radius + bottom_radius*bottom_radius); + float br_radius = sqrt(right_radius*right_radius + bottom_radius*bottom_radius); + + float trans_tl_radius = tan(tl_radius * dist)/d2t; + float trans_tr_radius = tan(tr_radius * dist)/d2t; + float trans_bl_radius = tan(bl_radius * dist)/d2t; + float trans_br_radius = tan(br_radius * dist)/d2t; + + //printf("trans_tl_radius: %f -> %f\n", tl_radius, trans_tl_radius); + //printf("trans_tr_radius: %f -> %f\n", tr_radius, trans_tr_radius); + //printf("trans_bl_radius: %f -> %f\n", bl_radius, trans_bl_radius); + //printf("trans_br_radius: %f -> %f\n", br_radius, trans_br_radius); + + + float hor = std::max(br_radius,tr_radius) + std::max(bl_radius,tl_radius); + float vert = std::max(tr_radius,tl_radius) + std::max(bl_radius,br_radius); + + float trans_hor = std::max(trans_br_radius,trans_tr_radius) + std::max(trans_bl_radius,trans_tl_radius); + float trans_vert = std::max(trans_tr_radius,trans_tl_radius) + std::max(trans_bl_radius,trans_br_radius); + + ofy = fy * ((vert) / (trans_vert)) * ((float)out_height / (float)in_height); + ocy = std::max(trans_tl_radius/tl_radius,trans_tr_radius/tr_radius) * ofy*cy/fy; + + ofx = fx * ((hor) / (trans_hor)) * ((float)out_width / (float)in_width); + ocx = std::max(trans_bl_radius/bl_radius,trans_tl_radius/tl_radius) * ofx*cx/fx; + + printf("new K: %f %f %f %f\n",ofx,ofy,ocx,ocy); + printf("old K: %f %f %f %f\n",fx,fy,cx,cy); + } + else + { + ofx = outputCalibration[0] * out_width; + ofy = outputCalibration[1] * out_height; + ocx = outputCalibration[2] * out_width-0.5; // TODO: -0.5 here or not? + ocy = outputCalibration[3] * out_height-0.5; + } + + outputCalibration[0] = ofx / out_width; + outputCalibration[1] = ofy / out_height; + outputCalibration[2] = (ocx+0.5) / out_width; + outputCalibration[3] = (ocy+0.5) / out_height; + outputCalibration[4] = 0; + + remapX = (float*)Eigen::internal::aligned_malloc(out_width * out_height *sizeof(float)); + remapY = (float*)Eigen::internal::aligned_malloc(out_width * out_height *sizeof(float)); + + for(int y=0;y 0 && iy > 0 && ix < in_width-1 && iy < in_height-1) + { + remapX[x+y*out_width] = ix; + remapY[x+y*out_width] = iy; + } + else + { + remapX[x+y*out_width] = -1; + remapY[x+y*out_width] = -1; + } + } + } + + printf("Prepped Warp matrices\n"); + } + else + { + printf("Not Rectifying\n"); + outputCalibration[0] = inputCalibration[0]; + outputCalibration[1] = inputCalibration[1]; + outputCalibration[2] = inputCalibration[2]; + outputCalibration[3] = inputCalibration[3]; + outputCalibration[4] = inputCalibration[4]; + out_width = in_width; + out_height = in_height; + } + + + originalK_ = cv::Mat(3, 3, CV_64F, cv::Scalar(0)); + originalK_.at(0, 0) = inputCalibration[0]; + originalK_.at(1, 1) = inputCalibration[1]; + originalK_.at(2, 2) = 1; + originalK_.at(2, 0) = inputCalibration[2]; + originalK_.at(2, 1) = inputCalibration[3]; + + K_ = cv::Mat(3, 3, CV_64F, cv::Scalar(0)); + K_.at(0, 0) = outputCalibration[0] * out_width; + K_.at(1, 1) = outputCalibration[1] * out_height; + K_.at(2, 2) = 1; + K_.at(2, 0) = outputCalibration[2] * out_width - 0.5; + K_.at(2, 1) = outputCalibration[3] * out_height - 0.5; } UndistorterPTAM::~UndistorterPTAM() { - Eigen::internal::aligned_free((void*)remapX); - Eigen::internal::aligned_free((void*)remapY); + Eigen::internal::aligned_free((void*)remapX); + Eigen::internal::aligned_free((void*)remapY); } void UndistorterPTAM::undistort(const cv::Mat& image, cv::OutputArray result) const { - if (!valid) - { - result.getMatRef() = image; - return; - } - - if (image.rows != in_height || image.cols != in_width) - { - printf("UndistorterPTAM: input image size differs from expected input size! Not undistorting.\n"); - result.getMatRef() = image; - return; - } - - if (in_height == out_height && in_width == out_width && inputCalibration[4] == 0) - { - // No transformation if neither distortion nor resize - result.getMatRef() = image; - return; - } - - result.create(out_height, out_width, CV_8U); - cv::Mat resultMat = result.getMatRef(); - assert(result.getMatRef().isContinuous()); - assert(image.isContinuous()); - - uchar* data = resultMat.data; - - for(int idx = out_width*out_height-1;idx>=0;idx--) - { - // get interp. values - float xx = remapX[idx]; - float yy = remapY[idx]; - - if(xx<0) - data[idx] = 0; - else - { - // get integer and rational parts - int xxi = xx; - int yyi = yy; - xx -= xxi; - yy -= yyi; - float xxyy = xx*yy; - - // get array base pointer - const uchar* src = (uchar*)image.data + xxi + yyi * in_width; - - // interpolate (bilinear) - data[idx] = xxyy * src[1+in_width] - + (yy-xxyy) * src[in_width] - + (xx-xxyy) * src[1] - + (1-xx-yy+xxyy) * src[0]; - } - } + if (!valid) + { + result.getMatRef() = image; + return; + } + + if (image.rows != in_height || image.cols != in_width) + { + printf("UndistorterPTAM: input image size differs from expected input size! Not undistorting.\n"); + result.getMatRef() = image; + return; + } + + if (in_height == out_height && in_width == out_width && inputCalibration[4] == 0) + { + // No transformation if neither distortion nor resize + result.getMatRef() = image; + return; + } + + result.create(out_height, out_width, CV_8U); + cv::Mat resultMat = result.getMatRef(); + assert(result.getMatRef().isContinuous()); + assert(image.isContinuous()); + + uchar* data = resultMat.data; + + for(int idx = out_width*out_height-1;idx>=0;idx--) + { + // get interp. values + float xx = remapX[idx]; + float yy = remapY[idx]; + + if(xx<0) + data[idx] = 0; + else + { + // get integer and rational parts + int xxi = xx; + int yyi = yy; + xx -= xxi; + yy -= yyi; + float xxyy = xx*yy; + + // get array base pointer + const uchar* src = (uchar*)image.data + xxi + yyi * in_width; + + // interpolate (bilinear) + data[idx] = xxyy * src[1+in_width] + + (yy-xxyy) * src[in_width] + + (xx-xxyy) * src[1] + + (1-xx-yy+xxyy) * src[0]; + } + } } const cv::Mat& UndistorterPTAM::getK() const { - return K_; + return K_; } const cv::Mat& UndistorterPTAM::getOriginalK() const { - return originalK_; + return originalK_; } int UndistorterPTAM::getOutputWidth() const { - return out_width; + return out_width; } int UndistorterPTAM::getOutputHeight() const { - return out_height; + return out_height; } int UndistorterPTAM::getInputWidth() const { - return in_width; + return in_width; } int UndistorterPTAM::getInputHeight() const { - return in_height; + return in_height; } bool UndistorterPTAM::isValid() const { - return valid; + return valid; } UndistorterOpenCV::UndistorterOpenCV(const char* configFileName) { - valid = true; - - // read parameters - std::ifstream infile(configFileName); - assert(infile.good()); - - std::string l1, l2, l3, l4; - - std::getline(infile,l1); - std::getline(infile,l2); - std::getline(infile,l3); - std::getline(infile,l4); - - // l1 & l2 - 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] - ) == 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\n", - inputCalibration[0], inputCalibration[1], inputCalibration[2], inputCalibration[3], inputCalibration[4], - inputCalibration[5], inputCalibration[6], inputCalibration[7]); - } - else - { - printf("Failed to read camera calibration (invalid format?)\nCalibration file: %s\n", configFileName); - valid = false; - } - - // l3 - if(l3 == "crop") - { - outputCalibration = -1; - printf("Out: Crop\n"); - } - else if(l3 == "full") - { - outputCalibration = -2; - printf("Out: Full\n"); - } - else if(l3 == "none") - { - printf("NO RECTIFICATION\n"); - valid = false; - } - else - { - printf("Out: Failed to Read Output pars... not rectifying.\n"); - valid = false; - } - - // l4 - if(std::sscanf(l4.c_str(), "%d %d", &out_width, &out_height) == 2) - { - printf("Output resolution: %d %d\n", out_width, out_height); - } - else - { - printf("Out: Failed to Read Output resolution... not rectifying.\n"); - valid = false; - } - - cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_32F); - for (int i = 0; i < 4; ++ i) - distCoeffs.at(i, 0) = inputCalibration[4 + i]; - - if(inputCalibration[2] < 1.0f) - { - printf("WARNING: cx = %f < 1, which should not be the case for normal cameras.!\n", inputCalibration[2]); - printf("Possibly this is due to a recent change in the calibration file format, please see the README.md.\n"); - - inputCalibration[0] *= in_width; - inputCalibration[2] *= in_width; - inputCalibration[1] *= in_height; - inputCalibration[3] *= in_height; - - printf("auto-changing calibration file to fx=%f, fy=%f, cx=%f, cy=%f\n", - inputCalibration[0], - inputCalibration[1], - inputCalibration[2], - inputCalibration[3]); - } - - originalK_ = cv::Mat(3, 3, CV_64F, cv::Scalar(0)); - originalK_.at(0, 0) = inputCalibration[0]; - originalK_.at(1, 1) = inputCalibration[1]; - originalK_.at(2, 2) = 1; - originalK_.at(0, 2) = inputCalibration[2]; - originalK_.at(1, 2) = inputCalibration[3]; - - if (valid) - { - K_ = cv::getOptimalNewCameraMatrix(originalK_, distCoeffs, cv::Size(in_width, in_height), (outputCalibration == -2) ? 1 : 0, cv::Size(out_width, out_height), nullptr, false); - - cv::initUndistortRectifyMap(originalK_, distCoeffs, cv::Mat(), K_, - cv::Size(out_width, out_height), CV_16SC2, map1, map2); - - originalK_.at(0, 0) /= in_width; - originalK_.at(0, 2) /= in_width; - originalK_.at(1, 1) /= in_height; - originalK_.at(1, 2) /= in_height; - } - - originalK_ = originalK_.t(); - K_ = K_.t(); + valid = true; + + // read parameters + std::ifstream infile(configFileName); + assert(infile.good()); + + std::string l1, l2, l3, l4; + + std::getline(infile,l1); + std::getline(infile,l2); + std::getline(infile,l3); + std::getline(infile,l4); + + // l1 & l2 + 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] + ) == 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\n", + inputCalibration[0], inputCalibration[1], inputCalibration[2], inputCalibration[3], inputCalibration[4], + inputCalibration[5], inputCalibration[6], inputCalibration[7]); + } + else + { + printf("Failed to read camera calibration (invalid format?)\nCalibration file: %s\n", configFileName); + valid = false; + } + + // l3 + if(l3 == "crop") + { + outputCalibration[0] = -1; + printf("Out: Crop\n"); + } + else if(l3 == "full") + { + outputCalibration[0] = -2; + printf("Out: Full\n"); + } + else if(l3 == "none") + { + printf("NO RECTIFICATION\n"); + valid = false; + } + else if(std::sscanf(l3.c_str(), "%f %f %f %f %f", &outputCalibration[0], &outputCalibration[1], &outputCalibration[2], &outputCalibration[3], &outputCalibration[4]) == 5) + { + printf("Out: %f %f %f %f %f\n", + outputCalibration[0], outputCalibration[1], outputCalibration[2], outputCalibration[3], outputCalibration[4]); + } + else + { + printf("Out: Failed to Read Output pars... not rectifying.\n"); + valid = false; + } + + // l4 + if(std::sscanf(l4.c_str(), "%d %d", &out_width, &out_height) == 2) + { + printf("Output resolution: %d %d\n", out_width, out_height); + } + else + { + printf("Out: Failed to Read Output resolution... not rectifying.\n"); + valid = false; + } + + cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_32F); + for (int i = 0; i < 4; ++ i) + distCoeffs.at(i, 0) = outputCalibration[i]; + + if(inputCalibration[2] < 1.0f) + { + printf("WARNING: cx = %f < 1, which should not be the case for normal cameras.!\n", inputCalibration[2]); + printf("Possibly this is due to a recent change in the calibration file format, please see the README.md.\n"); + + inputCalibration[0] *= in_width; + inputCalibration[2] *= in_width; + inputCalibration[1] *= in_height; + inputCalibration[3] *= in_height; + + printf("auto-changing calibration file to fx=%f, fy=%f, cx=%f, cy=%f\n", + inputCalibration[0], + inputCalibration[1], + inputCalibration[2], + inputCalibration[3]); + } + + originalK_ = cv::Mat(3, 3, CV_64F, cv::Scalar(0)); + originalK_.at(0, 0) = inputCalibration[0]; + originalK_.at(1, 1) = inputCalibration[1]; + originalK_.at(2, 2) = 1; + originalK_.at(0, 2) = inputCalibration[2]; + originalK_.at(1, 2) = inputCalibration[3]; + + if (valid) + { + K_ = cv::getOptimalNewCameraMatrix(originalK_, distCoeffs, cv::Size(in_width, in_height), (outputCalibration[0] == -2) ? 1 : 0, cv::Size(out_width, out_height), nullptr, false); + + cv::initUndistortRectifyMap(originalK_, distCoeffs, cv::Mat(), K_, + cv::Size(out_width, out_height), CV_16SC2, map1, map2); + + originalK_.at(0, 0) /= in_width; + originalK_.at(0, 2) /= in_width; + originalK_.at(1, 1) /= in_height; + originalK_.at(1, 2) /= in_height; + } + + originalK_ = originalK_.t(); + K_ = K_.t(); } UndistorterOpenCV::~UndistorterOpenCV() @@ -563,41 +568,41 @@ UndistorterOpenCV::~UndistorterOpenCV() void UndistorterOpenCV::undistort(const cv::Mat& image, cv::OutputArray result) const { - cv::remap(image, result, map1, map2, cv::INTER_LINEAR); + cv::remap(image, result, map1, map2, cv::INTER_LINEAR); } const cv::Mat& UndistorterOpenCV::getK() const { - return K_; + return K_; } const cv::Mat& UndistorterOpenCV::getOriginalK() const { - return originalK_; + return originalK_; } int UndistorterOpenCV::getOutputWidth() const { - return out_width; + return out_width; } int UndistorterOpenCV::getOutputHeight() const { - return out_height; + return out_height; } int UndistorterOpenCV::getInputWidth() const { - return in_width; + return in_width; } int UndistorterOpenCV::getInputHeight() const { - return in_height; + return in_height; } bool UndistorterOpenCV::isValid() const { - return valid; + return valid; } } diff --git a/lsd_slam_core/src/util/Undistorter.h b/lsd_slam_core/src/util/Undistorter.h index 8cc47dc9..7c571405 100644 --- a/lsd_slam_core/src/util/Undistorter.h +++ b/lsd_slam_core/src/util/Undistorter.h @@ -31,212 +31,212 @@ namespace lsd_slam class Undistorter { public: - virtual ~Undistorter(); - - /** - * Undistorts the given image and returns the result image. - */ - virtual void undistort(const cv::Mat& image, cv::OutputArray result) const = 0; - - /** - * Returns the intrinsic parameter matrix of the undistorted images. - */ - virtual const cv::Mat& getK() const = 0; - - /** - * Returns the intrinsic parameter matrix of the original images, - */ - virtual const cv::Mat& getOriginalK() const = 0; - - /** - * Returns the width of the undistorted images in pixels. - */ - virtual int getOutputWidth() const = 0; - - /** - * Returns the height of the undistorted images in pixels. - */ - 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. - */ - virtual bool isValid() const = 0; - - /** - * Creates and returns an Undistorter of the type used by the given - * configuration file. If the format is not recognized, returns nullptr. - */ - static Undistorter* getUndistorterForFile(const char* configFilename); + virtual ~Undistorter(); + + /** + * Undistorts the given image and returns the result image. + */ + virtual void undistort(const cv::Mat& image, cv::OutputArray result) const = 0; + + /** + * Returns the intrinsic parameter matrix of the undistorted images. + */ + virtual const cv::Mat& getK() const = 0; + + /** + * Returns the intrinsic parameter matrix of the original images, + */ + virtual const cv::Mat& getOriginalK() const = 0; + + /** + * Returns the width of the undistorted images in pixels. + */ + virtual int getOutputWidth() const = 0; + + /** + * Returns the height of the undistorted images in pixels. + */ + 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. + */ + virtual bool isValid() const = 0; + + /** + * Creates and returns an Undistorter of the type used by the given + * configuration file. If the format is not recognized, returns nullptr. + */ + static Undistorter* getUndistorterForFile(const char* configFilename); }; class UndistorterPTAM : public Undistorter { public: - /** - * Creates an Undistorter by reading the distortion parameters from a file. - * - * The file format is as follows: - * d1 d2 d3 d4 d5 - * inputWidth inputHeight - * crop / full / none - * outputWidth outputHeight - */ - UndistorterPTAM(const char* configFileName); - - /** - * Destructor. - */ - ~UndistorterPTAM(); - - UndistorterPTAM(const UndistorterPTAM&) = delete; - UndistorterPTAM& operator=(const UndistorterPTAM&) = delete; - - /** - * Undistorts the given image and returns the result image. - */ - void undistort(const cv::Mat& image, cv::OutputArray result) const; - - /** - * Returns the intrinsic parameter matrix of the undistorted images. - */ - const cv::Mat& getK() const; - - /** - * Returns the intrinsic parameter matrix of the original images, - */ - const cv::Mat& getOriginalK() const; - - /** - * Returns the width of the undistorted images in pixels. - */ - int getOutputWidth() const; - - /** - * Returns the height of the undistorted images in pixels. - */ - 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. - */ - bool isValid() const; - + /** + * Creates an Undistorter by reading the distortion parameters from a file. + * + * The file format is as follows: + * d1 d2 d3 d4 d5 + * inputWidth inputHeight + * crop / full / none + * outputWidth outputHeight + */ + UndistorterPTAM(const char* configFileName); + + /** + * Destructor. + */ + ~UndistorterPTAM(); + + UndistorterPTAM(const UndistorterPTAM&) = delete; + UndistorterPTAM& operator=(const UndistorterPTAM&) = delete; + + /** + * Undistorts the given image and returns the result image. + */ + void undistort(const cv::Mat& image, cv::OutputArray result) const; + + /** + * Returns the intrinsic parameter matrix of the undistorted images. + */ + const cv::Mat& getK() const; + + /** + * Returns the intrinsic parameter matrix of the original images, + */ + const cv::Mat& getOriginalK() const; + + /** + * Returns the width of the undistorted images in pixels. + */ + int getOutputWidth() const; + + /** + * Returns the height of the undistorted images in pixels. + */ + 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. + */ + bool isValid() const; + private: - cv::Mat K_; - cv::Mat originalK_; - - float inputCalibration[5]; - float outputCalibration[5]; - int out_width, out_height; - int in_width, in_height; - float* remapX; - float* remapY; - - - /// Is true if the undistorter object is valid (has been initialized with - /// a valid configuration) - bool valid; + cv::Mat K_; + cv::Mat originalK_; + + float inputCalibration[5]; + float outputCalibration[5]; + int out_width, out_height; + int in_width, in_height; + float* remapX; + float* remapY; + + + /// Is true if the undistorter object is valid (has been initialized with + /// a valid configuration) + bool valid; }; class UndistorterOpenCV : public Undistorter { public: - /** - * Creates an Undistorter by reading the distortion parameters from a file. - * - * The file format is as follows: - * fx fy cx cy d1 d2 d3 d4 d5 d6 - * inputWidth inputHeight - * crop / full / none - * outputWidth outputHeight - */ - UndistorterOpenCV(const char* configFileName); - - /** - * Destructor. - */ - ~UndistorterOpenCV(); - - UndistorterOpenCV(const UndistorterOpenCV&) = delete; - UndistorterOpenCV& operator=(const UndistorterOpenCV&) = delete; - - /** - * Undistorts the given image and returns the result image. - */ - void undistort(const cv::Mat& image, cv::OutputArray result) const; - - /** - * Returns the intrinsic parameter matrix of the undistorted images. - */ - const cv::Mat& getK() const; - - /** - * Returns the intrinsic parameter matrix of the original images, - */ - const cv::Mat& getOriginalK() const; - - /** - * Returns the width of the undistorted images in pixels. - */ - int getOutputWidth() const; - - /** - * Returns the height of the undistorted images in pixels. - */ - 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. - */ - bool isValid() const; - + /** + * Creates an Undistorter by reading the distortion parameters from a file. + * + * The file format is as follows: + * fx fy cx cy d1 d2 d3 d4 d5 d6 + * inputWidth inputHeight + * crop / full / none + * outputWidth outputHeight + */ + UndistorterOpenCV(const char* configFileName); + + /** + * Destructor. + */ + ~UndistorterOpenCV(); + + UndistorterOpenCV(const UndistorterOpenCV&) = delete; + UndistorterOpenCV& operator=(const UndistorterOpenCV&) = delete; + + /** + * Undistorts the given image and returns the result image. + */ + void undistort(const cv::Mat& image, cv::OutputArray result) const; + + /** + * Returns the intrinsic parameter matrix of the undistorted images. + */ + const cv::Mat& getK() const; + + /** + * Returns the intrinsic parameter matrix of the original images, + */ + const cv::Mat& getOriginalK() const; + + /** + * Returns the width of the undistorted images in pixels. + */ + int getOutputWidth() const; + + /** + * Returns the height of the undistorted images in pixels. + */ + 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. + */ + bool isValid() const; + private: - cv::Mat K_; - cv::Mat originalK_; - - float inputCalibration[10]; - float outputCalibration; - int out_width, out_height; - int in_width, in_height; - cv::Mat map1, map2; - - /// Is true if the undistorter object is valid (has been initialized with - /// a valid configuration) - bool valid; + cv::Mat K_; + cv::Mat originalK_; + + float inputCalibration[10]; + float outputCalibration[5]; + int out_width, out_height; + int in_width, in_height; + cv::Mat map1, map2; + + /// Is true if the undistorter object is valid (has been initialized with + /// a valid configuration) + bool valid; }; } From 6bb7af5884e1bb8f6140314e6dd97fa14e558c2d Mon Sep 17 00:00:00 2001 From: sebdi Date: Mon, 25 May 2015 19:36:44 -0400 Subject: [PATCH 2/2] Undo of formating --- lsd_slam_core/src/util/Undistorter.cpp | 948 ++++++++++++------------- lsd_slam_core/src/util/Undistorter.h | 386 +++++----- 2 files changed, 667 insertions(+), 667 deletions(-) diff --git a/lsd_slam_core/src/util/Undistorter.cpp b/lsd_slam_core/src/util/Undistorter.cpp index fc3f78c6..573d0bb2 100644 --- a/lsd_slam_core/src/util/Undistorter.cpp +++ b/lsd_slam_core/src/util/Undistorter.cpp @@ -37,529 +37,529 @@ Undistorter::~Undistorter() Undistorter* Undistorter::getUndistorterForFile(const char* configFilename) { - std::string completeFileName = configFilename; - - printf("Reading Calibration from file %s",completeFileName.c_str()); - - - std::ifstream f(completeFileName.c_str()); - if (!f.good()) - { - f.close(); - - completeFileName = packagePath+"calib/"+configFilename; - printf(" ... not found!\n Trying %s", completeFileName.c_str()); - - f.open(completeFileName.c_str()); - - if (!f.good()) - { - printf(" ... not found. Cannot operate without calibration, shutting down.\n"); - f.close(); - return 0; - } - } - - printf(" ... found!\n"); - - std::string l1; - std::getline(f,l1); - f.close(); - - - - float ic[10]; - 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]) == 8) - { - printf("found OpenCV camera model, building rectifier.\n"); - Undistorter* u = new UndistorterOpenCV(completeFileName.c_str()); - if(!u->isValid()) return 0; - return u; - } - else - { - printf("found ATAN camera model, building rectifier.\n"); - Undistorter* u = new UndistorterPTAM(completeFileName.c_str()); - if(!u->isValid()) return 0; - return u; - } + std::string completeFileName = configFilename; + + printf("Reading Calibration from file %s",completeFileName.c_str()); + + + std::ifstream f(completeFileName.c_str()); + if (!f.good()) + { + f.close(); + + completeFileName = packagePath+"calib/"+configFilename; + printf(" ... not found!\n Trying %s", completeFileName.c_str()); + + f.open(completeFileName.c_str()); + + if (!f.good()) + { + printf(" ... not found. Cannot operate without calibration, shutting down.\n"); + f.close(); + return 0; + } + } + + printf(" ... found!\n"); + + std::string l1; + std::getline(f,l1); + f.close(); + + + + float ic[10]; + 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]) == 8) + { + printf("found OpenCV camera model, building rectifier.\n"); + Undistorter* u = new UndistorterOpenCV(completeFileName.c_str()); + if(!u->isValid()) return 0; + return u; + } + else + { + printf("found ATAN camera model, building rectifier.\n"); + Undistorter* u = new UndistorterPTAM(completeFileName.c_str()); + if(!u->isValid()) return 0; + return u; + } } UndistorterPTAM::UndistorterPTAM(const char* configFileName) { - valid = true; - - remapX = nullptr; - remapY = nullptr; - - - - // read parameters - std::ifstream infile(configFileName); - assert(infile.good()); - - - std::string l1,l2,l3,l4; - - std::getline(infile,l1); - std::getline(infile,l2); - std::getline(infile,l3); - std::getline(infile,l4); - - - - - // l1 & l2 - if(std::sscanf(l1.c_str(), "%f %f %f %f %f", &inputCalibration[0], &inputCalibration[1], &inputCalibration[2], &inputCalibration[3], &inputCalibration[4]) == 5 && - 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\n", - inputCalibration[0], inputCalibration[1], inputCalibration[2], inputCalibration[3], inputCalibration[4]); - } - else - { - printf("Failed to read camera calibration (invalid format?)\nCalibration file: %s\n", configFileName); - valid = false; - } - - // l3 - if(l3 == "crop") - { - outputCalibration[0] = -1; - printf("Out: Crop\n"); - } - else if(l3 == "full") - { - outputCalibration[0] = -2; - printf("Out: Full\n"); - } - else if(l3 == "none") - { - printf("NO RECTIFICATION\n"); - } - else if(std::sscanf(l3.c_str(), "%f %f %f %f %f", &outputCalibration[0], &outputCalibration[1], &outputCalibration[2], &outputCalibration[3], &outputCalibration[4]) == 5) - { - printf("Out: %f %f %f %f %f\n", - outputCalibration[0], outputCalibration[1], outputCalibration[2], outputCalibration[3], outputCalibration[4]); - } - else - { - printf("Out: Failed to Read Output pars... not rectifying.\n"); - valid = false; - } - - - // l4 - if(std::sscanf(l4.c_str(), "%d %d", &out_width, &out_height) == 2) - { - printf("Output resolution: %d %d\n",out_width, out_height); - } - else - { - printf("Out: Failed to Read Output resolution... not rectifying.\n"); - valid = false; - } - - - - - // prep warp matrices - if(valid) - { - float dist = inputCalibration[4]; - float d2t = 2.0f * tan(dist / 2.0f); - - // current camera parameters - float fx = inputCalibration[0] * in_width; - float fy = inputCalibration[1] * in_height; - float cx = inputCalibration[2] * in_width - 0.5; - float cy = inputCalibration[3] * in_height - 0.5; - - // scale calibration parameters to input size - double xfactor = in_width / (1.0 * in_width); - double yfactor = in_height / (1.0 * in_height); - fx = fx * xfactor; - fy = fy * yfactor; - cx = (cx + 0.5) * xfactor - 0.5; - cy = (cy + 0.5) * yfactor - 0.5; - - // output camera parameters - float ofx, ofy, ocx, ocy; - - // find new camera matrix for "crop" and "full" - if (inputCalibration[4] == 0) - { - ofx = inputCalibration[0] * out_width; - ofy = inputCalibration[1] * out_height; - ocx = (inputCalibration[2] * out_width) - 0.5; - ocy = (inputCalibration[3] * out_height) - 0.5; - } - else if(outputCalibration[0] == -1) // "crop" - { - // find left-most and right-most radius - float left_radius = (cx)/fx; - float right_radius = (in_width-1 - cx)/fx; - float top_radius = (cy)/fy; - float bottom_radius = (in_height-1 - cy)/fy; - - float trans_left_radius = tan(left_radius * dist)/d2t; - float trans_right_radius = tan(right_radius * dist)/d2t; - float trans_top_radius = tan(top_radius * dist)/d2t; - float trans_bottom_radius = tan(bottom_radius * dist)/d2t; - - //printf("left_radius: %f -> %f\n", left_radius, trans_left_radius); - //printf("right_radius: %f -> %f\n", right_radius, trans_right_radius); - //printf("top_radius: %f -> %f\n", top_radius, trans_top_radius); - //printf("bottom_radius: %f -> %f\n", bottom_radius, trans_bottom_radius); - - - ofy = fy * ((top_radius + bottom_radius) / (trans_top_radius + trans_bottom_radius)) * ((float)out_height / (float)in_height); - ocy = (trans_top_radius/top_radius) * ofy*cy/fy; - - ofx = fx * ((left_radius + right_radius) / (trans_left_radius + trans_right_radius)) * ((float)out_width / (float)in_width); - ocx = (trans_left_radius/left_radius) * ofx*cx/fx; - - printf("new K: %f %f %f %f\n",ofx,ofy,ocx,ocy); - printf("old K: %f %f %f %f\n",fx,fy,cx,cy); - } - else if(outputCalibration[0] == -2) // "full" - { - float left_radius = cx/fx; - float right_radius = (in_width-1 - cx)/fx; - float top_radius = cy/fy; - float bottom_radius = (in_height-1 - cy)/fy; - - // find left-most and right-most radius - float tl_radius = sqrt(left_radius*left_radius + top_radius*top_radius); - float tr_radius = sqrt(right_radius*right_radius + top_radius*top_radius); - float bl_radius = sqrt(left_radius*left_radius + bottom_radius*bottom_radius); - float br_radius = sqrt(right_radius*right_radius + bottom_radius*bottom_radius); - - float trans_tl_radius = tan(tl_radius * dist)/d2t; - float trans_tr_radius = tan(tr_radius * dist)/d2t; - float trans_bl_radius = tan(bl_radius * dist)/d2t; - float trans_br_radius = tan(br_radius * dist)/d2t; - - //printf("trans_tl_radius: %f -> %f\n", tl_radius, trans_tl_radius); - //printf("trans_tr_radius: %f -> %f\n", tr_radius, trans_tr_radius); - //printf("trans_bl_radius: %f -> %f\n", bl_radius, trans_bl_radius); - //printf("trans_br_radius: %f -> %f\n", br_radius, trans_br_radius); - - - float hor = std::max(br_radius,tr_radius) + std::max(bl_radius,tl_radius); - float vert = std::max(tr_radius,tl_radius) + std::max(bl_radius,br_radius); - - float trans_hor = std::max(trans_br_radius,trans_tr_radius) + std::max(trans_bl_radius,trans_tl_radius); - float trans_vert = std::max(trans_tr_radius,trans_tl_radius) + std::max(trans_bl_radius,trans_br_radius); - - ofy = fy * ((vert) / (trans_vert)) * ((float)out_height / (float)in_height); - ocy = std::max(trans_tl_radius/tl_radius,trans_tr_radius/tr_radius) * ofy*cy/fy; - - ofx = fx * ((hor) / (trans_hor)) * ((float)out_width / (float)in_width); - ocx = std::max(trans_bl_radius/bl_radius,trans_tl_radius/tl_radius) * ofx*cx/fx; - - printf("new K: %f %f %f %f\n",ofx,ofy,ocx,ocy); - printf("old K: %f %f %f %f\n",fx,fy,cx,cy); - } - else - { - ofx = outputCalibration[0] * out_width; - ofy = outputCalibration[1] * out_height; - ocx = outputCalibration[2] * out_width-0.5; // TODO: -0.5 here or not? - ocy = outputCalibration[3] * out_height-0.5; - } - - outputCalibration[0] = ofx / out_width; - outputCalibration[1] = ofy / out_height; - outputCalibration[2] = (ocx+0.5) / out_width; - outputCalibration[3] = (ocy+0.5) / out_height; - outputCalibration[4] = 0; - - remapX = (float*)Eigen::internal::aligned_malloc(out_width * out_height *sizeof(float)); - remapY = (float*)Eigen::internal::aligned_malloc(out_width * out_height *sizeof(float)); - - for(int y=0;y 0 && iy > 0 && ix < in_width-1 && iy < in_height-1) - { - remapX[x+y*out_width] = ix; - remapY[x+y*out_width] = iy; - } - else - { - remapX[x+y*out_width] = -1; - remapY[x+y*out_width] = -1; - } - } - } - - printf("Prepped Warp matrices\n"); - } - else - { - printf("Not Rectifying\n"); - outputCalibration[0] = inputCalibration[0]; - outputCalibration[1] = inputCalibration[1]; - outputCalibration[2] = inputCalibration[2]; - outputCalibration[3] = inputCalibration[3]; - outputCalibration[4] = inputCalibration[4]; - out_width = in_width; - out_height = in_height; - } - - - originalK_ = cv::Mat(3, 3, CV_64F, cv::Scalar(0)); - originalK_.at(0, 0) = inputCalibration[0]; - originalK_.at(1, 1) = inputCalibration[1]; - originalK_.at(2, 2) = 1; - originalK_.at(2, 0) = inputCalibration[2]; - originalK_.at(2, 1) = inputCalibration[3]; - - K_ = cv::Mat(3, 3, CV_64F, cv::Scalar(0)); - K_.at(0, 0) = outputCalibration[0] * out_width; - K_.at(1, 1) = outputCalibration[1] * out_height; - K_.at(2, 2) = 1; - K_.at(2, 0) = outputCalibration[2] * out_width - 0.5; - K_.at(2, 1) = outputCalibration[3] * out_height - 0.5; + valid = true; + + remapX = nullptr; + remapY = nullptr; + + + + // read parameters + std::ifstream infile(configFileName); + assert(infile.good()); + + + std::string l1,l2,l3,l4; + + std::getline(infile,l1); + std::getline(infile,l2); + std::getline(infile,l3); + std::getline(infile,l4); + + + + + // l1 & l2 + if(std::sscanf(l1.c_str(), "%f %f %f %f %f", &inputCalibration[0], &inputCalibration[1], &inputCalibration[2], &inputCalibration[3], &inputCalibration[4]) == 5 && + 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\n", + inputCalibration[0], inputCalibration[1], inputCalibration[2], inputCalibration[3], inputCalibration[4]); + } + else + { + printf("Failed to read camera calibration (invalid format?)\nCalibration file: %s\n", configFileName); + valid = false; + } + + // l3 + if(l3 == "crop") + { + outputCalibration[0] = -1; + printf("Out: Crop\n"); + } + else if(l3 == "full") + { + outputCalibration[0] = -2; + printf("Out: Full\n"); + } + else if(l3 == "none") + { + printf("NO RECTIFICATION\n"); + } + else if(std::sscanf(l3.c_str(), "%f %f %f %f %f", &outputCalibration[0], &outputCalibration[1], &outputCalibration[2], &outputCalibration[3], &outputCalibration[4]) == 5) + { + printf("Out: %f %f %f %f %f\n", + outputCalibration[0], outputCalibration[1], outputCalibration[2], outputCalibration[3], outputCalibration[4]); + } + else + { + printf("Out: Failed to Read Output pars... not rectifying.\n"); + valid = false; + } + + + // l4 + if(std::sscanf(l4.c_str(), "%d %d", &out_width, &out_height) == 2) + { + printf("Output resolution: %d %d\n",out_width, out_height); + } + else + { + printf("Out: Failed to Read Output resolution... not rectifying.\n"); + valid = false; + } + + + + + // prep warp matrices + if(valid) + { + float dist = inputCalibration[4]; + float d2t = 2.0f * tan(dist / 2.0f); + + // current camera parameters + float fx = inputCalibration[0] * in_width; + float fy = inputCalibration[1] * in_height; + float cx = inputCalibration[2] * in_width - 0.5; + float cy = inputCalibration[3] * in_height - 0.5; + + // scale calibration parameters to input size + double xfactor = in_width / (1.0 * in_width); + double yfactor = in_height / (1.0 * in_height); + fx = fx * xfactor; + fy = fy * yfactor; + cx = (cx + 0.5) * xfactor - 0.5; + cy = (cy + 0.5) * yfactor - 0.5; + + // output camera parameters + float ofx, ofy, ocx, ocy; + + // find new camera matrix for "crop" and "full" + if (inputCalibration[4] == 0) + { + ofx = inputCalibration[0] * out_width; + ofy = inputCalibration[1] * out_height; + ocx = (inputCalibration[2] * out_width) - 0.5; + ocy = (inputCalibration[3] * out_height) - 0.5; + } + else if(outputCalibration[0] == -1) // "crop" + { + // find left-most and right-most radius + float left_radius = (cx)/fx; + float right_radius = (in_width-1 - cx)/fx; + float top_radius = (cy)/fy; + float bottom_radius = (in_height-1 - cy)/fy; + + float trans_left_radius = tan(left_radius * dist)/d2t; + float trans_right_radius = tan(right_radius * dist)/d2t; + float trans_top_radius = tan(top_radius * dist)/d2t; + float trans_bottom_radius = tan(bottom_radius * dist)/d2t; + + //printf("left_radius: %f -> %f\n", left_radius, trans_left_radius); + //printf("right_radius: %f -> %f\n", right_radius, trans_right_radius); + //printf("top_radius: %f -> %f\n", top_radius, trans_top_radius); + //printf("bottom_radius: %f -> %f\n", bottom_radius, trans_bottom_radius); + + + ofy = fy * ((top_radius + bottom_radius) / (trans_top_radius + trans_bottom_radius)) * ((float)out_height / (float)in_height); + ocy = (trans_top_radius/top_radius) * ofy*cy/fy; + + ofx = fx * ((left_radius + right_radius) / (trans_left_radius + trans_right_radius)) * ((float)out_width / (float)in_width); + ocx = (trans_left_radius/left_radius) * ofx*cx/fx; + + printf("new K: %f %f %f %f\n",ofx,ofy,ocx,ocy); + printf("old K: %f %f %f %f\n",fx,fy,cx,cy); + } + else if(outputCalibration[0] == -2) // "full" + { + float left_radius = cx/fx; + float right_radius = (in_width-1 - cx)/fx; + float top_radius = cy/fy; + float bottom_radius = (in_height-1 - cy)/fy; + + // find left-most and right-most radius + float tl_radius = sqrt(left_radius*left_radius + top_radius*top_radius); + float tr_radius = sqrt(right_radius*right_radius + top_radius*top_radius); + float bl_radius = sqrt(left_radius*left_radius + bottom_radius*bottom_radius); + float br_radius = sqrt(right_radius*right_radius + bottom_radius*bottom_radius); + + float trans_tl_radius = tan(tl_radius * dist)/d2t; + float trans_tr_radius = tan(tr_radius * dist)/d2t; + float trans_bl_radius = tan(bl_radius * dist)/d2t; + float trans_br_radius = tan(br_radius * dist)/d2t; + + //printf("trans_tl_radius: %f -> %f\n", tl_radius, trans_tl_radius); + //printf("trans_tr_radius: %f -> %f\n", tr_radius, trans_tr_radius); + //printf("trans_bl_radius: %f -> %f\n", bl_radius, trans_bl_radius); + //printf("trans_br_radius: %f -> %f\n", br_radius, trans_br_radius); + + + float hor = std::max(br_radius,tr_radius) + std::max(bl_radius,tl_radius); + float vert = std::max(tr_radius,tl_radius) + std::max(bl_radius,br_radius); + + float trans_hor = std::max(trans_br_radius,trans_tr_radius) + std::max(trans_bl_radius,trans_tl_radius); + float trans_vert = std::max(trans_tr_radius,trans_tl_radius) + std::max(trans_bl_radius,trans_br_radius); + + ofy = fy * ((vert) / (trans_vert)) * ((float)out_height / (float)in_height); + ocy = std::max(trans_tl_radius/tl_radius,trans_tr_radius/tr_radius) * ofy*cy/fy; + + ofx = fx * ((hor) / (trans_hor)) * ((float)out_width / (float)in_width); + ocx = std::max(trans_bl_radius/bl_radius,trans_tl_radius/tl_radius) * ofx*cx/fx; + + printf("new K: %f %f %f %f\n",ofx,ofy,ocx,ocy); + printf("old K: %f %f %f %f\n",fx,fy,cx,cy); + } + else + { + ofx = outputCalibration[0] * out_width; + ofy = outputCalibration[1] * out_height; + ocx = outputCalibration[2] * out_width-0.5; // TODO: -0.5 here or not? + ocy = outputCalibration[3] * out_height-0.5; + } + + outputCalibration[0] = ofx / out_width; + outputCalibration[1] = ofy / out_height; + outputCalibration[2] = (ocx+0.5) / out_width; + outputCalibration[3] = (ocy+0.5) / out_height; + outputCalibration[4] = 0; + + remapX = (float*)Eigen::internal::aligned_malloc(out_width * out_height *sizeof(float)); + remapY = (float*)Eigen::internal::aligned_malloc(out_width * out_height *sizeof(float)); + + for(int y=0;y 0 && iy > 0 && ix < in_width-1 && iy < in_height-1) + { + remapX[x+y*out_width] = ix; + remapY[x+y*out_width] = iy; + } + else + { + remapX[x+y*out_width] = -1; + remapY[x+y*out_width] = -1; + } + } + } + + printf("Prepped Warp matrices\n"); + } + else + { + printf("Not Rectifying\n"); + outputCalibration[0] = inputCalibration[0]; + outputCalibration[1] = inputCalibration[1]; + outputCalibration[2] = inputCalibration[2]; + outputCalibration[3] = inputCalibration[3]; + outputCalibration[4] = inputCalibration[4]; + out_width = in_width; + out_height = in_height; + } + + + originalK_ = cv::Mat(3, 3, CV_64F, cv::Scalar(0)); + originalK_.at(0, 0) = inputCalibration[0]; + originalK_.at(1, 1) = inputCalibration[1]; + originalK_.at(2, 2) = 1; + originalK_.at(2, 0) = inputCalibration[2]; + originalK_.at(2, 1) = inputCalibration[3]; + + K_ = cv::Mat(3, 3, CV_64F, cv::Scalar(0)); + K_.at(0, 0) = outputCalibration[0] * out_width; + K_.at(1, 1) = outputCalibration[1] * out_height; + K_.at(2, 2) = 1; + K_.at(2, 0) = outputCalibration[2] * out_width - 0.5; + K_.at(2, 1) = outputCalibration[3] * out_height - 0.5; } UndistorterPTAM::~UndistorterPTAM() { - Eigen::internal::aligned_free((void*)remapX); - Eigen::internal::aligned_free((void*)remapY); + Eigen::internal::aligned_free((void*)remapX); + Eigen::internal::aligned_free((void*)remapY); } void UndistorterPTAM::undistort(const cv::Mat& image, cv::OutputArray result) const { - if (!valid) - { - result.getMatRef() = image; - return; - } - - if (image.rows != in_height || image.cols != in_width) - { - printf("UndistorterPTAM: input image size differs from expected input size! Not undistorting.\n"); - result.getMatRef() = image; - return; - } - - if (in_height == out_height && in_width == out_width && inputCalibration[4] == 0) - { - // No transformation if neither distortion nor resize - result.getMatRef() = image; - return; - } - - result.create(out_height, out_width, CV_8U); - cv::Mat resultMat = result.getMatRef(); - assert(result.getMatRef().isContinuous()); - assert(image.isContinuous()); - - uchar* data = resultMat.data; - - for(int idx = out_width*out_height-1;idx>=0;idx--) - { - // get interp. values - float xx = remapX[idx]; - float yy = remapY[idx]; - - if(xx<0) - data[idx] = 0; - else - { - // get integer and rational parts - int xxi = xx; - int yyi = yy; - xx -= xxi; - yy -= yyi; - float xxyy = xx*yy; - - // get array base pointer - const uchar* src = (uchar*)image.data + xxi + yyi * in_width; - - // interpolate (bilinear) - data[idx] = xxyy * src[1+in_width] - + (yy-xxyy) * src[in_width] - + (xx-xxyy) * src[1] - + (1-xx-yy+xxyy) * src[0]; - } - } + if (!valid) + { + result.getMatRef() = image; + return; + } + + if (image.rows != in_height || image.cols != in_width) + { + printf("UndistorterPTAM: input image size differs from expected input size! Not undistorting.\n"); + result.getMatRef() = image; + return; + } + + if (in_height == out_height && in_width == out_width && inputCalibration[4] == 0) + { + // No transformation if neither distortion nor resize + result.getMatRef() = image; + return; + } + + result.create(out_height, out_width, CV_8U); + cv::Mat resultMat = result.getMatRef(); + assert(result.getMatRef().isContinuous()); + assert(image.isContinuous()); + + uchar* data = resultMat.data; + + for(int idx = out_width*out_height-1;idx>=0;idx--) + { + // get interp. values + float xx = remapX[idx]; + float yy = remapY[idx]; + + if(xx<0) + data[idx] = 0; + else + { + // get integer and rational parts + int xxi = xx; + int yyi = yy; + xx -= xxi; + yy -= yyi; + float xxyy = xx*yy; + + // get array base pointer + const uchar* src = (uchar*)image.data + xxi + yyi * in_width; + + // interpolate (bilinear) + data[idx] = xxyy * src[1+in_width] + + (yy-xxyy) * src[in_width] + + (xx-xxyy) * src[1] + + (1-xx-yy+xxyy) * src[0]; + } + } } const cv::Mat& UndistorterPTAM::getK() const { - return K_; + return K_; } const cv::Mat& UndistorterPTAM::getOriginalK() const { - return originalK_; + return originalK_; } int UndistorterPTAM::getOutputWidth() const { - return out_width; + return out_width; } int UndistorterPTAM::getOutputHeight() const { - return out_height; + return out_height; } int UndistorterPTAM::getInputWidth() const { - return in_width; + return in_width; } int UndistorterPTAM::getInputHeight() const { - return in_height; + return in_height; } bool UndistorterPTAM::isValid() const { - return valid; + return valid; } UndistorterOpenCV::UndistorterOpenCV(const char* configFileName) { - valid = true; - - // read parameters - std::ifstream infile(configFileName); - assert(infile.good()); - - std::string l1, l2, l3, l4; - - std::getline(infile,l1); - std::getline(infile,l2); - std::getline(infile,l3); - std::getline(infile,l4); - - // l1 & l2 - 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] - ) == 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\n", - inputCalibration[0], inputCalibration[1], inputCalibration[2], inputCalibration[3], inputCalibration[4], - inputCalibration[5], inputCalibration[6], inputCalibration[7]); - } - else - { - printf("Failed to read camera calibration (invalid format?)\nCalibration file: %s\n", configFileName); - valid = false; - } - - // l3 - if(l3 == "crop") - { + valid = true; + + // read parameters + std::ifstream infile(configFileName); + assert(infile.good()); + + std::string l1, l2, l3, l4; + + std::getline(infile,l1); + std::getline(infile,l2); + std::getline(infile,l3); + std::getline(infile,l4); + + // l1 & l2 + 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] + ) == 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\n", + inputCalibration[0], inputCalibration[1], inputCalibration[2], inputCalibration[3], inputCalibration[4], + inputCalibration[5], inputCalibration[6], inputCalibration[7]); + } + else + { + printf("Failed to read camera calibration (invalid format?)\nCalibration file: %s\n", configFileName); + valid = false; + } + + // l3 + if(l3 == "crop") + { outputCalibration[0] = -1; - printf("Out: Crop\n"); - } - else if(l3 == "full") - { + printf("Out: Crop\n"); + } + else if(l3 == "full") + { outputCalibration[0] = -2; - printf("Out: Full\n"); - } - else if(l3 == "none") - { - printf("NO RECTIFICATION\n"); - valid = false; - } + printf("Out: Full\n"); + } + else if(l3 == "none") + { + printf("NO RECTIFICATION\n"); + valid = false; + } else if(std::sscanf(l3.c_str(), "%f %f %f %f %f", &outputCalibration[0], &outputCalibration[1], &outputCalibration[2], &outputCalibration[3], &outputCalibration[4]) == 5) { printf("Out: %f %f %f %f %f\n", - outputCalibration[0], outputCalibration[1], outputCalibration[2], outputCalibration[3], outputCalibration[4]); + outputCalibration[0], outputCalibration[1], outputCalibration[2], outputCalibration[3], outputCalibration[4]); } - else - { - printf("Out: Failed to Read Output pars... not rectifying.\n"); - valid = false; - } - - // l4 - if(std::sscanf(l4.c_str(), "%d %d", &out_width, &out_height) == 2) - { - printf("Output resolution: %d %d\n", out_width, out_height); - } - else - { - printf("Out: Failed to Read Output resolution... not rectifying.\n"); - valid = false; - } - - cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_32F); - for (int i = 0; i < 4; ++ i) + else + { + printf("Out: Failed to Read Output pars... not rectifying.\n"); + valid = false; + } + + // l4 + if(std::sscanf(l4.c_str(), "%d %d", &out_width, &out_height) == 2) + { + printf("Output resolution: %d %d\n", out_width, out_height); + } + else + { + printf("Out: Failed to Read Output resolution... not rectifying.\n"); + valid = false; + } + + cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_32F); + for (int i = 0; i < 4; ++ i) distCoeffs.at(i, 0) = outputCalibration[i]; - if(inputCalibration[2] < 1.0f) - { - printf("WARNING: cx = %f < 1, which should not be the case for normal cameras.!\n", inputCalibration[2]); - printf("Possibly this is due to a recent change in the calibration file format, please see the README.md.\n"); - - inputCalibration[0] *= in_width; - inputCalibration[2] *= in_width; - inputCalibration[1] *= in_height; - inputCalibration[3] *= in_height; - - printf("auto-changing calibration file to fx=%f, fy=%f, cx=%f, cy=%f\n", - inputCalibration[0], - inputCalibration[1], - inputCalibration[2], - inputCalibration[3]); - } - - originalK_ = cv::Mat(3, 3, CV_64F, cv::Scalar(0)); - originalK_.at(0, 0) = inputCalibration[0]; - originalK_.at(1, 1) = inputCalibration[1]; - originalK_.at(2, 2) = 1; - originalK_.at(0, 2) = inputCalibration[2]; - originalK_.at(1, 2) = inputCalibration[3]; - - if (valid) - { + if(inputCalibration[2] < 1.0f) + { + printf("WARNING: cx = %f < 1, which should not be the case for normal cameras.!\n", inputCalibration[2]); + printf("Possibly this is due to a recent change in the calibration file format, please see the README.md.\n"); + + inputCalibration[0] *= in_width; + inputCalibration[2] *= in_width; + inputCalibration[1] *= in_height; + inputCalibration[3] *= in_height; + + printf("auto-changing calibration file to fx=%f, fy=%f, cx=%f, cy=%f\n", + inputCalibration[0], + inputCalibration[1], + inputCalibration[2], + inputCalibration[3]); + } + + originalK_ = cv::Mat(3, 3, CV_64F, cv::Scalar(0)); + originalK_.at(0, 0) = inputCalibration[0]; + originalK_.at(1, 1) = inputCalibration[1]; + originalK_.at(2, 2) = 1; + originalK_.at(0, 2) = inputCalibration[2]; + originalK_.at(1, 2) = inputCalibration[3]; + + if (valid) + { K_ = cv::getOptimalNewCameraMatrix(originalK_, distCoeffs, cv::Size(in_width, in_height), (outputCalibration[0] == -2) ? 1 : 0, cv::Size(out_width, out_height), nullptr, false); - - cv::initUndistortRectifyMap(originalK_, distCoeffs, cv::Mat(), K_, - cv::Size(out_width, out_height), CV_16SC2, map1, map2); - - originalK_.at(0, 0) /= in_width; - originalK_.at(0, 2) /= in_width; - originalK_.at(1, 1) /= in_height; - originalK_.at(1, 2) /= in_height; - } - - originalK_ = originalK_.t(); - K_ = K_.t(); + + cv::initUndistortRectifyMap(originalK_, distCoeffs, cv::Mat(), K_, + cv::Size(out_width, out_height), CV_16SC2, map1, map2); + + originalK_.at(0, 0) /= in_width; + originalK_.at(0, 2) /= in_width; + originalK_.at(1, 1) /= in_height; + originalK_.at(1, 2) /= in_height; + } + + originalK_ = originalK_.t(); + K_ = K_.t(); } UndistorterOpenCV::~UndistorterOpenCV() @@ -568,41 +568,41 @@ UndistorterOpenCV::~UndistorterOpenCV() void UndistorterOpenCV::undistort(const cv::Mat& image, cv::OutputArray result) const { - cv::remap(image, result, map1, map2, cv::INTER_LINEAR); + cv::remap(image, result, map1, map2, cv::INTER_LINEAR); } const cv::Mat& UndistorterOpenCV::getK() const { - return K_; + return K_; } const cv::Mat& UndistorterOpenCV::getOriginalK() const { - return originalK_; + return originalK_; } int UndistorterOpenCV::getOutputWidth() const { - return out_width; + return out_width; } int UndistorterOpenCV::getOutputHeight() const { - return out_height; + return out_height; } int UndistorterOpenCV::getInputWidth() const { - return in_width; + return in_width; } int UndistorterOpenCV::getInputHeight() const { - return in_height; + return in_height; } bool UndistorterOpenCV::isValid() const { - return valid; + return valid; } } diff --git a/lsd_slam_core/src/util/Undistorter.h b/lsd_slam_core/src/util/Undistorter.h index 7c571405..6dd35219 100644 --- a/lsd_slam_core/src/util/Undistorter.h +++ b/lsd_slam_core/src/util/Undistorter.h @@ -31,212 +31,212 @@ namespace lsd_slam class Undistorter { public: - virtual ~Undistorter(); - - /** - * Undistorts the given image and returns the result image. - */ - virtual void undistort(const cv::Mat& image, cv::OutputArray result) const = 0; - - /** - * Returns the intrinsic parameter matrix of the undistorted images. - */ - virtual const cv::Mat& getK() const = 0; - - /** - * Returns the intrinsic parameter matrix of the original images, - */ - virtual const cv::Mat& getOriginalK() const = 0; - - /** - * Returns the width of the undistorted images in pixels. - */ - virtual int getOutputWidth() const = 0; - - /** - * Returns the height of the undistorted images in pixels. - */ - 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. - */ - virtual bool isValid() const = 0; - - /** - * Creates and returns an Undistorter of the type used by the given - * configuration file. If the format is not recognized, returns nullptr. - */ - static Undistorter* getUndistorterForFile(const char* configFilename); + virtual ~Undistorter(); + + /** + * Undistorts the given image and returns the result image. + */ + virtual void undistort(const cv::Mat& image, cv::OutputArray result) const = 0; + + /** + * Returns the intrinsic parameter matrix of the undistorted images. + */ + virtual const cv::Mat& getK() const = 0; + + /** + * Returns the intrinsic parameter matrix of the original images, + */ + virtual const cv::Mat& getOriginalK() const = 0; + + /** + * Returns the width of the undistorted images in pixels. + */ + virtual int getOutputWidth() const = 0; + + /** + * Returns the height of the undistorted images in pixels. + */ + 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. + */ + virtual bool isValid() const = 0; + + /** + * Creates and returns an Undistorter of the type used by the given + * configuration file. If the format is not recognized, returns nullptr. + */ + static Undistorter* getUndistorterForFile(const char* configFilename); }; class UndistorterPTAM : public Undistorter { public: - /** - * Creates an Undistorter by reading the distortion parameters from a file. - * - * The file format is as follows: - * d1 d2 d3 d4 d5 - * inputWidth inputHeight - * crop / full / none - * outputWidth outputHeight - */ - UndistorterPTAM(const char* configFileName); - - /** - * Destructor. - */ - ~UndistorterPTAM(); - - UndistorterPTAM(const UndistorterPTAM&) = delete; - UndistorterPTAM& operator=(const UndistorterPTAM&) = delete; - - /** - * Undistorts the given image and returns the result image. - */ - void undistort(const cv::Mat& image, cv::OutputArray result) const; - - /** - * Returns the intrinsic parameter matrix of the undistorted images. - */ - const cv::Mat& getK() const; - - /** - * Returns the intrinsic parameter matrix of the original images, - */ - const cv::Mat& getOriginalK() const; - - /** - * Returns the width of the undistorted images in pixels. - */ - int getOutputWidth() const; - - /** - * Returns the height of the undistorted images in pixels. - */ - 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. - */ - bool isValid() const; - + /** + * Creates an Undistorter by reading the distortion parameters from a file. + * + * The file format is as follows: + * d1 d2 d3 d4 d5 + * inputWidth inputHeight + * crop / full / none + * outputWidth outputHeight + */ + UndistorterPTAM(const char* configFileName); + + /** + * Destructor. + */ + ~UndistorterPTAM(); + + UndistorterPTAM(const UndistorterPTAM&) = delete; + UndistorterPTAM& operator=(const UndistorterPTAM&) = delete; + + /** + * Undistorts the given image and returns the result image. + */ + void undistort(const cv::Mat& image, cv::OutputArray result) const; + + /** + * Returns the intrinsic parameter matrix of the undistorted images. + */ + const cv::Mat& getK() const; + + /** + * Returns the intrinsic parameter matrix of the original images, + */ + const cv::Mat& getOriginalK() const; + + /** + * Returns the width of the undistorted images in pixels. + */ + int getOutputWidth() const; + + /** + * Returns the height of the undistorted images in pixels. + */ + 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. + */ + bool isValid() const; + private: - cv::Mat K_; - cv::Mat originalK_; - - float inputCalibration[5]; - float outputCalibration[5]; - int out_width, out_height; - int in_width, in_height; - float* remapX; - float* remapY; - - - /// Is true if the undistorter object is valid (has been initialized with - /// a valid configuration) - bool valid; + cv::Mat K_; + cv::Mat originalK_; + + float inputCalibration[5]; + float outputCalibration[5]; + int out_width, out_height; + int in_width, in_height; + float* remapX; + float* remapY; + + + /// Is true if the undistorter object is valid (has been initialized with + /// a valid configuration) + bool valid; }; class UndistorterOpenCV : public Undistorter { public: - /** - * Creates an Undistorter by reading the distortion parameters from a file. - * - * The file format is as follows: - * fx fy cx cy d1 d2 d3 d4 d5 d6 - * inputWidth inputHeight - * crop / full / none - * outputWidth outputHeight - */ - UndistorterOpenCV(const char* configFileName); - - /** - * Destructor. - */ - ~UndistorterOpenCV(); - - UndistorterOpenCV(const UndistorterOpenCV&) = delete; - UndistorterOpenCV& operator=(const UndistorterOpenCV&) = delete; - - /** - * Undistorts the given image and returns the result image. - */ - void undistort(const cv::Mat& image, cv::OutputArray result) const; - - /** - * Returns the intrinsic parameter matrix of the undistorted images. - */ - const cv::Mat& getK() const; - - /** - * Returns the intrinsic parameter matrix of the original images, - */ - const cv::Mat& getOriginalK() const; - - /** - * Returns the width of the undistorted images in pixels. - */ - int getOutputWidth() const; - - /** - * Returns the height of the undistorted images in pixels. - */ - 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. - */ - bool isValid() const; - + /** + * Creates an Undistorter by reading the distortion parameters from a file. + * + * The file format is as follows: + * fx fy cx cy d1 d2 d3 d4 d5 d6 + * inputWidth inputHeight + * crop / full / none + * outputWidth outputHeight + */ + UndistorterOpenCV(const char* configFileName); + + /** + * Destructor. + */ + ~UndistorterOpenCV(); + + UndistorterOpenCV(const UndistorterOpenCV&) = delete; + UndistorterOpenCV& operator=(const UndistorterOpenCV&) = delete; + + /** + * Undistorts the given image and returns the result image. + */ + void undistort(const cv::Mat& image, cv::OutputArray result) const; + + /** + * Returns the intrinsic parameter matrix of the undistorted images. + */ + const cv::Mat& getK() const; + + /** + * Returns the intrinsic parameter matrix of the original images, + */ + const cv::Mat& getOriginalK() const; + + /** + * Returns the width of the undistorted images in pixels. + */ + int getOutputWidth() const; + + /** + * Returns the height of the undistorted images in pixels. + */ + 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. + */ + bool isValid() const; + private: - cv::Mat K_; - cv::Mat originalK_; - - float inputCalibration[10]; + cv::Mat K_; + cv::Mat originalK_; + + float inputCalibration[10]; float outputCalibration[5]; - int out_width, out_height; - int in_width, in_height; - cv::Mat map1, map2; - - /// Is true if the undistorter object is valid (has been initialized with - /// a valid configuration) - bool valid; + int out_width, out_height; + int in_width, in_height; + cv::Mat map1, map2; + + /// Is true if the undistorter object is valid (has been initialized with + /// a valid configuration) + bool valid; }; }