diff --git a/src/calibrationtool/extrinsicscalibrator.cpp b/src/calibrationtool/extrinsicscalibrator.cpp index dfb842bf..448c4cb3 100755 --- a/src/calibrationtool/extrinsicscalibrator.cpp +++ b/src/calibrationtool/extrinsicscalibrator.cpp @@ -41,16 +41,27 @@ ExtrinsicsCalibrator::ExtrinsicsCalibrator(CalibrationConfig *calibrationConfig, m_parametersSavePath = (m_calibrationConfig->calibrationSetPath + "/" + m_calibrationConfig->calibrationSetName).toStdString(); - m_charucoPattern = cv::Mat(cv::Size( m_calibrationConfig->patternWidth+1, + m_charucoPattern1 = cv::Mat(cv::Size( m_calibrationConfig->patternWidth+1, + m_calibrationConfig->patternHeight+1), CV_32SC1); + m_charucoPattern2 = cv::Mat(cv::Size( m_calibrationConfig->patternWidth+1, m_calibrationConfig->patternHeight+1), CV_32SC1); m_detectedPattern = cv::Mat(cv::Size( m_calibrationConfig->patternWidth+1, m_calibrationConfig->patternHeight+1), CV_32SC1); - m_charucoPattern = -1; + m_charucoPattern1 = -1; + m_charucoPattern2 = -1; int id_count = 0; for (int i = 0; i < m_calibrationConfig->patternWidth+1; i++) { for (int j = 0; j < m_calibrationConfig->patternHeight+1; j++) { if ((i+j)%2 != 0) { - m_charucoPattern.at(j,i) = id_count; + m_charucoPattern1.at(j,i) = id_count; + id_count++; + } + } + } + for (int i = 0; i < m_calibrationConfig->patternWidth+1; i++) { + for (int j = 0; j < m_calibrationConfig->patternHeight+1; j++) { + if ((i+j)%2 != 0) { + m_charucoPattern2.at(j,i) = id_count; id_count++; } } @@ -315,6 +326,12 @@ bool ExtrinsicsCalibrator::calibrateExtrinsicsPair(QList cameraPair, } +int reverse_column_index(int num_cols, int index) { + int row = index / num_cols; + int col = index % num_cols; + return row * num_cols + (num_cols - 1 - col); +} + bool ExtrinsicsCalibrator::calibrateExtrinsicsPairCharuco(QList cameraPair, Extrinsics &e, double &mean_repro_error) { @@ -330,11 +347,12 @@ bool ExtrinsicsCalibrator::calibrateExtrinsicsPairCharuco(QList cameraP dictionary = cv::aruco::getPredefinedDictionary(m_calibrationConfig->charucoPatternIdx); } - cv::Ptr board = - cv::aruco::CharucoBoard::create(m_calibrationConfig->patternWidth, - m_calibrationConfig->patternHeight, 0.04f, 0.02f, dictionary); - cv::Ptr charucoParams = - cv::aruco::DetectorParameters::create(); + cv::Ptr boardFront = cv::aruco::CharucoBoard::create(m_calibrationConfig->patternWidth, m_calibrationConfig->patternHeight, 0.04f, 0.02f, dictionary); + cv::Ptr boardBack = cv::aruco::CharucoBoard::create(m_calibrationConfig->patternWidth, m_calibrationConfig->patternHeight, 0.04f, 0.02f, dictionary); + for(int i=0; iids.size(); i++) { + boardBack->ids[i] += boardFront->ids.size(); + } + cv::Ptr charucoParams = cv::aruco::DetectorParameters::create(); std::vector checkerBoardPoints; for (int i = 0; i < m_calibrationConfig->patternHeight-1; i++) @@ -371,6 +389,9 @@ bool ExtrinsicsCalibrator::calibrateExtrinsicsPairCharuco(QList cameraP std::vector> objectPointsAll, objectPoints; std::vector> imagePointsAll1, imagePointsAll2, imagePoints1, imagePoints2; + + cv::Ptr detected_board1; + cv::Ptr detected_board2; cv::Size size; int iteration = 0; int skipIndex; @@ -426,21 +447,41 @@ bool ExtrinsicsCalibrator::calibrateExtrinsicsPairCharuco(QList cameraP std::vector> markerCorners1, markerCorners2; std::vector charucoCorners1, charucoCorners2; std::vector charucoIds1,charucoIds2; - cv::aruco::detectMarkers(img1, board->dictionary, markerCorners1, markerIds1, charucoParams); + cv::aruco::detectMarkers(img1, boardFront->dictionary, markerCorners1, markerIds1, charucoParams); + if (markerIds1.size() > 0 && markerIds1[0]ids.size()) { + detected_board1 = boardFront; + } else { + detected_board1 = boardBack; + } if (markerIds1.size() > 5) { - cv::aruco::interpolateCornersCharuco(markerCorners1, markerIds1, img1, board, charucoCorners1, charucoIds1); + cv::aruco::interpolateCornersCharuco(markerCorners1, markerIds1, img1, detected_board1, charucoCorners1, charucoIds1); if (charucoIds1.size() > m_calibrationConfig->patternHeight - 1 && charucoIds1.size() > m_calibrationConfig->patternWidth - 1) { cv::Scalar color = cv::Scalar(255, 0, 0); patternFound1 = true; + if (detected_board1->ids[0]>0) { + for(int i=0; ipatternWidth)-1, charucoIds1[i]); + } + } } } - cv::aruco::detectMarkers(img2, board->dictionary, markerCorners2, markerIds2, charucoParams); + cv::aruco::detectMarkers(img2, boardFront->dictionary, markerCorners2, markerIds2, charucoParams); + if (markerIds2.size() > 0 && markerIds2[0]ids.size()) { + detected_board2 = boardFront; + } else { + detected_board2 = boardBack; + } if (markerIds2.size() > 5) { - cv::aruco::interpolateCornersCharuco(markerCorners2, markerIds2, img2, board, charucoCorners2, charucoIds2); + cv::aruco::interpolateCornersCharuco(markerCorners2, markerIds2, img2, detected_board2, charucoCorners2, charucoIds2); if (charucoIds2.size() > m_calibrationConfig->patternHeight - 1 && charucoIds2.size() > m_calibrationConfig->patternWidth - 1) { patternFound2 = true; + if (detected_board2->ids[0]>0) { + for(int i=0; ipatternWidth)-1, charucoIds2[i]); + } + } } } if (patternFound1 && patternFound2) { @@ -666,21 +707,41 @@ bool ExtrinsicsCalibrator::checkRotation(std::vector< cv::Point2f> &corners1, int ExtrinsicsCalibrator::matchPattern() { int unrotCount = 0; int rotCount = 0; + int nMatched = 0; for (int i = 0; i < m_calibrationConfig->patternWidth+1; i++) { for (int j = 0; j < m_calibrationConfig->patternHeight+1; j++) { - if (m_charucoPattern.at(j,i) != -1) { - if (m_charucoPattern.at(j,i) == m_detectedPattern.at(j,i) || - m_charucoPattern.at(j,i) == m_detectedPattern.at(j,i)) { + if (m_charucoPattern1.at(j,i) != -1) { + if (m_charucoPattern1.at(j,i) == m_detectedPattern.at(j,i)) { unrotCount++; + nMatched++; } - if ((m_charucoPattern.at(j,i) == m_detectedPattern.at( + if ((m_charucoPattern1.at(j,i) == m_detectedPattern.at( m_calibrationConfig->patternHeight-j, m_calibrationConfig->patternWidth-i))) { rotCount++; + nMatched++; } } } } + if (nMatched == 0) { + for (int i = 0; i < m_calibrationConfig->patternWidth+1; i++) { + for (int j = 0; j < m_calibrationConfig->patternHeight+1; j++) { + if (m_charucoPattern2.at(j,i) != -1) { + if (m_charucoPattern2.at(j,i) == m_detectedPattern.at(j,i)) { + unrotCount++; + nMatched++; + } + if ((m_charucoPattern2.at(j,i) == m_detectedPattern.at( + m_calibrationConfig->patternHeight-j, + m_calibrationConfig->patternWidth-i))) { + rotCount++; + nMatched++; + } + } + } + } + } if (unrotCount == rotCount) { return 0; } diff --git a/src/calibrationtool/extrinsicscalibrator.hpp b/src/calibrationtool/extrinsicscalibrator.hpp index fc192dfc..3384c063 100755 --- a/src/calibrationtool/extrinsicscalibrator.hpp +++ b/src/calibrationtool/extrinsicscalibrator.hpp @@ -59,7 +59,8 @@ class ExtrinsicsCalibrator : public QObject, public QRunnable { cv::Mat F; }; - cv::Mat m_charucoPattern; + cv::Mat m_charucoPattern1; + cv::Mat m_charucoPattern2; cv::Mat m_detectedPattern; std::vector m_checkerBoardPoints; diff --git a/src/calibrationtool/intrinsicscalibrator.cpp b/src/calibrationtool/intrinsicscalibrator.cpp index 0c86e8f8..2da9863d 100755 --- a/src/calibrationtool/intrinsicscalibrator.cpp +++ b/src/calibrationtool/intrinsicscalibrator.cpp @@ -33,16 +33,27 @@ IntrinsicsCalibrator::IntrinsicsCalibrator(CalibrationConfig *calibrationConfig, m_parametersSavePath = (m_calibrationConfig->calibrationSetPath + "/" + m_calibrationConfig->calibrationSetName).toStdString(); - m_charucoPattern = cv::Mat(cv::Size( m_calibrationConfig->patternWidth+1, + m_charucoPattern1 = cv::Mat(cv::Size( m_calibrationConfig->patternWidth+1, + m_calibrationConfig->patternHeight+1), CV_32SC1); + m_charucoPattern2 = cv::Mat(cv::Size( m_calibrationConfig->patternWidth+1, m_calibrationConfig->patternHeight+1), CV_32SC1); m_detectedPattern = cv::Mat(cv::Size( m_calibrationConfig->patternWidth+1, m_calibrationConfig->patternHeight+1), CV_32SC1); - m_charucoPattern = -1; + m_charucoPattern1 = -1; + m_charucoPattern2 = -1; int id_count = 0; for (int i = 0; i < m_calibrationConfig->patternWidth+1; i++) { for (int j = 0; j < m_calibrationConfig->patternHeight+1; j++) { if ((i+j)%2 != 0) { - m_charucoPattern.at(j,i) = id_count; + m_charucoPattern1.at(j,i) = id_count; + id_count++; + } + } + } + for (int i = 0; i < m_calibrationConfig->patternWidth+1; i++) { + for (int j = 0; j < m_calibrationConfig->patternHeight+1; j++) { + if ((i+j)%2 != 0) { + m_charucoPattern2.at(j,i) = id_count; id_count++; } } @@ -244,25 +255,35 @@ void IntrinsicsCalibrator::run_charuco() { dictionary = cv::aruco::getPredefinedDictionary(m_calibrationConfig->charucoPatternIdx); } - cv::Ptr board = - cv::aruco::CharucoBoard::create(m_calibrationConfig->patternWidth, - m_calibrationConfig->patternHeight, m_calibrationConfig->patternSideLength, - m_calibrationConfig->markerSideLength, dictionary); - cv::Ptr charucoParams = - cv::aruco::DetectorParameters::create(); + cv::Ptr boardFront = cv::aruco::CharucoBoard::create(m_calibrationConfig->patternWidth, + m_calibrationConfig->patternHeight, + m_calibrationConfig->patternSideLength, + m_calibrationConfig->markerSideLength, dictionary); + + cv::Ptr boardBack = cv::aruco::CharucoBoard::create(m_calibrationConfig->patternWidth, + m_calibrationConfig->patternHeight, + m_calibrationConfig->patternSideLength, + m_calibrationConfig->markerSideLength, dictionary); + + for(int i=0; iids.size(); i++) { + boardBack->ids[i] += boardFront->ids.size(); + } + cv::Ptr charucoParams = cv::aruco::DetectorParameters::create(); cv::Mat imageCopy; if (m_calibrationConfig->debug) { cv::Mat boardImage; - board->draw(cv::Size(600, 500), boardImage, 10, 1); - cv::imwrite(m_parametersSavePath + "/debug/BoardPreview.jpg", boardImage); + boardFront->draw(cv::Size(600, 500), boardImage, 10, 1); + cv::imwrite(m_parametersSavePath + "/debug/BoardPreviewFront.jpg", boardImage); + boardBack->draw(cv::Size(600, 500), boardImage, 10, 1); + cv::imwrite(m_parametersSavePath + "/debug/BoardPreviewBack.jpg", boardImage); } std::vector> charucoIdsAll, charucoIds; std::vector> charucoCornersAll, charucoCorners; + cv::Ptr detected_board; - - while (charucoIdsAll.size() < m_calibrationConfig->framesForIntrinsics) { + while (charucoIdsAll.size() < m_calibrationConfig->framesForIntrinsics) { cv::VideoCapture cap(m_calibrationConfig->intrinsicsPath.toStdString() + "/" + m_cameraName + "." + format.toStdString()); int frameCount = cap.get(cv::CAP_PROP_FRAME_COUNT); @@ -299,14 +320,21 @@ void IntrinsicsCalibrator::run_charuco() { size = img.size(); int frameIndex = cap.get(cv::CAP_PROP_POS_FRAMES); cap.set(cv::CAP_PROP_POS_FRAMES, frameIndex+skipIndex); - if (frameIndex > frameCount) read_success = false; - std::vector markerIds; - std::vector> markerCorners; - cv::aruco::detectMarkers(img, board->dictionary, markerCorners, markerIds, charucoParams); - if (markerIds.size() > 5) { + if (frameIndex > frameCount) { + read_success = false; + } + std::vector markerIds; + std::vector> markerCorners; + cv::aruco::detectMarkers(img, boardFront->dictionary, markerCorners, markerIds, charucoParams); + if (markerIds.size() > 0 && markerIds[0]<35) { + detected_board = boardFront; + } else { + detected_board = boardBack; + } + if (markerIds.size() > 5) { std::vector charucoCorners; std::vector charucoIds; - cv::aruco::interpolateCornersCharuco(markerCorners, markerIds, img, board, charucoCorners, charucoIds); + cv::aruco::interpolateCornersCharuco(markerCorners, markerIds, img, detected_board, charucoCorners, charucoIds); if (charucoIds.size() > m_calibrationConfig->patternHeight-1 && charucoIds.size() > m_calibrationConfig->patternWidth-1) { charucoCornersAll.push_back(charucoCorners); @@ -356,14 +384,14 @@ void IntrinsicsCalibrator::run_charuco() { charucoCorners.size() << std::endl; double mean_repro_error = intrinsicsCalibrationStepCharuco(charucoCorners, charucoIds, - board, size, 1.25); + detected_board, size, 1.25); std::cout << m_cameraName << ": Mean Reprojection Error after Stage 1: " << mean_repro_error << std::endl; std::cout << "Number Images for Stage 2: " << charucoCorners.size() << std::endl; mean_repro_error = intrinsicsCalibrationStepCharuco(charucoCorners, charucoIds, - board, size, 1.5); + detected_board, size, 1.5); std::cout << m_cameraName << ": Mean Reprojection Error after Stage 2: " << mean_repro_error << std::endl; std::cout << m_cameraName << ": Number Images for Stage 3: " @@ -373,7 +401,7 @@ void IntrinsicsCalibrator::run_charuco() { std::vector< cv::Mat > rvecs, tvecs; cv::Mat stdDI, stdDE, errs; double repro_error = cv::aruco::calibrateCameraCharuco(charucoCorners, - charucoIds, board, size, K, + charucoIds, detected_board, size, K, D, rvecs, tvecs, stdDI, stdDE, errs, cv::CALIB_FIX_K3 | cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_SAME_FOCAL_LENGTH, cv::TermCriteria(cv::TermCriteria::MAX_ITER | @@ -499,14 +527,32 @@ bool IntrinsicsCalibrator::checkRotation(std::vector< cv::Point2f> &corners1, int IntrinsicsCalibrator::matchPattern() { int unrotCount = 0; int rotCount = 0; + int nMatched = 0; + for (int i = 0; i < m_calibrationConfig->patternWidth+1; i++) { + for (int j = 0; j < m_calibrationConfig->patternHeight+1; j++) { + if (m_charucoPattern1.at(j,i) != -1) { + if (m_charucoPattern1.at(j,i) == m_detectedPattern.at(j,i)) { + unrotCount++; + nMatched++; + } + if ((m_charucoPattern1.at(j,i) == + m_detectedPattern.at(m_calibrationConfig->patternHeight-j, + m_calibrationConfig->patternWidth-i))) { + rotCount++; + nMatched++; + + } + } + } + } + if (nMatched==0) { for (int i = 0; i < m_calibrationConfig->patternWidth+1; i++) { for (int j = 0; j < m_calibrationConfig->patternHeight+1; j++) { - if (m_charucoPattern.at(j,i) != -1) { - if (m_charucoPattern.at(j,i) == m_detectedPattern.at(j,i) || - m_charucoPattern.at(j,i) == m_detectedPattern.at(j,i)) { + if (m_charucoPattern2.at(j,i) != -1) { + if (m_charucoPattern2.at(j,i) == m_detectedPattern.at(j,i)) { unrotCount++; } - if ((m_charucoPattern.at(j,i) == + if ((m_charucoPattern2.at(j,i) == m_detectedPattern.at(m_calibrationConfig->patternHeight-j, m_calibrationConfig->patternWidth-i))) { rotCount++; @@ -514,6 +560,7 @@ int IntrinsicsCalibrator::matchPattern() { } } } + } if (unrotCount == rotCount) { return 0; } diff --git a/src/calibrationtool/intrinsicscalibrator.hpp b/src/calibrationtool/intrinsicscalibrator.hpp index a06bd0ad..bbf7f39b 100755 --- a/src/calibrationtool/intrinsicscalibrator.hpp +++ b/src/calibrationtool/intrinsicscalibrator.hpp @@ -53,7 +53,8 @@ class IntrinsicsCalibrator : public QObject, public QRunnable { cv::Mat D; }; - cv::Mat m_charucoPattern; + cv::Mat m_charucoPattern1; + cv::Mat m_charucoPattern2; cv::Mat m_detectedPattern; std::vector m_checkerBoardPoints;