Download modules/objdetect/src/aruco/charuco_detector.cpp from tgollogly/test: direct link, hf CLI and curl.
- Browser
- Download file 31.3 kB
-
https://huggingface.co/tgollogly/test/resolve/main/modules/objdetect/src/aruco/charuco_detector.cpp
- Command line
-
hf download hf://tgollogly/test/modules/objdetect/src/aruco/charuco_detector.cpp
-
curl -L -o charuco_detector.cpp https://huggingface.co/tgollogly/test/resolve/main/modules/objdetect/src/aruco/charuco_detector.cpp
31.3 kB
| // This file is part of OpenCV project. | |
| // It is subject to the license terms in the LICENSE file found in the top-level directory | |
| // of this distribution and at http://opencv.org/license.html | |
| namespace cv { | |
| namespace aruco { | |
| using namespace std; | |
| struct CharucoDetector::CharucoDetectorImpl { | |
| CharucoBoard board; | |
| CharucoParameters charucoParameters; | |
| ArucoDetector arucoDetector; | |
| CharucoDetectorImpl(const CharucoBoard& _board, const CharucoParameters _charucoParameters, | |
| const ArucoDetector& _arucoDetector): board(_board), charucoParameters(_charucoParameters), | |
| arucoDetector(_arucoDetector) | |
| {} | |
| bool checkBoard(InputArrayOfArrays markerCorners, InputArray markerIds, InputArray charucoCorners, InputArray charucoIds) { | |
| vector<Mat> mCorners; | |
| markerCorners.getMatVector(mCorners); | |
| const Mat mIds = markerIds.getMat(); | |
| const Mat chCorners = charucoCorners.getMat(); | |
| const Mat chIds = charucoIds.getMat(); | |
| const vector<int>& boardIds = board.getIds(); | |
| const vector<vector<int> > nearestMarkerIdx = board.getNearestMarkerIdx(); | |
| vector<Point2f> distance(board.getNearestMarkerIdx().size(), Point2f(0.f, std::numeric_limits<float>::max())); | |
| // distance[i].x: max distance from the i-th charuco corner to charuco corner-forming markers. | |
| // The two charuco corner-forming markers of i-th charuco corner are defined in getNearestMarkerIdx()[i] | |
| // distance[i].y: min distance from the charuco corner to other markers. | |
| for (size_t i = 0ull; i < chIds.total(); i++) { | |
| int chId = chIds.ptr<int>(0)[i]; | |
| Point2f charucoCorner(chCorners.ptr<Point2f>(0)[i]); | |
| for (size_t j = 0ull; j < mIds.total(); j++) { | |
| int idMaker = mIds.ptr<int>(0)[j]; | |
| // skip the check if the marker is not in the current board. | |
| if (find(boardIds.begin(), boardIds.end(), idMaker) == boardIds.end()) | |
| continue; | |
| Point2f centerMarker((mCorners[j].ptr<Point2f>(0)[0] + mCorners[j].ptr<Point2f>(0)[1] + | |
| mCorners[j].ptr<Point2f>(0)[2] + mCorners[j].ptr<Point2f>(0)[3]) / 4.f); | |
| float dist = sqrt(normL2Sqr<float>(centerMarker - charucoCorner)); | |
| // nearestMarkerIdx contains for each charuco corner, nearest marker index in ids array | |
| const int nearestMarkerId1 = boardIds[nearestMarkerIdx[chId][0]]; | |
| const int nearestMarkerId2 = boardIds[nearestMarkerIdx[chId][1]]; | |
| if (nearestMarkerId1 == idMaker || nearestMarkerId2 == idMaker) { | |
| int nearestCornerId = nearestMarkerId1 == idMaker ? board.getNearestMarkerCorners()[chId][0] : board.getNearestMarkerCorners()[chId][1]; | |
| Point2f nearestCorner = mCorners[j].ptr<Point2f>(0)[nearestCornerId]; | |
| // distToNearest: distance from the charuco corner to charuco corner-forming markers | |
| float distToNearest = sqrt(normL2Sqr<float>(nearestCorner - charucoCorner)); | |
| distance[chId].x = max(distance[chId].x, distToNearest); | |
| // check that nearestCorner is nearest point | |
| { | |
| Point2f mid1 = (mCorners[j].ptr<Point2f>(0)[(nearestCornerId + 1) % 4]+nearestCorner)*0.5f; | |
| Point2f mid2 = (mCorners[j].ptr<Point2f>(0)[(nearestCornerId + 3) % 4]+nearestCorner)*0.5f; | |
| float tmpDist = min(sqrt(normL2Sqr<float>(mid1 - charucoCorner)), sqrt(normL2Sqr<float>(mid2 - charucoCorner))); | |
| if (tmpDist < distToNearest) | |
| return false; | |
| } | |
| } | |
| // check distance from the charuco corner to other markers | |
| else | |
| distance[chId].y = min(distance[chId].y, dist); | |
| } | |
| // if distance from the charuco corner to charuco corner-forming markers more then distance from the charuco corner to other markers, | |
| // then a false board is found. | |
| if (distance[chId].x > 0.f && distance[chId].y < std::numeric_limits<float>::max() && distance[chId].x > distance[chId].y) | |
| return false; | |
| } | |
| return true; | |
| } | |
| /** Calculate the maximum window sizes for corner refinement for each charuco corner based on the distance | |
| * to their closest markers */ | |
| vector<Size> getMaximumSubPixWindowSizes(InputArrayOfArrays markerCorners, InputArray markerIds, | |
| InputArray charucoCorners) { | |
| size_t nCharucoCorners = charucoCorners.getMat().total(); | |
| CV_Assert(board.getNearestMarkerIdx().size() == nCharucoCorners); | |
| vector<Size> winSizes(nCharucoCorners, Size(-1, -1)); | |
| for(size_t i = 0ull; i < nCharucoCorners; i++) { | |
| if(charucoCorners.getMat().at<Point2f>((int)i) == Point2f(-1.f, -1.f)) continue; | |
| if(board.getNearestMarkerIdx()[i].empty()) continue; | |
| double minDist = -1; | |
| int counter = 0; | |
| // calculate the distance to each of the closest corner of each closest marker | |
| for(size_t j = 0; j < board.getNearestMarkerIdx()[i].size(); j++) { | |
| // find marker | |
| int markerId = board.getIds()[board.getNearestMarkerIdx()[i][j]]; | |
| int markerIdx = -1; | |
| for(size_t k = 0; k < markerIds.getMat().total(); k++) { | |
| if(markerIds.getMat().at<int>((int)k) == markerId) { | |
| markerIdx = (int)k; | |
| break; | |
| } | |
| } | |
| if(markerIdx == -1) continue; | |
| Point2f markerCorner = | |
| markerCorners.getMat(markerIdx).at<Point2f>(board.getNearestMarkerCorners()[i][j]); | |
| Point2f charucoCorner = charucoCorners.getMat().at<Point2f>((int)i); | |
| double dist = norm(markerCorner - charucoCorner); | |
| if(minDist == -1) minDist = dist; // if first distance, just assign it | |
| minDist = min(dist, minDist); | |
| counter++; | |
| } | |
| // if this is the first closest marker, dont do anything | |
| if(counter == 0) | |
| continue; | |
| else { | |
| // else, calculate the maximum window size | |
| int winSizeInt = int(minDist - 2); // remove 2 pixels for safety | |
| if(winSizeInt < 1) winSizeInt = 1; // minimum size is 1 | |
| if(winSizeInt > 10) winSizeInt = 10; // maximum size is 10 | |
| winSizes[i] = Size(winSizeInt, winSizeInt); | |
| } | |
| } | |
| return winSizes; | |
| } | |
| /** @brief From all projected chessboard corners, select those inside the image and apply subpixel refinement */ | |
| void selectAndRefineChessboardCorners(InputArray allCorners, InputArray image, OutputArray selectedCorners, | |
| OutputArray selectedIds, const vector<Size> &winSizes) { | |
| const int minDistToBorder = 2; // minimum distance of the corner to the image border | |
| // remaining corners, ids and window refinement sizes after removing corners outside the image | |
| vector<Point2f> filteredChessboardImgPoints; | |
| vector<Size> filteredWinSizes; | |
| vector<int> filteredIds; | |
| // filter corners outside the image | |
| Rect innerRect(minDistToBorder, minDistToBorder, image.getMat().cols - 2 * minDistToBorder, | |
| image.getMat().rows - 2 * minDistToBorder); | |
| for(unsigned int i = 0; i < allCorners.getMat().total(); i++) { | |
| if(innerRect.contains(allCorners.getMat().at<Point2f>(i))) { | |
| filteredChessboardImgPoints.push_back(allCorners.getMat().at<Point2f>(i)); | |
| filteredIds.push_back(i); | |
| filteredWinSizes.push_back(winSizes[i]); | |
| } | |
| } | |
| // if none valid, return 0 | |
| if(filteredChessboardImgPoints.empty()) return; | |
| // corner refinement, first convert input image to grey | |
| Mat grey; | |
| if(image.type() == CV_8UC3) | |
| cvtColor(image, grey, COLOR_BGR2GRAY); | |
| else | |
| grey = image.getMat(); | |
| //// For each of the charuco corners, apply subpixel refinement using its correspondind winSize | |
| parallel_for_(Range(0, (int)filteredChessboardImgPoints.size()), [&](const Range& range) { | |
| const int begin = range.start; | |
| const int end = range.end; | |
| for (int i = begin; i < end; i++) { | |
| vector<Point2f> in; | |
| in.push_back(filteredChessboardImgPoints[i] - Point2f(0.5, 0.5)); // adjust sub-pixel coordinates for cornerSubPix | |
| Size winSize = filteredWinSizes[i]; | |
| if (winSize.height == -1 || winSize.width == -1) | |
| winSize = Size(arucoDetector.getDetectorParameters().cornerRefinementWinSize, | |
| arucoDetector.getDetectorParameters().cornerRefinementWinSize); | |
| cornerSubPix(grey, in, winSize, Size(), | |
| TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS, | |
| arucoDetector.getDetectorParameters().cornerRefinementMaxIterations, | |
| arucoDetector.getDetectorParameters().cornerRefinementMinAccuracy)); | |
| filteredChessboardImgPoints[i] = in[0] + Point2f(0.5, 0.5); | |
| } | |
| }); | |
| // parse output | |
| Mat(filteredChessboardImgPoints).copyTo(selectedCorners); | |
| Mat(filteredIds).copyTo(selectedIds); | |
| } | |
| /** Interpolate charuco corners using approximated pose estimation */ | |
| void interpolateCornersCharucoApproxCalib(InputArrayOfArrays markerCorners, InputArray markerIds, | |
| InputArray image, OutputArray charucoCorners, OutputArray charucoIds) { | |
| CV_Assert(image.getMat().channels() == 1 || image.getMat().channels() == 3); | |
| CV_Assert(markerCorners.total() == markerIds.getMat().total()); | |
| // approximated pose estimation using marker corners | |
| Mat approximatedRvec, approximatedTvec; | |
| Mat objPoints, imgPoints; // object and image points for the solvePnP function | |
| Board simpleBoard(board.getObjPoints(), board.getDictionary(), board.getIds()); | |
| simpleBoard.matchImagePoints(markerCorners, markerIds, objPoints, imgPoints); | |
| if (objPoints.total() < 4ull) // need, at least, 4 corners | |
| return; | |
| solvePnP(objPoints, imgPoints, charucoParameters.cameraMatrix, charucoParameters.distCoeffs, approximatedRvec, approximatedTvec); | |
| // project chessboard corners | |
| vector<Point2f> allChessboardImgPoints; | |
| projectPoints(board.getChessboardCorners(), approximatedRvec, approximatedTvec, charucoParameters.cameraMatrix, | |
| charucoParameters.distCoeffs, allChessboardImgPoints); | |
| // calculate maximum window sizes for subpixel refinement. The size is limited by the distance | |
| // to the closes marker corner to avoid erroneous displacements to marker corners | |
| vector<Size> subPixWinSizes = getMaximumSubPixWindowSizes(markerCorners, markerIds, allChessboardImgPoints); | |
| // filter corners outside the image and subpixel-refine charuco corners | |
| selectAndRefineChessboardCorners(allChessboardImgPoints, image, charucoCorners, charucoIds, subPixWinSizes); | |
| } | |
| /** Interpolate charuco corners using local homography */ | |
| void interpolateCornersCharucoLocalHom(InputArrayOfArrays markerCorners, InputArray markerIds, InputArray image, | |
| OutputArray charucoCorners, OutputArray charucoIds) { | |
| CV_Assert(image.getMat().channels() == 1 || image.getMat().channels() == 3); | |
| CV_Assert(markerCorners.total() == markerIds.getMat().total()); | |
| size_t nMarkers = markerIds.getMat().total(); | |
| // calculate local homographies for each marker | |
| vector<Mat> transformations(nMarkers); | |
| vector<bool> validTransform(nMarkers, false); | |
| const auto& ids = board.getIds(); | |
| for(size_t i = 0ull; i < nMarkers; i++) { | |
| vector<Point2f> markerObjPoints2D; | |
| int markerId = markerIds.getMat().at<int>((int)i); | |
| auto it = find(ids.begin(), ids.end(), markerId); | |
| if(it == ids.end()) continue; | |
| auto boardIdx = it - ids.begin(); | |
| markerObjPoints2D.resize(4ull); | |
| for(size_t j = 0ull; j < 4ull; j++) | |
| markerObjPoints2D[j] = | |
| Point2f(board.getObjPoints()[boardIdx][j].x, board.getObjPoints()[boardIdx][j].y); | |
| transformations[i] = getPerspectiveTransform(markerObjPoints2D, markerCorners.getMat((int)i)); | |
| // set transform as valid if transformation is non-singular | |
| double det = determinant(transformations[i]); | |
| validTransform[i] = std::abs(det) > 1e-6; | |
| } | |
| size_t nCharucoCorners = (size_t)board.getChessboardCorners().size(); | |
| vector<Point2f> allChessboardImgPoints(nCharucoCorners, Point2f(-1, -1)); | |
| // for each charuco corner, calculate its interpolation position based on the closest markers | |
| // homographies | |
| for(size_t i = 0ull; i < nCharucoCorners; i++) { | |
| Point2f objPoint2D = Point2f(board.getChessboardCorners()[i].x, board.getChessboardCorners()[i].y); | |
| vector<Point2f> interpolatedPositions; | |
| for(size_t j = 0ull; j < board.getNearestMarkerIdx()[i].size(); j++) { | |
| int markerId = board.getIds()[board.getNearestMarkerIdx()[i][j]]; | |
| int markerIdx = -1; | |
| for(size_t k = 0ull; k < markerIds.getMat().total(); k++) { | |
| if(markerIds.getMat().at<int>((int)k) == markerId) { | |
| markerIdx = (int)k; | |
| break; | |
| } | |
| } | |
| if (markerIdx != -1 && | |
| validTransform[markerIdx]) | |
| { | |
| vector<Point2f> in, out; | |
| in.push_back(objPoint2D); | |
| perspectiveTransform(in, out, transformations[markerIdx]); | |
| interpolatedPositions.push_back(out[0]); | |
| } | |
| } | |
| // none of the closest markers detected | |
| if(interpolatedPositions.empty()) continue; | |
| // more than one closest marker detected, take middle point | |
| if(interpolatedPositions.size() > 1ull) { | |
| allChessboardImgPoints[i] = (interpolatedPositions[0] + interpolatedPositions[1]) / 2.; | |
| } | |
| // a single closest marker detected | |
| else allChessboardImgPoints[i] = interpolatedPositions[0]; | |
| } | |
| // calculate maximum window sizes for subpixel refinement. The size is limited by the distance | |
| // to the closes marker corner to avoid erroneous displacements to marker corners | |
| vector<Size> subPixWinSizes = getMaximumSubPixWindowSizes(markerCorners, markerIds, allChessboardImgPoints); | |
| // filter corners outside the image and subpixel-refine charuco corners | |
| selectAndRefineChessboardCorners(allChessboardImgPoints, image, charucoCorners, charucoIds, subPixWinSizes); | |
| } | |
| /** Remove charuco corners if any of their minMarkers closest markers has not been detected */ | |
| int filterCornersWithoutMinMarkers(InputArray _allCharucoCorners, InputArray allCharucoIds, InputArray allArucoIds, | |
| OutputArray _filteredCharucoCorners, OutputArray _filteredCharucoIds) { | |
| CV_Assert(charucoParameters.minMarkers >= 0 && charucoParameters.minMarkers <= 2); | |
| vector<Point2f> filteredCharucoCorners; | |
| vector<int> filteredCharucoIds; | |
| // for each charuco corner | |
| for(unsigned int i = 0; i < allCharucoIds.getMat().total(); i++) { | |
| int currentCharucoId = allCharucoIds.getMat().at<int>(i); | |
| int totalMarkers = 0; // nomber of closest marker detected | |
| // look for closest markers | |
| for(unsigned int m = 0; m < board.getNearestMarkerIdx()[currentCharucoId].size(); m++) { | |
| int markerId = board.getIds()[board.getNearestMarkerIdx()[currentCharucoId][m]]; | |
| bool found = false; | |
| for(unsigned int k = 0; k < allArucoIds.getMat().total(); k++) { | |
| if(allArucoIds.getMat().at<int>(k) == markerId) { | |
| found = true; | |
| break; | |
| } | |
| } | |
| if(found) totalMarkers++; | |
| } | |
| // if enough markers detected, add the charuco corner to the final list | |
| if(totalMarkers >= charucoParameters.minMarkers) { | |
| filteredCharucoIds.push_back(currentCharucoId); | |
| filteredCharucoCorners.push_back(_allCharucoCorners.getMat().at<Point2f>(i)); | |
| } | |
| } | |
| // parse output | |
| Mat(filteredCharucoCorners).copyTo(_filteredCharucoCorners); | |
| Mat(filteredCharucoIds).copyTo(_filteredCharucoIds); | |
| return (int)_filteredCharucoIds.total(); | |
| } | |
| void detectBoard(InputArray image, OutputArray charucoCorners, OutputArray charucoIds, | |
| InputOutputArrayOfArrays markerCorners, InputOutputArray markerIds) { | |
| CV_Assert((markerCorners.empty() && markerIds.empty() && !image.empty()) || (markerCorners.total() == markerIds.total())); | |
| vector<vector<Point2f>> tmpMarkerCorners; | |
| vector<int> tmpMarkerIds; | |
| InputOutputArrayOfArrays _markerCorners = markerCorners.needed() ? markerCorners : tmpMarkerCorners; | |
| InputOutputArray _markerIds = markerIds.needed() ? markerIds : tmpMarkerIds; | |
| if (markerCorners.empty() && markerIds.empty()) { | |
| vector<vector<Point2f> > rejectedMarkers; | |
| arucoDetector.detectMarkers(image, _markerCorners, _markerIds, rejectedMarkers); | |
| if (charucoParameters.tryRefineMarkers) | |
| arucoDetector.refineDetectedMarkers(image, board, _markerCorners, _markerIds, rejectedMarkers); | |
| if (_markerCorners.empty() && _markerIds.empty()) | |
| return; | |
| } | |
| // if camera parameters are avaible, use approximated calibration | |
| if(!charucoParameters.cameraMatrix.empty()) | |
| interpolateCornersCharucoApproxCalib(_markerCorners, _markerIds, image, charucoCorners, charucoIds); | |
| // else use local homography | |
| else | |
| interpolateCornersCharucoLocalHom(_markerCorners, _markerIds, image, charucoCorners, charucoIds); | |
| // to return a charuco corner, its closest aruco markers should have been detected | |
| filterCornersWithoutMinMarkers(charucoCorners, charucoIds, _markerIds, charucoCorners, charucoIds); | |
| } | |
| void detectBoardWithCheck(InputArray image, OutputArray charucoCorners, OutputArray charucoIds, | |
| InputOutputArrayOfArrays markerCorners, InputOutputArray markerIds) { | |
| vector<vector<Point2f>> tmpMarkerCorners; | |
| vector<int> tmpMarkerIds; | |
| InputOutputArrayOfArrays _markerCorners = markerCorners.needed() ? markerCorners : tmpMarkerCorners; | |
| InputOutputArray _markerIds = markerIds.needed() ? markerIds : tmpMarkerIds; | |
| detectBoard(image, charucoCorners, charucoIds, _markerCorners, _markerIds); | |
| if (checkBoard(_markerCorners, _markerIds, charucoCorners, charucoIds) == false) { | |
| CV_LOG_DEBUG(NULL, "ChArUco board is built incorrectly"); | |
| charucoCorners.release(); | |
| charucoIds.release(); | |
| } | |
| } | |
| }; | |
| CharucoDetector::CharucoDetector(const CharucoBoard &board, const CharucoParameters &charucoParams, | |
| const DetectorParameters &detectorParams, const RefineParameters& refineParams) { | |
| this->charucoDetectorImpl = makePtr<CharucoDetectorImpl>(board, charucoParams, ArucoDetector(board.getDictionary(), detectorParams, refineParams)); | |
| } | |
| const CharucoBoard& CharucoDetector::getBoard() const { | |
| return charucoDetectorImpl->board; | |
| } | |
| void CharucoDetector::setBoard(const CharucoBoard& board) { | |
| this->charucoDetectorImpl->board = board; | |
| charucoDetectorImpl->arucoDetector.setDictionary(board.getDictionary()); | |
| } | |
| const CharucoParameters &CharucoDetector::getCharucoParameters() const { | |
| return charucoDetectorImpl->charucoParameters; | |
| } | |
| void CharucoDetector::setCharucoParameters(CharucoParameters &charucoParameters) { | |
| charucoDetectorImpl->charucoParameters = charucoParameters; | |
| } | |
| const DetectorParameters& CharucoDetector::getDetectorParameters() const { | |
| return charucoDetectorImpl->arucoDetector.getDetectorParameters(); | |
| } | |
| void CharucoDetector::setDetectorParameters(const DetectorParameters& detectorParameters) { | |
| charucoDetectorImpl->arucoDetector.setDetectorParameters(detectorParameters); | |
| } | |
| const RefineParameters& CharucoDetector::getRefineParameters() const { | |
| return charucoDetectorImpl->arucoDetector.getRefineParameters(); | |
| } | |
| void CharucoDetector::setRefineParameters(const RefineParameters& refineParameters) { | |
| charucoDetectorImpl->arucoDetector.setRefineParameters(refineParameters); | |
| } | |
| void CharucoDetector::detectBoard(InputArray image, OutputArray charucoCorners, OutputArray charucoIds, | |
| InputOutputArrayOfArrays markerCorners, InputOutputArray markerIds) const { | |
| charucoDetectorImpl->detectBoardWithCheck(image, charucoCorners, charucoIds, markerCorners, markerIds); | |
| } | |
| void CharucoDetector::detectDiamonds(InputArray image, OutputArrayOfArrays _diamondCorners, OutputArray _diamondIds, | |
| InputOutputArrayOfArrays inMarkerCorners, InputOutputArray inMarkerIds) const { | |
| CV_Assert(getBoard().getChessboardSize() == Size(3, 3)); | |
| CV_Assert((inMarkerCorners.empty() && inMarkerIds.empty() && !image.empty()) || (inMarkerCorners.total() == inMarkerIds.total())); | |
| vector<vector<Point2f>> tmpMarkerCorners; | |
| vector<int> tmpMarkerIds; | |
| InputOutputArrayOfArrays _markerCorners = inMarkerCorners.needed() ? inMarkerCorners : tmpMarkerCorners; | |
| InputOutputArray _markerIds = inMarkerIds.needed() ? inMarkerIds : tmpMarkerIds; | |
| if (_markerCorners.empty() && _markerIds.empty()) { | |
| charucoDetectorImpl->arucoDetector.detectMarkers(image, _markerCorners, _markerIds); | |
| } | |
| const float minRepDistanceRate = 1.302455f; | |
| vector<vector<Point2f>> diamondCorners; | |
| vector<Vec4i> diamondIds; | |
| // stores if the detected markers have been assigned or not to a diamond | |
| vector<bool> assigned(_markerIds.total(), false); | |
| if(_markerIds.total() < 4ull) return; // a diamond need at least 4 markers | |
| // convert input image to grey | |
| Mat grey; | |
| if(image.type() == CV_8UC3) | |
| cvtColor(image, grey, COLOR_BGR2GRAY); | |
| else | |
| grey = image.getMat(); | |
| auto board = getBoard(); | |
| // for each of the detected markers, try to find a diamond | |
| for(unsigned int i = 0; i < (unsigned int)_markerIds.total(); i++) { | |
| if(assigned[i]) continue; | |
| // calculate marker perimeter | |
| float perimeterSq = 0; | |
| Mat corners = _markerCorners.getMat(i); | |
| for(int c = 0; c < 4; c++) { | |
| Point2f edge = corners.at<Point2f>(c) - corners.at<Point2f>((c + 1) % 4); | |
| perimeterSq += edge.x*edge.x + edge.y*edge.y; | |
| } | |
| // maximum reprojection error relative to perimeter | |
| float minRepDistance = sqrt(perimeterSq) * minRepDistanceRate; | |
| int currentId = _markerIds.getMat().at<int>(i); | |
| // prepare data to call refineDetectedMarkers() | |
| // detected markers (only the current one) | |
| vector<Mat> currentMarker; | |
| vector<int> currentMarkerId; | |
| currentMarker.push_back(_markerCorners.getMat(i)); | |
| currentMarkerId.push_back(currentId); | |
| // marker candidates (the rest of markers if they have not been assigned) | |
| vector<Mat> candidates; | |
| vector<int> candidatesIdxs; | |
| for(unsigned int k = 0; k < assigned.size(); k++) { | |
| if(k == i) continue; | |
| if(!assigned[k]) { | |
| candidates.push_back(_markerCorners.getMat(k)); | |
| candidatesIdxs.push_back(k); | |
| } | |
| } | |
| if(candidates.size() < 3ull) break; // we need at least 3 free markers | |
| // modify charuco layout id to make sure all the ids are different than current id | |
| vector<int> tmpIds(4ull); | |
| for(int k = 1; k < 4; k++) | |
| tmpIds[k] = currentId + 1 + k; | |
| // current id is assigned to [0], so it is the marker on the top | |
| tmpIds[0] = currentId; | |
| // create Charuco board layout for diamond (3x3 layout) | |
| charucoDetectorImpl->board = CharucoBoard(Size(3, 3), board.getSquareLength(), | |
| board.getMarkerLength(), board.getDictionary(), tmpIds); | |
| // try to find the rest of markers in the diamond | |
| vector<int> acceptedIdxs; | |
| if (currentMarker.size() != 4ull) | |
| { | |
| RefineParameters refineParameters(minRepDistance, -1.f, false); | |
| RefineParameters tmp = charucoDetectorImpl->arucoDetector.getRefineParameters(); | |
| charucoDetectorImpl->arucoDetector.setRefineParameters(refineParameters); | |
| charucoDetectorImpl->arucoDetector.refineDetectedMarkers(grey, getBoard(), currentMarker, currentMarkerId, | |
| candidates, | |
| noArray(), noArray(), acceptedIdxs); | |
| charucoDetectorImpl->arucoDetector.setRefineParameters(tmp); | |
| } | |
| // if found, we have a diamond | |
| if(currentMarker.size() == 4ull) { | |
| assigned[i] = true; | |
| // calculate diamond id, acceptedIdxs array indicates the markers taken from candidates array | |
| Vec4i markerId; | |
| markerId[0] = currentId; | |
| for(int k = 1; k < 4; k++) { | |
| int currentMarkerIdx = candidatesIdxs[acceptedIdxs[k - 1]]; | |
| markerId[k] = _markerIds.getMat().at<int>(currentMarkerIdx); | |
| assigned[currentMarkerIdx] = true; | |
| } | |
| // interpolate the charuco corners of the diamond | |
| vector<Point2f> currentMarkerCorners; | |
| Mat aux; | |
| charucoDetectorImpl->detectBoardWithCheck(grey, currentMarkerCorners, aux, currentMarker, currentMarkerId); | |
| // if everything is ok, save the diamond | |
| if(currentMarkerCorners.size() > 0ull) { | |
| // reorder corners | |
| vector<Point2f> currentMarkerCornersReorder; | |
| currentMarkerCornersReorder.resize(4); | |
| currentMarkerCornersReorder[0] = currentMarkerCorners[0]; | |
| currentMarkerCornersReorder[1] = currentMarkerCorners[1]; | |
| currentMarkerCornersReorder[2] = currentMarkerCorners[3]; | |
| currentMarkerCornersReorder[3] = currentMarkerCorners[2]; | |
| diamondCorners.push_back(currentMarkerCornersReorder); | |
| diamondIds.push_back(markerId); | |
| } | |
| } | |
| } | |
| charucoDetectorImpl->board = board; | |
| if(diamondIds.size() > 0ull) { | |
| // parse output | |
| Mat(diamondIds).copyTo(_diamondIds); | |
| _diamondCorners.create((int)diamondCorners.size(), 1, CV_32FC2); | |
| for(unsigned int i = 0; i < diamondCorners.size(); i++) { | |
| _diamondCorners.create(4, 1, CV_32FC2, i, true); | |
| for(int j = 0; j < 4; j++) { | |
| _diamondCorners.getMat(i).at<Point2f>(j) = diamondCorners[i][j]; | |
| } | |
| } | |
| } | |
| } | |
| void drawDetectedCornersCharuco(InputOutputArray _image, InputArray _charucoCorners, | |
| InputArray _charucoIds, Scalar cornerColor) { | |
| CV_Assert(!_image.getMat().empty() && | |
| (_image.getMat().channels() == 1 || _image.getMat().channels() == 3)); | |
| CV_Assert((_charucoCorners.total() == _charucoIds.total()) || | |
| _charucoIds.total() == 0); | |
| CV_Assert(_charucoCorners.channels() == 2); | |
| Mat charucoCorners = _charucoCorners.getMat(); | |
| if (charucoCorners.type() != CV_32SC2) | |
| charucoCorners.convertTo(charucoCorners, CV_32SC2); | |
| Mat charucoIds; | |
| if (!_charucoIds.empty()) | |
| charucoIds = _charucoIds.getMat(); | |
| size_t nCorners = charucoCorners.total(); | |
| for(size_t i = 0; i < nCorners; i++) { | |
| Point corner = charucoCorners.at<Point>((int)i); | |
| // draw first corner mark | |
| rectangle(_image, corner - Point(3, 3), corner + Point(3, 3), cornerColor, 1, LINE_AA); | |
| // draw ID | |
| if(!_charucoIds.empty()) { | |
| int id = charucoIds.at<int>((int)i); | |
| stringstream s; | |
| s << "id=" << id; | |
| putText(_image, s.str(), corner + Point(5, -5), FONT_HERSHEY_SIMPLEX, 0.5, | |
| cornerColor, 2); | |
| } | |
| } | |
| } | |
| void drawDetectedDiamonds(InputOutputArray _image, InputArrayOfArrays _corners, InputArray _ids, Scalar borderColor) { | |
| CV_Assert(_image.getMat().total() != 0 && | |
| (_image.getMat().channels() == 1 || _image.getMat().channels() == 3)); | |
| CV_Assert((_corners.total() == _ids.total()) || _ids.total() == 0); | |
| // calculate colors | |
| Scalar textColor, cornerColor; | |
| textColor = cornerColor = borderColor; | |
| swap(textColor.val[0], textColor.val[1]); // text color just sawp G and R | |
| swap(cornerColor.val[1], cornerColor.val[2]); // corner color just sawp G and B | |
| int nMarkers = (int)_corners.total(); | |
| for(int i = 0; i < nMarkers; i++) { | |
| Mat currentMarker = _corners.getMat(i); | |
| CV_Assert(currentMarker.total() == 4 && currentMarker.channels() == 2); | |
| if (currentMarker.type() != CV_32SC2) | |
| currentMarker.convertTo(currentMarker, CV_32SC2); | |
| // draw marker sides | |
| for(int j = 0; j < 4; j++) { | |
| Point p0, p1; | |
| p0 = currentMarker.at<Point>(j); | |
| p1 = currentMarker.at<Point>((j + 1) % 4); | |
| line(_image, p0, p1, borderColor, 1); | |
| } | |
| // draw first corner mark | |
| rectangle(_image, currentMarker.at<Point>(0) - Point(3, 3), | |
| currentMarker.at<Point>(0) + Point(3, 3), cornerColor, 1, LINE_AA); | |
| // draw id composed by four numbers | |
| if(_ids.total() != 0) { | |
| Point cent(0, 0); | |
| for(int p = 0; p < 4; p++) | |
| cent += currentMarker.at<Point>(p); | |
| cent = cent / 4.; | |
| stringstream s; | |
| s << "id=" << _ids.getMat().at< Vec4i >(i); | |
| putText(_image, s.str(), cent, FONT_HERSHEY_SIMPLEX, 0.5, textColor, 2); | |
| } | |
| } | |
| } | |
| } | |
| } | |