Chiebot-Mirror
/
opencv_contrib
8
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Merge pull request #769 from sovrasov:aruco_fix

Browse Source
pull/784/head
Vadim Pisarevsky 9 years ago
parent 0ea4ee2514 348dfeda05
commit 67a305ad6e
1 changed files with 31 additions and 34 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 65
      modules/aruco/src/charuco.cpp

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

@ -243,14 +243,14 @@ static unsigned int _filterCornersWithoutMinMarkers(Ptr<CharucoBoard> &_board,
vector< int > filteredCharucoIds;
// for each charuco corner
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
// look for closest markers
for(unsigned int m = 0; m < _board->nearestMarkerIdx[currentCharucoId].size(); m++) {
int markerId = _board->ids[_board->nearestMarkerIdx[currentCharucoId][m]];
bool found = false;
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;
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(totalMarkers >= minMarkers) {
filteredCharucoIds.push_back(currentCharucoId);
filteredCharucoCorners.push_back(_allCharucoCorners.getMat().ptr< Point2f >(0)[i]);
filteredCharucoCorners.push_back(_allCharucoCorners.getMat().at< Point2f >(i));
}
}
// parse output
_filteredCharucoCorners.create((int)filteredCharucoCorners.size(), 1, CV_32FC2);
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);
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();
@ -341,8 +341,8 @@ static unsigned int _selectAndRefineChessboardCorners(InputArray _allCorners, In
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().ptr< Point2f >(0)[i])) {
filteredChessboardImgPoints.push_back(_allCorners.getMat().ptr< Point2f >(0)[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]);
}
@ -382,12 +382,12 @@ static unsigned int _selectAndRefineChessboardCorners(InputArray _allCorners, In
// parse output
_selectedCorners.create((int)filteredChessboardImgPoints.size(), 1, CV_32FC2);
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);
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();
@ -406,7 +406,7 @@ static void _getMaximumSubPixWindowSizes(InputArrayOfArrays markerCorners, Input
sizes.resize(nCharucoCorners, Size(-1, -1));
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;
double minDist = -1;
@ -418,15 +418,15 @@ static void _getMaximumSubPixWindowSizes(InputArrayOfArrays markerCorners, Input
int markerId = board->ids[board->nearestMarkerIdx[i][j]];
int markerIdx = -1;
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;
break;
}
}
if(markerIdx == -1) continue;
Point2f markerCorner =
markerCorners.getMat(markerIdx).ptr< Point2f >(0)[board->nearestMarkerCorners[i][j]];
Point2f charucoCorner = charucoCorners.getMat().ptr< Point2f >(0)[i];
markerCorners.getMat(markerIdx).at< Point2f >(board->nearestMarkerCorners[i][j]);
Point2f charucoCorner = charucoCorners.getMat().at< Point2f >(i);
double dist = norm(markerCorner - charucoCorner);
if(minDist == -1) minDist = dist; // if first distance, just assign it
minDist = min(dist, minDist);
@ -519,7 +519,7 @@ static int _interpolateCornersCharucoLocalHom(InputArrayOfArrays _markerCorners,
transformations.resize(nMarkers);
for(unsigned int i = 0; i < nMarkers; i++) {
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);
if(it == _board->ids.end()) continue;
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 markerIdx = -1;
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;
break;
}
@ -623,14 +623,14 @@ void drawDetectedCornersCharuco(InputOutputArray _image, InputArray _charucoCorn
unsigned int nCorners = (unsigned int)_charucoCorners.getMat().total();
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
rectangle(_image, corner - Point2f(3, 3), corner + Point2f(3, 3), cornerColor, 1, LINE_AA);
// draw ID
if(_charucoIds.total() != 0) {
int id = _charucoIds.getMat().ptr< int >(0)[i];
int id = _charucoIds.getMat().at< int >(i);
stringstream s;
s << "id=" << id;
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;
objPoints.reserve(_charucoIds.getMat().total());
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());
objPoints.push_back(_board->chessboardCorners[currId]);
}
@ -730,7 +730,7 @@ double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfAr
allObjPoints[i].reserve(nCorners);
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());
allObjPoints[i].push_back(_board->chessboardCorners[pointId]);
}
@ -793,16 +793,13 @@ void detectCharucoDiamond(InputArray _image, InputArrayOfArrays _markerCorners,
float perimeterSq = 0;
Mat corners = _markerCorners.getMat(i);
for(int c = 0; c < 4; c++) {
perimeterSq +=
(corners.ptr< Point2f >()[c].x - corners.ptr< Point2f >()[(c + 1) % 4].x) *
(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);
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 = perimeterSq * minRepDistanceRate * minRepDistanceRate;
int currentId = _markerIds.getMat().ptr< int >()[i];
int currentId = _markerIds.getMat().at< int >(i);
// prepare data to call refineDetectedMarkers()
// detected markers (only the current one)
@ -848,7 +845,7 @@ void detectCharucoDiamond(InputArray _image, InputArrayOfArrays _markerCorners,
markerId[0] = currentId;
for(int k = 1; k < 4; k++) {
int currentMarkerIdx = candidatesIdxs[acceptedIdxs[k - 1]];
markerId[k] = _markerIds.getMat().ptr< int >()[currentMarkerIdx];
markerId[k] = _markerIds.getMat().at< int >(currentMarkerIdx);
assigned[currentMarkerIdx] = true;
}
@ -880,13 +877,13 @@ void detectCharucoDiamond(InputArray _image, InputArrayOfArrays _markerCorners,
// parse output
_diamondIds.create((int)diamondIds.size(), 1, CV_32SC4);
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);
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).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
for(int j = 0; j < 4; j++) {
Point2f p0, p1;
p0 = currentMarker.ptr< Point2f >(0)[j];
p1 = currentMarker.ptr< Point2f >(0)[(j + 1) % 4];
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.ptr< Point2f >(0)[0] - Point2f(3, 3),
currentMarker.ptr< Point2f >(0)[0] + Point2f(3, 3), cornerColor, 1, LINE_AA);
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.ptr< Point2f >(0)[p];
cent += currentMarker.at< Point2f >(p);
cent = cent / 4.;
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);
}
}

Write Preview
Loading…
Cancel
Save