238 lines
8.7 KiB
C++
238 lines
8.7 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "binocularMarkCam_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
#include "aruco.hpp"
|
||
#include "aruco/charuco.hpp"
|
||
#include <limits>
|
||
|
||
#define _OUTPUT_DEBUG_DATA 0
|
||
|
||
//version 1.0.0 : 初始版本
|
||
std::string m_strVersion = "1.0.0";
|
||
//读版本号
|
||
const char* wd_charuco3DMarkVersion(void)
|
||
{
|
||
return m_strVersion.c_str();
|
||
}
|
||
|
||
/*Breif：二维码标定板角点函数*/
|
||
void detectCharucoCorners(const cv::Mat& img,
|
||
const SWD_BQ_CharucoMarkInfo markInfo,
|
||
const SWD_BQ_MarkBoardInfo boardInfo,
|
||
std::vector<int>& markerIdsAll,
|
||
std::vector<std::vector<cv::Point2f> >& markerCornersAll,
|
||
std::vector<int>& charucoIds,
|
||
std::vector<std::vector<cv::Point2f>>& charucoCorners)
|
||
{
|
||
cv::Mat gray;
|
||
if (img.channels() == 3)
|
||
cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
|
||
else
|
||
gray = img.clone();
|
||
|
||
enum cv::aruco::PredefinedDictionaryType dictType;
|
||
if (1 == markInfo.dictType)
|
||
dictType = cv::aruco::DICT_6X6_1000;
|
||
else
|
||
dictType = cv::aruco::DICT_6X6_1000; //目前只支持DICT_6X6_1000
|
||
|
||
cv::aruco::Dictionary aruco_dict = cv::aruco::getPredefinedDictionary(dictType);
|
||
cv::aruco::DetectorParameters params = cv::aruco::DetectorParameters();
|
||
params.cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE;
|
||
|
||
cv::Ptr<cv::aruco::Dictionary> ptrDictionary = cv::makePtr<cv::aruco::Dictionary>(aruco_dict);
|
||
cv::Ptr<cv::aruco::DetectorParameters> ptrParams = cv::makePtr<cv::aruco::DetectorParameters>(params);
|
||
#if 0
|
||
// 优化检测器参数（提升检测稳定性）
|
||
ptrParams->adaptiveThreshWinSizeMin = 3;
|
||
ptrParams->adaptiveThreshWinSizeMax = 23;
|
||
ptrParams->adaptiveThreshWinSizeStep = 10;
|
||
ptrParams->minMarkerPerimeterRate = 0.01; // 检测更小的标记
|
||
#endif
|
||
//检测所有角点
|
||
cv::aruco::detectMarkers(gray, ptrDictionary, markerCornersAll, markerIdsAll, ptrParams);
|
||
|
||
//搜索Mark(搜索各个不同的标定板）
|
||
for (int board = 0; board < boardInfo.totalBoardNum; board++)
|
||
{
|
||
int startId = board * boardInfo.boardIdInterval;
|
||
int endId = startId + boardInfo.boardChaucoIDNum - 1;
|
||
|
||
std::vector<int> ids;
|
||
for (int i = 0; i < boardInfo.boardChaucoIDNum; i++)
|
||
ids.push_back(startId + i);
|
||
|
||
//搜索
|
||
std::vector<int> markerIds;
|
||
std::vector<std::vector<cv::Point2f> > markerCorners;
|
||
for (int i = 0; i < markerIdsAll.size(); i++)
|
||
{
|
||
if ((markerIdsAll[i] >= startId) && (markerIdsAll[i] <= endId))
|
||
{
|
||
markerIds.push_back(markerIdsAll[i]);
|
||
markerCorners.push_back(markerCornersAll[i]);
|
||
}
|
||
}
|
||
if (markerIds.size() > 0)
|
||
{
|
||
std::vector<int> board_charucoIds;
|
||
std::vector<cv::Point2f> board_charucoCorners;
|
||
|
||
cv::aruco::CharucoBoard board = cv::aruco::CharucoBoard(markInfo.patternSize, markInfo.checkerSize, markInfo.markerSize, aruco_dict, ids);
|
||
cv::Ptr<cv::aruco::CharucoBoard> ptrBoard = cv::makePtr<cv::aruco::CharucoBoard>(board);
|
||
cv::aruco::interpolateCornersCharuco(
|
||
markerCorners,
|
||
markerIds,
|
||
gray,
|
||
ptrBoard,
|
||
board_charucoCorners,
|
||
board_charucoIds);
|
||
if (board_charucoIds.size() > 0)
|
||
{
|
||
// 亚像素精确化
|
||
cv::cornerSubPix(gray, board_charucoCorners, cv::Size(11, 11), cv::Size(-1, -1),
|
||
cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 50, 0.1));
|
||
|
||
charucoIds.push_back(startId);
|
||
charucoCorners.push_back(board_charucoCorners);
|
||
}
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
SVzNL3DPoint _getStereoPoints3d(cv::Point2f left, cv::Point2f right, const cv::Mat& Q, double offset = 0)
|
||
{
|
||
const double cx = Q.at<double>(0, 3);
|
||
const double cy = Q.at<double>(1, 3);
|
||
const double f = Q.at<double>(2, 3);
|
||
const double t = 1.0 / Q.at<double>(3, 2);
|
||
//std::cout << "Cx: " << cx << ", Cy: " << cy << ", f: " << f << ", t: " << t << std::endl;
|
||
SVzNL3DPoint result;
|
||
double dis = left.x - right.x + offset;
|
||
result.x = (left.x + cx) * t / dis;
|
||
result.y = (left.y + cy) * t / dis;
|
||
result.z = f * t / dis;
|
||
//std::cout << "L: " << left[_i] << ", R: " << right[_i] << ", dis: " << dis << std::endl;
|
||
return result;
|
||
}
|
||
|
||
typedef struct
|
||
{
|
||
int markID;
|
||
std::vector<cv::Point2f> left;
|
||
std::vector<cv::Point2f> right;
|
||
}_MarkerPairing;
|
||
|
||
//提取查科Mark3D信息
|
||
void wd_BQ_getCharuco3DMark(
|
||
cv::Mat& leftImage,
|
||
cv::Mat& rightImage,
|
||
cv::Mat& cameraMatrixL,
|
||
cv::Mat& distCoeffsL,
|
||
cv::Mat& cameraMatrixR,
|
||
cv::Mat& distCoeffsR,
|
||
cv::Mat& R1,
|
||
cv::Mat& R2,
|
||
cv::Mat& P1,
|
||
cv::Mat& P2,
|
||
cv::Mat& Q,
|
||
SWD_BQ_CharucoMarkInfo markInfo,
|
||
SWD_BQ_MarkBoardInfo boardInfo,
|
||
double disparityOffset,
|
||
std::vector<SWD_charuco3DMark>& marks)
|
||
{
|
||
std::vector<int> markerIdsLeft;
|
||
std::vector<std::vector<cv::Point2f> > markerCornersLeft;
|
||
std::vector<int> charucoIdsLeft;
|
||
std::vector<std::vector<cv::Point2f>> charucoCornersLeft;//每个Mark有4个角点对应4个二维码
|
||
detectCharucoCorners(leftImage,
|
||
markInfo,
|
||
boardInfo,
|
||
markerIdsLeft,
|
||
markerCornersLeft,
|
||
charucoIdsLeft,
|
||
charucoCornersLeft);
|
||
|
||
std::vector<int> markerIdsRight;
|
||
std::vector<std::vector<cv::Point2f> > markerCornersRight;
|
||
std::vector<int> charucoIdsRight;
|
||
std::vector<std::vector<cv::Point2f>> charucoCornersRight;
|
||
detectCharucoCorners(rightImage,
|
||
markInfo,
|
||
boardInfo,
|
||
markerIdsRight,
|
||
markerCornersRight,
|
||
charucoIdsRight,
|
||
charucoCornersRight);
|
||
|
||
#if _OUTPUT_DEBUG_DATA
|
||
cv::Mat imageCopyLeft = leftImage.clone();
|
||
if (markerIdsLeft.size() > 0)
|
||
cv::aruco::drawDetectedMarkers(imageCopyLeft, markerCornersLeft, markerIdsLeft);
|
||
if (charucoIdsLeft.size() > 0)
|
||
cv::aruco::drawDetectedCornersCharuco(imageCopyLeft, charucoCornersLeft, charucoIdsLeft, cv::Scalar(0, 255, 255));
|
||
char markFilenameL[256];
|
||
sprintf_s(markFilenameL, "markersLeft.bmp");
|
||
cv::imwrite(markFilenameL, imageCopyLeft);
|
||
|
||
cv::Mat imageCopyRight = leftImage.clone();
|
||
if (markerIdsRight.size() > 0)
|
||
cv::aruco::drawDetectedMarkers(imageCopyRight, markerCornersRight, markerIdsRight);
|
||
if (charucoIdsRight.size() > 0)
|
||
cv::aruco::drawDetectedCornersCharuco(imageCopyRight, charucoCornersRight, charucoIdsRight, cv::Scalar(0, 255, 255));
|
||
char markFilenameR[256];
|
||
sprintf_s(markFilenameR, "markersRight.bmp");
|
||
cv::imwrite(markFilenameR, imageCopyRight);
|
||
#endif
|
||
|
||
//Marker配对
|
||
std::vector<_MarkerPairing> stereoMarks;
|
||
for (int i = 0, i_max = (int)charucoIdsLeft.size(); i < i_max; i++)
|
||
{
|
||
for (int j = 0, j_max = (int)charucoIdsRight.size(); j < j_max; j++)
|
||
{
|
||
if ( (charucoIdsLeft[i] == charucoIdsRight[j]) && (charucoCornersLeft[i].size() == charucoCornersRight[j].size()))
|
||
{
|
||
_MarkerPairing a_pair;
|
||
a_pair.markID = charucoIdsLeft[i];
|
||
a_pair.left.insert(a_pair.left.end(), charucoCornersLeft[i].begin(), charucoCornersLeft[i].end());
|
||
a_pair.right.insert(a_pair.right.end(), charucoCornersRight[j].begin(), charucoCornersRight[j].end());
|
||
stereoMarks.push_back(a_pair);
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
for (int i = 0, i_max = (int)stereoMarks.size(); i < i_max; i++)
|
||
{
|
||
//双目校正
|
||
std::vector<cv::Point2f> rectCornersLeft;
|
||
cv::undistortPoints(stereoMarks[i].left, rectCornersLeft, cameraMatrixL, distCoeffsL, R1, P1);
|
||
std::vector<cv::Point2f> rectCornersRight;
|
||
cv::undistortPoints(stereoMarks[i].right, rectCornersRight, cameraMatrixR, distCoeffsR, R2, P2);
|
||
|
||
std::vector<SVzNL3DPoint> corners;
|
||
for (int m = 0; m < rectCornersLeft.size(); m++)
|
||
{
|
||
double abs_diff = abs(rectCornersLeft[m].y - rectCornersRight[m].y);
|
||
if (abs_diff < 1.0) //极线误差小于1个像素
|
||
{
|
||
SVzNL3DPoint a_pt3d = _getStereoPoints3d(rectCornersLeft[m], rectCornersRight[m], Q, disparityOffset);
|
||
corners.push_back(a_pt3d);
|
||
}
|
||
}
|
||
if (corners.size() == 4)
|
||
{
|
||
SWD_charuco3DMark a_3dMark;
|
||
a_3dMark.markID = stereoMarks[i].markID;
|
||
a_3dMark.mark3D.x = (corners[0].x + corners[1].x + corners[2].x + corners[3].x) / 4;
|
||
a_3dMark.mark3D.y = (corners[0].y + corners[1].y + corners[2].y + corners[3].y) / 4;
|
||
a_3dMark.mark3D.z = (corners[0].z + corners[1].z + corners[2].z + corners[3].z) / 4;
|
||
marks.push_back(a_3dMark);
|
||
}
|
||
}
|
||
return;
|
||
} |