采用QHD分辨率使用kinect2_calibration，完成QHD图像校正-白红宇

采用QHD分辨率使用kinect2_calibration，完成QHD图像校正
阅读量：5008 次
发布时间：2019-06-12
本文共 33146 字，大约阅读时间需要 110 分钟。
//.................................................................................
//采用QHD分辨率使用kinect2_calibration，完成QHD图像校正
参考“http://www.bubuko.com/infodetail-2151412.html”
1.修改kinect2_calibration.cpp文件如下：
a)K2_TOPIC_HD 改为 K2_TOPIC_QHD
b)把文件中涉及到分辨率1920x1080都改成960x540，这一步非常重要，不然校正出来的内参还是按照HD分辨率进行校正
我的文件如下：
/** * Copyright 2014 University of Bremen, Institute for Artificial Intelligence * Author: Thiemo Wiedemeyer 
     
       * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * *     http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */#include 
      
       #include 
       
        #include 
        
         #include 
         
          #include 
          
           #include 
           
            #include 
            
             #include 
             
              #include 
              
               #include 
               
                #include 
                
                 #include 
                 
                  #include 
                  
                   #include 
                   
                    #include 
                    
                     #include 
                     
                      #include 
                      
                       #include 
                       
                        #include 
                        
                         #include 
                         
                          #include 
                          
                           #include 
                           
                            #include 
                            
                             #include 
                             
                              enum Mode{ RECORD, CALIBRATE};enum Source{ COLOR, IR, SYNC};class Recorder{private: const bool circleBoard; int circleFlags; const cv::Size boardDims; const float boardSize; const Source mode; const std::string path; const std::string topicColor, topicIr, topicDepth; std::mutex lock; bool update; bool foundColor, foundIr; cv::Mat color, ir, irGrey, depth; size_t frame; std::vector
                              
                                params; std::vector
                               
                                 board; std::vector
                                
                                  pointsColor, pointsIr; typedef message_filters::sync_policies::ExactTime
                                 
                                   ColorIrDepthSyncPolicy; ros::NodeHandle nh; ros::AsyncSpinner spinner; image_transport::ImageTransport it; image_transport::SubscriberFilter *subImageColor, *subImageIr, *subImageDepth; message_filters::Synchronizer
                                  
                                    *sync; int minIr, maxIr; cv::Ptr
                                   
                                     clahe;public: Recorder(const std::string &path, const std::string &topicColor, const std::string &topicIr, const std::string &topicDepth, const Source mode, const bool circleBoard, const bool symmetric, const cv::Size &boardDims, const float boardSize) : circleBoard(circleBoard), boardDims(boardDims), boardSize(boardSize), mode(mode), path(path), topicColor(topicColor), topicIr(topicIr), topicDepth(topicDepth), update(false), foundColor(false), foundIr(false), frame(0), nh("~"), spinner(0), it(nh), minIr(0), maxIr(0x7FFF) { if(symmetric) { circleFlags = cv::CALIB_CB_SYMMETRIC_GRID + cv::CALIB_CB_CLUSTERING; } else { circleFlags = cv::CALIB_CB_ASYMMETRIC_GRID + cv::CALIB_CB_CLUSTERING; } params.push_back(CV_IMWRITE_PNG_COMPRESSION); params.push_back(9); board.resize(boardDims.width * boardDims.height); for(size_t r = 0, i = 0; r < (size_t)boardDims.height; ++r) { for(size_t c = 0; c < (size_t)boardDims.width; ++c, ++i) { board[i] = cv::Point3f(c * boardSize, r * boardSize, 0); } } clahe = cv::createCLAHE(1.5, cv::Size(32, 32)); } ~Recorder() { } void run() { startRecord(); display(); stopRecord(); }private: void startRecord() { OUT_INFO("Controls:" << std::endl << FG_YELLOW " [ESC, q]" NO_COLOR " - Exit" << std::endl << FG_YELLOW " [SPACE, s]" NO_COLOR " - Save current frame" << std::endl << FG_YELLOW " [l]" NO_COLOR " - decrease min and max value for IR value range" << std::endl << FG_YELLOW " [h]" NO_COLOR " - increase min and max value for IR value range" << std::endl << FG_YELLOW " [1]" NO_COLOR " - decrease min value for IR value range" << std::endl << FG_YELLOW " [2]" NO_COLOR " - increase min value for IR value range" << std::endl << FG_YELLOW " [3]" NO_COLOR " - decrease max value for IR value range" << std::endl << FG_YELLOW " [4]" NO_COLOR " - increase max value for IR value range"); image_transport::TransportHints hints("compressed"); subImageColor = new image_transport::SubscriberFilter(it, topicColor, 4, hints); subImageIr = new image_transport::SubscriberFilter(it, topicIr, 4, hints); subImageDepth = new image_transport::SubscriberFilter(it, topicDepth, 4, hints); sync = new message_filters::Synchronizer
                                    
                                     (ColorIrDepthSyncPolicy(4), *subImageColor, *subImageIr, *subImageDepth); sync->registerCallback(boost::bind(&Recorder::callback, this, _1, _2, _3)); spinner.start(); } void stopRecord() { spinner.stop(); delete sync; delete subImageColor; delete subImageIr; delete subImageDepth; } 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); } 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) { std::vector
                                         
                                           pointsColor, pointsIr; cv::Mat color, ir, irGrey, irScaled, depth; bool foundColor = false; bool foundIr = false; if(mode == COLOR || mode == SYNC) { readImage(imageColor, color); } if(mode == IR || mode == SYNC) { readImage(imageIr, ir); 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: foundColor = cv::findChessboardCorners(color, boardDims, pointsColor, cv::CALIB_CB_FAST_CHECK); 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); } lock.lock(); this->color = color; this->ir = ir; this->irGrey = irGrey; this->depth = depth; this->foundColor = foundColor; this->foundIr = foundIr; this->pointsColor = pointsColor; this->pointsIr = pointsIr; update = true; lock.unlock(); } void display() { std::vector
                                          
                                            pointsColor, pointsIr; cv::Mat color, ir, irGrey, depth; cv::Mat colorDisp, irDisp; bool foundColor = false; bool foundIr = false; bool save = false; bool running = true; std::chrono::milliseconds duration(1); while(!update && ros::ok()) { std::this_thread::sleep_for(duration); } for(; ros::ok() && running;) { if(update) { lock.lock(); color = this->color; ir = this->ir; irGrey = this->irGrey; depth = this->depth; foundColor = this->foundColor; foundIr = this->foundIr; pointsColor = this->pointsColor; pointsIr = this->pointsIr; update = false; lock.unlock(); if(mode == COLOR || mode == SYNC) { cv::cvtColor(color, colorDisp, CV_GRAY2BGR); cv::drawChessboardCorners(colorDisp, boardDims, pointsColor, foundColor); //cv::resize(colorDisp, colorDisp, cv::Size(), 0.5, 0.5); //cv::flip(colorDisp, colorDisp, 1); } if(mode == IR || mode == SYNC) { cv::cvtColor(irGrey, irDisp, CV_GRAY2BGR); cv::drawChessboardCorners(irDisp, boardDims, pointsIr, foundIr); //cv::resize(irDisp, irDisp, cv::Size(), 0.5, 0.5); //cv::flip(irDisp, irDisp, 1); } } switch(mode) { case COLOR: cv::imshow("color", colorDisp); break; case IR: cv::imshow("ir", irDisp); break; case SYNC: cv::imshow("color", colorDisp); cv::imshow("ir", irDisp); break; } int key = cv::waitKey(10); switch(key & 0xFF) { case ' ': case 's': save = true; break; case 27: case 'q': running = false; break; case '1': minIr = std::max(0, minIr - 100); break; case '2': minIr = std::min(maxIr - 1, minIr + 100); break; case '3': maxIr = std::max(minIr + 1, maxIr - 100); break; case '4': maxIr = std::min(0xFFFF, maxIr + 100); break; case 'l': minIr = std::max(0, minIr - 100); maxIr = std::max(minIr + 1, maxIr - 100); break; case 'h': maxIr = std::min(0x7FFF, maxIr + 100); minIr = std::min(maxIr - 1, minIr + 100); break; } if(save && ((mode == COLOR && foundColor) || (mode == IR && foundIr) || (mode == SYNC && foundColor && foundIr))) { store(color, ir, irGrey, depth, pointsColor, pointsIr); save = false; } } cv::destroyAllWindows(); cv::waitKey(100); } void readImage(const sensor_msgs::Image::ConstPtr msgImage, cv::Mat &image) const { cv_bridge::CvImageConstPtr pCvImage; pCvImage = cv_bridge::toCvShare(msgImage, msgImage->encoding); pCvImage->image.copyTo(image); } void store(const cv::Mat &color, const cv::Mat &ir, const cv::Mat &irGrey, const cv::Mat &depth, const std::vector
                                           
                                             &pointsColor, std::vector
                                            
                                              &pointsIr) { std::ostringstream oss; oss << std::setfill('0') << std::setw(4) << frame++; const std::string frameNumber(oss.str()); OUT_INFO("storing frame: " << frameNumber); std::string base = path + frameNumber; for(size_t i = 0; i < pointsIr.size(); ++i) { pointsIr[i].x /= 2.0; pointsIr[i].y /= 2.0; } if(mode == SYNC) { base += CALIB_SYNC; } if(mode == COLOR || mode == SYNC) { cv::imwrite(base + CALIB_FILE_COLOR, color, params); cv::FileStorage file(base + CALIB_POINTS_COLOR, cv::FileStorage::WRITE); file << "points" << pointsColor; } if(mode == IR || mode == SYNC) { cv::imwrite(base + CALIB_FILE_IR, ir, params); cv::imwrite(base + CALIB_FILE_IR_GREY, irGrey, params); cv::imwrite(base + CALIB_FILE_DEPTH, depth, params); cv::FileStorage file(base + CALIB_POINTS_IR, cv::FileStorage::WRITE); file << "points" << pointsIr; } }};class CameraCalibration{private: const bool circleBoard; const cv::Size boardDims; const float boardSize; const int flags; const Source mode; const std::string path; std::vector
                                             
                                               board; std::vector
                                              
                                               
                                                 > pointsBoard; std::vector
                                                
                                                 
                                                   > pointsColor; std::vector
                                                  
                                                   
                                                     > pointsIr; cv::Size sizeColor; cv::Size sizeIr; cv::Mat cameraMatrixColor, distortionColor, rotationColor, translationColor, projectionColor; cv::Mat cameraMatrixIr, distortionIr, rotationIr, translationIr, projectionIr; cv::Mat rotation, translation, essential, fundamental, disparity; std::vector
                                                    
                                                      rvecsColor, tvecsColor; std::vector
                                                     
                                                       rvecsIr, tvecsIr;public: CameraCalibration(const std::string &path, const Source mode, const bool circleBoard, const cv::Size &boardDims, const float boardSize, const bool rational) : circleBoard(circleBoard), boardDims(boardDims), boardSize(boardSize), flags(rational ? cv::CALIB_RATIONAL_MODEL : 0), mode(mode), path(path), sizeColor(1920, 1080), sizeIr(512, 424) { board.resize(boardDims.width * boardDims.height); for(size_t r = 0, i = 0; r < (size_t)boardDims.height; ++r) { for(size_t c = 0; c < (size_t)boardDims.width; ++c, ++i) { board[i] = cv::Point3f(c * boardSize, r * boardSize, 0); } } } ~CameraCalibration() { } bool restore() { std::vector
                                                      
                                                        filesSync; std::vector
                                                       
                                                         filesColor; std::vector
                                                        
                                                          filesIr; DIR *dp; struct dirent *dirp; size_t posColor, posIr, posSync; if((dp = opendir(path.c_str())) == NULL) { OUT_ERROR("Error opening: " << path); return false; } while((dirp = readdir(dp)) != NULL) { std::string filename = dirp->d_name; if(dirp->d_type != DT_REG) { continue; } posSync = filename.rfind(CALIB_SYNC); posColor = filename.rfind(CALIB_FILE_COLOR); if(posSync != std::string::npos) { if(posColor != std::string::npos) { std::string frameName = filename.substr(0, posColor); filesSync.push_back(frameName); filesColor.push_back(frameName); filesIr.push_back(frameName); } continue; } if(posColor != std::string::npos) { std::string frameName = filename.substr(0, posColor); filesColor.push_back(frameName); continue; } posIr = filename.rfind(CALIB_FILE_IR_GREY); if(posIr != std::string::npos) { std::string frameName = filename.substr(0, posIr); filesIr.push_back(frameName); continue; } } closedir(dp); std::sort(filesColor.begin(), filesColor.end()); std::sort(filesIr.begin(), filesIr.end()); std::sort(filesSync.begin(), filesSync.end()); bool ret = true; switch(mode) { case COLOR: if(filesColor.empty()) { OUT_ERROR("no files found!"); return false; } pointsColor.resize(filesColor.size()); pointsBoard.resize(filesColor.size(), board); ret = ret && readFiles(filesColor, CALIB_POINTS_COLOR, pointsColor); break; case IR: if(filesIr.empty()) { OUT_ERROR("no files found!"); return false; } pointsIr.resize(filesIr.size()); pointsBoard.resize(filesIr.size(), board); ret = ret && readFiles(filesIr, CALIB_POINTS_IR, pointsIr); break; case SYNC: if(filesColor.empty() || filesIr.empty()) { OUT_ERROR("no files found!"); return false; } pointsColor.resize(filesColor.size()); pointsIr.resize(filesSync.size()); pointsColor.resize(filesSync.size()); pointsBoard.resize(filesSync.size(), board); ret = ret && readFiles(filesSync, CALIB_POINTS_COLOR, pointsColor); ret = ret && readFiles(filesSync, CALIB_POINTS_IR, pointsIr); ret = ret && checkSyncPointsOrder(); ret = ret && loadCalibration(); break; } return ret; } void calibrate() { switch(mode) { case COLOR: calibrateIntrinsics(sizeColor, pointsBoard, pointsColor, cameraMatrixColor, distortionColor, rotationColor, projectionColor, rvecsColor, tvecsColor); break; case IR: calibrateIntrinsics(sizeIr, pointsBoard, pointsIr, cameraMatrixIr, distortionIr, rotationIr, projectionIr, rvecsIr, tvecsIr); break; case SYNC: calibrateExtrinsics(); break; } storeCalibration(); }private: bool readFiles(const std::vector
                                                         
                                                           &files, const std::string &ext, std::vector
                                                          
                                                           
                                                             > &points) const { bool ret = true; #pragma omp parallel for for(size_t i = 0; i < files.size(); ++i) { std::string pointsname = path + files[i] + ext; #pragma omp critical OUT_INFO("restoring file: " << files[i] << ext); cv::FileStorage file(pointsname, cv::FileStorage::READ); if(!file.isOpened()) { #pragma omp critical { ret = false; OUT_ERROR("couldn't open file: " << files[i] << ext); } } else { file["points"] >> points[i]; } } return ret; } bool checkSyncPointsOrder() { if(pointsColor.size() != pointsIr.size()) { OUT_ERROR("number of detected color and ir patterns does not match!"); return false; } for(size_t i = 0; i < pointsColor.size(); ++i) { const std::vector
                                                            
                                                              &pColor = pointsColor[i]; const std::vector
                                                             
                                                               &pIr = pointsIr[i]; if(pColor.front().y > pColor.back().y || pColor.front().x > pColor.back().x) { std::reverse(pointsColor[i].begin(), pointsColor[i].end()); } if(pIr.front().y > pIr.back().y || pIr.front().x > pIr.back().x) { std::reverse(pointsIr[i].begin(), pointsIr[i].end()); } } return true; } void calibrateIntrinsics(const cv::Size &size, const std::vector
                                                              
                                                               
                                                                 > &pointsBoard, const std::vector
                                                                
                                                                 
                                                                   > &points, cv::Mat &cameraMatrix, cv::Mat &distortion, cv::Mat &rotation, cv::Mat &projection, std::vector
                                                                  
                                                                    &rvecs, std::vector
                                                                   
                                                                     &tvecs) { if(points.empty()) { OUT_ERROR("no data for calibration provided!"); return; } const cv::TermCriteria termCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON); double error; OUT_INFO("calibrating intrinsics..."); error = cv::calibrateCamera(pointsBoard, points, size, cameraMatrix, distortion, rvecs, tvecs, flags, termCriteria); OUT_INFO("re-projection error: " << error << std::endl); OUT_INFO("Camera Matrix:" << std::endl << cameraMatrix); OUT_INFO("Distortion Coeeficients:" << std::endl << distortion << std::endl); rotation = cv::Mat::eye(3, 3, CV_64F); projection = cv::Mat::eye(4, 4, CV_64F); cameraMatrix.copyTo(projection(cv::Rect(0, 0, 3, 3))); } void calibrateExtrinsics() { if(pointsColor.size() != pointsIr.size()) { OUT_ERROR("number of detected color and ir patterns does not match!"); return; } if(pointsColor.empty() || pointsIr.empty()) { OUT_ERROR("no data for calibration provided!"); return; } const cv::TermCriteria termCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 100, DBL_EPSILON); double error; OUT_INFO("Camera Matrix Color:" << std::endl << cameraMatrixColor); OUT_INFO("Distortion Coeeficients Color:" << std::endl << distortionColor << std::endl); OUT_INFO("Camera Matrix Ir:" << std::endl << cameraMatrixIr); OUT_INFO("Distortion Coeeficients Ir:" << std::endl << distortionIr << std::endl); OUT_INFO("calibrating Color and Ir extrinsics...");#if CV_MAJOR_VERSION == 2 error = cv::stereoCalibrate(pointsBoard, pointsIr, pointsColor, cameraMatrixIr, distortionIr, cameraMatrixColor, distortionColor, sizeColor, rotation, translation, essential, fundamental, termCriteria, cv::CALIB_FIX_INTRINSIC);#elif CV_MAJOR_VERSION == 3 error = cv::stereoCalibrate(pointsBoard, pointsIr, pointsColor, cameraMatrixIr, distortionIr, cameraMatrixColor, distortionColor, sizeColor, rotation, translation, essential, fundamental, cv::CALIB_FIX_INTRINSIC, termCriteria);#endif OUT_INFO("re-projection error: " << error << std::endl); OUT_INFO("Rotation:" << std::endl << rotation); OUT_INFO("Translation:" << std::endl << translation); OUT_INFO("Essential:" << std::endl << essential); OUT_INFO("Fundamental:" << std::endl << fundamental << std::endl); } void storeCalibration() { cv::FileStorage fs; switch(mode) { case SYNC: fs.open(path + K2_CALIB_POSE, cv::FileStorage::WRITE); break; case COLOR: fs.open(path + K2_CALIB_COLOR, cv::FileStorage::WRITE); break; case IR: fs.open(path + K2_CALIB_IR, cv::FileStorage::WRITE); break; } if(!fs.isOpened()) { OUT_ERROR("couldn't store calibration data!"); return; } switch(mode) { case SYNC: fs << K2_CALIB_ROTATION << rotation; fs << K2_CALIB_TRANSLATION << translation; fs << K2_CALIB_ESSENTIAL << essential; fs << K2_CALIB_FUNDAMENTAL << fundamental; break; case COLOR: fs << K2_CALIB_CAMERA_MATRIX << cameraMatrixColor; fs << K2_CALIB_DISTORTION << distortionColor; fs << K2_CALIB_ROTATION << rotationColor; fs << K2_CALIB_PROJECTION << projectionColor; break; case IR: fs << K2_CALIB_CAMERA_MATRIX << cameraMatrixIr; fs << K2_CALIB_DISTORTION << distortionIr; fs << K2_CALIB_ROTATION << rotationIr; fs << K2_CALIB_PROJECTION << projectionIr; break; } fs.release(); } bool loadCalibration() { cv::FileStorage fs; if(fs.open(path + K2_CALIB_COLOR, cv::FileStorage::READ)) { fs[K2_CALIB_CAMERA_MATRIX] >> cameraMatrixColor; fs[K2_CALIB_DISTORTION] >> distortionColor; fs[K2_CALIB_ROTATION] >> rotationColor; fs[K2_CALIB_PROJECTION] >> projectionColor; fs.release(); } else { OUT_ERROR("couldn't load color calibration data!"); return false; } if(fs.open(path + K2_CALIB_IR, cv::FileStorage::READ)) { fs[K2_CALIB_CAMERA_MATRIX] >> cameraMatrixIr; fs[K2_CALIB_DISTORTION] >> distortionIr; fs[K2_CALIB_ROTATION] >> rotationIr; fs[K2_CALIB_PROJECTION] >> projectionIr; fs.release(); } else { OUT_ERROR("couldn't load ir calibration data!"); return false; } return true; }};class DepthCalibration{private: const std::string path; std::vector
                                                                    
                                                                      board; std::vector
                                                                     
                                                                      
                                                                        > points; std::vector
                                                                       
                                                                         images; cv::Size size; cv::Mat cameraMatrix, distortion, rotation, translation; cv::Mat mapX, mapY; double fx, fy, cx, cy; std::ofstream plot;public: DepthCalibration(const std::string &path, const cv::Size &boardDims, const float boardSize) : path(path), size(512, 424) { board.resize(boardDims.width * boardDims.height); for(size_t r = 0, i = 0; r < (size_t)boardDims.height; ++r) { for(size_t c = 0; c < (size_t)boardDims.width; ++c, ++i) { board[i] = cv::Point3f(c * boardSize, r * boardSize, 0); } } } ~DepthCalibration() { } bool restore() { std::vector
                                                                        
                                                                          files; DIR *dp; struct dirent *dirp; size_t pos; if((dp = opendir(path.c_str())) == NULL) { OUT_ERROR("Error opening: " << path); return false; } while((dirp = readdir(dp)) != NULL) { std::string filename = dirp->d_name; if(dirp->d_type != DT_REG) { continue; } /*pos = filename.rfind(CALIB_SYNC); if(pos != std::string::npos) { continue; }*/ pos = filename.rfind(CALIB_FILE_IR_GREY); if(pos != std::string::npos) { std::string frameName = filename.substr(0, pos); files.push_back(frameName); continue; } } closedir(dp); std::sort(files.begin(), files.end()); if(files.empty()) { OUT_ERROR("no files found!"); return false; } bool ret = readFiles(files); ret = ret && loadCalibration(); if(ret) { cv::initUndistortRectifyMap(cameraMatrix, distortion, cv::Mat(), cameraMatrix, size, CV_32FC1, mapX, mapY); fx = cameraMatrix.at
                                                                         
                                                                          (0, 0); fy = cameraMatrix.at
                                                                          
                                                                           (1, 1); cx = cameraMatrix.at
                                                                           
                                                                            (0, 2); cy = cameraMatrix.at
                                                                            
                                                                             (1, 2); } return ret; } void calibrate() { plot.open(path + "plot.dat", std::ios_base::trunc); if(!plot.is_open()) { OUT_ERROR("couldn't open 'plot.dat'!"); return; } if(images.empty()) { OUT_ERROR("no images found!"); return; } plot << "# Columns:" << std::endl << "# 1: X" << std::endl << "# 2: Y" << std::endl << "# 3: computed depth" << std::endl << "# 4: measured depth" << std::endl << "# 5: difference between computed and measured depth" << std::endl; std::vector
                                                                             
                                                                               depthDists, imageDists; for(size_t i = 0; i < images.size(); ++i) { OUT_INFO("frame: " << images[i]); plot << "# frame: " << images[i] << std::endl; cv::Mat depth, planeNormal, region; double planeDistance; cv::Rect roi; depth = cv::imread(images[i], cv::IMREAD_ANYDEPTH); if(depth.empty()) { OUT_ERROR("couldn't load image '" << images[i] << "'!"); return; } cv::remap(depth, depth, mapX, mapY, cv::INTER_NEAREST); computeROI(depth, points[i], region, roi); getPlane(i, planeNormal, planeDistance); computePointDists(planeNormal, planeDistance, region, roi, depthDists, imageDists); } compareDists(imageDists, depthDists); }private: void compareDists(const std::vector
                                                                              
                                                                                &imageDists, const std::vector
                                                                               
                                                                                 &depthDists) const { if(imageDists.size() != depthDists.size()) { OUT_ERROR("number of real and computed distance samples does not match!"); return; } if(imageDists.empty() || depthDists.empty()) { OUT_ERROR("no distance sample data!"); return; } double avg = 0, sqavg = 0, var = 0, stddev = 0; std::vector
                                                                                
                                                                                  diffs(imageDists.size()); for(size_t i = 0; i < imageDists.size(); ++i) { diffs[i] = imageDists[i] - depthDists[i]; avg += diffs[i]; sqavg += diffs[i] * diffs[i]; } sqavg = sqrt(sqavg / imageDists.size()); avg /= imageDists.size(); for(size_t i = 0; i < imageDists.size(); ++i) { const double diff = diffs[i] - avg; var += diff * diff; } var = var / (imageDists.size()); stddev = sqrt(var); std::sort(diffs.begin(), diffs.end()); OUT_INFO("stats on difference:" << std::endl << " avg: " << avg << std::endl << " var: " << var << std::endl << " stddev: " << stddev << std::endl << " rms: " << sqavg << std::endl << " median: " << diffs[diffs.size() / 2]); storeCalibration(avg * 1000.0); } void computePointDists(const cv::Mat &normal, const double distance, const cv::Mat &region, const cv::Rect &roi, std::vector
                                                                                 
                                                                                   &depthDists, std::vector
                                                                                  
                                                                                    &imageDists) { for(int r = 0; r < region.rows; ++r) { const uint16_t *itD = region.ptr
                                                                                   
                                                                                    (r); cv::Point p(roi.x, roi.y + r); for(int c = 0; c < region.cols; ++c, ++itD, ++p.x) { const double dDist = *itD / 1000.0; if(dDist < 0.1) { continue; } const double iDist = computeDistance(p, normal, distance); const double diff = iDist - dDist; if(std::abs(diff) > 0.08) { continue; } depthDists.push_back(dDist); imageDists.push_back(iDist); plot << p.x << ' ' << p.y << ' ' << iDist << ' ' << dDist << ' ' << diff << std::endl; } } } double computeDistance(const cv::Point &pointImage, const cv::Mat &normal, const double distance) const { cv::Mat point = cv::Mat(3, 1, CV_64F); point.at
                                                                                    
                                                                                     (0) = (pointImage.x - cx) / fx; point.at
                                                                                     
                                                                                      (1) = (pointImage.y - cy) / fy; point.at
                                                                                      
                                                                                       (2) = 1; double t = distance / normal.dot(point); point = point * t; return point.at
                                                                                       
                                                                                        (2); } void getPlane(const size_t index, cv::Mat &normal, double &distance) const { cv::Mat rvec, rotation, translation; //cv::solvePnP(board, points[index], cameraMatrix, distortion, rvec, translation, false, cv::EPNP);#if CV_MAJOR_VERSION == 2 cv::solvePnPRansac(board, points[index], cameraMatrix, distortion, rvec, translation, false, 300, 0.05, board.size(), cv::noArray(), cv::ITERATIVE);#elif CV_MAJOR_VERSION == 3 cv::solvePnPRansac(board, points[index], cameraMatrix, distortion, rvec, translation, false, 300, 0.05, 0.99, cv::noArray(), cv::SOLVEPNP_ITERATIVE);#endif cv::Rodrigues(rvec, rotation); normal = cv::Mat(3, 1, CV_64F); normal.at
                                                                                        
                                                                                         (0) = 0; normal.at
                                                                                         
                                                                                          (1) = 0; normal.at
                                                                                          
                                                                                           (2) = 1; normal = rotation * normal; distance = normal.dot(translation); } void computeROI(const cv::Mat &depth, const std::vector
                                                                                           
                                                                                             &points, cv::Mat &region, cv::Rect &roi) const { std::vector
                                                                                            
                                                                                              norm; std::vector
                                                                                             
                                                                                               undist, hull; cv::undistortPoints(points, norm, cameraMatrix, distortion); undist.reserve(norm.size()); for(size_t i = 0; i < norm.size(); ++i) { cv::Point p; p.x = (int)round(norm[i].x * fx + cx); p.y = (int)round(norm[i].y * fy + cy); if(p.x >= 0 && p.x < depth.cols && p.y >= 0 && p.y < depth.rows) { undist.push_back(p); } } roi = cv::boundingRect(undist); cv::Mat mask = cv::Mat::zeros(depth.rows, depth.cols, CV_8U); cv::convexHull(undist, hull); cv::fillConvexPoly(mask, hull, CV_RGB(255, 255, 255)); cv::Mat tmp; depth.copyTo(tmp, mask); tmp(roi).copyTo(region); } bool readFiles(const std::vector
                                                                                              
                                                                                                &files) { points.resize(files.size()); images.resize(files.size()); bool ret = true; #pragma omp parallel for for(size_t i = 0; i < files.size(); ++i) { std::string pointsname = path + files[i] + CALIB_POINTS_IR; #pragma omp critical OUT_INFO("restoring file: " << files[i]); cv::FileStorage file(pointsname, cv::FileStorage::READ); if(!file.isOpened()) { #pragma omp critical { OUT_ERROR("couldn't read '" << pointsname << "'!"); ret = false; } } else { file["points"] >> points[i]; file.release(); images[i] = path + files[i] + CALIB_FILE_DEPTH; } } return ret; } bool loadCalibration() { cv::FileStorage fs; if(fs.open(path + K2_CALIB_IR, cv::FileStorage::READ)) { fs[K2_CALIB_CAMERA_MATRIX] >> cameraMatrix; fs[K2_CALIB_DISTORTION] >> distortion; fs.release(); } else { OUT_ERROR("couldn't read calibration '" << path + K2_CALIB_IR << "'!"); return false; } return true; } void storeCalibration(const double depthShift) const { cv::FileStorage fs; if(fs.open(path + K2_CALIB_DEPTH, cv::FileStorage::WRITE)) { fs << K2_CALIB_DEPTH_SHIFT << depthShift; fs.release(); } else { OUT_ERROR("couldn't store depth calibration!"); } }};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 " 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 " 'circle
                                                                                               
                                                                                                x
                                                                                                
                                                                                                 x
                                                                                                 
                                                                                                  ' " NO_COLOR "for symmetric circle grid" << std::endl << FG_YELLOW " 'acircle
                                                                                                  
                                                                                                   x
                                                                                                   
                                                                                                    x
                                                                                                    
                                                                                                     ' " NO_COLOR "for asymmetric circle grid" << std::endl << FG_YELLOW " 'chess
                                                                                                     
                                                                                                      x
                                                                                                      
                                                                                                       x
                                                                                                       
                                                                                                        ' " NO_COLOR "for chessboard pattern" << std::endl << FG_GREEN " distortion model" NO_COLOR ": " FG_YELLOW "'rational'" NO_COLOR " for using model with 8 instead of 5 coefficients" << std::endl << FG_GREEN " output path" NO_COLOR ": " FG_YELLOW "'-path 
                                                                                                        
                                                                                                         '" NO_COLOR << std::endl;}int main(int argc, char **argv){#if EXTENDED_OUTPUT ROSCONSOLE_AUTOINIT; if(!getenv("ROSCONSOLE_FORMAT")) { ros::console::g_formatter.tokens_.clear(); ros::console::g_formatter.init("[${severity}] ${message}"); }#endif Mode mode = RECORD; Source source = SYNC; bool circleBoard = false; bool symmetric = true; bool rational = false; bool calibDepth = false; cv::Size boardDims = cv::Size(7, 6); float boardSize = 0.108; std::string ns = K2_DEFAULT_NS; std::string path = "./"; ros::init(argc, argv, "kinect2_calib", ros::init_options::AnonymousName); if(!ros::ok()) { return 0; } for(int argI = 1; argI < argc; ++ argI) { std::string arg(argv[argI]); if(arg == "--help" || arg == "--h" || arg == "-h" || arg == "-?" || arg == "--?") { help(argv[0]); ros::shutdown(); return 0; } else if(arg == "record") { mode = RECORD; } else if(arg == "calibrate") { mode = CALIBRATE; } else if(arg == "color") { source = COLOR; } else if(arg == "ir") { source = IR; } else if(arg == "sync") { source = SYNC; } else if(arg == "depth") { calibDepth = true; } else if(arg == "rational") { rational = true; } else if(arg.find("circle") == 0 && arg.find('x') != arg.rfind('x') && arg.rfind('x') != std::string::npos) { circleBoard = true; const size_t start = 6; const size_t leftX = arg.find('x'); const size_t rightX = arg.rfind('x'); const size_t end = arg.size(); int width = atoi(arg.substr(start, leftX - start).c_str()); int height = atoi(arg.substr(leftX + 1, rightX - leftX + 1).c_str()); boardSize = atof(arg.substr(rightX + 1, end - rightX + 1).c_str()); boardDims = cv::Size(width, height); } else if((arg.find("circle") == 0 || arg.find("acircle") == 0) && arg.find('x') != arg.rfind('x') && arg.rfind('x') != std::string::npos) { symmetric = arg.find("circle") == 0; circleBoard = true; const size_t start = 6 + (symmetric ? 0 : 1); const size_t leftX = arg.find('x'); const size_t rightX = arg.rfind('x'); const size_t end = arg.size(); int width = atoi(arg.substr(start, leftX - start).c_str()); int height = atoi(arg.substr(leftX + 1, rightX - leftX + 1).c_str()); boardSize = atof(arg.substr(rightX + 1, end - rightX + 1).c_str()); boardDims = cv::Size(width, height); } else if(arg.find("chess") == 0 && arg.find('x') != arg.rfind('x') && arg.rfind('x') != std::string::npos) { circleBoard = false; const size_t start = 5; const size_t leftX = arg.find('x'); const size_t rightX = arg.rfind('x'); const size_t end = arg.size(); int width = atoi(arg.substr(start, leftX - start).c_str()); int height = atoi(arg.substr(leftX + 1, rightX - leftX + 1).c_str()); boardSize = atof(arg.substr(rightX + 1, end - rightX + 1).c_str()); boardDims = cv::Size(width, height); } else if(arg == "-path" && ++argI < argc) { arg = argv[argI]; struct stat fileStat; if(stat(arg.c_str(), &fileStat) == 0 && S_ISDIR(fileStat.st_mode)) { path = arg; } else { OUT_ERROR("Unknown path: " << arg); help(argv[0]); ros::shutdown(); return 0; } } else { ns = arg; } } std::string topicColor = "/" + ns + K2_TOPIC_QHD + K2_TOPIC_IMAGE_MONO; std::string topicIr = "/" + ns + K2_TOPIC_SD + K2_TOPIC_IMAGE_IR; std::string topicDepth = "/" + ns + K2_TOPIC_SD + K2_TOPIC_IMAGE_DEPTH; OUT_INFO("Start settings:" << std::endl << " Mode: " FG_CYAN << (mode == RECORD ? "record" : "calibrate") << NO_COLOR << std::endl << " Source: " FG_CYAN << (calibDepth ? "depth" : (source == COLOR ? "color" : (source == IR ? "ir" : "sync"))) << NO_COLOR << std::endl << " Board: " FG_CYAN << (circleBoard ? "circles" : "chess") << NO_COLOR << std::endl << " Dimensions: " FG_CYAN << boardDims.width << " x " << boardDims.height << NO_COLOR << std::endl << " Field size: " FG_CYAN << boardSize << NO_COLOR << std::endl << "Dist. model: " FG_CYAN << (rational ? '8' : '5') << " coefficients" << NO_COLOR << std::endl << "Topic color: " FG_CYAN << topicColor << NO_COLOR << std::endl << " Topic ir: " FG_CYAN << topicIr << NO_COLOR << std::endl << "Topic depth: " FG_CYAN << topicDepth << NO_COLOR << std::endl << " Path: " FG_CYAN << path << NO_COLOR << std::endl); if(!ros::master::check()) { OUT_ERROR("checking ros master failed."); return -1; } if(mode == RECORD) { Recorder recorder(path, topicColor, topicIr, topicDepth, source, circleBoard, symmetric, boardDims, boardSize); OUT_INFO("starting recorder..."); recorder.run(); OUT_INFO("stopped recording..."); } else if(calibDepth) { DepthCalibration calib(path, boardDims, boardSize); OUT_INFO("restoring files..."); calib.restore(); OUT_INFO("starting calibration..."); calib.calibrate(); } else { CameraCalibration calib(path, source, circleBoard, boardDims, boardSize, rational); OUT_INFO("restoring files..."); calib.restore(); OUT_INFO("starting calibration..."); calib.calibrate(); } return 0;}