opencv/opencv_contrib
1
0
Fork 0
mirror of https://github.com/opencv/opencv_contrib.git synced 2025-10-22 16:08:41 +08:00
Code Issues Releases Wiki Activity

Merge pull request #769 from sovrasov:aruco_fix

Browse Source
This commit is contained in:
Vadim Pisarevsky
2016-09-20 19:52:58 +00:00
parent 0ea4ee2514 348dfeda05
commit 67a305ad6e
1 changed files with 31 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
modules/aruco/src/charuco.cpp
View File

@@ -243,14 +243,14 @@ static unsigned int _filterCornersWithoutMinMarkers(Ptr<CharucoBoard> &_board,
vector< int > filteredCharucoIds; vector< int > filteredCharucoIds;
// for each charuco corner // for each charuco corner
for(unsigned int i = 0; i < _allCharucoIds.getMat().total(); i++) { for(unsigned int i = 0; i < _allCharucoIds.getMat().total(); i++) {
int currentCharucoId = _allCharucoIds.getMat().ptr< int >(0)[i]; int currentCharucoId = _allCharucoIds.getMat().at< int >(i);
int totalMarkers = 0; // nomber of closest marker detected int totalMarkers = 0; // nomber of closest marker detected
// look for closest markers // look for closest markers
for(unsigned int m = 0; m < _board->nearestMarkerIdx[currentCharucoId].size(); m++) { for(unsigned int m = 0; m < _board->nearestMarkerIdx[currentCharucoId].size(); m++) {
int markerId = _board->ids[_board->nearestMarkerIdx[currentCharucoId][m]]; int markerId = _board->ids[_board->nearestMarkerIdx[currentCharucoId][m]];
bool found = false; bool found = false;
for(unsigned int k = 0; k < _allArucoIds.getMat().total(); k++) { for(unsigned int k = 0; k < _allArucoIds.getMat().total(); k++) {
if(_allArucoIds.getMat().ptr< int >(0)[k] == markerId) { if(_allArucoIds.getMat().at< int >(k) == markerId) {
found = true; found = true;
break; break;
} }
@@ -260,19 +260,19 @@ static unsigned int _filterCornersWithoutMinMarkers(Ptr<CharucoBoard> &_board,
// if enough markers detected, add the charuco corner to the final list // if enough markers detected, add the charuco corner to the final list
if(totalMarkers >= minMarkers) { if(totalMarkers >= minMarkers) {
filteredCharucoIds.push_back(currentCharucoId); filteredCharucoIds.push_back(currentCharucoId);
filteredCharucoCorners.push_back(_allCharucoCorners.getMat().ptr< Point2f >(0)[i]); filteredCharucoCorners.push_back(_allCharucoCorners.getMat().at< Point2f >(i));
} }
} }
// parse output // parse output
_filteredCharucoCorners.create((int)filteredCharucoCorners.size(), 1, CV_32FC2); _filteredCharucoCorners.create((int)filteredCharucoCorners.size(), 1, CV_32FC2);
for(unsigned int i = 0; i < filteredCharucoCorners.size(); i++) { for(unsigned int i = 0; i < filteredCharucoCorners.size(); i++) {
_filteredCharucoCorners.getMat().ptr< Point2f >(0)[i] = filteredCharucoCorners[i]; _filteredCharucoCorners.getMat().at< Point2f >(i) = filteredCharucoCorners[i];
} }
_filteredCharucoIds.create((int)filteredCharucoIds.size(), 1, CV_32SC1); _filteredCharucoIds.create((int)filteredCharucoIds.size(), 1, CV_32SC1);
for(unsigned int i = 0; i < filteredCharucoIds.size(); i++) { for(unsigned int i = 0; i < filteredCharucoIds.size(); i++) {
_filteredCharucoIds.getMat().ptr< int >(0)[i] = filteredCharucoIds[i]; _filteredCharucoIds.getMat().at< int >(i) = filteredCharucoIds[i];
} }
return (unsigned int)filteredCharucoCorners.size(); return (unsigned int)filteredCharucoCorners.size();
@@ -341,8 +341,8 @@ static unsigned int _selectAndRefineChessboardCorners(InputArray _allCorners, In
Rect innerRect(minDistToBorder, minDistToBorder, _image.getMat().cols - 2 * minDistToBorder, Rect innerRect(minDistToBorder, minDistToBorder, _image.getMat().cols - 2 * minDistToBorder,
_image.getMat().rows - 2 * minDistToBorder); _image.getMat().rows - 2 * minDistToBorder);
for(unsigned int i = 0; i < _allCorners.getMat().total(); i++) { for(unsigned int i = 0; i < _allCorners.getMat().total(); i++) {
if(innerRect.contains(_allCorners.getMat().ptr< Point2f >(0)[i])) { if(innerRect.contains(_allCorners.getMat().at< Point2f >(i))) {
filteredChessboardImgPoints.push_back(_allCorners.getMat().ptr< Point2f >(0)[i]); filteredChessboardImgPoints.push_back(_allCorners.getMat().at< Point2f >(i));
filteredIds.push_back(i); filteredIds.push_back(i);
filteredWinSizes.push_back(winSizes[i]); filteredWinSizes.push_back(winSizes[i]);
} }
@@ -382,12 +382,12 @@ static unsigned int _selectAndRefineChessboardCorners(InputArray _allCorners, In
// parse output // parse output
_selectedCorners.create((int)filteredChessboardImgPoints.size(), 1, CV_32FC2); _selectedCorners.create((int)filteredChessboardImgPoints.size(), 1, CV_32FC2);
for(unsigned int i = 0; i < filteredChessboardImgPoints.size(); i++) { for(unsigned int i = 0; i < filteredChessboardImgPoints.size(); i++) {
_selectedCorners.getMat().ptr< Point2f >(0)[i] = filteredChessboardImgPoints[i]; _selectedCorners.getMat().at< Point2f >(i) = filteredChessboardImgPoints[i];
} }
_selectedIds.create((int)filteredIds.size(), 1, CV_32SC1); _selectedIds.create((int)filteredIds.size(), 1, CV_32SC1);
for(unsigned int i = 0; i < filteredIds.size(); i++) { for(unsigned int i = 0; i < filteredIds.size(); i++) {
_selectedIds.getMat().ptr< int >(0)[i] = filteredIds[i]; _selectedIds.getMat().at< int >(i) = filteredIds[i];
} }
return (unsigned int)filteredChessboardImgPoints.size(); return (unsigned int)filteredChessboardImgPoints.size();
@@ -406,7 +406,7 @@ static void _getMaximumSubPixWindowSizes(InputArrayOfArrays markerCorners, Input
sizes.resize(nCharucoCorners, Size(-1, -1)); sizes.resize(nCharucoCorners, Size(-1, -1));
for(unsigned int i = 0; i < nCharucoCorners; i++) { for(unsigned int i = 0; i < nCharucoCorners; i++) {
if(charucoCorners.getMat().ptr< Point2f >(0)[i] == Point2f(-1, -1)) continue; if(charucoCorners.getMat().at< Point2f >(i) == Point2f(-1, -1)) continue;
if(board->nearestMarkerIdx[i].size() == 0) continue; if(board->nearestMarkerIdx[i].size() == 0) continue;
double minDist = -1; double minDist = -1;
@@ -418,15 +418,15 @@ static void _getMaximumSubPixWindowSizes(InputArrayOfArrays markerCorners, Input
int markerId = board->ids[board->nearestMarkerIdx[i][j]]; int markerId = board->ids[board->nearestMarkerIdx[i][j]];
int markerIdx = -1; int markerIdx = -1;
for(unsigned int k = 0; k < markerIds.getMat().total(); k++) { for(unsigned int k = 0; k < markerIds.getMat().total(); k++) {
if(markerIds.getMat().ptr< int >(0)[k] == markerId) { if(markerIds.getMat().at< int >(k) == markerId) {
markerIdx = k; markerIdx = k;
break; break;
} }
} }
if(markerIdx == -1) continue; if(markerIdx == -1) continue;
Point2f markerCorner = Point2f markerCorner =
markerCorners.getMat(markerIdx).ptr< Point2f >(0)[board->nearestMarkerCorners[i][j]]; markerCorners.getMat(markerIdx).at< Point2f >(board->nearestMarkerCorners[i][j]);
Point2f charucoCorner = charucoCorners.getMat().ptr< Point2f >(0)[i]; Point2f charucoCorner = charucoCorners.getMat().at< Point2f >(i);
double dist = norm(markerCorner - charucoCorner); double dist = norm(markerCorner - charucoCorner);
if(minDist == -1) minDist = dist; // if first distance, just assign it if(minDist == -1) minDist = dist; // if first distance, just assign it
minDist = min(dist, minDist); minDist = min(dist, minDist);
@@ -519,7 +519,7 @@ static int _interpolateCornersCharucoLocalHom(InputArrayOfArrays _markerCorners,
transformations.resize(nMarkers); transformations.resize(nMarkers);
for(unsigned int i = 0; i < nMarkers; i++) { for(unsigned int i = 0; i < nMarkers; i++) {
vector< Point2f > markerObjPoints2D; vector< Point2f > markerObjPoints2D;
int markerId = _markerIds.getMat().ptr< int >(0)[i]; int markerId = _markerIds.getMat().at< int >(i);
vector< int >::const_iterator it = find(_board->ids.begin(), _board->ids.end(), markerId); vector< int >::const_iterator it = find(_board->ids.begin(), _board->ids.end(), markerId);
if(it == _board->ids.end()) continue; if(it == _board->ids.end()) continue;
int boardIdx = (int)std::distance<std::vector<int>::const_iterator>(_board->ids.begin(), it); int boardIdx = (int)std::distance<std::vector<int>::const_iterator>(_board->ids.begin(), it);
@@ -544,7 +544,7 @@ static int _interpolateCornersCharucoLocalHom(InputArrayOfArrays _markerCorners,
int markerId = _board->ids[_board->nearestMarkerIdx[i][j]]; int markerId = _board->ids[_board->nearestMarkerIdx[i][j]];
int markerIdx = -1; int markerIdx = -1;
for(unsigned int k = 0; k < _markerIds.getMat().total(); k++) { for(unsigned int k = 0; k < _markerIds.getMat().total(); k++) {
if(_markerIds.getMat().ptr< int >(0)[k] == markerId) { if(_markerIds.getMat().at< int >(k) == markerId) {
markerIdx = k; markerIdx = k;
break; break;
} }
@@ -623,14 +623,14 @@ void drawDetectedCornersCharuco(InputOutputArray _image, InputArray _charucoCorn
unsigned int nCorners = (unsigned int)_charucoCorners.getMat().total(); unsigned int nCorners = (unsigned int)_charucoCorners.getMat().total();
for(unsigned int i = 0; i < nCorners; i++) { for(unsigned int i = 0; i < nCorners; i++) {
Point2f corner = _charucoCorners.getMat().ptr< Point2f >(0)[i]; Point2f corner = _charucoCorners.getMat().at< Point2f >(i);
// draw first corner mark // draw first corner mark
rectangle(_image, corner - Point2f(3, 3), corner + Point2f(3, 3), cornerColor, 1, LINE_AA); rectangle(_image, corner - Point2f(3, 3), corner + Point2f(3, 3), cornerColor, 1, LINE_AA);
// draw ID // draw ID
if(_charucoIds.total() != 0) { if(_charucoIds.total() != 0) {
int id = _charucoIds.getMat().ptr< int >(0)[i]; int id = _charucoIds.getMat().at< int >(i);
stringstream s; stringstream s;
s << "id=" << id; s << "id=" << id;
putText(_image, s.str(), corner + Point2f(5, -5), FONT_HERSHEY_SIMPLEX, 0.5, putText(_image, s.str(), corner + Point2f(5, -5), FONT_HERSHEY_SIMPLEX, 0.5,
@@ -692,7 +692,7 @@ bool estimatePoseCharucoBoard(InputArray _charucoCorners, InputArray _charucoIds
vector< Point3f > objPoints; vector< Point3f > objPoints;
objPoints.reserve(_charucoIds.getMat().total()); objPoints.reserve(_charucoIds.getMat().total());
for(unsigned int i = 0; i < _charucoIds.getMat().total(); i++) { for(unsigned int i = 0; i < _charucoIds.getMat().total(); i++) {
int currId = _charucoIds.getMat().ptr< int >(0)[i]; int currId = _charucoIds.getMat().at< int >(i);
CV_Assert(currId >= 0 && currId < (int)_board->chessboardCorners.size()); CV_Assert(currId >= 0 && currId < (int)_board->chessboardCorners.size());
objPoints.push_back(_board->chessboardCorners[currId]); objPoints.push_back(_board->chessboardCorners[currId]);
} }
@@ -730,7 +730,7 @@ double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfAr
allObjPoints[i].reserve(nCorners); allObjPoints[i].reserve(nCorners);
for(unsigned int j = 0; j < nCorners; j++) { for(unsigned int j = 0; j < nCorners; j++) {
int pointId = _charucoIds.getMat(i).ptr< int >(0)[j]; int pointId = _charucoIds.getMat(i).at< int >(j);
CV_Assert(pointId >= 0 && pointId < (int)_board->chessboardCorners.size()); CV_Assert(pointId >= 0 && pointId < (int)_board->chessboardCorners.size());
allObjPoints[i].push_back(_board->chessboardCorners[pointId]); allObjPoints[i].push_back(_board->chessboardCorners[pointId]);
} }
@@ -793,16 +793,13 @@ void detectCharucoDiamond(InputArray _image, InputArrayOfArrays _markerCorners,
float perimeterSq = 0; float perimeterSq = 0;
Mat corners = _markerCorners.getMat(i); Mat corners = _markerCorners.getMat(i);
for(int c = 0; c < 4; c++) { for(int c = 0; c < 4; c++) {
perimeterSq += Point2f edge = corners.at< Point2f >(c) - corners.at< Point2f >((c + 1) % 4);
(corners.ptr< Point2f >()[c].x - corners.ptr< Point2f >()[(c + 1) % 4].x) * perimeterSq += edge.x*edge.x + edge.y*edge.y;
(corners.ptr< Point2f >()[c].x - corners.ptr< Point2f >()[(c + 1) % 4].x) +
(corners.ptr< Point2f >()[c].y - corners.ptr< Point2f >()[(c + 1) % 4].y) *
(corners.ptr< Point2f >()[c].y - corners.ptr< Point2f >()[(c + 1) % 4].y);
} }
// maximum reprojection error relative to perimeter // maximum reprojection error relative to perimeter
float minRepDistance = perimeterSq * minRepDistanceRate * minRepDistanceRate; float minRepDistance = perimeterSq * minRepDistanceRate * minRepDistanceRate;
int currentId = _markerIds.getMat().ptr< int >()[i]; int currentId = _markerIds.getMat().at< int >(i);
// prepare data to call refineDetectedMarkers() // prepare data to call refineDetectedMarkers()
// detected markers (only the current one) // detected markers (only the current one)
@@ -848,7 +845,7 @@ void detectCharucoDiamond(InputArray _image, InputArrayOfArrays _markerCorners,
markerId[0] = currentId; markerId[0] = currentId;
for(int k = 1; k < 4; k++) { for(int k = 1; k < 4; k++) {
int currentMarkerIdx = candidatesIdxs[acceptedIdxs[k - 1]]; int currentMarkerIdx = candidatesIdxs[acceptedIdxs[k - 1]];
markerId[k] = _markerIds.getMat().ptr< int >()[currentMarkerIdx]; markerId[k] = _markerIds.getMat().at< int >(currentMarkerIdx);
assigned[currentMarkerIdx] = true; assigned[currentMarkerIdx] = true;
} }
@@ -880,13 +877,13 @@ void detectCharucoDiamond(InputArray _image, InputArrayOfArrays _markerCorners,
// parse output // parse output
_diamondIds.create((int)diamondIds.size(), 1, CV_32SC4); _diamondIds.create((int)diamondIds.size(), 1, CV_32SC4);
for(unsigned int i = 0; i < diamondIds.size(); i++) for(unsigned int i = 0; i < diamondIds.size(); i++)
_diamondIds.getMat().ptr< Vec4i >(0)[i] = diamondIds[i]; _diamondIds.getMat().at< Vec4i >(i) = diamondIds[i];
_diamondCorners.create((int)diamondCorners.size(), 1, CV_32FC2); _diamondCorners.create((int)diamondCorners.size(), 1, CV_32FC2);
for(unsigned int i = 0; i < diamondCorners.size(); i++) { for(unsigned int i = 0; i < diamondCorners.size(); i++) {
_diamondCorners.create(4, 1, CV_32FC2, i, true); _diamondCorners.create(4, 1, CV_32FC2, i, true);
for(int j = 0; j < 4; j++) { for(int j = 0; j < 4; j++) {
_diamondCorners.getMat(i).ptr< Point2f >()[j] = diamondCorners[i][j]; _diamondCorners.getMat(i).at< Point2f >(j) = diamondCorners[i][j];
} }
} }
} }
@@ -940,23 +937,23 @@ void drawDetectedDiamonds(InputOutputArray _image, InputArrayOfArrays _corners,
// draw marker sides // draw marker sides
for(int j = 0; j < 4; j++) { for(int j = 0; j < 4; j++) {
Point2f p0, p1; Point2f p0, p1;
p0 = currentMarker.ptr< Point2f >(0)[j]; p0 = currentMarker.at< Point2f >(j);
p1 = currentMarker.ptr< Point2f >(0)[(j + 1) % 4]; p1 = currentMarker.at< Point2f >((j + 1) % 4);
line(_image, p0, p1, borderColor, 1); line(_image, p0, p1, borderColor, 1);
} }
// draw first corner mark // draw first corner mark
rectangle(_image, currentMarker.ptr< Point2f >(0)[0] - Point2f(3, 3), rectangle(_image, currentMarker.at< Point2f >(0) - Point2f(3, 3),
currentMarker.ptr< Point2f >(0)[0] + Point2f(3, 3), cornerColor, 1, LINE_AA); currentMarker.at< Point2f >(0) + Point2f(3, 3), cornerColor, 1, LINE_AA);
// draw id composed by four numbers // draw id composed by four numbers
if(_ids.total() != 0) { if(_ids.total() != 0) {
Point2f cent(0, 0); Point2f cent(0, 0);
for(int p = 0; p < 4; p++) for(int p = 0; p < 4; p++)
cent += currentMarker.ptr< Point2f >(0)[p]; cent += currentMarker.at< Point2f >(p);
cent = cent / 4.; cent = cent / 4.;
stringstream s; stringstream s;
s << "id=" << _ids.getMat().ptr< Vec4i >(0)[i]; s << "id=" << _ids.getMat().at< Vec4i >(i);
putText(_image, s.str(), cent, FONT_HERSHEY_SIMPLEX, 0.5, textColor, 2); putText(_image, s.str(), cent, FONT_HERSHEY_SIMPLEX, 0.5, textColor, 2);
} }
} }