diff --git a/kinect2_calibration/src/kinect2_calibration.cpp b/kinect2_calibration/src/kinect2_calibration.cpp index a03260dc..d25590ec 100644 --- a/kinect2_calibration/src/kinect2_calibration.cpp +++ b/kinect2_calibration/src/kinect2_calibration.cpp @@ -51,7 +51,8 @@ enum Mode { RECORD, - CALIBRATE + CALIBRATE, + REPROCESS }; enum Source @@ -61,6 +62,37 @@ enum Source SYNC }; + + static void findMinMax(const cv::Mat &ir,int &minIr, int & maxIr) + { + for(size_t r = 0; r < (size_t)ir.rows; ++r) + { + const uint16_t *it = ir.ptr(r); + + for(size_t c = 0; c < (size_t)ir.cols; ++c, ++it) + { + minIr = std::min(minIr, (int) * it); + maxIr = std::max(maxIr, (int) * it); + } + } + } + + + static void findMinMax(const cv::Mat &ir, const std::vector &pointsIr,int &minIr, int & maxIr) + { + minIr = 0xFFFF; + maxIr = 0; + for(size_t i = 0; i < pointsIr.size(); ++i) + { + const cv::Point2f &p = pointsIr[i]; + cv::Rect roi(std::max(0, (int)p.x - 2), std::max(0, (int)p.y - 2), 9, 9); + roi.width = std::min(roi.width, ir.cols - roi.x); + roi.height = std::min(roi.height, ir.rows - roi.y); + + findMinMax(ir(roi),minIr,maxIr); + } + } + class Recorder { private: @@ -191,34 +223,7 @@ class Recorder clahe->apply(grey, grey); } - void findMinMax(const cv::Mat &ir, const std::vector &pointsIr) - { - minIr = 0xFFFF; - maxIr = 0; - for(size_t i = 0; i < pointsIr.size(); ++i) - { - const cv::Point2f &p = pointsIr[i]; - cv::Rect roi(std::max(0, (int)p.x - 2), std::max(0, (int)p.y - 2), 9, 9); - roi.width = std::min(roi.width, ir.cols - roi.x); - roi.height = std::min(roi.height, ir.rows - roi.y); - findMinMax(ir(roi)); - } - } - - void findMinMax(const cv::Mat &ir) - { - for(size_t r = 0; r < (size_t)ir.rows; ++r) - { - const uint16_t *it = ir.ptr(r); - - for(size_t c = 0; c < (size_t)ir.cols; ++c, ++it) - { - minIr = std::min(minIr, (int) * it); - maxIr = std::max(maxIr, (int) * it); - } - } - } void callback(const sensor_msgs::Image::ConstPtr imageColor, const sensor_msgs::Image::ConstPtr imageIr, const sensor_msgs::Image::ConstPtr imageDepth) { @@ -285,7 +290,7 @@ class Recorder if(foundIr) { // Update min and max ir value based on checkerboard values - findMinMax(irScaled, pointsIr); + findMinMax(irScaled, pointsIr,minIr,maxIr); } lock.lock(); @@ -455,6 +460,7 @@ class Recorder class CameraCalibration { private: + int circleFlags; const bool circleBoard; const cv::Size boardDims; const float boardSize; @@ -478,11 +484,23 @@ class CameraCalibration std::vector rvecsColor, tvecsColor; std::vector rvecsIr, tvecsIr; + int minIr=0,maxIr=0x7FFF; + cv::Ptr clahe; public: - CameraCalibration(const std::string &path, const Source mode, const bool circleBoard, const cv::Size &boardDims, const float boardSize, const bool rational) + CameraCalibration(const std::string &path, const Source mode, const bool circleBoard, const cv::Size &boardDims, const float boardSize, const bool rational,const bool symmetric) : circleBoard(circleBoard), boardDims(boardDims), boardSize(boardSize), flags(rational ? cv::CALIB_RATIONAL_MODEL : 0), mode(mode), path(path), sizeColor(1920, 1080), sizeIr(512, 424) { + + if(symmetric) + { + circleFlags = cv::CALIB_CB_SYMMETRIC_GRID + cv::CALIB_CB_CLUSTERING; + } + else + { + circleFlags = cv::CALIB_CB_ASYMMETRIC_GRID + cv::CALIB_CB_CLUSTERING; + } + board.resize(boardDims.width * boardDims.height); for(size_t r = 0, i = 0; r < (size_t)boardDims.height; ++r) { @@ -491,12 +509,190 @@ class CameraCalibration board[i] = cv::Point3f(c * boardSize, r * boardSize, 0); } } + + clahe = cv::createCLAHE(1.5, cv::Size(32, 32)); + } + + + + void convertIr(const cv::Mat &ir, cv::Mat &grey) + { + const float factor = 255.0f / (maxIr - minIr); + grey.create(ir.rows, ir.cols, CV_8U); + + #pragma omp parallel for + for(size_t r = 0; r < (size_t)ir.rows; ++r) + { + const uint16_t *itI = ir.ptr(r); + uint8_t *itO = grey.ptr(r); + + for(size_t c = 0; c < (size_t)ir.cols; ++c, ++itI, ++itO) + { + *itO = std::min(std::max(*itI - minIr, 0) * factor, 255.0f); + } + } + clahe->apply(grey, grey); } + + ~CameraCalibration() { } + + void processimage(cv::Mat image,std::string base,std::string filename, bool imagecolor) + { + std::cout << "reprocessing " << filename << std::endl; + std::vector pointsColor, pointsIr; + cv::Mat color, ir, irGrey, irScaled, depth; + bool foundColor = false; + bool foundIr = false; + + if(imagecolor) + { + color = image; + } + else + { + ir = image; + /// readImage(imageDepth, depth); + cv::resize(ir, irScaled, cv::Size(), 2.0, 2.0, cv::INTER_CUBIC); + + convertIr(irScaled, irGrey); + } + + if(circleBoard) + { + switch(mode) + { + case COLOR: + foundColor = cv::findCirclesGrid(color, boardDims, pointsColor, circleFlags); + break; + case IR: + foundIr = cv::findCirclesGrid(irGrey, boardDims, pointsIr, circleFlags); + break; + case SYNC: + foundColor = cv::findCirclesGrid(color, boardDims, pointsColor, circleFlags); + foundIr = cv::findCirclesGrid(irGrey, boardDims, pointsIr, circleFlags); + break; + } + } + else + { + const cv::TermCriteria termCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::COUNT, 100, DBL_EPSILON); + switch(mode) + { + case COLOR: + foundColor = cv::findChessboardCorners(color, boardDims, pointsColor, cv::CALIB_CB_FAST_CHECK); + break; + case IR: + foundIr = cv::findChessboardCorners(irGrey, boardDims, pointsIr, cv::CALIB_CB_ADAPTIVE_THRESH); + break; + case SYNC: + if(imagecolor) + foundColor = cv::findChessboardCorners(color, boardDims, pointsColor, cv::CALIB_CB_FAST_CHECK); + else + foundIr = cv::findChessboardCorners(irGrey, boardDims, pointsIr, cv::CALIB_CB_ADAPTIVE_THRESH); + break; + } + if(foundColor) + { + cv::cornerSubPix(color, pointsColor, cv::Size(11, 11), cv::Size(-1, -1), termCriteria); + } + if(foundIr) + { + cv::cornerSubPix(irGrey, pointsIr, cv::Size(11, 11), cv::Size(-1, -1), termCriteria); + } + } + + if(foundIr) + { + // Update min and max ir value based on checkerboard values + findMinMax(irScaled, pointsIr,minIr,maxIr); + } + + + std::cerr << "\toutput " << base << " found color: " << foundColor << " " << pointsColor.size() << " ir: " << foundIr << " " << pointsIr.size() << std::endl; + for(size_t i = 0; i < pointsIr.size(); ++i) + { + pointsIr[i].x /= 2.0; + pointsIr[i].y /= 2.0; + } + + + if(imagecolor) + { + + cv::FileStorage file(base + CALIB_POINTS_COLOR, cv::FileStorage::WRITE); + file << "points" << pointsColor; + } + else + { + cv::FileStorage file(base + CALIB_POINTS_IR, cv::FileStorage::WRITE); + file << "points" << pointsIr; + } + + } + + + void reprocess() + { + std::vector filesSync; + std::vector filesColor; + std::vector filesIr; + + DIR *dp; + struct dirent *dirp; + size_t posColor; + + if((dp = opendir(path.c_str())) == NULL) + { + OUT_ERROR("Error opening: " << path); + return ; + } + + while((dirp = readdir(dp)) != NULL) + { + std::string filename = dirp->d_name; + + if(dirp->d_type != DT_REG) + { + continue; + } + posColor = filename.rfind(CALIB_FILE_COLOR); + if(posColor != std::string::npos) + { + cv::Mat image = cv::imread(filename, cv::IMREAD_ANYDEPTH); + processimage(image,filename.substr(0,posColor),filename,true); + continue; + } + posColor = filename.rfind(CALIB_FILE_IR_GREY); + if(posColor != std::string::npos) + { + cv::Mat image = cv::imread(filename, cv::IMREAD_ANYDEPTH); + processimage(image,filename.substr(0,posColor),filename,false); + continue; + } + posColor = filename.rfind(CALIB_FILE_IR); + if(posColor != std::string::npos) + { + //cv::Mat image = cv::imread(filename, cv::IMREAD_ANYDEPTH); + //processimage(image,filename.substr(0,posColor),filename); + continue; + } + posColor = filename.rfind(CALIB_FILE_DEPTH); + if(posColor != std::string::npos) + { + //cv::Mat image = cv::imread(filename, cv::IMREAD_ANYDEPTH); + // processimage(image,filename.substr(0,posColor),filename); + continue; + } + + } + + } + bool restore() { std::vector filesSync; @@ -617,6 +813,7 @@ class CameraCalibration storeCalibration(); } + private: bool readFiles(const std::vector &files, const std::string &ext, std::vector > &points) const { @@ -849,6 +1046,8 @@ class DepthCalibration { } + void reprocess() + {} bool restore() { std::vector files; @@ -1169,7 +1368,7 @@ void help(const std::string &path) { std::cout << path << FG_BLUE " [options]" << std::endl << FG_GREEN " name" NO_COLOR ": " FG_YELLOW "'any string'" NO_COLOR " equals to the kinect2_bridge topic base name" << std::endl - << FG_GREEN " mode" NO_COLOR ": " FG_YELLOW "'record'" NO_COLOR " or " FG_YELLOW "'calibrate'" << std::endl + << FG_GREEN " mode" NO_COLOR ": " FG_YELLOW "'record'" NO_COLOR " or " FG_YELLOW "'calibrate' or reprocess" << std::endl << FG_GREEN " source" NO_COLOR ": " FG_YELLOW "'color'" NO_COLOR ", " FG_YELLOW "'ir'" NO_COLOR ", " FG_YELLOW "'sync'" NO_COLOR ", " FG_YELLOW "'depth'" << std::endl << FG_GREEN " board" NO_COLOR ":" << std::endl << FG_YELLOW " 'circlexx' " NO_COLOR "for symmetric circle grid" << std::endl @@ -1226,6 +1425,10 @@ int main(int argc, char **argv) { mode = CALIBRATE; } + else if(arg == "reprocess") + { + mode = REPROCESS; + } else if(arg == "color") { source = COLOR; @@ -1336,6 +1539,16 @@ int main(int argc, char **argv) recorder.run(); OUT_INFO("stopped recording..."); + } + else if(mode == REPROCESS) + { + + CameraCalibration calib(path, source, circleBoard, boardDims, boardSize, rational,symmetric); + + OUT_INFO("restoring files..."); + calib.reprocess(); + + } else if(calibDepth) { @@ -1349,7 +1562,7 @@ int main(int argc, char **argv) } else { - CameraCalibration calib(path, source, circleBoard, boardDims, boardSize, rational); + CameraCalibration calib(path, source, circleBoard, boardDims, boardSize, rational,symmetric); OUT_INFO("restoring files..."); calib.restore();