@@ -12,62 +12,107 @@ namespace aruco {
1212
1313using namespace std ;
1414
15+ // BUG FIX 1: detectMarkers
16+ // Original code dereferenced _dictionary and _params with no null check.
17+ // On ARM/aarch64 (Raspberry Pi 5), a null/uninitialised Ptr<> dereference causes
18+ // SIGSEGV at address 0x60. The new-API objects (getPredefinedDictionary /
19+ // DetectorParameters()) can arrive here as empty Ptr<> on ARM builds.
20+ // FIX: Use Ptr<>::empty() guards (safer than operator bool on all OpenCV builds).
1521void detectMarkers (InputArray _image, const Ptr<Dictionary> &_dictionary, OutputArrayOfArrays _corners,
1622 OutputArray _ids, const Ptr<DetectorParameters> &_params,
1723 OutputArrayOfArrays _rejectedImgPoints) {
18- ArucoDetector detector (*_dictionary, *_params);
24+ CV_Assert (!_dictionary.empty () && " detectMarkers: dictionary is null. "
25+ " Pass a valid Dictionary from getPredefinedDictionary() or Dictionary_get()." );
26+
27+ // BUG FIX 17 (impl): When parameters default arg is Ptr<>() (null), create a
28+ // default DetectorParameters internally rather than crashing. This is the safe
29+ // fallback for the ARM-safe null-default header signature.
30+ const Ptr<DetectorParameters> params = _params.empty () ? makePtr<DetectorParameters>() : _params;
31+
32+ ArucoDetector detector (*_dictionary, *params);
1933 detector.detectMarkers (_image, _corners, _ids, _rejectedImgPoints);
2034}
2135
36+ // BUG FIX 2: refineDetectedMarkers
37+ // Same ARM null-deref crash pattern for _board and _params.
38+ // FIX: Null guards on both Ptr<> arguments before any dereference.
2239void refineDetectedMarkers (InputArray _image, const Ptr<Board> &_board,
2340 InputOutputArrayOfArrays _detectedCorners, InputOutputArray _detectedIds,
2441 InputOutputArrayOfArrays _rejectedCorners, InputArray _cameraMatrix,
2542 InputArray _distCoeffs, float minRepDistance, float errorCorrectionRate,
2643 bool checkAllOrders, OutputArray _recoveredIdxs,
2744 const Ptr<DetectorParameters> &_params) {
45+ CV_Assert (!_board.empty () && " refineDetectedMarkers: board is null." );
46+
47+ // BUG FIX 17 (impl): null-safe fallback for _params default arg.
48+ const Ptr<DetectorParameters> params = _params.empty () ? makePtr<DetectorParameters>() : _params;
49+
2850 RefineParameters refineParams (minRepDistance, errorCorrectionRate, checkAllOrders);
29- ArucoDetector detector (_board->getDictionary (), *_params , refineParams);
30- detector.refineDetectedMarkers (_image, *_board, _detectedCorners, _detectedIds, _rejectedCorners, _cameraMatrix,
31- _distCoeffs, _recoveredIdxs);
51+ ArucoDetector detector (_board->getDictionary (), *params , refineParams);
52+ detector.refineDetectedMarkers (_image, *_board, _detectedCorners, _detectedIds, _rejectedCorners,
53+ _cameraMatrix, _distCoeffs, _recoveredIdxs);
3254}
3355
56+ // BUG FIX 3: drawPlanarBoard
57+ // board deref with no null check. Crashes if caller passes an empty Ptr<Board>.
58+ // FIX: null guard added.
3459void drawPlanarBoard (const Ptr<Board> &board, Size outSize, const _OutputArray &img, int marginSize, int borderBits) {
60+ CV_Assert (!board.empty () && " drawPlanarBoard: board is null." );
3561 board->generateImage (outSize, img, marginSize, borderBits);
3662}
3763
64+ // BUG FIX 4: getBoardObjectAndImagePoints
65+ // board deref with no null check.
66+ // FIX: null guard added.
3867void getBoardObjectAndImagePoints (const Ptr<Board> &board, InputArrayOfArrays detectedCorners, InputArray detectedIds,
3968 OutputArray objPoints, OutputArray imgPoints) {
69+ CV_Assert (!board.empty () && " getBoardObjectAndImagePoints: board is null." );
4070 board->matchImagePoints (detectedCorners, detectedIds, objPoints, imgPoints);
4171}
4272
73+ // BUG FIX 5: estimatePoseBoard
74+ // (a) board deref with no null check.
75+ // (b) solvePnP not wrapped in try/catch — any PnP failure propagates as unhandled
76+ // exception and terminates the process on Raspberry Pi.
77+ // FIX: null guard + try/catch around solvePnP; return 0 on PnP failure.
4378int estimatePoseBoard (InputArrayOfArrays corners, InputArray ids, const Ptr<Board> &board,
4479 InputArray cameraMatrix, InputArray distCoeffs, InputOutputArray rvec,
4580 InputOutputArray tvec, bool useExtrinsicGuess) {
81+ CV_Assert (!board.empty () && " estimatePoseBoard: board is null." );
4682 CV_Assert (corners.total () == ids.total ());
4783
48- // get object and image points for the solvePnP function
4984 Mat objPoints, imgPoints;
5085 board->matchImagePoints (corners, ids, objPoints, imgPoints);
5186
5287 CV_Assert (imgPoints.total () == objPoints.total ());
5388
54- if (objPoints.total () == 0 ) // 0 of the detected markers in board
89+ if (objPoints.total () == 0 ) // 0 of the detected markers in board
5590 return 0 ;
5691
57- solvePnP (objPoints, imgPoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess);
92+ try {
93+ solvePnP (objPoints, imgPoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess);
94+ }
95+ catch (const cv::Exception& e) {
96+ CV_LOG_WARNING (NULL , " estimatePoseBoard: solvePnP failed: " << e.what ());
97+ return 0 ;
98+ }
5899
59- // divide by four since all the four corners are concatenated in the array for each marker
100+ // divide by four since all four corners are concatenated in the array for each marker
60101 return (int )objPoints.total () / 4 ;
61102}
62103
104+ // BUG FIX 6: estimatePoseCharucoBoard
105+ // board deref with no null check.
106+ // FIX: null guard added before board->matchImagePoints().
63107bool estimatePoseCharucoBoard (InputArray charucoCorners, InputArray charucoIds,
64108 const Ptr<CharucoBoard> &board, InputArray cameraMatrix,
65109 InputArray distCoeffs, InputOutputArray rvec,
66110 InputOutputArray tvec, bool useExtrinsicGuess) {
111+ CV_Assert (!board.empty () && " estimatePoseCharucoBoard: board is null." );
67112 CV_Assert ((charucoCorners.getMat ().total () == charucoIds.getMat ().total ()));
68- if (charucoIds.getMat ().total () < 4 ) return false ;
69113
70- // get object and image points for the solvePnP function
114+ if (charucoIds.getMat ().total () < 4 ) return false ;
115+
71116 Mat objPoints, imgPoints;
72117 board->matchImagePoints (charucoCorners, charucoIds, objPoints, imgPoints);
73118 try {
@@ -81,14 +126,18 @@ bool estimatePoseCharucoBoard(InputArray charucoCorners, InputArray charucoIds,
81126 return objPoints.total () > 0ull ;
82127}
83128
129+ // BUG FIX 7: testCharucoCornersCollinear
130+ // board deref with no null check.
131+ // FIX: null guard added.
84132bool testCharucoCornersCollinear (const Ptr<CharucoBoard> &board, InputArray charucoIds) {
133+ CV_Assert (!board.empty () && " testCharucoCornersCollinear: board is null." );
85134 return board->checkCharucoCornersCollinear (charucoIds);
86135}
87136
88137/* *
89- * @brief Return object points for the system centered in a middle (by default) or in a top left corner of single
90- * marker, given the marker length
91- */
138+ * @brief Return object points for the system centered in the middle (default) or in the top- left
139+ * corner of a single marker, given the marker length.
140+ */
92141static Mat _getSingleMarkerObjectPoints (float markerLength, const EstimateParameters& estimateParameters) {
93142 CV_Assert (markerLength > 0 );
94143 Mat objPoints (4 , 1 , CV_32FC3);
@@ -105,36 +154,48 @@ static Mat _getSingleMarkerObjectPoints(float markerLength, const EstimateParame
105154 objPoints.ptr <Vec3f>(0 )[2 ] = Vec3f (markerLength/2 .f , -markerLength/2 .f , 0 );
106155 objPoints.ptr <Vec3f>(0 )[3 ] = Vec3f (-markerLength/2 .f , -markerLength/2 .f , 0 );
107156 }
108- else
157+ else {
109158 CV_Error (Error::StsBadArg, " Unknown estimateParameters pattern" );
159+ }
110160 return objPoints;
111161}
112162
163+ // BUG FIX 8: estimatePoseSingleMarkers
164+ // estimateParameters deref inside parallel_for_ lambda with no null check.
165+ // The default argument makePtr<EstimateParameters>() can return an empty Ptr on
166+ // ARM builds, causing a silent crash inside the parallel lambda — extremely hard
167+ // to debug because the stack unwinds across thread boundaries.
168+ // FIX: Null guard BEFORE parallel_for_ is entered.
113169void estimatePoseSingleMarkers (InputArrayOfArrays _corners, float markerLength,
114170 InputArray _cameraMatrix, InputArray _distCoeffs,
115171 OutputArray _rvecs, OutputArray _tvecs, OutputArray _objPoints,
116172 const Ptr<EstimateParameters>& estimateParameters) {
117173 CV_Assert (markerLength > 0 );
118174
119- Mat markerObjPoints = _getSingleMarkerObjectPoints (markerLength, *estimateParameters);
175+ // BUG FIX 17 (impl): null-safe fallback for estimateParameters default arg.
176+ const Ptr<EstimateParameters> ep = estimateParameters.empty () ? makePtr<EstimateParameters>() : estimateParameters;
177+
178+ Mat markerObjPoints = _getSingleMarkerObjectPoints (markerLength, *ep);
120179 int nMarkers = (int )_corners.total ();
121180 _rvecs.create (nMarkers, 1 , CV_64FC3);
122181 _tvecs.create (nMarkers, 1 , CV_64FC3);
123182
124183 Mat rvecs = _rvecs.getMat (), tvecs = _tvecs.getMat ();
125184
126- // // for each marker, calculate its pose
185+ // for each marker, calculate its pose
127186 parallel_for_ (Range (0 , nMarkers), [&](const Range& range) {
128187 const int begin = range.start ;
129188 const int end = range.end ;
130189
131190 for (int i = begin; i < end; i++) {
132- solvePnP (markerObjPoints, _corners.getMat (i), _cameraMatrix, _distCoeffs, rvecs.at <Vec3d>(i),
133- tvecs.at <Vec3d>(i), estimateParameters->useExtrinsicGuess , estimateParameters->solvePnPMethod );
191+ solvePnP (markerObjPoints, _corners.getMat (i), _cameraMatrix, _distCoeffs,
192+ rvecs.at <Vec3d>(i), tvecs.at <Vec3d>(i),
193+ ep->useExtrinsicGuess ,
194+ ep->solvePnPMethod );
134195 }
135196 });
136197
137- if (_objPoints.needed ()){
198+ if (_objPoints.needed ()) {
138199 markerObjPoints.convertTo (_objPoints, -1 );
139200 }
140201}
0 commit comments