diff --git a/BinocularMarkCam_test/BinocularMarkCam_test.cpp b/BinocularMarkCam_test/BinocularMarkCam_test.cpp new file mode 100644 index 0000000..1b9f5fd --- /dev/null +++ b/BinocularMarkCam_test/BinocularMarkCam_test.cpp @@ -0,0 +1,104 @@ +// BinocularMarkCam_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// +#include +#include +#include +#include +#include +#include +#include "binocularMarkCam_Export.h" +#include +#include +#include +#include + +#define TEST_GROUP 2 +int main(int argc, char** argv) +{ + const char* dataPath[TEST_GROUP] = { + "F:/标定采图/charucoMark图/MarkTest/", //0 + "F:/标定采图/charucoMark图/Mark_13度/", + }; + + SVzNLRange fileIdx[TEST_GROUP] = { + {1,4}, {1,5}, + }; + + const char* ver = wd_charuco3DMarkVersion(); + printf("ver:%s\n", ver); + + for (int grp = 1; grp < 2; grp++) + { + SWD_BQ_CharucoMarkInfo markInfo; + markInfo.patternSize = cv::Size(3, 3); + markInfo.dictType = 1; //DICT_6x6 + markInfo.checkerSize = 60.0; + markInfo.markerSize = 45.0; + + SWD_BQ_MarkBoardInfo boardInfo; + boardInfo.totalBoardNum = 10; + boardInfo.boardIdInterval = 8; + boardInfo.boardChaucoIDNum = 4; + + // 1. 打开 XML 文件（READ 模式） + char calibFile[250]; + sprintf_s(calibFile, sizeof(calibFile), "%sStereoCamera.xml", dataPath[grp]); + cv::FileStorage fs("F:/标定采图/charucoMark图/Mark_13度/StereoCamera.xml", cv::FileStorage::READ); + + // 检查文件是否成功打开 + if (!fs.isOpened()) { + printf( "错误：无法打开 XML 文件！请检查路径是否正确。\n" ); + return -1; + } + cv::FileNode LeftCamera = fs["LeftCamera"]; + cv::Mat cameraMatrixL, distCoeffsL; + LeftCamera["CameraMatrix"] >> cameraMatrixL; + LeftCamera["DistCoeffs"] >> distCoeffsL; + cv::FileNode RightCamera = fs["RightCamera"]; + cv::Mat cameraMatrixR, distCoeffsR; + RightCamera["CameraMatrix"] >> cameraMatrixR; + RightCamera["DistCoeffs"] >> distCoeffsR; + cv::FileNode Rectification = fs["Rectification"]; + cv::Mat R1, R2,P1,P2,Q; + Rectification["R1"] >> R1; + Rectification["R2"] >> R2; + Rectification["P1"] >> P1; + Rectification["P2"] >> P2; + Rectification["Q"] >> Q; + + int fidx1 = fileIdx[grp].nMin; + int fidx2 = fileIdx[grp].nMax; + for (int index = fidx1; index < fidx2; index ++) + { + char filename[256]; + sprintf_s(filename, "%s/left%d.bmp", dataPath[grp], index); + cv::Mat leftimg = cv::imread(filename, cv::IMREAD_GRAYSCALE); + + sprintf_s(filename, "%s/right%d.bmp", dataPath[grp], index); + cv::Mat rightimg = cv::imread(filename, cv::IMREAD_GRAYSCALE); + + if (leftimg.empty() || rightimg.empty()) + break; + + double disparityOffset = 0; + std::vector marks; + wd_BQ_getCharuco3DMark( + leftimg, rightimg, + cameraMatrixL, distCoeffsL, cameraMatrixR, distCoeffsR, R1, R2, P1, P2, Q, + markInfo, boardInfo, + disparityOffset, + marks); + } + } +} + +// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 +// 调试程序: F5 或调试 >“开始调试”菜单 + +// 入门使用技巧: +// 1. 使用解决方案资源管理器窗口添加/管理文件 +// 2. 使用团队资源管理器窗口连接到源代码管理 +// 3. 使用输出窗口查看生成输出和其他消息 +// 4. 使用错误列表窗口查看错误 +// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目 +// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件 diff --git a/BinocularMarkCam_test/BinocularMarkCam_test.vcxproj b/BinocularMarkCam_test/BinocularMarkCam_test.vcxproj new file mode 100644 index 0000000..992e5cd --- /dev/null +++ b/BinocularMarkCam_test/BinocularMarkCam_test.vcxproj @@ -0,0 +1,158 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {fb3bcf92-df8b-4cec-aa39-4ecd31f82718} + BinocularMarkCamtest + 10.0 + binocularMarkCam_test + + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + $(SolutionDir)build\$(Platform)\$(Configuration)\ + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + Level3 + true + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv\build\include; + + + Console + true + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world480d.lib;binocularMarkCam.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv\build\include; + + + Console + true + true + true + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world480.lib;binocularMarkCam.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/binocularMarkCam/binocularMarkCam.vcxproj b/binocularMarkCam/binocularMarkCam.vcxproj new file mode 100644 index 0000000..4cf1ae5 --- /dev/null +++ b/binocularMarkCam/binocularMarkCam.vcxproj @@ -0,0 +1,176 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + + + + 16.0 + Win32Proj + {ab25a4fe-6457-4cb4-a190-ebef603335c7} + binocularMarkCam + 10.0 + binocularMarkCam + + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;BINOCULARMARKCAM_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;BINOCULARMARKCAM_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + true + true + false + + + + + Level3 + true + _DEBUG;BINOCULARMARKCAM_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + false + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug; + opencv_world480d.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + NDEBUG;BINOCULARMARKCAM_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + true + true + false + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release; + opencv_world480.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + + + + + + \ No newline at end of file diff --git a/sourceCode/aruco/aruco.cpp b/sourceCode/aruco/aruco.cpp new file mode 100644 index 0000000..c734a75 --- /dev/null +++ b/sourceCode/aruco/aruco.cpp @@ -0,0 +1,143 @@ +// 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 "../aruco.hpp" +#include +#include + +namespace cv { +namespace aruco { + +using namespace std; + +void detectMarkers(InputArray _image, const Ptr &_dictionary, OutputArrayOfArrays _corners, + OutputArray _ids, const Ptr &_params, + OutputArrayOfArrays _rejectedImgPoints) { + ArucoDetector detector(*_dictionary, *_params); + detector.detectMarkers(_image, _corners, _ids, _rejectedImgPoints); +} + +void refineDetectedMarkers(InputArray _image, const Ptr &_board, + InputOutputArrayOfArrays _detectedCorners, InputOutputArray _detectedIds, + InputOutputArrayOfArrays _rejectedCorners, InputArray _cameraMatrix, + InputArray _distCoeffs, float minRepDistance, float errorCorrectionRate, + bool checkAllOrders, OutputArray _recoveredIdxs, + const Ptr &_params) { + RefineParameters refineParams(minRepDistance, errorCorrectionRate, checkAllOrders); + ArucoDetector detector(_board->getDictionary(), *_params, refineParams); + detector.refineDetectedMarkers(_image, *_board, _detectedCorners, _detectedIds, _rejectedCorners, _cameraMatrix, + _distCoeffs, _recoveredIdxs); +} + +void drawPlanarBoard(const Ptr &board, Size outSize, const _OutputArray &img, int marginSize, int borderBits) { + board->generateImage(outSize, img, marginSize, borderBits); +} + +void getBoardObjectAndImagePoints(const Ptr &board, InputArrayOfArrays detectedCorners, InputArray detectedIds, + OutputArray objPoints, OutputArray imgPoints) { + board->matchImagePoints(detectedCorners, detectedIds, objPoints, imgPoints); +} + +int estimatePoseBoard(InputArrayOfArrays corners, InputArray ids, const Ptr &board, + InputArray cameraMatrix, InputArray distCoeffs, InputOutputArray rvec, + InputOutputArray tvec, bool useExtrinsicGuess) { + CV_Assert(corners.total() == ids.total()); + + // get object and image points for the solvePnP function + Mat objPoints, imgPoints; + board->matchImagePoints(corners, ids, objPoints, imgPoints); + + CV_Assert(imgPoints.total() == objPoints.total()); + + if(objPoints.total() == 0) // 0 of the detected markers in board + return 0; + + solvePnP(objPoints, imgPoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess); + + // divide by four since all the four corners are concatenated in the array for each marker + return (int)objPoints.total() / 4; +} + +bool estimatePoseCharucoBoard(InputArray charucoCorners, InputArray charucoIds, + const Ptr &board, InputArray cameraMatrix, + InputArray distCoeffs, InputOutputArray rvec, + InputOutputArray tvec, bool useExtrinsicGuess) { + CV_Assert((charucoCorners.getMat().total() == charucoIds.getMat().total())); + if(charucoIds.getMat().total() < 4) return false; + + // get object and image points for the solvePnP function + Mat objPoints, imgPoints; + board->matchImagePoints(charucoCorners, charucoIds, objPoints, imgPoints); + try { + solvePnP(objPoints, imgPoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess); + } + catch (const cv::Exception& e) { + CV_LOG_WARNING(NULL, "estimatePoseCharucoBoard: " << std::endl << e.what()); + return false; + } + + return objPoints.total() > 0ull; +} + +bool testCharucoCornersCollinear(const Ptr &board, InputArray charucoIds) { + return board->checkCharucoCornersCollinear(charucoIds); +} + +/** + * @brief Return object points for the system centered in a middle (by default) or in a top left corner of single + * marker, given the marker length + */ +static Mat _getSingleMarkerObjectPoints(float markerLength, const EstimateParameters& estimateParameters) { + CV_Assert(markerLength > 0); + Mat objPoints(4, 1, CV_32FC3); + // set coordinate system in the top-left corner of the marker, with Z pointing out + if (estimateParameters.pattern == ARUCO_CW_TOP_LEFT_CORNER) { + objPoints.ptr(0)[0] = Vec3f(0.f, 0.f, 0); + objPoints.ptr(0)[1] = Vec3f(markerLength, 0.f, 0); + objPoints.ptr(0)[2] = Vec3f(markerLength, markerLength, 0); + objPoints.ptr(0)[3] = Vec3f(0.f, markerLength, 0); + } + else if (estimateParameters.pattern == ARUCO_CCW_CENTER) { + objPoints.ptr(0)[0] = Vec3f(-markerLength/2.f, markerLength/2.f, 0); + objPoints.ptr(0)[1] = Vec3f(markerLength/2.f, markerLength/2.f, 0); + objPoints.ptr(0)[2] = Vec3f(markerLength/2.f, -markerLength/2.f, 0); + objPoints.ptr(0)[3] = Vec3f(-markerLength/2.f, -markerLength/2.f, 0); + } + else + CV_Error(Error::StsBadArg, "Unknown estimateParameters pattern"); + return objPoints; +} + +void estimatePoseSingleMarkers(InputArrayOfArrays _corners, float markerLength, + InputArray _cameraMatrix, InputArray _distCoeffs, + OutputArray _rvecs, OutputArray _tvecs, OutputArray _objPoints, + const Ptr& estimateParameters) { + CV_Assert(markerLength > 0); + + Mat markerObjPoints = _getSingleMarkerObjectPoints(markerLength, *estimateParameters); + int nMarkers = (int)_corners.total(); + _rvecs.create(nMarkers, 1, CV_64FC3); + _tvecs.create(nMarkers, 1, CV_64FC3); + + Mat rvecs = _rvecs.getMat(), tvecs = _tvecs.getMat(); + + //// for each marker, calculate its pose + parallel_for_(Range(0, nMarkers), [&](const Range& range) { + const int begin = range.start; + const int end = range.end; + + for (int i = begin; i < end; i++) { + solvePnP(markerObjPoints, _corners.getMat(i), _cameraMatrix, _distCoeffs, rvecs.at(i), + tvecs.at(i), estimateParameters->useExtrinsicGuess, estimateParameters->solvePnPMethod); + } + }); + + if(_objPoints.needed()){ + markerObjPoints.convertTo(_objPoints, -1); + } +} + +} +} diff --git a/sourceCode/aruco/aruco_calib.cpp b/sourceCode/aruco/aruco_calib.cpp new file mode 100644 index 0000000..643c69f --- /dev/null +++ b/sourceCode/aruco/aruco_calib.cpp @@ -0,0 +1,99 @@ +// 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 "aruco_calib.hpp" +#include + +namespace cv { +namespace aruco { +using namespace std; +using namespace cv; + +EstimateParameters::EstimateParameters() : pattern(ARUCO_CCW_CENTER), useExtrinsicGuess(false), + solvePnPMethod(SOLVEPNP_ITERATIVE) {} + +double calibrateCameraAruco(InputArrayOfArrays _corners, InputArray _ids, InputArray _counter, + const Ptr &board, Size imageSize, InputOutputArray _cameraMatrix, + InputOutputArray _distCoeffs, OutputArrayOfArrays _rvecs, + OutputArrayOfArrays _tvecs, + OutputArray _stdDeviationsIntrinsics, + OutputArray _stdDeviationsExtrinsics, + OutputArray _perViewErrors, + int flags, const TermCriteria& criteria) { + // for each frame, get properly processed imagePoints and objectPoints for the calibrateCamera + // function + vector processedObjectPoints, processedImagePoints; + size_t nFrames = _counter.total(); + int markerCounter = 0; + for(size_t frame = 0; frame < nFrames; frame++) { + int nMarkersInThisFrame = _counter.getMat().ptr< int >()[frame]; + vector thisFrameCorners; + vector thisFrameIds; + + CV_Assert(nMarkersInThisFrame > 0); + + thisFrameCorners.reserve((size_t) nMarkersInThisFrame); + thisFrameIds.reserve((size_t) nMarkersInThisFrame); + for(int j = markerCounter; j < markerCounter + nMarkersInThisFrame; j++) { + thisFrameCorners.push_back(_corners.getMat(j)); + thisFrameIds.push_back(_ids.getMat().ptr< int >()[j]); + } + markerCounter += nMarkersInThisFrame; + Mat currentImgPoints, currentObjPoints; + board->matchImagePoints(thisFrameCorners, thisFrameIds, currentObjPoints, + currentImgPoints); + if(currentImgPoints.total() > 0 && currentObjPoints.total() > 0) { + processedImagePoints.push_back(currentImgPoints); + processedObjectPoints.push_back(currentObjPoints); + } + } + return calibrateCamera(processedObjectPoints, processedImagePoints, imageSize, _cameraMatrix, _distCoeffs, _rvecs, + _tvecs, _stdDeviationsIntrinsics, _stdDeviationsExtrinsics, _perViewErrors, flags, criteria); +} + +double calibrateCameraAruco(InputArrayOfArrays _corners, InputArray _ids, InputArray _counter, const Ptr &board, + Size imageSize, InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs, + OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, int flags, const TermCriteria& criteria) { + return calibrateCameraAruco(_corners, _ids, _counter, board, imageSize, _cameraMatrix, _distCoeffs, + _rvecs, _tvecs, noArray(), noArray(), noArray(), flags, criteria); +} + +double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfArrays _charucoIds, + const Ptr &_board, Size imageSize, + InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs, + OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, + OutputArray _stdDeviationsIntrinsics, + OutputArray _stdDeviationsExtrinsics, + OutputArray _perViewErrors, + int flags, const TermCriteria& criteria) { + CV_Assert(_charucoIds.total() > 0 && (_charucoIds.total() == _charucoCorners.total())); + + // Join object points of charuco corners in a single vector for calibrateCamera() function + vector > allObjPoints; + allObjPoints.resize(_charucoIds.total()); + for(unsigned int i = 0; i < _charucoIds.total(); i++) { + unsigned int nCorners = (unsigned int)_charucoIds.getMat(i).total(); + CV_Assert(nCorners > 0 && nCorners == _charucoCorners.getMat(i).total()); + allObjPoints[i].reserve(nCorners); + + for(unsigned int j = 0; j < nCorners; j++) { + int pointId = _charucoIds.getMat(i).at< int >(j); + CV_Assert(pointId >= 0 && pointId < (int)_board->getChessboardCorners().size()); + allObjPoints[i].push_back(_board->getChessboardCorners()[pointId]); + } + } + return calibrateCamera(allObjPoints, _charucoCorners, imageSize, _cameraMatrix, _distCoeffs, _rvecs, _tvecs, + _stdDeviationsIntrinsics, _stdDeviationsExtrinsics, _perViewErrors, flags, criteria); +} + +double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfArrays _charucoIds, + const Ptr &_board, Size imageSize, InputOutputArray _cameraMatrix, + InputOutputArray _distCoeffs, OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, + int flags, const TermCriteria& criteria) { +return calibrateCameraCharuco(_charucoCorners, _charucoIds, _board, imageSize, _cameraMatrix, _distCoeffs, _rvecs, + _tvecs, noArray(), noArray(), noArray(), flags, criteria); +} + +} +} diff --git a/sourceCode/aruco/aruco_calib.hpp b/sourceCode/aruco/aruco_calib.hpp new file mode 100644 index 0000000..2fd344d --- /dev/null +++ b/sourceCode/aruco/aruco_calib.hpp @@ -0,0 +1,180 @@ +// 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 +#ifndef OPENCV_ARUCO_CALIB_POSE_HPP +#define OPENCV_ARUCO_CALIB_POSE_HPP +#include + +namespace cv { +namespace aruco { + +//! @addtogroup aruco +//! @{ + +/** @brief rvec/tvec define the right handed coordinate system of the marker. + * + * PatternPositionType defines center this system and axes direction. + * Axis X (red color) - first coordinate, axis Y (green color) - second coordinate, + * axis Z (blue color) - third coordinate. + * + * @deprecated Use Board::matchImagePoints and cv::solvePnP + * + * @sa estimatePoseSingleMarkers() + */ +enum PatternPositionType { + /** @brief The marker coordinate system is centered on the middle of the marker. + * + * The coordinates of the four corners (CCW order) of the marker in its own coordinate system are: + * (-markerLength/2, markerLength/2, 0), (markerLength/2, markerLength/2, 0), + * (markerLength/2, -markerLength/2, 0), (-markerLength/2, -markerLength/2, 0). + */ + ARUCO_CCW_CENTER, + /** @brief The marker coordinate system is centered on the top-left corner of the marker. + * + * The coordinates of the four corners (CW order) of the marker in its own coordinate system are: + * (0, 0, 0), (markerLength, 0, 0), + * (markerLength, markerLength, 0), (0, markerLength, 0). + * + * These pattern dots are convenient to use with a chessboard/ChArUco board. + */ + ARUCO_CW_TOP_LEFT_CORNER +}; + +/** @brief Pose estimation parameters + * + * @param pattern Defines center this system and axes direction (default PatternPositionType::ARUCO_CCW_CENTER). + * @param useExtrinsicGuess Parameter used for SOLVEPNP_ITERATIVE. If true (1), the function uses the provided + * rvec and tvec values as initial approximations of the rotation and translation vectors, respectively, and further + * optimizes them (default false). + * @param solvePnPMethod Method for solving a PnP problem: see @ref calib3d_solvePnP_flags (default SOLVEPNP_ITERATIVE). + * + * @deprecated Use Board::matchImagePoints and cv::solvePnP + * + * @sa PatternPositionType, solvePnP() + */ +struct CV_EXPORTS_W_SIMPLE EstimateParameters { + CV_PROP_RW aruco::PatternPositionType pattern; + CV_PROP_RW bool useExtrinsicGuess; + CV_PROP_RW int solvePnPMethod; + + CV_WRAP EstimateParameters(); +}; + +/** + * @brief Calibrate a camera using aruco markers + * + * @param corners vector of detected marker corners in all frames. + * The corners should have the same format returned by detectMarkers (see #detectMarkers). + * @param ids list of identifiers for each marker in corners + * @param counter number of markers in each frame so that corners and ids can be split + * @param board Marker Board layout + * @param imageSize Size of the image used only to initialize the intrinsic camera matrix. + * @param cameraMatrix Output 3x3 floating-point camera matrix + * \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ . If CV\_CALIB\_USE\_INTRINSIC\_GUESS + * and/or CV_CALIB_FIX_ASPECT_RATIO are specified, some or all of fx, fy, cx, cy must be + * initialized before calling the function. + * @param distCoeffs Output vector of distortion coefficients + * \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6],[s_1, s_2, s_3, s_4]])\f$ of 4, 5, 8 or 12 elements + * @param rvecs Output vector of rotation vectors (see Rodrigues ) estimated for each board view + * (e.g. std::vector>). That is, each k-th rotation vector together with the corresponding + * k-th translation vector (see the next output parameter description) brings the board pattern + * from the model coordinate space (in which object points are specified) to the world coordinate + * space, that is, a real position of the board pattern in the k-th pattern view (k=0.. *M* -1). + * @param tvecs Output vector of translation vectors estimated for each pattern view. + * @param stdDeviationsIntrinsics Output vector of standard deviations estimated for intrinsic parameters. + * Order of deviations values: + * \f$(f_x, f_y, c_x, c_y, k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6 , s_1, s_2, s_3, + * s_4, \tau_x, \tau_y)\f$ If one of parameters is not estimated, it's deviation is equals to zero. + * @param stdDeviationsExtrinsics Output vector of standard deviations estimated for extrinsic parameters. + * Order of deviations values: \f$(R_1, T_1, \dotsc , R_M, T_M)\f$ where M is number of pattern views, + * \f$R_i, T_i\f$ are concatenated 1x3 vectors. + * @param perViewErrors Output vector of average re-projection errors estimated for each pattern view. + * @param flags flags Different flags for the calibration process (see #calibrateCamera for details). + * @param criteria Termination criteria for the iterative optimization algorithm. + * + * This function calibrates a camera using an Aruco Board. The function receives a list of + * detected markers from several views of the Board. The process is similar to the chessboard + * calibration in calibrateCamera(). The function returns the final re-projection error. + * + * @deprecated Use Board::matchImagePoints and cv::solvePnP + */ +CV_EXPORTS_AS(calibrateCameraArucoExtended) +double calibrateCameraAruco(InputArrayOfArrays corners, InputArray ids, InputArray counter, const Ptr &board, + Size imageSize, InputOutputArray cameraMatrix, InputOutputArray distCoeffs, + OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, OutputArray stdDeviationsIntrinsics, + OutputArray stdDeviationsExtrinsics, OutputArray perViewErrors, int flags = 0, + const TermCriteria& criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON)); + +/** @overload + * @brief It's the same function as #calibrateCameraAruco but without calibration error estimation. + * @deprecated Use Board::matchImagePoints and cv::solvePnP + */ +CV_EXPORTS_W double calibrateCameraAruco(InputArrayOfArrays corners, InputArray ids, InputArray counter, + const Ptr &board, Size imageSize, InputOutputArray cameraMatrix, + InputOutputArray distCoeffs, OutputArrayOfArrays rvecs = noArray(), + OutputArrayOfArrays tvecs = noArray(), int flags = 0, + const TermCriteria& criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, + 30, DBL_EPSILON)); + + +/** + * @brief Calibrate a camera using Charuco corners + * + * @param charucoCorners vector of detected charuco corners per frame + * @param charucoIds list of identifiers for each corner in charucoCorners per frame + * @param board Marker Board layout + * @param imageSize input image size + * @param cameraMatrix Output 3x3 floating-point camera matrix + * \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ . If CV\_CALIB\_USE\_INTRINSIC\_GUESS + * and/or CV_CALIB_FIX_ASPECT_RATIO are specified, some or all of fx, fy, cx, cy must be + * initialized before calling the function. + * @param distCoeffs Output vector of distortion coefficients + * \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6],[s_1, s_2, s_3, s_4]])\f$ of 4, 5, 8 or 12 elements + * @param rvecs Output vector of rotation vectors (see Rodrigues ) estimated for each board view + * (e.g. std::vector>). That is, each k-th rotation vector together with the corresponding + * k-th translation vector (see the next output parameter description) brings the board pattern + * from the model coordinate space (in which object points are specified) to the world coordinate + * space, that is, a real position of the board pattern in the k-th pattern view (k=0.. *M* -1). + * @param tvecs Output vector of translation vectors estimated for each pattern view. + * @param stdDeviationsIntrinsics Output vector of standard deviations estimated for intrinsic parameters. + * Order of deviations values: + * \f$(f_x, f_y, c_x, c_y, k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6 , s_1, s_2, s_3, + * s_4, \tau_x, \tau_y)\f$ If one of parameters is not estimated, it's deviation is equals to zero. + * @param stdDeviationsExtrinsics Output vector of standard deviations estimated for extrinsic parameters. + * Order of deviations values: \f$(R_1, T_1, \dotsc , R_M, T_M)\f$ where M is number of pattern views, + * \f$R_i, T_i\f$ are concatenated 1x3 vectors. + * @param perViewErrors Output vector of average re-projection errors estimated for each pattern view. + * @param flags flags Different flags for the calibration process (see #calibrateCamera for details). + * @param criteria Termination criteria for the iterative optimization algorithm. + * + * This function calibrates a camera using a set of corners of a Charuco Board. The function + * receives a list of detected corners and its identifiers from several views of the Board. + * The function returns the final re-projection error. + * + * @deprecated Use CharucoBoard::matchImagePoints and cv::solvePnP + */ +CV_EXPORTS_AS(calibrateCameraCharucoExtended) +double calibrateCameraCharuco(InputArrayOfArrays charucoCorners, InputArrayOfArrays charucoIds, + const Ptr &board, Size imageSize, InputOutputArray cameraMatrix, + InputOutputArray distCoeffs, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, + OutputArray stdDeviationsIntrinsics, OutputArray stdDeviationsExtrinsics, + OutputArray perViewErrors, int flags = 0, const TermCriteria& criteria = TermCriteria( + TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON)); + +/** + * @brief It's the same function as #calibrateCameraCharuco but without calibration error estimation. + * + * @deprecated Use CharucoBoard::matchImagePoints and cv::solvePnP + */ +CV_EXPORTS_W double calibrateCameraCharuco(InputArrayOfArrays charucoCorners, InputArrayOfArrays charucoIds, + const Ptr &board, Size imageSize, + InputOutputArray cameraMatrix, InputOutputArray distCoeffs, + OutputArrayOfArrays rvecs = noArray(), + OutputArrayOfArrays tvecs = noArray(), int flags = 0, + const TermCriteria& criteria=TermCriteria(TermCriteria::COUNT + + TermCriteria::EPS, 30, DBL_EPSILON)); +//! @} + +} +} +#endif diff --git a/sourceCode/aruco/charuco.cpp b/sourceCode/aruco/charuco.cpp new file mode 100644 index 0000000..3abd8b0 --- /dev/null +++ b/sourceCode/aruco/charuco.cpp @@ -0,0 +1,62 @@ +// 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 +#include "charuco.hpp" +#include + +namespace cv { +namespace aruco { + +using namespace std; + +int interpolateCornersCharuco(InputArrayOfArrays _markerCorners, InputArray _markerIds, + InputArray _image, const Ptr &_board, + OutputArray _charucoCorners, OutputArray _charucoIds, + InputArray _cameraMatrix, InputArray _distCoeffs, int minMarkers) { + CharucoParameters params; + params.minMarkers = minMarkers; + params.cameraMatrix = _cameraMatrix.getMat(); + params.distCoeffs = _distCoeffs.getMat(); + CharucoDetector detector(*_board, params); + vector markerCorners; + _markerCorners.getMatVector(markerCorners); + detector.detectBoard(_image, _charucoCorners, _charucoIds, markerCorners, _markerIds.getMat()); + return (int)_charucoIds.total(); +} + + +void detectCharucoDiamond(InputArray _image, InputArrayOfArrays _markerCorners, InputArray _markerIds, + float squareMarkerLengthRate, OutputArrayOfArrays _diamondCorners, OutputArray _diamondIds, + InputArray _cameraMatrix, InputArray _distCoeffs, Ptr dictionary) { + CharucoParameters params; + params.cameraMatrix = _cameraMatrix.getMat(); + params.distCoeffs = _distCoeffs.getMat(); + CharucoBoard board({3, 3}, squareMarkerLengthRate, 1.f, *dictionary); + CharucoDetector detector(board, params); + vector markerCorners; + _markerCorners.getMatVector(markerCorners); + + detector.detectDiamonds(_image, _diamondCorners, _diamondIds, markerCorners, _markerIds.getMat()); +} + + +void drawCharucoDiamond(const Ptr &dictionary, Vec4i ids, int squareLength, int markerLength, + OutputArray _img, int marginSize, int borderBits) { + CV_Assert(squareLength > 0 && markerLength > 0 && squareLength > markerLength); + CV_Assert(marginSize >= 0 && borderBits > 0); + + // assign the charuco marker ids + vector tmpIds(4); + for(int i = 0; i < 4; i++) + tmpIds[i] = ids[i]; + // create a charuco board similar to a charuco marker and print it + CharucoBoard board(Size(3, 3), (float)squareLength, (float)markerLength, *dictionary, tmpIds); + Size outSize(3 * squareLength + 2 * marginSize, 3 * squareLength + 2 * marginSize); + board.generateImage(outSize, _img, marginSize, borderBits); +} + +} +} diff --git a/sourceCode/aruco/charuco.hpp b/sourceCode/aruco/charuco.hpp new file mode 100644 index 0000000..60ba818 --- /dev/null +++ b/sourceCode/aruco/charuco.hpp @@ -0,0 +1,110 @@ +// 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 +#ifndef OPENCV_CHARUCO_HPP +#define OPENCV_CHARUCO_HPP + +#include +#include +#include "../aruco.hpp" +#include +#include "aruco_calib.hpp" + + +namespace cv { +namespace aruco { + +//! @addtogroup aruco +//! @{ + +/** + * @brief Interpolate position of ChArUco board corners + * @param markerCorners vector of already detected markers corners. For each marker, its four + * corners are provided, (e.g std::vector > ). For N detected markers, the + * dimensions of this array should be Nx4. The order of the corners should be clockwise. + * @param markerIds list of identifiers for each marker in corners + * @param image input image necesary for corner refinement. Note that markers are not detected and + * should be sent in corners and ids parameters. + * @param board layout of ChArUco board. + * @param charucoCorners interpolated chessboard corners + * @param charucoIds interpolated chessboard corners identifiers + * @param cameraMatrix optional 3x3 floating-point camera matrix + * \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ + * @param distCoeffs optional vector of distortion coefficients + * \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6],[s_1, s_2, s_3, s_4]])\f$ of 4, 5, 8 or 12 elements + * @param minMarkers number of adjacent markers that must be detected to return a charuco corner + * + * This function receives the detected markers and returns the 2D position of the chessboard corners + * from a ChArUco board using the detected Aruco markers. If camera parameters are provided, + * the process is based in an approximated pose estimation, else it is based on local homography. + * Only visible corners are returned. For each corner, its corresponding identifier is + * also returned in charucoIds. + * The function returns the number of interpolated corners. + * + * @deprecated Use CharucoDetector::detectBoard + */ +CV_EXPORTS_W int interpolateCornersCharuco(InputArrayOfArrays markerCorners, InputArray markerIds, + InputArray image, const Ptr &board, + OutputArray charucoCorners, OutputArray charucoIds, + InputArray cameraMatrix = noArray(), + InputArray distCoeffs = noArray(), int minMarkers = 2); + +/** + * @brief Detect ChArUco Diamond markers + * + * @param image input image necessary for corner subpixel. + * @param markerCorners list of detected marker corners from detectMarkers function. + * @param markerIds list of marker ids in markerCorners. + * @param squareMarkerLengthRate rate between square and marker length: + * squareMarkerLengthRate = squareLength/markerLength. The real units are not necessary. + * @param diamondCorners output list of detected diamond corners (4 corners per diamond). The order + * is the same than in marker corners: top left, top right, bottom right and bottom left. Similar + * format than the corners returned by detectMarkers (e.g std::vector > ). + * @param diamondIds ids of the diamonds in diamondCorners. The id of each diamond is in fact of + * type Vec4i, so each diamond has 4 ids, which are the ids of the aruco markers composing the + * diamond. + * @param cameraMatrix Optional camera calibration matrix. + * @param distCoeffs Optional camera distortion coefficients. + * @param dictionary dictionary of markers indicating the type of markers. + * + * This function detects Diamond markers from the previous detected ArUco markers. The diamonds + * are returned in the diamondCorners and diamondIds parameters. If camera calibration parameters + * are provided, the diamond search is based on reprojection. If not, diamond search is based on + * homography. Homography is faster than reprojection, but less accurate. + * + * @deprecated Use CharucoDetector::detectDiamonds + */ +CV_EXPORTS_W void detectCharucoDiamond(InputArray image, InputArrayOfArrays markerCorners, + InputArray markerIds, float squareMarkerLengthRate, + OutputArrayOfArrays diamondCorners, OutputArray diamondIds, + InputArray cameraMatrix = noArray(), + InputArray distCoeffs = noArray(), + Ptr dictionary = makePtr + (getPredefinedDictionary(PredefinedDictionaryType::DICT_4X4_50))); + + +/** + * @brief Draw a ChArUco Diamond marker + * + * @param dictionary dictionary of markers indicating the type of markers. + * @param ids list of 4 ids for each ArUco marker in the ChArUco marker. + * @param squareLength size of the chessboard squares in pixels. + * @param markerLength size of the markers in pixels. + * @param img output image with the marker. The size of this image will be + * 3*squareLength + 2*marginSize,. + * @param marginSize minimum margins (in pixels) of the marker in the output image + * @param borderBits width of the marker borders. + * + * This function return the image of a ChArUco marker, ready to be printed. + * + * @deprecated Use CharucoBoard::generateImage() + */ +CV_EXPORTS_W void drawCharucoDiamond(const Ptr &dictionary, Vec4i ids, int squareLength, + int markerLength, OutputArray img, int marginSize = 0, + int borderBits = 1); + +//! @} +} +} + +#endif diff --git a/sourceCode/aruco/precomp.hpp b/sourceCode/aruco/precomp.hpp new file mode 100644 index 0000000..e49b075 --- /dev/null +++ b/sourceCode/aruco/precomp.hpp @@ -0,0 +1,11 @@ +// 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 + +#ifndef __OPENCV_CCALIB_PRECOMP__ +#define __OPENCV_CCALIB_PRECOMP__ + +#include +#include + +#endif diff --git a/sourceCode/binocularMarkCam.cpp b/sourceCode/binocularMarkCam.cpp new file mode 100644 index 0000000..a54500a --- /dev/null +++ b/sourceCode/binocularMarkCam.cpp @@ -0,0 +1,238 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "binocularMarkCam_Export.h" +#include +#include "aruco.hpp" +#include "aruco/charuco.hpp" +#include + +#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& markerIdsAll, + std::vector >& markerCornersAll, + std::vector& charucoIds, + std::vector>& 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 ptrDictionary = cv::makePtr(aruco_dict); + cv::Ptr ptrParams = cv::makePtr(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 ids; + for (int i = 0; i < boardInfo.boardChaucoIDNum; i++) + ids.push_back(startId + i); + + // + std::vector markerIds; + std::vector > 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 board_charucoIds; + std::vector board_charucoCorners; + + cv::aruco::CharucoBoard board = cv::aruco::CharucoBoard(markInfo.patternSize, markInfo.checkerSize, markInfo.markerSize, aruco_dict, ids); + cv::Ptr ptrBoard = cv::makePtr(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(0, 3); + const double cy = Q.at(1, 3); + const double f = Q.at(2, 3); + const double t = 1.0 / Q.at(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 left; + std::vector 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& marks) +{ + std::vector markerIdsLeft; + std::vector > markerCornersLeft; + std::vector charucoIdsLeft; + std::vector> charucoCornersLeft;//ÿMark4ǵӦ4ά + detectCharucoCorners(leftImage, + markInfo, + boardInfo, + markerIdsLeft, + markerCornersLeft, + charucoIdsLeft, + charucoCornersLeft); + + std::vector markerIdsRight; + std::vector > markerCornersRight; + std::vector charucoIdsRight; + std::vector> 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 rectCornersLeft; + cv::undistortPoints(stereoMarks[i].left, rectCornersLeft, cameraMatrixL, distCoeffsL, R1, P1); + std::vector rectCornersRight; + cv::undistortPoints(stereoMarks[i].right, rectCornersRight, cameraMatrixR, distCoeffsR, R2, P2); + + std::vector 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; +} \ No newline at end of file diff --git a/sourceCode/binocularMarkCam_Export.h b/sourceCode/binocularMarkCam_Export.h new file mode 100644 index 0000000..cb80557 --- /dev/null +++ b/sourceCode/binocularMarkCam_Export.h @@ -0,0 +1,47 @@ +#pragma once + +#if defined(SG_API_LIBRARY) +# define WD_APISHARED_EXPORT __declspec(dllexport) +#else +# define WD_APISHARED_EXPORT __declspec(dllimport) +#endif + +#include "SG_baseDataType.h" +#include +#include + + +typedef struct +{ + cv::Size patternSize; //3x3 mark + float checkerSize; //60mm + float markerSize; //45mm + int dictType; //1: DICT_6x6 +}SWD_BQ_CharucoMarkInfo; + +typedef struct +{ + int totalBoardNum; //mark + int boardIdInterval; //ÿMarkcharucoļһ0ʼڶ8ʼΪ8 + int boardChaucoIDNum; //ÿMarkcharuco3x3charuco, άĸΪ4 +}SWD_BQ_MarkBoardInfo; +//汾 +WD_APISHARED_EXPORT const char* wd_charuco3DMarkVersion(void); + +//ȡMark3DϢ +WD_APISHARED_EXPORT 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& marks); \ No newline at end of file