mirror of
https://github.com/opencv/opencv.git
synced 2026-07-30 15:53:03 +04:00
Merge pull request #22986 from AleksandrPanov:move_contrib_charuco_to_main_objdetect
merge with https://github.com/opencv/opencv_contrib/pull/3394 move Charuco API from contrib to main repo: - add CharucoDetector: ``` CharucoDetector::detectBoard(InputArray image, InputOutputArrayOfArrays markerCorners, InputOutputArray markerIds, OutputArray charucoCorners, OutputArray charucoIds) const // detect charucoCorners and/or markerCorners CharucoDetector::detectDiamonds(InputArray image, InputOutputArrayOfArrays _markerCorners, InputOutputArrayOfArrays _markerIds, OutputArrayOfArrays _diamondCorners, OutputArray _diamondIds) const ``` - add `matchImagePoints()` for `CharucoBoard` - remove contrib aruco dependencies from interactive-calibration tool - move almost all aruco tests to objdetect ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [x] There is a reference to the original bug report and related work - [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [x] The feature is well documented and sample code can be built with the project CMake
This commit is contained in:
Alexander Panov
GitHub
@@ -3,6 +3,8 @@
|
||||
// of this distribution and at http://opencv.org/license.html
|
||||
|
||||
#include "../precomp.hpp"
|
||||
#include "opencv2/objdetect/aruco_board.hpp"
|
||||
|
||||
#include <opencv2/objdetect/aruco_dictionary.hpp>
|
||||
#include <numeric>
|
||||
|
||||
@@ -10,72 +12,60 @@ namespace cv {
|
||||
namespace aruco {
|
||||
using namespace std;
|
||||
|
||||
struct Board::BoardImpl {
|
||||
std::vector<std::vector<Point3f> > objPoints;
|
||||
struct Board::Impl {
|
||||
Dictionary dictionary;
|
||||
Point3f rightBottomBorder;
|
||||
std::vector<int> ids;
|
||||
std::vector<std::vector<Point3f> > objPoints;
|
||||
Point3f rightBottomBorder;
|
||||
|
||||
BoardImpl() {
|
||||
dictionary = Dictionary(getPredefinedDictionary(PredefinedDictionaryType::DICT_4X4_50));
|
||||
}
|
||||
explicit Impl(const Dictionary& _dictionary):
|
||||
dictionary(_dictionary)
|
||||
{}
|
||||
|
||||
virtual ~Impl() {}
|
||||
|
||||
Impl(const Impl&) = delete;
|
||||
Impl& operator=(const Impl&) = delete;
|
||||
|
||||
virtual void matchImagePoints(InputArray detectedCorners, InputArray detectedIds, OutputArray _objPoints,
|
||||
OutputArray imgPoints) const;
|
||||
|
||||
virtual void generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const;
|
||||
};
|
||||
|
||||
Board::Board(): boardImpl(makePtr<BoardImpl>()) {}
|
||||
void Board::Impl::matchImagePoints(InputArray detectedCorners, InputArray detectedIds, OutputArray _objPoints,
|
||||
OutputArray imgPoints) const {
|
||||
|
||||
Board::~Board() {}
|
||||
CV_Assert(ids.size() == objPoints.size());
|
||||
CV_Assert(detectedIds.total() == detectedCorners.total());
|
||||
|
||||
Ptr<Board> Board::create(InputArrayOfArrays objPoints, const Dictionary &dictionary, InputArray ids) {
|
||||
CV_Assert(objPoints.total() == ids.total());
|
||||
CV_Assert(objPoints.type() == CV_32FC3 || objPoints.type() == CV_32FC1);
|
||||
size_t nDetectedMarkers = detectedIds.total();
|
||||
|
||||
vector<vector<Point3f> > obj_points_vector;
|
||||
Point3f rightBottomBorder = Point3f(0.f, 0.f, 0.f);
|
||||
for (unsigned int i = 0; i < objPoints.total(); i++) {
|
||||
vector<Point3f> corners;
|
||||
Mat corners_mat = objPoints.getMat(i);
|
||||
vector<Point3f> objPnts;
|
||||
objPnts.reserve(nDetectedMarkers);
|
||||
|
||||
if (corners_mat.type() == CV_32FC1)
|
||||
corners_mat = corners_mat.reshape(3);
|
||||
CV_Assert(corners_mat.total() == 4);
|
||||
vector<Point2f> imgPnts;
|
||||
imgPnts.reserve(nDetectedMarkers);
|
||||
|
||||
for (int j = 0; j < 4; j++) {
|
||||
const Point3f &corner = corners_mat.at<Point3f>(j);
|
||||
corners.push_back(corner);
|
||||
rightBottomBorder.x = std::max(rightBottomBorder.x, corner.x);
|
||||
rightBottomBorder.y = std::max(rightBottomBorder.y, corner.y);
|
||||
rightBottomBorder.z = std::max(rightBottomBorder.z, corner.z);
|
||||
// look for detected markers that belong to the board and get their information
|
||||
for(unsigned int i = 0; i < nDetectedMarkers; i++) {
|
||||
int currentId = detectedIds.getMat().ptr< int >(0)[i];
|
||||
for(unsigned int j = 0; j < ids.size(); j++) {
|
||||
if(currentId == ids[j]) {
|
||||
for(int p = 0; p < 4; p++) {
|
||||
objPnts.push_back(objPoints[j][p]);
|
||||
imgPnts.push_back(detectedCorners.getMat(i).ptr<Point2f>(0)[p]);
|
||||
}
|
||||
}
|
||||
}
|
||||
obj_points_vector.push_back(corners);
|
||||
}
|
||||
Board board;
|
||||
Ptr<Board> res = makePtr<Board>(board);
|
||||
ids.copyTo(res->boardImpl->ids);
|
||||
res->boardImpl->objPoints = obj_points_vector;
|
||||
res->boardImpl->dictionary = dictionary;
|
||||
res->boardImpl->rightBottomBorder = rightBottomBorder;
|
||||
return res;
|
||||
|
||||
// create output
|
||||
Mat(objPnts).copyTo(_objPoints);
|
||||
Mat(imgPnts).copyTo(imgPoints);
|
||||
}
|
||||
|
||||
const Dictionary& Board::getDictionary() const {
|
||||
return this->boardImpl->dictionary;
|
||||
}
|
||||
|
||||
const vector<vector<Point3f> >& Board::getObjPoints() const {
|
||||
return this->boardImpl->objPoints;
|
||||
}
|
||||
|
||||
const Point3f& Board::getRightBottomCorner() const {
|
||||
return this->boardImpl->rightBottomBorder;
|
||||
}
|
||||
|
||||
const vector<int>& Board::getIds() const {
|
||||
return this->boardImpl->ids;
|
||||
}
|
||||
|
||||
/** @brief Implementation of draw planar board that accepts a raw Board pointer.
|
||||
*/
|
||||
void Board::generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const {
|
||||
void Board::Impl::generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const {
|
||||
CV_Assert(!outSize.empty());
|
||||
CV_Assert(marginSize >= 0);
|
||||
|
||||
@@ -85,17 +75,17 @@ void Board::generateImage(Size outSize, OutputArray img, int marginSize, int bor
|
||||
out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
|
||||
|
||||
// calculate max and min values in XY plane
|
||||
CV_Assert(this->getObjPoints().size() > 0);
|
||||
CV_Assert(objPoints.size() > 0);
|
||||
float minX, maxX, minY, maxY;
|
||||
minX = maxX = this->getObjPoints()[0][0].x;
|
||||
minY = maxY = this->getObjPoints()[0][0].y;
|
||||
minX = maxX = objPoints[0][0].x;
|
||||
minY = maxY = objPoints[0][0].y;
|
||||
|
||||
for(unsigned int i = 0; i < this->getObjPoints().size(); i++) {
|
||||
for(unsigned int i = 0; i < objPoints.size(); i++) {
|
||||
for(int j = 0; j < 4; j++) {
|
||||
minX = min(minX, this->getObjPoints()[i][j].x);
|
||||
maxX = max(maxX, this->getObjPoints()[i][j].x);
|
||||
minY = min(minY, this->getObjPoints()[i][j].y);
|
||||
maxY = max(maxY, this->getObjPoints()[i][j].y);
|
||||
minX = min(minX, objPoints[i][j].x);
|
||||
maxX = max(maxX, objPoints[i][j].x);
|
||||
minY = min(minY, objPoints[i][j].y);
|
||||
maxY = max(maxY, objPoints[i][j].y);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -121,10 +111,10 @@ void Board::generateImage(Size outSize, OutputArray img, int marginSize, int bor
|
||||
Mat marker;
|
||||
Point2f outCorners[3];
|
||||
Point2f inCorners[3];
|
||||
for(unsigned int m = 0; m < this->getObjPoints().size(); m++) {
|
||||
for(unsigned int m = 0; m < objPoints.size(); m++) {
|
||||
// transform corners to markerZone coordinates
|
||||
for(int j = 0; j < 3; j++) {
|
||||
Point2f pf = Point2f(this->getObjPoints()[m][j].x, this->getObjPoints()[m][j].y);
|
||||
Point2f pf = Point2f(objPoints[m][j].x, objPoints[m][j].y);
|
||||
// move top left to 0, 0
|
||||
pf -= Point2f(minX, minY);
|
||||
pf.x = pf.x / sizeX * float(out.cols);
|
||||
@@ -135,7 +125,7 @@ void Board::generateImage(Size outSize, OutputArray img, int marginSize, int bor
|
||||
// get marker
|
||||
Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
|
||||
dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
|
||||
getDictionary().generateImageMarker(this->getIds()[m], dst_sz.width, marker, borderBits);
|
||||
dictionary.generateImageMarker(ids[m], dst_sz.width, marker, borderBits);
|
||||
|
||||
if((outCorners[0].y == outCorners[1].y) && (outCorners[1].x == outCorners[2].x)) {
|
||||
// marker is aligned to image axes
|
||||
@@ -155,70 +145,119 @@ void Board::generateImage(Size outSize, OutputArray img, int marginSize, int bor
|
||||
}
|
||||
}
|
||||
|
||||
void Board::matchImagePoints(InputArray detectedCorners, InputArray detectedIds,
|
||||
OutputArray _objPoints, OutputArray imgPoints) const {
|
||||
CV_Assert(getIds().size() == getObjPoints().size());
|
||||
CV_Assert(detectedIds.total() == detectedCorners.total());
|
||||
|
||||
size_t nDetectedMarkers = detectedIds.total();
|
||||
|
||||
vector<Point3f> objPnts;
|
||||
objPnts.reserve(nDetectedMarkers);
|
||||
|
||||
vector<Point2f> imgPnts;
|
||||
imgPnts.reserve(nDetectedMarkers);
|
||||
|
||||
// look for detected markers that belong to the board and get their information
|
||||
for(unsigned int i = 0; i < nDetectedMarkers; i++) {
|
||||
int currentId = detectedIds.getMat().ptr< int >(0)[i];
|
||||
for(unsigned int j = 0; j < getIds().size(); j++) {
|
||||
if(currentId == getIds()[j]) {
|
||||
for(int p = 0; p < 4; p++) {
|
||||
objPnts.push_back(getObjPoints()[j][p]);
|
||||
imgPnts.push_back(detectedCorners.getMat(i).ptr<Point2f>(0)[p]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// create output
|
||||
Mat(objPnts).copyTo(_objPoints);
|
||||
Mat(imgPnts).copyTo(imgPoints);
|
||||
Board::Board(const Ptr<Impl>& _impl):
|
||||
impl(_impl)
|
||||
{
|
||||
CV_Assert(impl);
|
||||
}
|
||||
|
||||
struct GridBoard::GridImpl {
|
||||
GridImpl(){};
|
||||
Board::Board():
|
||||
impl(nullptr)
|
||||
{}
|
||||
|
||||
Board::Board(InputArrayOfArrays objPoints, const Dictionary &dictionary, InputArray ids):
|
||||
Board(new Board::Impl(dictionary)) {
|
||||
CV_Assert(ids.size() == objPoints.size());
|
||||
CV_Assert(objPoints.total() == ids.total());
|
||||
CV_Assert(objPoints.type() == CV_32FC3 || objPoints.type() == CV_32FC1);
|
||||
|
||||
vector<vector<Point3f> > obj_points_vector;
|
||||
Point3f rightBottomBorder = Point3f(0.f, 0.f, 0.f);
|
||||
for (unsigned int i = 0; i < objPoints.total(); i++) {
|
||||
vector<Point3f> corners;
|
||||
Mat corners_mat = objPoints.getMat(i);
|
||||
|
||||
if (corners_mat.type() == CV_32FC1)
|
||||
corners_mat = corners_mat.reshape(3);
|
||||
CV_Assert(corners_mat.total() == 4);
|
||||
|
||||
for (int j = 0; j < 4; j++) {
|
||||
const Point3f &corner = corners_mat.at<Point3f>(j);
|
||||
corners.push_back(corner);
|
||||
rightBottomBorder.x = std::max(rightBottomBorder.x, corner.x);
|
||||
rightBottomBorder.y = std::max(rightBottomBorder.y, corner.y);
|
||||
rightBottomBorder.z = std::max(rightBottomBorder.z, corner.z);
|
||||
}
|
||||
obj_points_vector.push_back(corners);
|
||||
}
|
||||
|
||||
ids.copyTo(impl->ids);
|
||||
impl->objPoints = obj_points_vector;
|
||||
impl->rightBottomBorder = rightBottomBorder;
|
||||
}
|
||||
|
||||
const Dictionary& Board::getDictionary() const {
|
||||
CV_Assert(this->impl);
|
||||
return this->impl->dictionary;
|
||||
}
|
||||
|
||||
const vector<vector<Point3f> >& Board::getObjPoints() const {
|
||||
CV_Assert(this->impl);
|
||||
return this->impl->objPoints;
|
||||
}
|
||||
|
||||
const Point3f& Board::getRightBottomCorner() const {
|
||||
CV_Assert(this->impl);
|
||||
return this->impl->rightBottomBorder;
|
||||
}
|
||||
|
||||
const vector<int>& Board::getIds() const {
|
||||
CV_Assert(this->impl);
|
||||
return this->impl->ids;
|
||||
}
|
||||
|
||||
/** @brief Implementation of draw planar board that accepts a raw Board pointer.
|
||||
*/
|
||||
void Board::generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const {
|
||||
CV_Assert(this->impl);
|
||||
impl->generateImage(outSize, img, marginSize, borderBits);
|
||||
}
|
||||
|
||||
void Board::matchImagePoints(InputArray detectedCorners, InputArray detectedIds, OutputArray objPoints,
|
||||
OutputArray imgPoints) const {
|
||||
CV_Assert(this->impl);
|
||||
impl->matchImagePoints(detectedCorners, detectedIds, objPoints, imgPoints);
|
||||
}
|
||||
|
||||
struct GridBoardImpl : public Board::Impl {
|
||||
GridBoardImpl(const Dictionary& _dictionary, const Size& _size, float _markerLength, float _markerSeparation):
|
||||
Board::Impl(_dictionary),
|
||||
size(_size),
|
||||
markerLength(_markerLength),
|
||||
markerSeparation(_markerSeparation)
|
||||
{
|
||||
CV_Assert(size.width*size.height > 0 && markerLength > 0 && markerSeparation > 0);
|
||||
}
|
||||
|
||||
// number of markers in X and Y directions
|
||||
int sizeX = 3, sizeY = 3;
|
||||
|
||||
const Size size;
|
||||
// marker side length (normally in meters)
|
||||
float markerLength = 1.f;
|
||||
|
||||
float markerLength;
|
||||
// separation between markers in the grid
|
||||
float markerSeparation = .5f;
|
||||
float markerSeparation;
|
||||
};
|
||||
|
||||
GridBoard::GridBoard(): gridImpl(makePtr<GridImpl>()) {}
|
||||
GridBoard::GridBoard() {}
|
||||
|
||||
Ptr<GridBoard> GridBoard::create(int markersX, int markersY, float markerLength, float markerSeparation,
|
||||
const Dictionary &dictionary, InputArray ids) {
|
||||
CV_Assert(markersX > 0 && markersY > 0 && markerLength > 0 && markerSeparation > 0);
|
||||
GridBoard board;
|
||||
Ptr<GridBoard> res = makePtr<GridBoard>(board);
|
||||
res->gridImpl->sizeX = markersX;
|
||||
res->gridImpl->sizeY = markersY;
|
||||
res->gridImpl->markerLength = markerLength;
|
||||
res->gridImpl->markerSeparation = markerSeparation;
|
||||
res->boardImpl->dictionary = dictionary;
|
||||
GridBoard::GridBoard(const Size& size, float markerLength, float markerSeparation,
|
||||
const Dictionary &dictionary, InputArray ids):
|
||||
Board(new GridBoardImpl(dictionary, size, markerLength, markerSeparation)) {
|
||||
|
||||
size_t totalMarkers = (size_t) markersX * markersY;
|
||||
CV_Assert(totalMarkers == ids.total());
|
||||
size_t totalMarkers = (size_t) size.width*size.height;
|
||||
CV_Assert(ids.empty() || totalMarkers == ids.total());
|
||||
vector<vector<Point3f> > objPoints;
|
||||
objPoints.reserve(totalMarkers);
|
||||
ids.copyTo(res->boardImpl->ids);
|
||||
|
||||
if(!ids.empty()) {
|
||||
ids.copyTo(impl->ids);
|
||||
} else {
|
||||
impl->ids = std::vector<int>(totalMarkers);
|
||||
std::iota(impl->ids.begin(), impl->ids.end(), 0);
|
||||
}
|
||||
|
||||
// calculate Board objPoints
|
||||
for (int y = 0; y < markersY; y++) {
|
||||
for (int x = 0; x < markersX; x++) {
|
||||
for (int y = 0; y < size.height; y++) {
|
||||
for (int x = 0; x < size.width; x++) {
|
||||
vector <Point3f> corners(4);
|
||||
corners[0] = Point3f(x * (markerLength + markerSeparation),
|
||||
y * (markerLength + markerSeparation), 0);
|
||||
@@ -228,67 +267,141 @@ Ptr<GridBoard> GridBoard::create(int markersX, int markersY, float markerLength,
|
||||
objPoints.push_back(corners);
|
||||
}
|
||||
}
|
||||
res->boardImpl->objPoints = objPoints;
|
||||
res->boardImpl->rightBottomBorder = Point3f(markersX * markerLength + markerSeparation * (markersX - 1),
|
||||
markersY * markerLength + markerSeparation * (markersY - 1), 0.f);
|
||||
return res;
|
||||
}
|
||||
|
||||
Ptr<GridBoard> GridBoard::create(int markersX, int markersY, float markerLength, float markerSeparation,
|
||||
const Dictionary &dictionary, int firstMarker) {
|
||||
vector<int> ids(markersX*markersY);
|
||||
std::iota(ids.begin(), ids.end(), firstMarker);
|
||||
return GridBoard::create(markersX, markersY, markerLength, markerSeparation, dictionary, ids);
|
||||
}
|
||||
|
||||
void GridBoard::generateImage(Size outSize, OutputArray _img, int marginSize, int borderBits) const {
|
||||
Board::generateImage(outSize, _img, marginSize, borderBits);
|
||||
impl->objPoints = objPoints;
|
||||
impl->rightBottomBorder = Point3f(size.width * markerLength + markerSeparation * (size.width - 1),
|
||||
size.height * markerLength + markerSeparation * (size.height - 1), 0.f);
|
||||
}
|
||||
|
||||
Size GridBoard::getGridSize() const {
|
||||
return Size(gridImpl->sizeX, gridImpl->sizeY);
|
||||
CV_Assert(impl);
|
||||
return static_pointer_cast<GridBoardImpl>(impl)->size;
|
||||
}
|
||||
|
||||
float GridBoard::getMarkerLength() const {
|
||||
return gridImpl->markerLength;
|
||||
CV_Assert(impl);
|
||||
return static_pointer_cast<GridBoardImpl>(impl)->markerLength;
|
||||
}
|
||||
|
||||
float GridBoard::getMarkerSeparation() const {
|
||||
return gridImpl->markerSeparation;
|
||||
CV_Assert(impl);
|
||||
return static_pointer_cast<GridBoardImpl>(impl)->markerSeparation;
|
||||
}
|
||||
|
||||
struct CharucoBoard::CharucoImpl : GridBoard::GridImpl {
|
||||
// size of chessboard squares side (normally in meters)
|
||||
struct CharucoBoardImpl : Board::Impl {
|
||||
CharucoBoardImpl(const Dictionary& _dictionary, const Size& _size, float _squareLength, float _markerLength):
|
||||
Board::Impl(_dictionary),
|
||||
size(_size),
|
||||
squareLength(_squareLength),
|
||||
markerLength(_markerLength)
|
||||
{}
|
||||
|
||||
// chessboard size
|
||||
Size size;
|
||||
|
||||
// Physical size of chessboard squares side (normally in meters)
|
||||
float squareLength;
|
||||
|
||||
// marker side length (normally in meters)
|
||||
// Physical marker side length (normally in meters)
|
||||
float markerLength;
|
||||
|
||||
static void _getNearestMarkerCorners(CharucoBoard &board, float squareLength);
|
||||
|
||||
// vector of chessboard 3D corners precalculated
|
||||
std::vector<Point3f> chessboardCorners;
|
||||
|
||||
// for each charuco corner, nearest marker id and nearest marker corner id of each marker
|
||||
std::vector<std::vector<int> > nearestMarkerIdx;
|
||||
std::vector<std::vector<int> > nearestMarkerCorners;
|
||||
|
||||
void calcNearestMarkerCorners();
|
||||
|
||||
void matchImagePoints(InputArrayOfArrays detectedCorners, InputArray detectedIds,
|
||||
OutputArray objPoints, OutputArray imgPoints) const override;
|
||||
|
||||
void generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const override;
|
||||
};
|
||||
|
||||
CharucoBoard::CharucoBoard(): charucoImpl(makePtr<CharucoImpl>()) {}
|
||||
/** Fill nearestMarkerIdx and nearestMarkerCorners arrays */
|
||||
void CharucoBoardImpl::calcNearestMarkerCorners() {
|
||||
nearestMarkerIdx.resize(chessboardCorners.size());
|
||||
nearestMarkerCorners.resize(chessboardCorners.size());
|
||||
unsigned int nMarkers = (unsigned int)objPoints.size();
|
||||
unsigned int nCharucoCorners = (unsigned int)chessboardCorners.size();
|
||||
for(unsigned int i = 0; i < nCharucoCorners; i++) {
|
||||
double minDist = -1; // distance of closest markers
|
||||
Point3f charucoCorner = chessboardCorners[i];
|
||||
for(unsigned int j = 0; j < nMarkers; j++) {
|
||||
// calculate distance from marker center to charuco corner
|
||||
Point3f center = Point3f(0, 0, 0);
|
||||
for(unsigned int k = 0; k < 4; k++)
|
||||
center += objPoints[j][k];
|
||||
center /= 4.;
|
||||
double sqDistance;
|
||||
Point3f distVector = charucoCorner - center;
|
||||
sqDistance = distVector.x * distVector.x + distVector.y * distVector.y;
|
||||
if(j == 0 || fabs(sqDistance - minDist) < cv::pow(0.01 * squareLength, 2)) {
|
||||
// if same minimum distance (or first iteration), add to nearestMarkerIdx vector
|
||||
nearestMarkerIdx[i].push_back(j);
|
||||
minDist = sqDistance;
|
||||
} else if(sqDistance < minDist) {
|
||||
// if finding a closest marker to the charuco corner
|
||||
nearestMarkerIdx[i].clear(); // remove any previous added marker
|
||||
nearestMarkerIdx[i].push_back(j); // add the new closest marker index
|
||||
minDist = sqDistance;
|
||||
}
|
||||
}
|
||||
// for each of the closest markers, search the marker corner index closer
|
||||
// to the charuco corner
|
||||
for(unsigned int j = 0; j < nearestMarkerIdx[i].size(); j++) {
|
||||
nearestMarkerCorners[i].resize(nearestMarkerIdx[i].size());
|
||||
double minDistCorner = -1;
|
||||
for(unsigned int k = 0; k < 4; k++) {
|
||||
double sqDistance;
|
||||
Point3f distVector = charucoCorner - objPoints[nearestMarkerIdx[i][j]][k];
|
||||
sqDistance = distVector.x * distVector.x + distVector.y * distVector.y;
|
||||
if(k == 0 || sqDistance < minDistCorner) {
|
||||
// if this corner is closer to the charuco corner, assing its index
|
||||
// to nearestMarkerCorners
|
||||
minDistCorner = sqDistance;
|
||||
nearestMarkerCorners[i][j] = k;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CharucoBoard::generateImage(Size outSize, OutputArray _img, int marginSize, int borderBits) const {
|
||||
void CharucoBoardImpl::matchImagePoints(InputArrayOfArrays detectedCorners, InputArray detectedIds,
|
||||
OutputArray _objPoints, OutputArray imgPoints) const {
|
||||
if (detectedCorners.kind() == _InputArray::STD_VECTOR_VECTOR ||
|
||||
detectedCorners.isMatVector() || detectedCorners.isUMatVector())
|
||||
Board::Impl::matchImagePoints(detectedCorners, detectedIds, _objPoints, imgPoints);
|
||||
else {
|
||||
CV_Assert(detectedCorners.isMat() || detectedCorners.isVector());
|
||||
size_t nDetected = detectedCorners.total();
|
||||
vector<Point3f> objPnts(nDetected);
|
||||
vector<Point2f> imgPnts(nDetected);
|
||||
for(size_t i = 0ull; i < nDetected; i++) {
|
||||
int pointId = detectedIds.getMat().at<int>((int)i);
|
||||
CV_Assert(pointId >= 0 && pointId < (int)chessboardCorners.size());
|
||||
objPnts[i] = chessboardCorners[pointId];
|
||||
imgPnts[i] = detectedCorners.getMat().at<Point2f>((int)i);
|
||||
}
|
||||
Mat(objPnts).copyTo(_objPoints);
|
||||
Mat(imgPnts).copyTo(imgPoints);
|
||||
}
|
||||
}
|
||||
|
||||
void CharucoBoardImpl::generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const {
|
||||
CV_Assert(!outSize.empty());
|
||||
CV_Assert(marginSize >= 0);
|
||||
|
||||
_img.create(outSize, CV_8UC1);
|
||||
_img.setTo(255);
|
||||
Mat out = _img.getMat();
|
||||
img.create(outSize, CV_8UC1);
|
||||
img.setTo(255);
|
||||
Mat out = img.getMat();
|
||||
Mat noMarginsImg =
|
||||
out.colRange(marginSize, out.cols - marginSize).rowRange(marginSize, out.rows - marginSize);
|
||||
|
||||
double totalLengthX, totalLengthY;
|
||||
totalLengthX = charucoImpl->squareLength * charucoImpl->sizeX;
|
||||
totalLengthY = charucoImpl->squareLength * charucoImpl->sizeY;
|
||||
totalLengthX = squareLength * size.width;
|
||||
totalLengthY = squareLength * size.height;
|
||||
|
||||
// proportional transformation
|
||||
double xReduction = totalLengthX / double(noMarginsImg.cols);
|
||||
@@ -308,21 +421,21 @@ void CharucoBoard::generateImage(Size outSize, OutputArray _img, int marginSize,
|
||||
|
||||
// determine the margins to draw only the markers
|
||||
// take the minimum just to be sure
|
||||
double squareSizePixels = min(double(chessboardZoneImg.cols) / double(charucoImpl->sizeX),
|
||||
double(chessboardZoneImg.rows) / double(charucoImpl->sizeY));
|
||||
double squareSizePixels = min(double(chessboardZoneImg.cols) / double(size.width),
|
||||
double(chessboardZoneImg.rows) / double(size.height));
|
||||
|
||||
double diffSquareMarkerLength = (charucoImpl->squareLength - charucoImpl->markerLength) / 2;
|
||||
double diffSquareMarkerLength = (squareLength - markerLength) / 2;
|
||||
int diffSquareMarkerLengthPixels =
|
||||
int(diffSquareMarkerLength * squareSizePixels / charucoImpl->squareLength);
|
||||
int(diffSquareMarkerLength * squareSizePixels / squareLength);
|
||||
|
||||
// draw markers
|
||||
Mat markersImg;
|
||||
Board::generateImage(chessboardZoneImg.size(), markersImg, diffSquareMarkerLengthPixels, borderBits);
|
||||
Board::Impl::generateImage(chessboardZoneImg.size(), markersImg, diffSquareMarkerLengthPixels, borderBits);
|
||||
markersImg.copyTo(chessboardZoneImg);
|
||||
|
||||
// now draw black squares
|
||||
for(int y = 0; y < charucoImpl->sizeY; y++) {
|
||||
for(int x = 0; x < charucoImpl->sizeX; x++) {
|
||||
for(int y = 0; y < size.height; y++) {
|
||||
for(int x = 0; x < size.width; x++) {
|
||||
|
||||
if(y % 2 != x % 2) continue; // white corner, dont do anything
|
||||
|
||||
@@ -338,78 +451,22 @@ void CharucoBoard::generateImage(Size outSize, OutputArray _img, int marginSize,
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Fill nearestMarkerIdx and nearestMarkerCorners arrays
|
||||
*/
|
||||
void CharucoBoard::CharucoImpl::_getNearestMarkerCorners(CharucoBoard &board, float squareLength) {
|
||||
board.charucoImpl->nearestMarkerIdx.resize(board.charucoImpl->chessboardCorners.size());
|
||||
board.charucoImpl->nearestMarkerCorners.resize(board.charucoImpl->chessboardCorners.size());
|
||||
CharucoBoard::CharucoBoard(){}
|
||||
|
||||
unsigned int nMarkers = (unsigned int)board.getIds().size();
|
||||
unsigned int nCharucoCorners = (unsigned int)board.charucoImpl->chessboardCorners.size();
|
||||
for(unsigned int i = 0; i < nCharucoCorners; i++) {
|
||||
double minDist = -1; // distance of closest markers
|
||||
Point3f charucoCorner = board.charucoImpl->chessboardCorners[i];
|
||||
for(unsigned int j = 0; j < nMarkers; j++) {
|
||||
// calculate distance from marker center to charuco corner
|
||||
Point3f center = Point3f(0, 0, 0);
|
||||
for(unsigned int k = 0; k < 4; k++)
|
||||
center += board.getObjPoints()[j][k];
|
||||
center /= 4.;
|
||||
double sqDistance;
|
||||
Point3f distVector = charucoCorner - center;
|
||||
sqDistance = distVector.x * distVector.x + distVector.y * distVector.y;
|
||||
if(j == 0 || fabs(sqDistance - minDist) < cv::pow(0.01 * squareLength, 2)) {
|
||||
// if same minimum distance (or first iteration), add to nearestMarkerIdx vector
|
||||
board.charucoImpl->nearestMarkerIdx[i].push_back(j);
|
||||
minDist = sqDistance;
|
||||
} else if(sqDistance < minDist) {
|
||||
// if finding a closest marker to the charuco corner
|
||||
board.charucoImpl->nearestMarkerIdx[i].clear(); // remove any previous added marker
|
||||
board.charucoImpl->nearestMarkerIdx[i].push_back(j); // add the new closest marker index
|
||||
minDist = sqDistance;
|
||||
}
|
||||
}
|
||||
// for each of the closest markers, search the marker corner index closer
|
||||
// to the charuco corner
|
||||
for(unsigned int j = 0; j < board.charucoImpl->nearestMarkerIdx[i].size(); j++) {
|
||||
board.charucoImpl->nearestMarkerCorners[i].resize(board.charucoImpl->nearestMarkerIdx[i].size());
|
||||
double minDistCorner = -1;
|
||||
for(unsigned int k = 0; k < 4; k++) {
|
||||
double sqDistance;
|
||||
Point3f distVector = charucoCorner - board.getObjPoints()[board.charucoImpl->nearestMarkerIdx[i][j]][k];
|
||||
sqDistance = distVector.x * distVector.x + distVector.y * distVector.y;
|
||||
if(k == 0 || sqDistance < minDistCorner) {
|
||||
// if this corner is closer to the charuco corner, assing its index
|
||||
// to nearestMarkerCorners
|
||||
minDistCorner = sqDistance;
|
||||
board.charucoImpl->nearestMarkerCorners[i][j] = k;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
CharucoBoard::CharucoBoard(const Size& size, float squareLength, float markerLength,
|
||||
const Dictionary &dictionary, InputArray ids):
|
||||
Board(new CharucoBoardImpl(dictionary, size, squareLength, markerLength)) {
|
||||
|
||||
Ptr<CharucoBoard> CharucoBoard::create(int squaresX, int squaresY, float squareLength, float markerLength,
|
||||
const Dictionary &dictionary, InputArray ids) {
|
||||
CV_Assert(squaresX > 1 && squaresY > 1 && markerLength > 0 && squareLength > markerLength);
|
||||
CharucoBoard board;
|
||||
Ptr<CharucoBoard> res = makePtr<CharucoBoard>(board);
|
||||
CV_Assert(size.width > 1 && size.height > 1 && markerLength > 0 && squareLength > markerLength);
|
||||
|
||||
res->charucoImpl->sizeX = squaresX;
|
||||
res->charucoImpl->sizeY = squaresY;
|
||||
res->charucoImpl->squareLength = squareLength;
|
||||
res->charucoImpl->markerLength = markerLength;
|
||||
res->boardImpl->dictionary = dictionary;
|
||||
vector<vector<Point3f> > objPoints;
|
||||
|
||||
float diffSquareMarkerLength = (squareLength - markerLength) / 2;
|
||||
int totalMarkers = (int)(ids.total());
|
||||
ids.copyTo(res->boardImpl->ids);
|
||||
ids.copyTo(impl->ids);
|
||||
// calculate Board objPoints
|
||||
int nextId = 0;
|
||||
for(int y = 0; y < squaresY; y++) {
|
||||
for(int x = 0; x < squaresX; x++) {
|
||||
for(int y = 0; y < size.height; y++) {
|
||||
for(int x = 0; x < size.width; x++) {
|
||||
|
||||
if(y % 2 == x % 2) continue; // black corner, no marker here
|
||||
|
||||
@@ -422,48 +479,60 @@ Ptr<CharucoBoard> CharucoBoard::create(int squaresX, int squaresY, float squareL
|
||||
objPoints.push_back(corners);
|
||||
// first ids in dictionary
|
||||
if (totalMarkers == 0)
|
||||
res->boardImpl->ids.push_back(nextId);
|
||||
impl->ids.push_back(nextId);
|
||||
nextId++;
|
||||
}
|
||||
}
|
||||
if (totalMarkers > 0 && nextId != totalMarkers)
|
||||
CV_Error(cv::Error::StsBadSize, "Size of ids must be equal to the number of markers: "+std::to_string(nextId));
|
||||
res->boardImpl->objPoints = objPoints;
|
||||
impl->objPoints = objPoints;
|
||||
|
||||
// now fill chessboardCorners
|
||||
for(int y = 0; y < squaresY - 1; y++) {
|
||||
for(int x = 0; x < squaresX - 1; x++) {
|
||||
std::vector<Point3f> & c = static_pointer_cast<CharucoBoardImpl>(impl)->chessboardCorners;
|
||||
for(int y = 0; y < size.height - 1; y++) {
|
||||
for(int x = 0; x < size.width - 1; x++) {
|
||||
Point3f corner;
|
||||
corner.x = (x + 1) * squareLength;
|
||||
corner.y = (y + 1) * squareLength;
|
||||
corner.z = 0;
|
||||
res->charucoImpl->chessboardCorners.push_back(corner);
|
||||
c.push_back(corner);
|
||||
}
|
||||
}
|
||||
res->boardImpl->rightBottomBorder = Point3f(squaresX * squareLength, squaresY * squareLength, 0.f);
|
||||
CharucoBoard::CharucoImpl::_getNearestMarkerCorners(*res, res->charucoImpl->squareLength);
|
||||
return res;
|
||||
impl->rightBottomBorder = Point3f(size.width * squareLength, size.height * squareLength, 0.f);
|
||||
static_pointer_cast<CharucoBoardImpl>(impl)->calcNearestMarkerCorners();
|
||||
}
|
||||
|
||||
Size CharucoBoard::getChessboardSize() const { return Size(charucoImpl->sizeX, charucoImpl->sizeY); }
|
||||
Size CharucoBoard::getChessboardSize() const {
|
||||
CV_Assert(impl);
|
||||
return static_pointer_cast<CharucoBoardImpl>(impl)->size;
|
||||
}
|
||||
|
||||
float CharucoBoard::getSquareLength() const { return charucoImpl->squareLength; }
|
||||
float CharucoBoard::getSquareLength() const {
|
||||
CV_Assert(impl);
|
||||
return static_pointer_cast<CharucoBoardImpl>(impl)->squareLength;
|
||||
}
|
||||
|
||||
float CharucoBoard::getMarkerLength() const { return charucoImpl->markerLength; }
|
||||
float CharucoBoard::getMarkerLength() const {
|
||||
CV_Assert(impl);
|
||||
return static_pointer_cast<CharucoBoardImpl>(impl)->markerLength;
|
||||
}
|
||||
|
||||
bool CharucoBoard::checkCharucoCornersCollinear(InputArray charucoIds) const {
|
||||
CV_Assert(impl);
|
||||
|
||||
unsigned int nCharucoCorners = (unsigned int)charucoIds.getMat().total();
|
||||
if (nCharucoCorners <= 2)
|
||||
return true;
|
||||
|
||||
// only test if there are 3 or more corners
|
||||
CV_Assert(charucoImpl->chessboardCorners.size() >= charucoIds.getMat().total());
|
||||
auto board = static_pointer_cast<CharucoBoardImpl>(impl);
|
||||
CV_Assert(board->chessboardCorners.size() >= charucoIds.getMat().total());
|
||||
|
||||
Vec<double, 3> point0(charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(0)].x,
|
||||
charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(0)].y, 1);
|
||||
Vec<double, 3> point0(board->chessboardCorners[charucoIds.getMat().at<int>(0)].x,
|
||||
board->chessboardCorners[charucoIds.getMat().at<int>(0)].y, 1);
|
||||
|
||||
Vec<double, 3> point1(charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(1)].x,
|
||||
charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(1)].y, 1);
|
||||
Vec<double, 3> point1(board->chessboardCorners[charucoIds.getMat().at<int>(1)].x,
|
||||
board->chessboardCorners[charucoIds.getMat().at<int>(1)].y, 1);
|
||||
|
||||
// create a line from the first two points.
|
||||
Vec<double, 3> testLine = point0.cross(point1);
|
||||
@@ -477,8 +546,8 @@ bool CharucoBoard::checkCharucoCornersCollinear(InputArray charucoIds) const {
|
||||
|
||||
double dotProduct;
|
||||
for (unsigned int i = 2; i < nCharucoCorners; i++){
|
||||
testPoint(0) = charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(i)].x;
|
||||
testPoint(1) = charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(i)].y;
|
||||
testPoint(0) = board->chessboardCorners[charucoIds.getMat().at<int>(i)].x;
|
||||
testPoint(1) = board->chessboardCorners[charucoIds.getMat().at<int>(i)].y;
|
||||
|
||||
// if testPoint is on testLine, dotProduct will be zero (or very, very close)
|
||||
dotProduct = testPoint.dot(testLine);
|
||||
@@ -492,15 +561,18 @@ bool CharucoBoard::checkCharucoCornersCollinear(InputArray charucoIds) const {
|
||||
}
|
||||
|
||||
std::vector<Point3f> CharucoBoard::getChessboardCorners() const {
|
||||
return charucoImpl->chessboardCorners;
|
||||
CV_Assert(impl);
|
||||
return static_pointer_cast<CharucoBoardImpl>(impl)->chessboardCorners;
|
||||
}
|
||||
|
||||
std::vector<std::vector<int> > CharucoBoard::getNearestMarkerIdx() const {
|
||||
return charucoImpl->nearestMarkerIdx;
|
||||
CV_Assert(impl);
|
||||
return static_pointer_cast<CharucoBoardImpl>(impl)->nearestMarkerIdx;
|
||||
}
|
||||
|
||||
std::vector<std::vector<int> > CharucoBoard::getNearestMarkerCorners() const {
|
||||
return charucoImpl->nearestMarkerCorners;
|
||||
CV_Assert(impl);
|
||||
return static_pointer_cast<CharucoBoardImpl>(impl)->nearestMarkerCorners;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -856,7 +856,7 @@ ArucoDetector::ArucoDetector(const Dictionary &_dictionary,
|
||||
}
|
||||
|
||||
void ArucoDetector::detectMarkers(InputArray _image, OutputArrayOfArrays _corners, OutputArray _ids,
|
||||
OutputArrayOfArrays _rejectedImgPoints) {
|
||||
OutputArrayOfArrays _rejectedImgPoints) const {
|
||||
CV_Assert(!_image.empty());
|
||||
DetectorParameters& detectorParams = arucoDetectorImpl->detectorParams;
|
||||
const Dictionary& dictionary = arucoDetectorImpl->dictionary;
|
||||
@@ -994,37 +994,25 @@ void ArucoDetector::detectMarkers(InputArray _image, OutputArrayOfArrays _corner
|
||||
/**
|
||||
* Project board markers that are not included in the list of detected markers
|
||||
*/
|
||||
static void _projectUndetectedMarkers(const Ptr<Board> &_board, InputOutputArrayOfArrays _detectedCorners,
|
||||
InputOutputArray _detectedIds, InputArray _cameraMatrix, InputArray _distCoeffs,
|
||||
vector<vector<Point2f> >& _undetectedMarkersProjectedCorners,
|
||||
OutputArray _undetectedMarkersIds) {
|
||||
// first estimate board pose with the current avaible markers
|
||||
Mat rvec, tvec;
|
||||
int boardDetectedMarkers = 0;
|
||||
{
|
||||
CV_Assert(_detectedCorners.total() == _detectedIds.total());
|
||||
// get object and image points for the solvePnP function
|
||||
Mat detectedObjPoints, imgPoints;
|
||||
_board->matchImagePoints(_detectedCorners, _detectedIds, detectedObjPoints, imgPoints);
|
||||
CV_Assert(imgPoints.total() == detectedObjPoints.total());
|
||||
if(detectedObjPoints.total() > 0) // 0 of the detected markers in board
|
||||
{
|
||||
solvePnP(detectedObjPoints, imgPoints, _cameraMatrix, _distCoeffs, rvec, tvec);
|
||||
// divide by four since all the four corners are concatenated in the array for each marker
|
||||
boardDetectedMarkers = static_cast<int>(detectedObjPoints.total()) / 4;
|
||||
}
|
||||
}
|
||||
|
||||
// at least one marker from board so rvec and tvec are valid
|
||||
if(boardDetectedMarkers == 0) return;
|
||||
static inline void _projectUndetectedMarkers(const Board &board, InputOutputArrayOfArrays detectedCorners,
|
||||
InputOutputArray detectedIds, InputArray cameraMatrix, InputArray distCoeffs,
|
||||
vector<vector<Point2f> >& undetectedMarkersProjectedCorners,
|
||||
OutputArray undetectedMarkersIds) {
|
||||
Mat rvec, tvec; // first estimate board pose with the current avaible markers
|
||||
Mat objPoints, imgPoints; // object and image points for the solvePnP function
|
||||
board.matchImagePoints(detectedCorners, detectedIds, objPoints, imgPoints);
|
||||
if (objPoints.total() < 4ull) // at least one marker from board so rvec and tvec are valid
|
||||
return;
|
||||
solvePnP(objPoints, imgPoints, cameraMatrix, distCoeffs, rvec, tvec);
|
||||
|
||||
// search undetected markers and project them using the previous pose
|
||||
vector<vector<Point2f> > undetectedCorners;
|
||||
const std::vector<int>& ids = board.getIds();
|
||||
vector<int> undetectedIds;
|
||||
for(unsigned int i = 0; i < _board->getIds().size(); i++) {
|
||||
for(unsigned int i = 0; i < ids.size(); i++) {
|
||||
int foundIdx = -1;
|
||||
for(unsigned int j = 0; j < _detectedIds.total(); j++) {
|
||||
if(_board->getIds()[i] == _detectedIds.getMat().ptr<int>()[j]) {
|
||||
for(unsigned int j = 0; j < detectedIds.total(); j++) {
|
||||
if(ids[i] == detectedIds.getMat().ptr<int>()[j]) {
|
||||
foundIdx = j;
|
||||
break;
|
||||
}
|
||||
@@ -1033,31 +1021,31 @@ static void _projectUndetectedMarkers(const Ptr<Board> &_board, InputOutputArray
|
||||
// not detected
|
||||
if(foundIdx == -1) {
|
||||
undetectedCorners.push_back(vector<Point2f>());
|
||||
undetectedIds.push_back(_board->getIds()[i]);
|
||||
projectPoints(_board->getObjPoints()[i], rvec, tvec, _cameraMatrix, _distCoeffs,
|
||||
undetectedIds.push_back(ids[i]);
|
||||
projectPoints(board.getObjPoints()[i], rvec, tvec, cameraMatrix, distCoeffs,
|
||||
undetectedCorners.back());
|
||||
}
|
||||
}
|
||||
// parse output
|
||||
Mat(undetectedIds).copyTo(_undetectedMarkersIds);
|
||||
_undetectedMarkersProjectedCorners = undetectedCorners;
|
||||
Mat(undetectedIds).copyTo(undetectedMarkersIds);
|
||||
undetectedMarkersProjectedCorners = undetectedCorners;
|
||||
}
|
||||
|
||||
/**
|
||||
* Interpolate board markers that are not included in the list of detected markers using
|
||||
* global homography
|
||||
*/
|
||||
static void _projectUndetectedMarkers(const Ptr<Board> &_board, InputOutputArrayOfArrays _detectedCorners,
|
||||
static void _projectUndetectedMarkers(const Board &_board, InputOutputArrayOfArrays _detectedCorners,
|
||||
InputOutputArray _detectedIds,
|
||||
vector<vector<Point2f> >& _undetectedMarkersProjectedCorners,
|
||||
OutputArray _undetectedMarkersIds) {
|
||||
// check board points are in the same plane, if not, global homography cannot be applied
|
||||
CV_Assert(_board->getObjPoints().size() > 0);
|
||||
CV_Assert(_board->getObjPoints()[0].size() > 0);
|
||||
float boardZ = _board->getObjPoints()[0][0].z;
|
||||
for(unsigned int i = 0; i < _board->getObjPoints().size(); i++) {
|
||||
for(unsigned int j = 0; j < _board->getObjPoints()[i].size(); j++)
|
||||
CV_Assert(boardZ == _board->getObjPoints()[i][j].z);
|
||||
CV_Assert(_board.getObjPoints().size() > 0);
|
||||
CV_Assert(_board.getObjPoints()[0].size() > 0);
|
||||
float boardZ = _board.getObjPoints()[0][0].z;
|
||||
for(unsigned int i = 0; i < _board.getObjPoints().size(); i++) {
|
||||
for(unsigned int j = 0; j < _board.getObjPoints()[i].size(); j++)
|
||||
CV_Assert(boardZ == _board.getObjPoints()[i][j].z);
|
||||
}
|
||||
|
||||
vector<Point2f> detectedMarkersObj2DAll; // Object coordinates (without Z) of all the detected
|
||||
@@ -1067,14 +1055,14 @@ static void _projectUndetectedMarkers(const Ptr<Board> &_board, InputOutputArray
|
||||
// missing markers in different vectors
|
||||
vector<int> undetectedMarkersIds; // ids of missing markers
|
||||
// find markers included in board, and missing markers from board. Fill the previous vectors
|
||||
for(unsigned int j = 0; j < _board->getIds().size(); j++) {
|
||||
for(unsigned int j = 0; j < _board.getIds().size(); j++) {
|
||||
bool found = false;
|
||||
for(unsigned int i = 0; i < _detectedIds.total(); i++) {
|
||||
if(_detectedIds.getMat().ptr<int>()[i] == _board->getIds()[j]) {
|
||||
if(_detectedIds.getMat().ptr<int>()[i] == _board.getIds()[j]) {
|
||||
for(int c = 0; c < 4; c++) {
|
||||
imageCornersAll.push_back(_detectedCorners.getMat(i).ptr<Point2f>()[c]);
|
||||
detectedMarkersObj2DAll.push_back(
|
||||
Point2f(_board->getObjPoints()[j][c].x, _board->getObjPoints()[j][c].y));
|
||||
Point2f(_board.getObjPoints()[j][c].x, _board.getObjPoints()[j][c].y));
|
||||
}
|
||||
found = true;
|
||||
break;
|
||||
@@ -1084,9 +1072,9 @@ static void _projectUndetectedMarkers(const Ptr<Board> &_board, InputOutputArray
|
||||
undetectedMarkersObj2D.push_back(vector<Point2f>());
|
||||
for(int c = 0; c < 4; c++) {
|
||||
undetectedMarkersObj2D.back().push_back(
|
||||
Point2f(_board->getObjPoints()[j][c].x, _board->getObjPoints()[j][c].y));
|
||||
Point2f(_board.getObjPoints()[j][c].x, _board.getObjPoints()[j][c].y));
|
||||
}
|
||||
undetectedMarkersIds.push_back(_board->getIds()[j]);
|
||||
undetectedMarkersIds.push_back(_board.getIds()[j]);
|
||||
}
|
||||
}
|
||||
if(imageCornersAll.size() == 0) return;
|
||||
@@ -1103,10 +1091,10 @@ static void _projectUndetectedMarkers(const Ptr<Board> &_board, InputOutputArray
|
||||
Mat(undetectedMarkersIds).copyTo(_undetectedMarkersIds);
|
||||
}
|
||||
|
||||
void ArucoDetector::refineDetectedMarkers(InputArray _image, const Ptr<Board> &_board,
|
||||
void ArucoDetector::refineDetectedMarkers(InputArray _image, const Board& _board,
|
||||
InputOutputArrayOfArrays _detectedCorners, InputOutputArray _detectedIds,
|
||||
InputOutputArrayOfArrays _rejectedCorners, InputArray _cameraMatrix,
|
||||
InputArray _distCoeffs, OutputArray _recoveredIdxs) {
|
||||
InputArray _distCoeffs, OutputArray _recoveredIdxs) const {
|
||||
DetectorParameters& detectorParams = arucoDetectorImpl->detectorParams;
|
||||
const Dictionary& dictionary = arucoDetectorImpl->dictionary;
|
||||
RefineParameters& refineParams = arucoDetectorImpl->refineParams;
|
||||
|
||||
@@ -0,0 +1,521 @@
|
||||
// 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
|
||||
|
||||
#include "../precomp.hpp"
|
||||
|
||||
#include <opencv2/calib3d.hpp>
|
||||
#include "opencv2/objdetect/charuco_detector.hpp"
|
||||
#include "aruco_utils.hpp"
|
||||
|
||||
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)
|
||||
{}
|
||||
|
||||
/** 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
|
||||
printf("before board.matchImagePoints(markerCorners, markerIds, objPoints, imgPoints);\n");
|
||||
board.matchImagePoints(markerCorners, markerIds, objPoints, imgPoints);
|
||||
printf("after board.matchImagePoints(markerCorners, markerIds, objPoints, imgPoints);\n");
|
||||
if (objPoints.total() < 4ull) // need, at least, 4 corners
|
||||
return;
|
||||
|
||||
solvePnP(objPoints, imgPoints, charucoParameters.cameraMatrix, charucoParameters.distCoeffs, approximatedRvec, approximatedTvec);
|
||||
printf("after solvePnP\n");
|
||||
|
||||
// project chessboard corners
|
||||
vector<Point2f> allChessboardImgPoints;
|
||||
projectPoints(board.getChessboardCorners(), approximatedRvec, approximatedTvec, charucoParameters.cameraMatrix,
|
||||
charucoParameters.distCoeffs, allChessboardImgPoints);
|
||||
printf("after projectPoints\n");
|
||||
// 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
|
||||
printf("before selectAndRefineChessboardCorners\n");
|
||||
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();
|
||||
}
|
||||
};
|
||||
|
||||
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 {
|
||||
CV_Assert((markerCorners.empty() && markerIds.empty() && !image.empty()) || (markerCorners.size() == markerIds.size()));
|
||||
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;
|
||||
charucoDetectorImpl->arucoDetector.detectMarkers(image, _markerCorners, _markerIds, rejectedMarkers);
|
||||
if (charucoDetectorImpl->charucoParameters.tryRefineMarkers)
|
||||
charucoDetectorImpl->arucoDetector.refineDetectedMarkers(image, charucoDetectorImpl->board, _markerCorners,
|
||||
_markerIds, rejectedMarkers);
|
||||
}
|
||||
// if camera parameters are avaible, use approximated calibration
|
||||
if(!charucoDetectorImpl->charucoParameters.cameraMatrix.empty())
|
||||
charucoDetectorImpl->interpolateCornersCharucoApproxCalib(_markerCorners, _markerIds, image, charucoCorners,
|
||||
charucoIds);
|
||||
// else use local homography
|
||||
else
|
||||
charucoDetectorImpl->interpolateCornersCharucoLocalHom(_markerCorners, _markerIds, image, charucoCorners,
|
||||
charucoIds);
|
||||
// to return a charuco corner, its closest aruco markers should have been detected
|
||||
charucoDetectorImpl->filterCornersWithoutMinMarkers(charucoCorners, charucoIds, _markerIds, charucoCorners,
|
||||
charucoIds);
|
||||
}
|
||||
|
||||
void CharucoDetector::detectDiamonds(InputArray image, OutputArrayOfArrays _diamondCorners, OutputArray _diamondIds,
|
||||
InputOutputArrayOfArrays inMarkerCorners, InputOutputArrayOfArrays inMarkerIds) const {
|
||||
CV_Assert(getBoard().getChessboardSize() == Size(3, 3));
|
||||
CV_Assert((inMarkerCorners.empty() && inMarkerIds.empty() && !image.empty()) || (inMarkerCorners.size() == inMarkerIds.size()));
|
||||
|
||||
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;
|
||||
detectBoard(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.getMat().total() == _charucoIds.getMat().total()) ||
|
||||
_charucoIds.getMat().total() == 0);
|
||||
|
||||
size_t nCorners = _charucoCorners.getMat().total();
|
||||
for(size_t i = 0; i < nCorners; i++) {
|
||||
Point2f corner = _charucoCorners.getMat().at<Point2f>((int)i);
|
||||
// draw first corner mark
|
||||
rectangle(_image, corner - Point2f(3, 3), corner + Point2f(3, 3), cornerColor, 1, LINE_AA);
|
||||
// draw ID
|
||||
if(!_charucoIds.empty()) {
|
||||
int id = _charucoIds.getMat().at<int>((int)i);
|
||||
stringstream s;
|
||||
s << "id=" << id;
|
||||
putText(_image, s.str(), corner + Point2f(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.type() == CV_32FC2);
|
||||
|
||||
// draw marker sides
|
||||
for(int j = 0; j < 4; j++) {
|
||||
Point2f p0, p1;
|
||||
p0 = currentMarker.at< Point2f >(j);
|
||||
p1 = currentMarker.at< Point2f >((j + 1) % 4);
|
||||
line(_image, p0, p1, borderColor, 1);
|
||||
}
|
||||
|
||||
// draw first corner mark
|
||||
rectangle(_image, currentMarker.at< Point2f >(0) - Point2f(3, 3),
|
||||
currentMarker.at< Point2f >(0) + Point2f(3, 3), cornerColor, 1, LINE_AA);
|
||||
|
||||
// draw id composed by four numbers
|
||||
if(_ids.total() != 0) {
|
||||
Point2f cent(0, 0);
|
||||
for(int p = 0; p < 4; p++)
|
||||
cent += currentMarker.at< Point2f >(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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user