aruco_pose: Vendor in aruco library from OpenCV 3.4.6

aruco_pose/CMakeLists.txt

@@ -127,6 +127,8 @@ catkin_package(
 ## Build ##
 ###########
 
+add_subdirectory(src/aruco_lib)
+
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 include_directories(
@@ -163,6 +165,7 @@ add_dependencies(${PROJECT_NAME} aruco_pose_generate_messages_cpp)
 target_link_libraries(aruco_pose
   ${catkin_LIBRARIES}
   ${OpenCV_LIBRARIES}
+  aruco_lib
 )
 
 #############

aruco_pose/src/aruco_detect.cpp

@@ -40,7 +40,7 @@
 
 #include <opencv2/opencv.hpp>
 #include <opencv2/highgui.hpp>
-#include <opencv2/aruco.hpp>
+#include <aruco.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/calib3d/calib3d.hpp>
 
@@ -58,8 +58,8 @@ private:
 	tf2_ros::TransformBroadcaster br_;
 	tf2_ros::Buffer tf_buffer_;
 	tf2_ros::TransformListener tf_listener_{tf_buffer_};
-	cv::Ptr<cv::aruco::Dictionary> dictionary_;
-	cv::Ptr<cv::aruco::DetectorParameters> parameters_;
+	cv::Ptr<aruco_lib::Dictionary> dictionary_;
+	cv::Ptr<aruco_lib::DetectorParameters> parameters_;
 	image_transport::Publisher debug_pub_;
 	image_transport::CameraSubscriber img_sub_;
 	ros::Publisher markers_pub_, vis_markers_pub_;
@@ -97,9 +97,9 @@ public:
 		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
 		dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);
 
-		dictionary_ = cv::aruco::getPredefinedDictionary(static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(dictionary));
-		parameters_ = cv::aruco::DetectorParameters::create();
-		parameters_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
+		dictionary_ = aruco_lib::getPredefinedDictionary(static_cast<aruco_lib::PREDEFINED_DICTIONARY_NAME>(dictionary));
+		parameters_ = aruco_lib::DetectorParameters::create();
+		parameters_->cornerRefinementMethod = aruco_lib::CORNER_REFINE_SUBPIX;
 
 		image_transport::ImageTransport it(nh_);
 		image_transport::ImageTransport it_priv(nh_priv_);
@@ -125,7 +125,7 @@ private:
 		geometry_msgs::TransformStamped snap_to;
 
 		// Detect markers
-		cv::aruco::detectMarkers(image, dictionary_, corners, ids, parameters_, rejected);
+		aruco_lib::detectMarkers(image, dictionary_, corners, ids, parameters_, rejected);
 
 		array_.header.stamp = msg->header.stamp;
 		array_.header.frame_id = msg->header.frame_id;
@@ -136,7 +136,7 @@ private:
 
 			// Estimate individual markers' poses
 			if (estimate_poses_) {
-				cv::aruco::estimatePoseSingleMarkers(corners, length_, camera_matrix_, dist_coeffs_,
+				aruco_lib::estimatePoseSingleMarkers(corners, length_, camera_matrix_, dist_coeffs_,
 				                                     rvecs, tvecs);
 
 				// process length override, TODO: efficiency
@@ -148,7 +148,7 @@ private:
 							vector<cv::Vec3d> rvecs_current, tvecs_current;
 							vector<vector<cv::Point2f>> corners_current;
 							corners_current.push_back(corners[i]);
-							cv::aruco::estimatePoseSingleMarkers(corners_current, item->second,
+							aruco_lib::estimatePoseSingleMarkers(corners_current, item->second,
 							                                     camera_matrix_, dist_coeffs_,
 										                         rvecs_current, tvecs_current);
 							rvecs[i] = rvecs_current[0];
@@ -223,10 +223,10 @@ private:
 		// Publish debug image
 		if (debug_pub_.getNumSubscribers() != 0) {
 			Mat debug = image.clone();
-			cv::aruco::drawDetectedMarkers(debug, corners, ids); // draw markers
+			aruco_lib::drawDetectedMarkers(debug, corners, ids); // draw markers
 			if (estimate_poses_)
 				for (unsigned int i = 0; i < ids.size(); i++)
-					cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_,
+					aruco_lib::drawAxis(debug, camera_matrix_, dist_coeffs_,
 					                    rvecs[i], tvecs[i], getMarkerLength(ids[i]));
 
 			cv_bridge::CvImage out_msg;

aruco_pose/src/aruco_lib/CMakeLists.txt

@@ -0,0 +1,69 @@
+# aruco_lib: ArUco detection and pose estimation routines
+# Lifted directly from OpenCV, modified to not conflict with it
+
+# I'm specifying static linkage here, since I'm not sure where catkin is going to put
+# my shared libraries (and whether they're going to be packaged)
+add_library(aruco_lib STATIC
+        # ArUco detection and pose estimation
+        aruco/apriltag_quad_thresh.cpp
+        aruco/aruco.cpp
+        aruco/charuco.cpp
+        aruco/dictionary.cpp
+        aruco/zmaxheap.cpp
+        # Camera calibration routines (for SolvePnP)
+        # cv_calib_camera/ap3p.cpp
+        # cv_calib_camera/calib3d_c_api.cpp
+        # cv_calib_camera/calibinit.cpp
+        # cv_calib_camera/calibration.cpp
+        # cv_calib_camera/calibration_handeye.cpp
+        # cv_calib_camera/circlesgrid.cpp
+        # cv_calib_camera/compat_ptsetreg.cpp
+        # cv_calib_camera/compat_stereo.cpp
+        # cv_calib_camera/dls.cpp
+        # cv_calib_camera/epnp.cpp
+        # cv_calib_camera/fisheye.cpp
+        # cv_calib_camera/five-point.cpp
+        # cv_calib_camera/fundam.cpp
+        # cv_calib_camera/homography_decomp.cpp
+        # cv_calib_camera/levmarq.cpp
+        # cv_calib_camera/main.cpp
+        # cv_calib_camera/p3p.cpp
+        # cv_calib_camera/polynom_solver.cpp
+        # cv_calib_camera/posit.cpp
+        # cv_calib_camera/ptsetreg.cpp
+        # cv_calib_camera/quadsubpix.cpp
+        # cv_calib_camera/rho.cpp
+        # cv_calib_camera/solvepnp.cpp
+        # cv_calib_camera/stereobm.cpp
+        # cv_calib_camera/stereosgbm.cpp
+        # cv_calib_camera/triangulate.cpp
+        # cv_calib_camera/upnp.cpp
+        )
+
+target_link_libraries(aruco_lib
+        PUBLIC
+        ${OpenCV_LIBRARIES}
+        )
+
+target_include_directories(aruco_lib
+        PRIVATE
+        aruco
+        # cv_calib_camera
+        )
+
+target_include_directories(aruco_lib
+        PUBLIC
+        aruco/include
+        # cv_calib_camera/include
+        ${OpenCV_INCLUDE_DIRS}
+        )
+
+target_compile_definitions(aruco_lib
+        PRIVATE
+        CV_OVERRIDE=override
+        )
+
+target_compile_options(aruco_lib
+        PRIVATE
+        -fpic -fPIC
+        )

aruco_pose/src/aruco_lib/aruco/apriltag_quad_thresh.hpp

@@ -0,0 +1,125 @@
+// 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.
+//
+// Copyright (C) 2013-2016, The Regents of The University of Michigan.
+//
+// This software was developed in the APRIL Robotics Lab under the
+// direction of Edwin Olson, ebolson@umich.edu. This software may be
+// available under alternative licensing terms; contact the address above.
+//
+// The views and conclusions contained in the software and documentation are those
+// of the authors and should not be interpreted as representing official policies,
+// either expressed or implied, of the Regents of The University of Michigan.
+
+// limitation: image size must be <32768 in width and height. This is
+// because we use a fixed-point 16 bit integer representation with one
+// fractional bit.
+
+#ifndef _OPENCV_APRIL_QUAD_THRESH_HPP_
+#define _OPENCV_APRIL_QUAD_THRESH_HPP_
+
+#include "aruco.hpp"
+#include "unionfind.hpp"
+#include "zmaxheap.hpp"
+#include "zarray.hpp"
+
+namespace aruco_lib {
+
+static inline uint32_t u64hash_2(uint64_t x) {
+	return uint32_t((2654435761UL * x) >> 32);
+}
+
+struct uint64_zarray_entry{
+	uint64_t id;
+	zarray_t *cluster;
+
+	struct uint64_zarray_entry *next;
+};
+
+struct pt{
+	// Note: these represent 2*actual value.
+	uint16_t x, y;
+	float theta;
+	int16_t gx, gy;
+};
+
+struct remove_vertex{
+	int i;           // which vertex to remove?
+	int left, right; // left vertex, right vertex
+
+	double err;
+};
+
+struct segment{
+	int is_vertex;
+
+	// always greater than zero, but right can be > size, which denotes
+	// a wrap around back to the beginning of the points. and left < right.
+	int left, right;
+};
+
+struct line_fit_pt{
+	double Mx, My;
+	double Mxx, Myy, Mxy;
+	double W; // total weight
+};
+
+/**
+ * lfps contains *cumulative* moments for N points, with
+ * index j reflecting points [0,j] (inclusive).
+ * fit a line to the points [i0, i1] (inclusive). i0, i1 are both (0, sz)
+ * if i1 < i0, we treat this as a wrap around.
+ */
+void fit_line(struct line_fit_pt *lfps, int sz, int i0, int i1, double *lineparm, double *err, double *mse);
+
+int err_compare_descending(const void *_a, const void *_b);
+
+/**
+  1. Identify A) white points near a black point and B) black points near a white point.
+
+  2. Find the connected components within each of the classes above,
+  yielding clusters of "white-near-black" and
+  "black-near-white". (These two classes are kept separate). Each
+  segment has a unique id.
+
+  3. For every pair of "white-near-black" and "black-near-white"
+  clusters, find the set of points that are in one and adjacent to the
+  other. In other words, a "boundary" layer between the two
+  clusters. (This is actually performed by iterating over the pixels,
+  rather than pairs of clusters.) Critically, this helps keep nearby
+  edges from becoming connected.
+ **/
+int quad_segment_maxima(const cv::Ptr<DetectorParameters> &td, int sz, struct line_fit_pt *lfps, int indices[4]);
+
+/**
+ * returns 0 if the cluster looks bad.
+ */
+int quad_segment_agg(int sz, struct line_fit_pt *lfps, int indices[4]);
+
+/**
+ *  return 1 if the quad looks okay, 0 if it should be discarded
+ *  quad
+ **/
+int fit_quad(const cv::Ptr<DetectorParameters> &_params, const cv::Mat im, zarray_t *cluster, struct sQuad *quad);
+
+/**
+ *
+ * @param mIm
+ * @param parameters
+ * @param mThresh
+ */
+void threshold(const cv::Mat mIm, const cv::Ptr<DetectorParameters> &parameters, cv::Mat& mThresh);
+
+/**
+ *
+ * @param parameters
+ * @param mImg
+ * @param contours
+ * @return
+ */
+zarray_t *apriltag_quad_thresh(const cv::Ptr<DetectorParameters> &parameters, const cv::Mat & mImg, std::vector< std::vector< cv::Point > > &contours);
+
+}
+
+#endif

aruco_pose/src/aruco_lib/aruco/charuco.cpp

@@ -0,0 +1,931 @@
+/*
+By downloading, copying, installing or using the software you agree to this
+license. If you do not agree to this license, do not download, install,
+copy or use the software.
+
+                          License Agreement
+               For Open Source Computer Vision Library
+                       (3-clause BSD License)
+
+Copyright (C) 2013, OpenCV Foundation, all rights reserved.
+Third party copyrights are property of their respective owners.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+  * Redistributions of source code must retain the above copyright notice,
+    this list of conditions and the following disclaimer.
+
+  * Redistributions in binary form must reproduce the above copyright notice,
+    this list of conditions and the following disclaimer in the documentation
+    and/or other materials provided with the distribution.
+
+  * Neither the names of the copyright holders nor the names of the contributors
+    may be used to endorse or promote products derived from this software
+    without specific prior written permission.
+
+This software is provided by the copyright holders and contributors "as is" and
+any express or implied warranties, including, but not limited to, the implied
+warranties of merchantability and fitness for a particular purpose are
+disclaimed. In no event shall copyright holders or contributors be liable for
+any direct, indirect, incidental, special, exemplary, or consequential damages
+(including, but not limited to, procurement of substitute goods or services;
+loss of use, data, or profits; or business interruption) however caused
+and on any theory of liability, whether in contract, strict liability,
+or tort (including negligence or otherwise) arising in any way out of
+the use of this software, even if advised of the possibility of such damage.
+*/
+
+#include "precomp.hpp"
+#include "aruco/charuco.hpp"
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+
+
+namespace aruco_lib {
+
+using namespace std;
+using namespace cv;
+
+
+/**
+ */
+void CharucoBoard::draw(Size outSize, OutputArray _img, int marginSize, int borderBits) {
+
+    CV_Assert(!(outSize.width <= 0 || outSize.height <= 0));
+    CV_Assert(marginSize >= 0);
+
+    _img.create(outSize, CV_8UC1);
+    _img.setTo(255);
+    Mat out = _img.getMat();
+    Mat noMarginsImg =
+        out.colRange(marginSize, out.cols - marginSize).rowRange(marginSize, out.rows - marginSize);
+
+    double totalLengthX, totalLengthY;
+    totalLengthX = _squareLength * _squaresX;
+    totalLengthY = _squareLength * _squaresY;
+
+    // proportional transformation
+    double xReduction = totalLengthX / double(noMarginsImg.cols);
+    double yReduction = totalLengthY / double(noMarginsImg.rows);
+
+    // determine the zone where the chessboard is placed
+    Mat chessboardZoneImg;
+    if(xReduction > yReduction) {
+        int nRows = int(totalLengthY / xReduction);
+        int rowsMargins = (noMarginsImg.rows - nRows) / 2;
+        chessboardZoneImg = noMarginsImg.rowRange(rowsMargins, noMarginsImg.rows - rowsMargins);
+    } else {
+        int nCols = int(totalLengthX / yReduction);
+        int colsMargins = (noMarginsImg.cols - nCols) / 2;
+        chessboardZoneImg = noMarginsImg.colRange(colsMargins, noMarginsImg.cols - colsMargins);
+    }
+
+    // determine the margins to draw only the markers
+    // take the minimum just to be sure
+    double squareSizePixels = min(double(chessboardZoneImg.cols) / double(_squaresX),
+                                  double(chessboardZoneImg.rows) / double(_squaresY));
+
+    double diffSquareMarkerLength = (_squareLength - _markerLength) / 2;
+    int diffSquareMarkerLengthPixels =
+        int(diffSquareMarkerLength * squareSizePixels / _squareLength);
+
+    // draw markers
+    Mat markersImg;
+    aruco_lib::_drawPlanarBoardImpl(this, chessboardZoneImg.size(), markersImg,
+                                diffSquareMarkerLengthPixels, borderBits);
+
+    markersImg.copyTo(chessboardZoneImg);
+
+    // now draw black squares
+    for(int y = 0; y < _squaresY; y++) {
+        for(int x = 0; x < _squaresX; x++) {
+
+            if(y % 2 != x % 2) continue; // white corner, dont do anything
+
+            double startX, startY;
+            startX = squareSizePixels * double(x);
+            startY = double(chessboardZoneImg.rows) - squareSizePixels * double(y + 1);
+
+            Mat squareZone = chessboardZoneImg.rowRange(int(startY), int(startY + squareSizePixels))
+                                 .colRange(int(startX), int(startX + squareSizePixels));
+
+            squareZone.setTo(0);
+        }
+    }
+}
+
+
+
+/**
+ */
+Ptr<CharucoBoard> CharucoBoard::create(int squaresX, int squaresY, float squareLength,
+                                  float markerLength, const Ptr<Dictionary> &dictionary) {
+
+    CV_Assert(squaresX > 1 && squaresY > 1 && markerLength > 0 && squareLength > markerLength);
+    Ptr<CharucoBoard> res = makePtr<CharucoBoard>();
+
+    res->_squaresX = squaresX;
+    res->_squaresY = squaresY;
+    res->_squareLength = squareLength;
+    res->_markerLength = markerLength;
+    res->dictionary = dictionary;
+
+    float diffSquareMarkerLength = (squareLength - markerLength) / 2;
+
+    // calculate Board objPoints
+    for(int y = squaresY - 1; y >= 0; y--) {
+        for(int x = 0; x < squaresX; x++) {
+
+            if(y % 2 == x % 2) continue; // black corner, no marker here
+
+            vector< Point3f > corners;
+            corners.resize(4);
+            corners[0] = Point3f(x * squareLength + diffSquareMarkerLength,
+                                 y * squareLength + diffSquareMarkerLength + markerLength, 0);
+            corners[1] = corners[0] + Point3f(markerLength, 0, 0);
+            corners[2] = corners[0] + Point3f(markerLength, -markerLength, 0);
+            corners[3] = corners[0] + Point3f(0, -markerLength, 0);
+            res->objPoints.push_back(corners);
+            // first ids in dictionary
+            int nextId = (int)res->ids.size();
+            res->ids.push_back(nextId);
+        }
+    }
+
+    // now fill chessboardCorners
+    for(int y = 0; y < squaresY - 1; y++) {
+        for(int x = 0; x < squaresX - 1; x++) {
+            Point3f corner;
+            corner.x = (x + 1) * squareLength;
+            corner.y = (y + 1) * squareLength;
+            corner.z = 0;
+            res->chessboardCorners.push_back(corner);
+        }
+    }
+
+    res->_getNearestMarkerCorners();
+
+    return res;
+}
+
+
+
+/**
+  * Fill nearestMarkerIdx and nearestMarkerCorners arrays
+  */
+void CharucoBoard::_getNearestMarkerCorners() {
+
+    nearestMarkerIdx.resize(chessboardCorners.size());
+    nearestMarkerCorners.resize(chessboardCorners.size());
+
+    unsigned int nMarkers = (unsigned int)ids.size();
+    unsigned int nCharucoCorners = (unsigned int)chessboardCorners.size();
+    for(unsigned int i = 0; i < nCharucoCorners; i++) {
+        double minDist = -1; // distance of closest markers
+        Point3f charucoCorner = chessboardCorners[i];
+        for(unsigned int j = 0; j < nMarkers; j++) {
+            // calculate distance from marker center to charuco corner
+            Point3f center = Point3f(0, 0, 0);
+            for(unsigned int k = 0; k < 4; k++)
+                center += objPoints[j][k];
+            center /= 4.;
+            double sqDistance;
+            Point3f distVector = charucoCorner - center;
+            sqDistance = distVector.x * distVector.x + distVector.y * distVector.y;
+            if(j == 0 || fabs(sqDistance - minDist) < cv::pow(0.01 * _squareLength, 2)) {
+                // if same minimum distance (or first iteration), add to nearestMarkerIdx vector
+                nearestMarkerIdx[i].push_back(j);
+                minDist = sqDistance;
+            } else if(sqDistance < minDist) {
+                // if finding a closest marker to the charuco corner
+                nearestMarkerIdx[i].clear(); // remove any previous added marker
+                nearestMarkerIdx[i].push_back(j); // add the new closest marker index
+                minDist = sqDistance;
+            }
+        }
+
+        // for each of the closest markers, search the marker corner index closer
+        // to the charuco corner
+        for(unsigned int j = 0; j < nearestMarkerIdx[i].size(); j++) {
+            nearestMarkerCorners[i].resize(nearestMarkerIdx[i].size());
+            double minDistCorner = -1;
+            for(unsigned int k = 0; k < 4; k++) {
+                double sqDistance;
+                Point3f distVector = charucoCorner - objPoints[nearestMarkerIdx[i][j]][k];
+                sqDistance = distVector.x * distVector.x + distVector.y * distVector.y;
+                if(k == 0 || sqDistance < minDistCorner) {
+                    // if this corner is closer to the charuco corner, assing its index
+                    // to nearestMarkerCorners
+                    minDistCorner = sqDistance;
+                    nearestMarkerCorners[i][j] = k;
+                }
+            }
+        }
+    }
+}
+
+
+/**
+  * Remove charuco corners if any of their minMarkers closest markers has not been detected
+  */
+static int _filterCornersWithoutMinMarkers(const Ptr<CharucoBoard> &_board,
+                                                    InputArray _allCharucoCorners,
+                                                    InputArray _allCharucoIds,
+                                                    InputArray _allArucoIds, int minMarkers,
+                                                    OutputArray _filteredCharucoCorners,
+                                                    OutputArray _filteredCharucoIds) {
+
+    CV_Assert(minMarkers >= 0 && minMarkers <= 2);
+
+    vector< Point2f > filteredCharucoCorners;
+    vector< int > filteredCharucoIds;
+    // for each charuco corner
+    for(unsigned int i = 0; i < _allCharucoIds.getMat().total(); 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().at< int >(k) == markerId) {
+                    found = true;
+                    break;
+                }
+            }
+            if(found) totalMarkers++;
+        }
+        // if enough markers detected, add the charuco corner to the final list
+        if(totalMarkers >= minMarkers) {
+            filteredCharucoIds.push_back(currentCharucoId);
+            filteredCharucoCorners.push_back(_allCharucoCorners.getMat().at< Point2f >(i));
+        }
+    }
+
+    // parse output
+    Mat(filteredCharucoCorners).copyTo(_filteredCharucoCorners);
+    Mat(filteredCharucoIds).copyTo(_filteredCharucoIds);
+    return (int)_filteredCharucoIds.total();
+}
+
+
+/**
+  * ParallelLoopBody class for the parallelization of the charuco corners subpixel refinement
+  * Called from function _selectAndRefineChessboardCorners()
+  */
+class CharucoSubpixelParallel : public ParallelLoopBody {
+    public:
+    CharucoSubpixelParallel(const Mat *_grey, vector< Point2f > *_filteredChessboardImgPoints,
+                            vector< Size > *_filteredWinSizes, const Ptr<DetectorParameters> &_params)
+        : grey(_grey), filteredChessboardImgPoints(_filteredChessboardImgPoints),
+          filteredWinSizes(_filteredWinSizes), params(_params) {}
+
+    void operator()(const Range &range) const CV_OVERRIDE {
+        const int begin = range.start;
+        const int end = range.end;
+
+        for(int i = begin; i < end; i++) {
+            vector< Point2f > in;
+            in.push_back((*filteredChessboardImgPoints)[i]);
+            Size winSize = (*filteredWinSizes)[i];
+            if(winSize.height == -1 || winSize.width == -1)
+                winSize = Size(params->cornerRefinementWinSize, params->cornerRefinementWinSize);
+
+            cornerSubPix(*grey, in, winSize, Size(),
+                         TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS,
+                                      params->cornerRefinementMaxIterations,
+                                      params->cornerRefinementMinAccuracy));
+
+            (*filteredChessboardImgPoints)[i] = in[0];
+        }
+    }
+
+    private:
+    CharucoSubpixelParallel &operator=(const CharucoSubpixelParallel &); // to quiet MSVC
+
+    const Mat *grey;
+    vector< Point2f > *filteredChessboardImgPoints;
+    vector< Size > *filteredWinSizes;
+    const Ptr<DetectorParameters> &params;
+};
+
+
+
+
+/**
+  * @brief From all projected chessboard corners, select those inside the image and apply subpixel
+  * refinement. Returns number of valid corners.
+  */
+static int _selectAndRefineChessboardCorners(InputArray _allCorners, InputArray _image,
+                                                      OutputArray _selectedCorners,
+                                                      OutputArray _selectedIds,
+                                                      const vector< Size > &winSizes) {
+
+    const int minDistToBorder = 2; // minimum distance of the corner to the image border
+    // remaining corners, ids and window refinement sizes after removing corners outside the image
+    vector< Point2f > filteredChessboardImgPoints;
+    vector< Size > filteredWinSizes;
+    vector< int > filteredIds;
+
+    // filter corners outside the image
+    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().at< Point2f >(i))) {
+            filteredChessboardImgPoints.push_back(_allCorners.getMat().at< Point2f >(i));
+            filteredIds.push_back(i);
+            filteredWinSizes.push_back(winSizes[i]);
+        }
+    }
+
+    // if none valid, return 0
+    if(filteredChessboardImgPoints.size() == 0) return 0;
+
+    // corner refinement, first convert input image to grey
+    Mat grey;
+    if(_image.type() == CV_8UC3)
+        cvtColor(_image, grey, COLOR_BGR2GRAY);
+    else
+        _image.copyTo(grey);
+
+    const Ptr<DetectorParameters> params = DetectorParameters::create(); // use default params for corner refinement
+
+    //// For each of the charuco corners, apply subpixel refinement using its correspondind winSize
+    // for(unsigned int i=0; i<filteredChessboardImgPoints.size(); i++) {
+    //    vector<Point2f> in;
+    //    in.push_back(filteredChessboardImgPoints[i]);
+    //    Size winSize = filteredWinSizes[i];
+    //    if(winSize.height == -1 || winSize.width == -1)
+    //        winSize = Size(params.cornerRefinementWinSize, params.cornerRefinementWinSize);
+    //    cornerSubPix(grey, in, winSize, Size(),
+    //                 TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS,
+    //                              params->cornerRefinementMaxIterations,
+    //                              params->cornerRefinementMinAccuracy));
+    //    filteredChessboardImgPoints[i] = in[0];
+    //}
+
+    // this is the parallel call for the previous commented loop (result is equivalent)
+    parallel_for_(
+        Range(0, (int)filteredChessboardImgPoints.size()),
+        CharucoSubpixelParallel(&grey, &filteredChessboardImgPoints, &filteredWinSizes, params));
+
+    // parse output
+    Mat(filteredChessboardImgPoints).copyTo(_selectedCorners);
+    Mat(filteredIds).copyTo(_selectedIds);
+    return (int)filteredChessboardImgPoints.size();
+}
+
+
+/**
+  * Calculate the maximum window sizes for corner refinement for each charuco corner based on the
+  * distance to their closest markers
+  */
+static void _getMaximumSubPixWindowSizes(InputArrayOfArrays markerCorners, InputArray markerIds,
+                                         InputArray charucoCorners, const Ptr<CharucoBoard> &board,
+                                         vector< Size > &sizes) {
+
+    unsigned int nCharucoCorners = (unsigned int)charucoCorners.getMat().total();
+    sizes.resize(nCharucoCorners, Size(-1, -1));
+
+    for(unsigned int i = 0; i < nCharucoCorners; i++) {
+        if(charucoCorners.getMat().at< Point2f >(i) == Point2f(-1, -1)) continue;
+        if(board->nearestMarkerIdx[i].size() == 0) continue;
+
+        double minDist = -1;
+        int counter = 0;
+
+        // calculate the distance to each of the closest corner of each closest marker
+        for(unsigned int j = 0; j < board->nearestMarkerIdx[i].size(); j++) {
+            // find marker
+            int markerId = board->ids[board->nearestMarkerIdx[i][j]];
+            int markerIdx = -1;
+            for(unsigned int k = 0; k < markerIds.getMat().total(); k++) {
+                if(markerIds.getMat().at< int >(k) == markerId) {
+                    markerIdx = k;
+                    break;
+                }
+            }
+            if(markerIdx == -1) continue;
+            Point2f markerCorner =
+                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);
+            counter++;
+        }
+
+        // if this is the first closest marker, dont do anything
+        if(counter == 0)
+            continue;
+        else {
+            // else, calculate the maximum window size
+            int winSizeInt = int(minDist - 2); // remove 2 pixels for safety
+            if(winSizeInt < 1) winSizeInt = 1; // minimum size is 1
+            if(winSizeInt > 10) winSizeInt = 10; // maximum size is 10
+            sizes[i] = Size(winSizeInt, winSizeInt);
+        }
+    }
+}
+
+
+
+/**
+  * Interpolate charuco corners using approximated pose estimation
+  */
+static int _interpolateCornersCharucoApproxCalib(InputArrayOfArrays _markerCorners,
+                                                 InputArray _markerIds, InputArray _image,
+                                                 const Ptr<CharucoBoard> &_board,
+                                                 InputArray _cameraMatrix, InputArray _distCoeffs,
+                                                 OutputArray _charucoCorners,
+                                                 OutputArray _charucoIds) {
+
+    CV_Assert(_image.getMat().channels() == 1 || _image.getMat().channels() == 3);
+    CV_Assert(_markerCorners.total() == _markerIds.getMat().total() &&
+              _markerIds.getMat().total() > 0);
+
+    // approximated pose estimation using marker corners
+    Mat approximatedRvec, approximatedTvec;
+    int detectedBoardMarkers;
+    Ptr<Board> _b = _board.staticCast<Board>();
+    detectedBoardMarkers =
+        aruco_lib::estimatePoseBoard(_markerCorners, _markerIds, _b,
+                                 _cameraMatrix, _distCoeffs, approximatedRvec, approximatedTvec);
+
+    if(detectedBoardMarkers == 0) return 0;
+
+    // project chessboard corners
+    vector< Point2f > allChessboardImgPoints;
+
+    projectPoints(_board->chessboardCorners, approximatedRvec, approximatedTvec, _cameraMatrix,
+                  _distCoeffs, allChessboardImgPoints);
+
+
+    // calculate maximum window sizes for subpixel refinement. The size is limited by the distance
+    // to the closes marker corner to avoid erroneous displacements to marker corners
+    vector< Size > subPixWinSizes;
+    _getMaximumSubPixWindowSizes(_markerCorners, _markerIds, allChessboardImgPoints, _board,
+                                 subPixWinSizes);
+
+    // filter corners outside the image and subpixel-refine charuco corners
+    return _selectAndRefineChessboardCorners(allChessboardImgPoints, _image, _charucoCorners,
+                                             _charucoIds, subPixWinSizes);
+}
+
+
+
+/**
+  * Interpolate charuco corners using local homography
+  */
+static int _interpolateCornersCharucoLocalHom(InputArrayOfArrays _markerCorners,
+                                              InputArray _markerIds, InputArray _image,
+                                              const Ptr<CharucoBoard> &_board,
+                                              OutputArray _charucoCorners,
+                                              OutputArray _charucoIds) {
+
+    CV_Assert(_image.getMat().channels() == 1 || _image.getMat().channels() == 3);
+    CV_Assert(_markerCorners.total() == _markerIds.getMat().total() &&
+              _markerIds.getMat().total() > 0);
+
+    unsigned int nMarkers = (unsigned int)_markerIds.getMat().total();
+
+    // calculate local homographies for each marker
+    vector< Mat > transformations;
+    transformations.resize(nMarkers);
+    for(unsigned int i = 0; i < nMarkers; i++) {
+        vector< Point2f > markerObjPoints2D;
+        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);
+        markerObjPoints2D.resize(4);
+        for(unsigned int j = 0; j < 4; j++)
+            markerObjPoints2D[j] =
+                Point2f(_board->objPoints[boardIdx][j].x, _board->objPoints[boardIdx][j].y);
+
+        transformations[i] = getPerspectiveTransform(markerObjPoints2D, _markerCorners.getMat(i));
+    }
+
+    unsigned int nCharucoCorners = (unsigned int)_board->chessboardCorners.size();
+    vector< Point2f > allChessboardImgPoints(nCharucoCorners, Point2f(-1, -1));
+
+    // for each charuco corner, calculate its interpolation position based on the closest markers
+    // homographies
+    for(unsigned int i = 0; i < nCharucoCorners; i++) {
+        Point2f objPoint2D = Point2f(_board->chessboardCorners[i].x, _board->chessboardCorners[i].y);
+
+        vector< Point2f > interpolatedPositions;
+        for(unsigned int j = 0; j < _board->nearestMarkerIdx[i].size(); j++) {
+            int markerId = _board->ids[_board->nearestMarkerIdx[i][j]];
+            int markerIdx = -1;
+            for(unsigned int k = 0; k < _markerIds.getMat().total(); k++) {
+                if(_markerIds.getMat().at< int >(k) == markerId) {
+                    markerIdx = k;
+                    break;
+                }
+            }
+            if(markerIdx != -1) {
+                vector< Point2f > in, out;
+                in.push_back(objPoint2D);
+                perspectiveTransform(in, out, transformations[markerIdx]);
+                interpolatedPositions.push_back(out[0]);
+            }
+        }
+
+        // none of the closest markers detected
+        if(interpolatedPositions.size() == 0) continue;
+
+        // more than one closest marker detected, take middle point
+        if(interpolatedPositions.size() > 1) {
+            allChessboardImgPoints[i] = (interpolatedPositions[0] + interpolatedPositions[1]) / 2.;
+        }
+        // a single closest marker detected
+        else allChessboardImgPoints[i] = interpolatedPositions[0];
+    }
+
+    // calculate maximum window sizes for subpixel refinement. The size is limited by the distance
+    // to the closes marker corner to avoid erroneous displacements to marker corners
+    vector< Size > subPixWinSizes;
+    _getMaximumSubPixWindowSizes(_markerCorners, _markerIds, allChessboardImgPoints, _board,
+                                 subPixWinSizes);
+
+
+    // filter corners outside the image and subpixel-refine charuco corners
+    return _selectAndRefineChessboardCorners(allChessboardImgPoints, _image, _charucoCorners,
+                                             _charucoIds, subPixWinSizes);
+}
+
+
+
+/**
+  */
+int interpolateCornersCharuco(InputArrayOfArrays _markerCorners, InputArray _markerIds,
+                              InputArray _image, const Ptr<CharucoBoard> &_board,
+                              OutputArray _charucoCorners, OutputArray _charucoIds,
+                              InputArray _cameraMatrix, InputArray _distCoeffs, int minMarkers) {
+
+    // if camera parameters are avaible, use approximated calibration
+    if(_cameraMatrix.total() != 0) {
+        _interpolateCornersCharucoApproxCalib(_markerCorners, _markerIds, _image, _board,
+                                                     _cameraMatrix, _distCoeffs, _charucoCorners,
+                                                     _charucoIds);
+    }
+    // else use local homography
+    else {
+        _interpolateCornersCharucoLocalHom(_markerCorners, _markerIds, _image, _board,
+                                                  _charucoCorners, _charucoIds);
+    }
+
+    // to return a charuco corner, its closest aruco markers should have been detected
+    return _filterCornersWithoutMinMarkers(_board, _charucoCorners, _charucoIds, _markerIds,
+                                           minMarkers, _charucoCorners, _charucoIds);
+}
+
+
+
+/**
+  */
+void drawDetectedCornersCharuco(InputOutputArray _image, InputArray _charucoCorners,
+                                InputArray _charucoIds, Scalar cornerColor) {
+
+    CV_Assert(_image.getMat().total() != 0 &&
+              (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
+    CV_Assert((_charucoCorners.getMat().total() == _charucoIds.getMat().total()) ||
+              _charucoIds.getMat().total() == 0);
+
+    unsigned int nCorners = (unsigned int)_charucoCorners.getMat().total();
+    for(unsigned int i = 0; i < nCorners; 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().at< int >(i);
+            stringstream s;
+            s << "id=" << id;
+            putText(_image, s.str(), corner + Point2f(5, -5), FONT_HERSHEY_SIMPLEX, 0.5,
+                    cornerColor, 2);
+        }
+    }
+}
+
+
+/**
+  * Check if a set of 3d points are enough for calibration. Z coordinate is ignored.
+  * Only axis paralel lines are considered
+  */
+static bool _arePointsEnoughForPoseEstimation(const vector< Point3f > &points) {
+
+    if(points.size() < 4) return false;
+
+    vector< double > sameXValue; // different x values in points
+    vector< int > sameXCounter;  // number of points with the x value in sameXValue
+    for(unsigned int i = 0; i < points.size(); i++) {
+        bool found = false;
+        for(unsigned int j = 0; j < sameXValue.size(); j++) {
+            if(sameXValue[j] == points[i].x) {
+                found = true;
+                sameXCounter[j]++;
+            }
+        }
+        if(!found) {
+            sameXValue.push_back(points[i].x);
+            sameXCounter.push_back(1);
+        }
+    }
+
+    // count how many x values has more than 2 points
+    int moreThan2 = 0;
+    for(unsigned int i = 0; i < sameXCounter.size(); i++) {
+        if(sameXCounter[i] >= 2) moreThan2++;
+    }
+
+    // if we have more than 1 two xvalues with more than 2 points, calibration is ok
+    if(moreThan2 > 1)
+        return true;
+    else
+        return false;
+}
+
+
+/**
+  */
+bool estimatePoseCharucoBoard(InputArray _charucoCorners, InputArray _charucoIds,
+                              const Ptr<CharucoBoard> &_board, InputArray _cameraMatrix, InputArray _distCoeffs,
+                              OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess) {
+
+    CV_Assert((_charucoCorners.getMat().total() == _charucoIds.getMat().total()));
+
+    // need, at least, 4 corners
+    if(_charucoIds.getMat().total() < 4) return false;
+
+    vector< Point3f > objPoints;
+    objPoints.reserve(_charucoIds.getMat().total());
+    for(unsigned int i = 0; i < _charucoIds.getMat().total(); i++) {
+        int currId = _charucoIds.getMat().at< int >(i);
+        CV_Assert(currId >= 0 && currId < (int)_board->chessboardCorners.size());
+        objPoints.push_back(_board->chessboardCorners[currId]);
+    }
+
+    // points need to be in different lines, check if detected points are enough
+    if(!_arePointsEnoughForPoseEstimation(objPoints)) return false;
+
+    solvePnP(objPoints, _charucoCorners, _cameraMatrix, _distCoeffs, _rvec, _tvec, useExtrinsicGuess);
+
+    return true;
+}
+
+
+
+
+/**
+  */
+double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfArrays _charucoIds,
+                              const Ptr<CharucoBoard> &_board, Size imageSize,
+                              InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs,
+                              OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs,
+                              OutputArray _stdDeviationsIntrinsics,
+                              OutputArray _stdDeviationsExtrinsics,
+                              OutputArray _perViewErrors,
+                              int flags, 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< vector< Point3f > > 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->chessboardCorners.size());
+            allObjPoints[i].push_back(_board->chessboardCorners[pointId]);
+        }
+    }
+
+    return calibrateCamera(allObjPoints, _charucoCorners, imageSize, _cameraMatrix, _distCoeffs,
+                           _rvecs, _tvecs, _stdDeviationsIntrinsics, _stdDeviationsExtrinsics,
+                           _perViewErrors, flags, criteria);
+}
+
+
+
+/**
+ */
+double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfArrays _charucoIds,
+  const Ptr<CharucoBoard> &_board, Size imageSize,
+  InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs,
+  OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, int flags,
+  TermCriteria criteria) {
+    return calibrateCameraCharuco(_charucoCorners, _charucoIds, _board, imageSize, _cameraMatrix, _distCoeffs, _rvecs,
+      _tvecs, noArray(), noArray(), noArray(), flags, criteria);
+}
+
+
+/**
+ */
+void detectCharucoDiamond(InputArray _image, InputArrayOfArrays _markerCorners,
+                          InputArray _markerIds, float squareMarkerLengthRate,
+                          OutputArrayOfArrays _diamondCorners, OutputArray _diamondIds,
+                          InputArray _cameraMatrix, InputArray _distCoeffs) {
+
+    CV_Assert(_markerIds.total() > 0 && _markerIds.total() == _markerCorners.total());
+
+    const float minRepDistanceRate = 1.302455f;
+
+    // create Charuco board layout for diamond (3x3 layout)
+    Ptr<Dictionary> dict = getPredefinedDictionary(PREDEFINED_DICTIONARY_NAME(0));
+    Ptr<CharucoBoard> _charucoDiamondLayout = CharucoBoard::create(3, 3, squareMarkerLengthRate, 1., dict);
+
+
+    vector< vector< Point2f > > diamondCorners;
+    vector< Vec4i > diamondIds;
+
+    // stores if the detected markers have been assigned or not to a diamond
+    vector< bool > assigned(_markerIds.total(), false);
+    if(_markerIds.total() < 4) return; // a diamond need at least 4 markers
+
+    // convert input image to grey
+    Mat grey;
+    if(_image.type() == CV_8UC3)
+        cvtColor(_image, grey, COLOR_BGR2GRAY);
+    else
+        _image.copyTo(grey);
+
+    // for each of the detected markers, try to find a diamond
+    for(unsigned int i = 0; i < _markerIds.total(); i++) {
+        if(assigned[i]) continue;
+
+        // calculate marker perimeter
+        float perimeterSq = 0;
+        Mat corners = _markerCorners.getMat(i);
+        for(int c = 0; c < 4; c++) {
+          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 = sqrt(perimeterSq) * minRepDistanceRate;
+
+        int currentId = _markerIds.getMat().at< int >(i);
+
+        // prepare data to call refineDetectedMarkers()
+        // detected markers (only the current one)
+        vector< Mat > currentMarker;
+        vector< int > currentMarkerId;
+        currentMarker.push_back(_markerCorners.getMat(i));
+        currentMarkerId.push_back(currentId);
+
+        // marker candidates (the rest of markers if they have not been assigned)
+        vector< Mat > candidates;
+        vector< int > candidatesIdxs;
+        for(unsigned int k = 0; k < assigned.size(); k++) {
+            if(k == i) continue;
+            if(!assigned[k]) {
+                candidates.push_back(_markerCorners.getMat(k));
+                candidatesIdxs.push_back(k);
+            }
+        }
+        if(candidates.size() < 3) break; // we need at least 3 free markers
+
+        // modify charuco layout id to make sure all the ids are different than current id
+        for(int k = 1; k < 4; k++)
+            _charucoDiamondLayout->ids[k] = currentId + 1 + k;
+        // current id is assigned to [0], so it is the marker on the top
+        _charucoDiamondLayout->ids[0] = currentId;
+
+        // try to find the rest of markers in the diamond
+        vector< int > acceptedIdxs;
+        Ptr<Board> _b = _charucoDiamondLayout.staticCast<Board>();
+        aruco_lib::refineDetectedMarkers(grey, _b,
+                                     currentMarker, currentMarkerId,
+                                     candidates, noArray(), noArray(), minRepDistance, -1, false,
+                                     acceptedIdxs);
+
+        // if found, we have a diamond
+        if(currentMarker.size() == 4) {
+
+            assigned[i] = true;
+
+            // calculate diamond id, acceptedIdxs array indicates the markers taken from candidates
+            // array
+            Vec4i markerId;
+            markerId[0] = currentId;
+            for(int k = 1; k < 4; k++) {
+                int currentMarkerIdx = candidatesIdxs[acceptedIdxs[k - 1]];
+                markerId[k] = _markerIds.getMat().at< int >(currentMarkerIdx);
+                assigned[currentMarkerIdx] = true;
+            }
+
+            // interpolate the charuco corners of the diamond
+            vector< Point2f > currentMarkerCorners;
+            Mat aux;
+            interpolateCornersCharuco(currentMarker, currentMarkerId, grey, _charucoDiamondLayout,
+                                      currentMarkerCorners, aux, _cameraMatrix, _distCoeffs);
+
+            // if everything is ok, save the diamond
+            if(currentMarkerCorners.size() > 0) {
+                // reorder corners
+                vector< Point2f > currentMarkerCornersReorder;
+                currentMarkerCornersReorder.resize(4);
+                currentMarkerCornersReorder[0] = currentMarkerCorners[2];
+                currentMarkerCornersReorder[1] = currentMarkerCorners[3];
+                currentMarkerCornersReorder[2] = currentMarkerCorners[1];
+                currentMarkerCornersReorder[3] = currentMarkerCorners[0];
+
+                diamondCorners.push_back(currentMarkerCornersReorder);
+                diamondIds.push_back(markerId);
+            }
+        }
+    }
+
+
+    if(diamondIds.size() > 0) {
+        // parse output
+        Mat(diamondIds).copyTo(_diamondIds);
+
+        _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).at< Point2f >(j) = diamondCorners[i][j];
+            }
+        }
+    }
+}
+
+
+
+
+/**
+  */
+void drawCharucoDiamond(const Ptr<Dictionary> &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);
+
+    // create a charuco board similar to a charuco marker and print it
+    Ptr<CharucoBoard> board =
+        CharucoBoard::create(3, 3, (float)squareLength, (float)markerLength, dictionary);
+
+    // assign the charuco marker ids
+    for(int i = 0; i < 4; i++)
+        board->ids[i] = ids[i];
+
+    Size outSize(3 * squareLength + 2 * marginSize, 3 * squareLength + 2 * marginSize);
+    board->draw(outSize, _img, marginSize, borderBits);
+}
+
+
+/**
+ */
+void drawDetectedDiamonds(InputOutputArray _image, InputArrayOfArrays _corners,
+                          InputArray _ids, Scalar borderColor) {
+
+
+    CV_Assert(_image.getMat().total() != 0 &&
+              (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
+    CV_Assert((_corners.total() == _ids.total()) || _ids.total() == 0);
+
+    // calculate colors
+    Scalar textColor, cornerColor;
+    textColor = cornerColor = borderColor;
+    swap(textColor.val[0], textColor.val[1]);     // text color just sawp G and R
+    swap(cornerColor.val[1], cornerColor.val[2]); // corner color just sawp G and B
+
+    int nMarkers = (int)_corners.total();
+    for(int i = 0; i < nMarkers; i++) {
+        Mat currentMarker = _corners.getMat(i);
+        CV_Assert(currentMarker.total() == 4 && currentMarker.type() == CV_32FC2);
+
+        // draw marker sides
+        for(int j = 0; j < 4; j++) {
+            Point2f p0, p1;
+            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.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.at< Point2f >(p);
+            cent = cent / 4.;
+            stringstream s;
+            s << "id=" << _ids.getMat().at< Vec4i >(i);
+            putText(_image, s.str(), cent, FONT_HERSHEY_SIMPLEX, 0.5, textColor, 2);
+        }
+    }
+}
+}

aruco_pose/src/aruco_lib/aruco/dictionary.cpp

@@ -0,0 +1,493 @@
+/*
+By downloading, copying, installing or using the software you agree to this
+license. If you do not agree to this license, do not download, install,
+copy or use the software.
+
+						  License Agreement
+			   For Open Source Computer Vision Library
+					   (3-clause BSD License)
+
+Copyright (C) 2013, OpenCV Foundation, all rights reserved.
+Third party copyrights are property of their respective owners.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+  * Redistributions of source code must retain the above copyright notice,
+	this list of conditions and the following disclaimer.
+
+  * Redistributions in binary form must reproduce the above copyright notice,
+	this list of conditions and the following disclaimer in the documentation
+	and/or other materials provided with the distribution.
+
+  * Neither the names of the copyright holders nor the names of the contributors
+	may be used to endorse or promote products derived from this software
+	without specific prior written permission.
+
+This software is provided by the copyright holders and contributors "as is" and
+any express or implied warranties, including, but not limited to, the implied
+warranties of merchantability and fitness for a particular purpose are
+disclaimed. In no event shall copyright holders or contributors be liable for
+any direct, indirect, incidental, special, exemplary, or consequential damages
+(including, but not limited to, procurement of substitute goods or services;
+loss of use, data, or profits; or business interruption) however caused
+and on any theory of liability, whether in contract, strict liability,
+or tort (including negligence or otherwise) arising in any way out of
+the use of this software, even if advised of the possibility of such damage.
+*/
+
+#include "precomp.hpp"
+#include "aruco/dictionary.hpp"
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+#include "predefined_dictionaries.hpp"
+#include "predefined_dictionaries_apriltag.hpp"
+#include "opencv2/core/hal/hal.hpp"
+
+
+namespace aruco_lib {
+
+using namespace std;
+using namespace cv;
+
+
+/**
+  */
+Dictionary::Dictionary(const Ptr<Dictionary> &_dictionary) {
+	markerSize = _dictionary->markerSize;
+	maxCorrectionBits = _dictionary->maxCorrectionBits;
+	bytesList = _dictionary->bytesList.clone();
+}
+
+
+/**
+  */
+Dictionary::Dictionary(const Mat &_bytesList, int _markerSize, int _maxcorr) {
+	markerSize = _markerSize;
+	maxCorrectionBits = _maxcorr;
+	bytesList = _bytesList;
+}
+
+
+/**
+ */
+Ptr<Dictionary> Dictionary::create(int nMarkers, int markerSize, int randomSeed) {
+	const Ptr<Dictionary> baseDictionary = makePtr<Dictionary>();
+	return create(nMarkers, markerSize, baseDictionary, randomSeed);
+}
+
+
+/**
+ */
+Ptr<Dictionary> Dictionary::create(int nMarkers, int markerSize,
+								   const Ptr<Dictionary> &baseDictionary, int randomSeed) {
+
+	return generateCustomDictionary(nMarkers, markerSize, baseDictionary, randomSeed);
+}
+
+
+/**
+ */
+Ptr<Dictionary> Dictionary::get(int dict) {
+	return getPredefinedDictionary(dict);
+}
+
+
+/**
+ */
+bool Dictionary::identify(const Mat &onlyBits, int &idx, int &rotation,
+						  double maxCorrectionRate) const {
+
+	CV_Assert(onlyBits.rows == markerSize && onlyBits.cols == markerSize);
+
+	int maxCorrectionRecalculed = int(double(maxCorrectionBits) * maxCorrectionRate);
+
+	// get as a byte list
+	Mat candidateBytes = getByteListFromBits(onlyBits);
+
+	idx = -1; // by default, not found
+
+	// search closest marker in dict
+	for(int m = 0; m < bytesList.rows; m++) {
+		int currentMinDistance = markerSize * markerSize + 1;
+		int currentRotation = -1;
+		for(unsigned int r = 0; r < 4; r++) {
+			int currentHamming = cv::hal::normHamming(
+					bytesList.ptr(m)+r*candidateBytes.cols,
+					candidateBytes.ptr(),
+					candidateBytes.cols);
+
+			if(currentHamming < currentMinDistance) {
+				currentMinDistance = currentHamming;
+				currentRotation = r;
+			}
+		}
+
+		// if maxCorrection is fullfilled, return this one
+		if(currentMinDistance <= maxCorrectionRecalculed) {
+			idx = m;
+			rotation = currentRotation;
+			break;
+		}
+	}
+
+	return idx != -1;
+}
+
+
+/**
+  */
+int Dictionary::getDistanceToId(InputArray bits, int id, bool allRotations) const {
+
+	CV_Assert(id >= 0 && id < bytesList.rows);
+
+	unsigned int nRotations = 4;
+	if(!allRotations) nRotations = 1;
+
+	Mat candidateBytes = getByteListFromBits(bits.getMat());
+	int currentMinDistance = int(bits.total() * bits.total());
+	for(unsigned int r = 0; r < nRotations; r++) {
+		int currentHamming = cv::hal::normHamming(
+				bytesList.ptr(id) + r*candidateBytes.cols,
+				candidateBytes.ptr(),
+				candidateBytes.cols);
+
+		if(currentHamming < currentMinDistance) {
+			currentMinDistance = currentHamming;
+		}
+	}
+	return currentMinDistance;
+}
+
+
+
+/**
+ * @brief Draw a canonical marker image
+ */
+void Dictionary::drawMarker(int id, int sidePixels, OutputArray _img, int borderBits) const {
+
+	CV_Assert(sidePixels >= (markerSize + 2*borderBits));
+	CV_Assert(id < bytesList.rows);
+	CV_Assert(borderBits > 0);
+
+	_img.create(sidePixels, sidePixels, CV_8UC1);
+
+	// create small marker with 1 pixel per bin
+	Mat tinyMarker(markerSize + 2 * borderBits, markerSize + 2 * borderBits, CV_8UC1,
+				   Scalar::all(0));
+	Mat innerRegion = tinyMarker.rowRange(borderBits, tinyMarker.rows - borderBits)
+						  .colRange(borderBits, tinyMarker.cols - borderBits);
+	// put inner bits
+	Mat bits = 255 * getBitsFromByteList(bytesList.rowRange(id, id + 1), markerSize);
+	CV_Assert(innerRegion.total() == bits.total());
+	bits.copyTo(innerRegion);
+
+	// resize tiny marker to output size
+	cv::resize(tinyMarker, _img.getMat(), _img.getMat().size(), 0, 0, INTER_NEAREST);
+}
+
+
+
+
+/**
+  * @brief Transform matrix of bits to list of bytes in the 4 rotations
+  */
+Mat Dictionary::getByteListFromBits(const Mat &bits) {
+	// integer ceil
+	int nbytes = (bits.cols * bits.rows + 8 - 1) / 8;
+
+	Mat candidateByteList(1, nbytes, CV_8UC4, Scalar::all(0));
+	unsigned char currentBit = 0;
+	int currentByte = 0;
+
+	// the 4 rotations
+	uchar* rot0 = candidateByteList.ptr();
+	uchar* rot1 = candidateByteList.ptr() + 1*nbytes;
+	uchar* rot2 = candidateByteList.ptr() + 2*nbytes;
+	uchar* rot3 = candidateByteList.ptr() + 3*nbytes;
+
+	for(int row = 0; row < bits.rows; row++) {
+		for(int col = 0; col < bits.cols; col++) {
+			// circular shift
+			rot0[currentByte] <<= 1;
+			rot1[currentByte] <<= 1;
+			rot2[currentByte] <<= 1;
+			rot3[currentByte] <<= 1;
+			// set bit
+			rot0[currentByte] |= bits.at<uchar>(row, col);
+			rot1[currentByte] |= bits.at<uchar>(col, bits.cols - 1 - row);
+			rot2[currentByte] |= bits.at<uchar>(bits.rows - 1 - row, bits.cols - 1 - col);
+			rot3[currentByte] |= bits.at<uchar>(bits.rows - 1 - col, row);
+			currentBit++;
+			if(currentBit == 8) {
+				// next byte
+				currentBit = 0;
+				currentByte++;
+			}
+		}
+	}
+	return candidateByteList;
+}
+
+
+
+/**
+  * @brief Transform list of bytes to matrix of bits
+  */
+Mat Dictionary::getBitsFromByteList(const Mat &byteList, int markerSize) {
+	CV_Assert(byteList.total() > 0 &&
+			  byteList.total() >= (unsigned int)markerSize * markerSize / 8 &&
+			  byteList.total() <= (unsigned int)markerSize * markerSize / 8 + 1);
+	Mat bits(markerSize, markerSize, CV_8UC1, Scalar::all(0));
+
+	unsigned char base2List[] = { 128, 64, 32, 16, 8, 4, 2, 1 };
+	int currentByteIdx = 0;
+	// we only need the bytes in normal rotation
+	unsigned char currentByte = byteList.ptr()[0];
+	int currentBit = 0;
+	for(int row = 0; row < bits.rows; row++) {
+		for(int col = 0; col < bits.cols; col++) {
+			if(currentByte >= base2List[currentBit]) {
+				bits.at< unsigned char >(row, col) = 1;
+				currentByte -= base2List[currentBit];
+			}
+			currentBit++;
+			if(currentBit == 8) {
+				currentByteIdx++;
+				currentByte = byteList.ptr()[currentByteIdx];
+				// if not enough bits for one more byte, we are in the end
+				// update bit position accordingly
+				if(8 * (currentByteIdx + 1) > (int)bits.total())
+					currentBit = 8 * (currentByteIdx + 1) - (int)bits.total();
+				else
+					currentBit = 0; // ok, bits enough for next byte
+			}
+		}
+	}
+	return bits;
+}
+
+
+
+
+// DictionaryData constructors calls
+const Dictionary DICT_ARUCO_DATA = Dictionary(Mat(1024, (5*5 + 7)/8, CV_8UC4, (uchar*)DICT_ARUCO_BYTES), 5, 0);
+
+const Dictionary DICT_4X4_50_DATA = Dictionary(Mat(50, (4*4 + 7)/8, CV_8UC4, (uchar*)DICT_4X4_1000_BYTES), 4, 1);
+const Dictionary DICT_4X4_100_DATA = Dictionary(Mat(100, (4*4 + 7)/8, CV_8UC4, (uchar*)DICT_4X4_1000_BYTES), 4, 1);
+const Dictionary DICT_4X4_250_DATA = Dictionary(Mat(250, (4*4 + 7)/8, CV_8UC4, (uchar*)DICT_4X4_1000_BYTES), 4, 1);
+const Dictionary DICT_4X4_1000_DATA = Dictionary(Mat(1000, (4*4 + 7)/8, CV_8UC4, (uchar*)DICT_4X4_1000_BYTES), 4, 0);
+
+const Dictionary DICT_5X5_50_DATA = Dictionary(Mat(50, (5*5 + 7)/8, CV_8UC4, (uchar*)DICT_5X5_1000_BYTES), 5, 3);
+const Dictionary DICT_5X5_100_DATA = Dictionary(Mat(100, (5*5 + 7)/8, CV_8UC4, (uchar*)DICT_5X5_1000_BYTES), 5, 3);
+const Dictionary DICT_5X5_250_DATA = Dictionary(Mat(250, (5*5 + 7)/8, CV_8UC4, (uchar*)DICT_5X5_1000_BYTES), 5, 2);
+const Dictionary DICT_5X5_1000_DATA = Dictionary(Mat(1000, (5*5 + 7)/8, CV_8UC4, (uchar*)DICT_5X5_1000_BYTES), 5, 2);
+
+const Dictionary DICT_6X6_50_DATA = Dictionary(Mat(50, (6*6 + 7)/8 ,CV_8UC4, (uchar*)DICT_6X6_1000_BYTES), 6, 6);
+const Dictionary DICT_6X6_100_DATA = Dictionary(Mat(100, (6*6 + 7)/8 ,CV_8UC4, (uchar*)DICT_6X6_1000_BYTES), 6, 5);
+const Dictionary DICT_6X6_250_DATA = Dictionary(Mat(250, (6*6 + 7)/8 ,CV_8UC4, (uchar*)DICT_6X6_1000_BYTES), 6, 5);
+const Dictionary DICT_6X6_1000_DATA = Dictionary(Mat(1000, (6*6 + 7)/8 ,CV_8UC4, (uchar*)DICT_6X6_1000_BYTES), 6, 4);
+
+const Dictionary DICT_7X7_50_DATA = Dictionary(Mat(50, (7*7 + 7)/8 ,CV_8UC4, (uchar*)DICT_7X7_1000_BYTES), 7, 9);
+const Dictionary DICT_7X7_100_DATA = Dictionary(Mat(100, (7*7 + 7)/8 ,CV_8UC4, (uchar*)DICT_7X7_1000_BYTES), 7, 8);
+const Dictionary DICT_7X7_250_DATA = Dictionary(Mat(250, (7*7 + 7)/8 ,CV_8UC4, (uchar*)DICT_7X7_1000_BYTES), 7, 8);
+const Dictionary DICT_7X7_1000_DATA = Dictionary(Mat(1000, (7*7 + 7)/8 ,CV_8UC4, (uchar*)DICT_7X7_1000_BYTES), 7, 6);
+
+const Dictionary DICT_APRILTAG_16h5_DATA = Dictionary(Mat(30, (4*4 + 7)/8, CV_8UC4, (uchar*)DICT_APRILTAG_16h5_BYTES), 4, 0);
+const Dictionary DICT_APRILTAG_25h9_DATA = Dictionary(Mat(35, (5*5 + 7)/8, CV_8UC4, (uchar*)DICT_APRILTAG_25h9_BYTES), 5, 0);
+const Dictionary DICT_APRILTAG_36h10_DATA = Dictionary(Mat(2320, (6*6 + 7)/8, CV_8UC4, (uchar*)DICT_APRILTAG_36h10_BYTES), 6, 0);
+const Dictionary DICT_APRILTAG_36h11_DATA = Dictionary(Mat(587, (6*6 + 7)/8, CV_8UC4, (uchar*)DICT_APRILTAG_36h11_BYTES), 6, 0);
+
+
+Ptr<Dictionary> getPredefinedDictionary(PREDEFINED_DICTIONARY_NAME name) {
+	switch(name) {
+
+	case DICT_ARUCO_ORIGINAL:
+		return makePtr<Dictionary>(DICT_ARUCO_DATA);
+
+	case DICT_4X4_50:
+		return makePtr<Dictionary>(DICT_4X4_50_DATA);
+	case DICT_4X4_100:
+		return makePtr<Dictionary>(DICT_4X4_100_DATA);
+	case DICT_4X4_250:
+		return makePtr<Dictionary>(DICT_4X4_250_DATA);
+	case DICT_4X4_1000:
+		return makePtr<Dictionary>(DICT_4X4_1000_DATA);
+
+	case DICT_5X5_50:
+		return makePtr<Dictionary>(DICT_5X5_50_DATA);
+	case DICT_5X5_100:
+		return makePtr<Dictionary>(DICT_5X5_100_DATA);
+	case DICT_5X5_250:
+		return makePtr<Dictionary>(DICT_5X5_250_DATA);
+	case DICT_5X5_1000:
+		return makePtr<Dictionary>(DICT_5X5_1000_DATA);
+
+	case DICT_6X6_50:
+		return makePtr<Dictionary>(DICT_6X6_50_DATA);
+	case DICT_6X6_100:
+		return makePtr<Dictionary>(DICT_6X6_100_DATA);
+	case DICT_6X6_250:
+		return makePtr<Dictionary>(DICT_6X6_250_DATA);
+	case DICT_6X6_1000:
+		return makePtr<Dictionary>(DICT_6X6_1000_DATA);
+
+	case DICT_7X7_50:
+		return makePtr<Dictionary>(DICT_7X7_50_DATA);
+	case DICT_7X7_100:
+		return makePtr<Dictionary>(DICT_7X7_100_DATA);
+	case DICT_7X7_250:
+		return makePtr<Dictionary>(DICT_7X7_250_DATA);
+	case DICT_7X7_1000:
+		return makePtr<Dictionary>(DICT_7X7_1000_DATA);
+
+	case DICT_APRILTAG_16h5:
+		return makePtr<Dictionary>(DICT_APRILTAG_16h5_DATA);
+	case DICT_APRILTAG_25h9:
+		return makePtr<Dictionary>(DICT_APRILTAG_25h9_DATA);
+	case DICT_APRILTAG_36h10:
+		return makePtr<Dictionary>(DICT_APRILTAG_36h10_DATA);
+	case DICT_APRILTAG_36h11:
+		return makePtr<Dictionary>(DICT_APRILTAG_36h11_DATA);
+
+	}
+	return makePtr<Dictionary>(DICT_4X4_50_DATA);
+}
+
+
+Ptr<Dictionary> getPredefinedDictionary(int dict) {
+	return getPredefinedDictionary(PREDEFINED_DICTIONARY_NAME(dict));
+}
+
+
+/**
+ * @brief Generates a random marker Mat of size markerSize x markerSize
+ */
+static Mat _generateRandomMarker(int markerSize, RNG &rng) {
+	Mat marker(markerSize, markerSize, CV_8UC1, Scalar::all(0));
+	for(int i = 0; i < markerSize; i++) {
+		for(int j = 0; j < markerSize; j++) {
+			unsigned char bit = (unsigned char) (rng.uniform(0,2));
+			marker.at< unsigned char >(i, j) = bit;
+		}
+	}
+	return marker;
+}
+
+/**
+ * @brief Calculate selfDistance of the codification of a marker Mat. Self distance is the Hamming
+ * distance of the marker to itself in the other rotations.
+ * See S. Garrido-Jurado, R. Muñoz-Salinas, F. J. Madrid-Cuevas, and M. J. Marín-Jiménez. 2014.
+ * "Automatic generation and detection of highly reliable fiducial markers under occlusion".
+ * Pattern Recogn. 47, 6 (June 2014), 2280-2292. DOI=10.1016/j.patcog.2014.01.005
+ */
+static int _getSelfDistance(const Mat &marker) {
+	Mat bytes = Dictionary::getByteListFromBits(marker);
+	int minHamming = (int)marker.total() + 1;
+	for(int r = 1; r < 4; r++) {
+		int currentHamming = cv::hal::normHamming(bytes.ptr(), bytes.ptr() + bytes.cols*r, bytes.cols);
+		if(currentHamming < minHamming) minHamming = currentHamming;
+	}
+	return minHamming;
+}
+
+/**
+ */
+Ptr<Dictionary> generateCustomDictionary(int nMarkers, int markerSize,
+										 const Ptr<Dictionary> &baseDictionary, int randomSeed) {
+	RNG rng((uint64)(randomSeed));
+
+	Ptr<Dictionary> out = makePtr<Dictionary>();
+	out->markerSize = markerSize;
+
+	// theoretical maximum intermarker distance
+	// See S. Garrido-Jurado, R. Muñoz-Salinas, F. J. Madrid-Cuevas, and M. J. Marín-Jiménez. 2014.
+	// "Automatic generation and detection of highly reliable fiducial markers under occlusion".
+	// Pattern Recogn. 47, 6 (June 2014), 2280-2292. DOI=10.1016/j.patcog.2014.01.005
+	int C = (int)std::floor(float(markerSize * markerSize) / 4.f);
+	int tau = 2 * (int)std::floor(float(C) * 4.f / 3.f);
+
+	// if baseDictionary is provided, calculate its intermarker distance
+	if(baseDictionary->bytesList.rows > 0) {
+		CV_Assert(baseDictionary->markerSize == markerSize);
+		out->bytesList = baseDictionary->bytesList.clone();
+
+		int minDistance = markerSize * markerSize + 1;
+		for(int i = 0; i < out->bytesList.rows; i++) {
+			Mat markerBytes = out->bytesList.rowRange(i, i + 1);
+			Mat markerBits = Dictionary::getBitsFromByteList(markerBytes, markerSize);
+			minDistance = min(minDistance, _getSelfDistance(markerBits));
+			for(int j = i + 1; j < out->bytesList.rows; j++) {
+				minDistance = min(minDistance, out->getDistanceToId(markerBits, j));
+			}
+		}
+		tau = minDistance;
+	}
+
+	// current best option
+	int bestTau = 0;
+	Mat bestMarker;
+
+	// after these number of unproductive iterations, the best option is accepted
+	const int maxUnproductiveIterations = 5000;
+	int unproductiveIterations = 0;
+
+	while(out->bytesList.rows < nMarkers) {
+		Mat currentMarker = _generateRandomMarker(markerSize, rng);
+
+		int selfDistance = _getSelfDistance(currentMarker);
+		int minDistance = selfDistance;
+
+		// if self distance is better or equal than current best option, calculate distance
+		// to previous accepted markers
+		if(selfDistance >= bestTau) {
+			for(int i = 0; i < out->bytesList.rows; i++) {
+				int currentDistance = out->getDistanceToId(currentMarker, i);
+				minDistance = min(currentDistance, minDistance);
+				if(minDistance <= bestTau) {
+					break;
+				}
+			}
+		}
+
+		// if distance is high enough, accept the marker
+		if(minDistance >= tau) {
+			unproductiveIterations = 0;
+			bestTau = 0;
+			Mat bytes = Dictionary::getByteListFromBits(currentMarker);
+			out->bytesList.push_back(bytes);
+		} else {
+			unproductiveIterations++;
+
+			// if distance is not enough, but is better than the current best option
+			if(minDistance > bestTau) {
+				bestTau = minDistance;
+				bestMarker = currentMarker;
+			}
+
+			// if number of unproductive iterarions has been reached, accept the current best option
+			if(unproductiveIterations == maxUnproductiveIterations) {
+				unproductiveIterations = 0;
+				tau = bestTau;
+				bestTau = 0;
+				Mat bytes = Dictionary::getByteListFromBits(bestMarker);
+				out->bytesList.push_back(bytes);
+			}
+		}
+	}
+
+	// update the maximum number of correction bits for the generated dictionary
+	out->maxCorrectionBits = (tau - 1) / 2;
+
+	return out;
+}
+
+
+/**
+ */
+Ptr<Dictionary> generateCustomDictionary(int nMarkers, int markerSize, int randomSeed) {
+	Ptr<Dictionary> baseDictionary = makePtr<Dictionary>();
+	return generateCustomDictionary(nMarkers, markerSize, baseDictionary, randomSeed);
+}
+
+}

aruco_pose/src/aruco_lib/aruco/include/aruco.hpp

@@ -0,0 +1,595 @@
+/*
+By downloading, copying, installing or using the software you agree to this
+license. If you do not agree to this license, do not download, install,
+copy or use the software.
+
+						  License Agreement
+			   For Open Source Computer Vision Library
+					   (3-clause BSD License)
+
+Copyright (C) 2013, OpenCV Foundation, all rights reserved.
+Third party copyrights are property of their respective owners.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+  * Redistributions of source code must retain the above copyright notice,
+	this list of conditions and the following disclaimer.
+
+  * Redistributions in binary form must reproduce the above copyright notice,
+	this list of conditions and the following disclaimer in the documentation
+	and/or other materials provided with the distribution.
+
+  * Neither the names of the copyright holders nor the names of the contributors
+	may be used to endorse or promote products derived from this software
+	without specific prior written permission.
+
+This software is provided by the copyright holders and contributors "as is" and
+any express or implied warranties, including, but not limited to, the implied
+warranties of merchantability and fitness for a particular purpose are
+disclaimed. In no event shall copyright holders or contributors be liable for
+any direct, indirect, incidental, special, exemplary, or consequential damages
+(including, but not limited to, procurement of substitute goods or services;
+loss of use, data, or profits; or business interruption) however caused
+and on any theory of liability, whether in contract, strict liability,
+or tort (including negligence or otherwise) arising in any way out of
+the use of this software, even if advised of the possibility of such damage.
+*/
+
+#ifndef __OPENCV_ARUCO_HPP__
+#define __OPENCV_ARUCO_HPP__
+
+#include <opencv2/core.hpp>
+#include <vector>
+#include "aruco/dictionary.hpp"
+
+/**
+ * @defgroup aruco ArUco Marker Detection
+ * This module is dedicated to square fiducial markers (also known as Augmented Reality Markers)
+ * These markers are useful for easy, fast and robust camera pose estimation.ç
+ *
+ * The main functionalities are:
+ * - Detection of markers in an image
+ * - Pose estimation from a single marker or from a board/set of markers
+ * - Detection of ChArUco board for high subpixel accuracy
+ * - Camera calibration from both, ArUco boards and ChArUco boards.
+ * - Detection of ChArUco diamond markers
+ * The samples directory includes easy examples of how to use the module.
+ *
+ * The implementation is based on the ArUco Library by R. Muñoz-Salinas and S. Garrido-Jurado @cite Aruco2014.
+ *
+ * Markers can also be detected based on the AprilTag 2 @cite wang2016iros fiducial detection method.
+ *
+ * @sa S. Garrido-Jurado, R. Muñoz-Salinas, F. J. Madrid-Cuevas, and M. J. Marín-Jiménez. 2014.
+ * "Automatic generation and detection of highly reliable fiducial markers under occlusion".
+ * Pattern Recogn. 47, 6 (June 2014), 2280-2292. DOI=10.1016/j.patcog.2014.01.005
+ *
+ * @sa http://www.uco.es/investiga/grupos/ava/node/26
+ *
+ * This module has been originally developed by Sergio Garrido-Jurado as a project
+ * for Google Summer of Code 2015 (GSoC 15).
+ *
+ *
+*/
+
+namespace aruco_lib {
+
+//! @addtogroup aruco
+//! @{
+
+enum CornerRefineMethod{
+	CORNER_REFINE_NONE,     ///< Tag and corners detection based on the ArUco approach
+	CORNER_REFINE_SUBPIX,   ///< ArUco approach and refine the corners locations using corner subpixel accuracy
+	CORNER_REFINE_CONTOUR,  ///< ArUco approach and refine the corners locations using the contour-points line fitting
+	CORNER_REFINE_APRILTAG, ///< Tag and corners detection based on the AprilTag 2 approach @cite wang2016iros
+};
+
+/**
+ * @brief Parameters for the detectMarker process:
+ * - adaptiveThreshWinSizeMin: minimum window size for adaptive thresholding before finding
+ *   contours (default 3).
+ * - adaptiveThreshWinSizeMax: maximum window size for adaptive thresholding before finding
+ *   contours (default 23).
+ * - adaptiveThreshWinSizeStep: increments from adaptiveThreshWinSizeMin to adaptiveThreshWinSizeMax
+ *   during the thresholding (default 10).
+ * - adaptiveThreshConstant: constant for adaptive thresholding before finding contours (default 7)
+ * - minMarkerPerimeterRate: determine minimum perimeter for marker contour to be detected. This
+ *   is defined as a rate respect to the maximum dimension of the input image (default 0.03).
+ * - maxMarkerPerimeterRate:  determine maximum perimeter for marker contour to be detected. This
+ *   is defined as a rate respect to the maximum dimension of the input image (default 4.0).
+ * - polygonalApproxAccuracyRate: minimum accuracy during the polygonal approximation process to
+ *   determine which contours are squares.
+ * - minCornerDistanceRate: minimum distance between corners for detected markers relative to its
+ *   perimeter (default 0.05)
+ * - minDistanceToBorder: minimum distance of any corner to the image border for detected markers
+ *   (in pixels) (default 3)
+ * - minMarkerDistanceRate: minimum mean distance beetween two marker corners to be considered
+ *   similar, so that the smaller one is removed. The rate is relative to the smaller perimeter
+ *   of the two markers (default 0.05).
+ * - cornerRefinementMethod: corner refinement method. (CORNER_REFINE_NONE, no refinement.
+ *   CORNER_REFINE_SUBPIX, do subpixel refinement. CORNER_REFINE_CONTOUR use contour-Points,
+ *   CORNER_REFINE_APRILTAG  use the AprilTag2 approach)
+ * - cornerRefinementWinSize: window size for the corner refinement process (in pixels) (default 5).
+ * - cornerRefinementMaxIterations: maximum number of iterations for stop criteria of the corner
+ *   refinement process (default 30).
+ * - cornerRefinementMinAccuracy: minimum error for the stop cristeria of the corner refinement
+ *   process (default: 0.1)
+ * - markerBorderBits: number of bits of the marker border, i.e. marker border width (default 1).
+ * - perspectiveRemovePixelPerCell: number of bits (per dimension) for each cell of the marker
+ *   when removing the perspective (default 8).
+ * - perspectiveRemoveIgnoredMarginPerCell: width of the margin of pixels on each cell not
+ *   considered for the determination of the cell bit. Represents the rate respect to the total
+ *   size of the cell, i.e. perspectiveRemovePixelPerCell (default 0.13)
+ * - maxErroneousBitsInBorderRate: maximum number of accepted erroneous bits in the border (i.e.
+ *   number of allowed white bits in the border). Represented as a rate respect to the total
+ *   number of bits per marker (default 0.35).
+ * - minOtsuStdDev: minimun standard deviation in pixels values during the decodification step to
+ *   apply Otsu thresholding (otherwise, all the bits are set to 0 or 1 depending on mean higher
+ *   than 128 or not) (default 5.0)
+ * - errorCorrectionRate error correction rate respect to the maximun error correction capability
+ *   for each dictionary. (default 0.6).
+ * - aprilTagMinClusterPixels: reject quads containing too few pixels.
+ * - aprilTagMaxNmaxima: how many corner candidates to consider when segmenting a group of pixels into a quad.
+ * - aprilTagCriticalRad: Reject quads where pairs of edges have angles that are close to straight or close to
+ *   180 degrees. Zero means that no quads are rejected. (In radians).
+ * - aprilTagMaxLineFitMse:  When fitting lines to the contours, what is the maximum mean squared error
+ *   allowed?  This is useful in rejecting contours that are far from being quad shaped; rejecting
+ *   these quads "early" saves expensive decoding processing.
+ * - aprilTagMinWhiteBlackDiff: When we build our model of black & white pixels, we add an extra check that
+ *   the white model must be (overall) brighter than the black model.  How much brighter? (in pixel values, [0,255]).
+ * - aprilTagDeglitch:  should the thresholded image be deglitched? Only useful for very noisy images
+ * - aprilTagQuadDecimate: Detection of quads can be done on a lower-resolution image, improving speed at a
+ *   cost of pose accuracy and a slight decrease in detection rate. Decoding the binary payload is still
+ *   done at full resolution.
+ * - aprilTagQuadSigma: What Gaussian blur should be applied to the segmented image (used for quad detection?)
+ *   Parameter is the standard deviation in pixels.  Very noisy images benefit from non-zero values (e.g. 0.8).
+ */
+struct CV_EXPORTS_W DetectorParameters {
+
+	DetectorParameters();
+
+	CV_WRAP static cv::Ptr<DetectorParameters> create();
+
+	CV_PROP_RW int adaptiveThreshWinSizeMin;
+	CV_PROP_RW int adaptiveThreshWinSizeMax;
+	CV_PROP_RW int adaptiveThreshWinSizeStep;
+	CV_PROP_RW double adaptiveThreshConstant;
+	CV_PROP_RW double minMarkerPerimeterRate;
+	CV_PROP_RW double maxMarkerPerimeterRate;
+	CV_PROP_RW double polygonalApproxAccuracyRate;
+	CV_PROP_RW double minCornerDistanceRate;
+	CV_PROP_RW int minDistanceToBorder;
+	CV_PROP_RW double minMarkerDistanceRate;
+	CV_PROP_RW int cornerRefinementMethod;
+	CV_PROP_RW int cornerRefinementWinSize;
+	CV_PROP_RW int cornerRefinementMaxIterations;
+	CV_PROP_RW double cornerRefinementMinAccuracy;
+	CV_PROP_RW int markerBorderBits;
+	CV_PROP_RW int perspectiveRemovePixelPerCell;
+	CV_PROP_RW double perspectiveRemoveIgnoredMarginPerCell;
+	CV_PROP_RW double maxErroneousBitsInBorderRate;
+	CV_PROP_RW double minOtsuStdDev;
+	CV_PROP_RW double errorCorrectionRate;
+
+	// April :: User-configurable parameters.
+	CV_PROP_RW float aprilTagQuadDecimate;
+	CV_PROP_RW float aprilTagQuadSigma;
+
+	// April :: Internal variables
+	CV_PROP_RW int aprilTagMinClusterPixels;
+	CV_PROP_RW int aprilTagMaxNmaxima;
+	CV_PROP_RW float aprilTagCriticalRad;
+	CV_PROP_RW float aprilTagMaxLineFitMse;
+	CV_PROP_RW int aprilTagMinWhiteBlackDiff;
+	CV_PROP_RW int aprilTagDeglitch;
+};
+
+
+
+/**
+ * @brief Basic marker detection
+ *
+ * @param image input image
+ * @param dictionary indicates the type of markers that will be searched
+ * @param corners vector of detected marker corners. For each marker, its four corners
+ * are provided, (e.g std::vector<std::vector<cv::Point2f> > ). For N detected markers,
+ * the dimensions of this array is Nx4. The order of the corners is clockwise.
+ * @param ids vector of identifiers of the detected markers. The identifier is of type int
+ * (e.g. std::vector<int>). For N detected markers, the size of ids is also N.
+ * The identifiers have the same order than the markers in the imgPoints array.
+ * @param parameters marker detection parameters
+ * @param rejectedImgPoints contains the imgPoints of those squares whose inner code has not a
+ * correct codification. Useful for debugging purposes.
+ * @param cameraMatrix optional input 3x3 floating-point camera matrix
+ * \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$
+ * @param distCoeff 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
+ *
+ * Performs marker detection in the input image. Only markers included in the specific dictionary
+ * are searched. For each detected marker, it returns the 2D position of its corner in the image
+ * and its corresponding identifier.
+ * Note that this function does not perform pose estimation.
+ * @sa estimatePoseSingleMarkers,  estimatePoseBoard
+ *
+ */
+CV_EXPORTS_W void detectMarkers(cv::InputArray image, const cv::Ptr<Dictionary> &dictionary, cv::OutputArrayOfArrays corners,
+                                cv::OutputArray ids, const cv::Ptr<DetectorParameters> &parameters = DetectorParameters::create(), // editorconfig-checker-disable-line
+                                cv::OutputArrayOfArrays rejectedImgPoints = cv::noArray(), cv::InputArray cameraMatrix= cv::noArray(), cv::InputArray distCoeff= cv::noArray()); // editorconfig-checker-disable-line
+
+
+
+/**
+ * @brief Pose estimation for single markers
+ *
+ * @param corners vector of already detected markers corners. For each marker, its four corners
+ * are provided, (e.g std::vector<std::vector<cv::Point2f> > ). For N detected markers,
+ * the dimensions of this array should be Nx4. The order of the corners should be clockwise.
+ * @sa detectMarkers
+ * @param markerLength the length of the markers' side. The returning translation vectors will
+ * be in the same unit. Normally, unit is meters.
+ * @param cameraMatrix input 3x3 floating-point camera matrix
+ * \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$
+ * @param distCoeffs 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 array of output rotation vectors (@sa Rodrigues) (e.g. std::vector<cv::Vec3d>).
+ * Each element in rvecs corresponds to the specific marker in imgPoints.
+ * @param tvecs array of output translation vectors (e.g. std::vector<cv::Vec3d>).
+ * Each element in tvecs corresponds to the specific marker in imgPoints.
+ * @param _objPoints array of object points of all the marker corners
+ *
+ * This function receives the detected markers and returns their pose estimation respect to
+ * the camera individually. So for each marker, one rotation and translation vector is returned.
+ * The returned transformation is the one that transforms points from each marker coordinate system
+ * to the camera coordinate system.
+ * The marker corrdinate system is centered on the middle of the marker, with the Z axis
+ * perpendicular to the marker plane.
+ * The coordinates of the four corners 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)
+ */
+CV_EXPORTS_W void estimatePoseSingleMarkers(cv::InputArrayOfArrays corners, float markerLength,
+                                            cv::InputArray cameraMatrix, cv::InputArray distCoeffs, // editorconfig-checker-disable-line
+                                            cv::OutputArray rvecs, cv::OutputArray tvecs, cv::OutputArray _objPoints = cv::noArray()); // editorconfig-checker-disable-line
+
+
+
+/**
+ * @brief Board of markers
+ *
+ * A board is a set of markers in the 3D space with a common cordinate system.
+ * The common form of a board of marker is a planar (2D) board, however any 3D layout can be used.
+ * A Board object is composed by:
+ * - The object points of the marker corners, i.e. their coordinates respect to the board system.
+ * - The dictionary which indicates the type of markers of the board
+ * - The identifier of all the markers in the board.
+ */
+class CV_EXPORTS_W Board {
+
+	public:
+	/**
+	* @brief Provide way to create Board by passing nessesary data. Specially needed in Python.
+	*
+	* @param objPoints array of object points of all the marker corners in the board
+	* @param dictionary the dictionary of markers employed for this board
+	* @param ids vector of the identifiers of the markers in the board
+	*
+	*/
+	CV_WRAP static cv::Ptr<Board> create(cv::InputArrayOfArrays objPoints, const cv::Ptr<Dictionary> &dictionary, cv::InputArray ids);
+	/// array of object points of all the marker corners in the board
+	/// each marker include its 4 corners in CCW order. For M markers, the size is Mx4.
+	CV_PROP std::vector< std::vector< cv::Point3f > > objPoints;
+
+	/// the dictionary of markers employed for this board
+	CV_PROP cv::Ptr<Dictionary> dictionary;
+
+	/// vector of the identifiers of the markers in the board (same size than objPoints)
+	/// The identifiers refers to the board dictionary
+	CV_PROP std::vector< int > ids;
+};
+
+
+
+/**
+ * @brief Planar board with grid arrangement of markers
+ * More common type of board. All markers are placed in the same plane in a grid arrangment.
+ * The board can be drawn using drawPlanarBoard() function (@sa drawPlanarBoard)
+ */
+class CV_EXPORTS_W GridBoard : public Board {
+
+	public:
+	/**
+	 * @brief Draw a GridBoard
+	 *
+	 * @param outSize size of the output image in pixels.
+	 * @param img output image with the board. The size of this image will be outSize
+	 * and the board will be on the center, keeping the board proportions.
+	 * @param marginSize minimum margins (in pixels) of the board in the output image
+	 * @param borderBits width of the marker borders.
+	 *
+	 * This function return the image of the GridBoard, ready to be printed.
+	 */
+	CV_WRAP void draw(cv::Size outSize, cv::OutputArray img, int marginSize = 0, int borderBits = 1);
+
+
+	/**
+	 * @brief Create a GridBoard object
+	 *
+	 * @param markersX number of markers in X direction
+	 * @param markersY number of markers in Y direction
+	 * @param markerLength marker side length (normally in meters)
+	 * @param markerSeparation separation between two markers (same unit as markerLength)
+	 * @param dictionary dictionary of markers indicating the type of markers
+	 * @param firstMarker id of first marker in dictionary to use on board.
+	 * @return the output GridBoard object
+	 *
+	 * This functions creates a GridBoard object given the number of markers in each direction and
+	 * the marker size and marker separation.
+	 */
+	CV_WRAP static cv::Ptr<GridBoard> create(int markersX, int markersY, float markerLength,
+											 float markerSeparation, const cv::Ptr<Dictionary> &dictionary, int firstMarker = 0);
+
+	/**
+	  *
+	  */
+	CV_WRAP cv::Size getGridSize() const { return cv::Size(_markersX, _markersY); }
+
+	/**
+	  *
+	  */
+	CV_WRAP float getMarkerLength() const { return _markerLength; }
+
+	/**
+	  *
+	  */
+	CV_WRAP float getMarkerSeparation() const { return _markerSeparation; }
+
+
+	private:
+	// number of markers in X and Y directions
+	int _markersX, _markersY;
+
+	// marker side lenght (normally in meters)
+	float _markerLength;
+
+	// separation between markers in the grid
+	float _markerSeparation;
+};
+
+
+
+/**
+ * @brief Pose estimation for a board of markers
+ *
+ * @param corners vector of already detected markers corners. For each marker, its four corners
+ * are provided, (e.g std::vector<std::vector<cv::Point2f> > ). For N detected markers, the
+ * dimensions of this array should be Nx4. The order of the corners should be clockwise.
+ * @param ids list of identifiers for each marker in corners
+ * @param board layout of markers in the board. The layout is composed by the marker identifiers
+ * and the positions of each marker corner in the board reference system.
+ * @param cameraMatrix input 3x3 floating-point camera matrix
+ * \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$
+ * @param distCoeffs 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 rvec Output vector (e.g. cv::Mat) corresponding to the rotation vector of the board
+ * (see cv::Rodrigues). Used as initial guess if not empty.
+ * @param tvec Output vector (e.g. cv::Mat) corresponding to the translation vector of the board.
+ * @param useExtrinsicGuess defines whether initial guess for \b rvec and \b tvec will be used or not.
+ * Used as initial guess if not empty.
+ *
+ * This function receives the detected markers and returns the pose of a marker board composed
+ * by those markers.
+ * A Board of marker has a single world coordinate system which is defined by the board layout.
+ * The returned transformation is the one that transforms points from the board coordinate system
+ * to the camera coordinate system.
+ * Input markers that are not included in the board layout are ignored.
+ * The function returns the number of markers from the input employed for the board pose estimation.
+ * Note that returning a 0 means the pose has not been estimated.
+ */
+CV_EXPORTS_W int estimatePoseBoard(cv::InputArrayOfArrays corners, cv::InputArray ids, const cv::Ptr<Board> &board,
+                                   cv::InputArray cameraMatrix, cv::InputArray distCoeffs, cv::OutputArray rvec, // editorconfig-checker-disable-line
+                                   cv::OutputArray tvec, bool useExtrinsicGuess = false); // editorconfig-checker-disable-line
+
+
+
+
+/**
+ * @brief Refind not detected markers based on the already detected and the board layout
+ *
+ * @param image input image
+ * @param board layout of markers in the board.
+ * @param detectedCorners vector of already detected marker corners.
+ * @param detectedIds vector of already detected marker identifiers.
+ * @param rejectedCorners vector of rejected candidates during the marker detection process.
+ * @param cameraMatrix optional input 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 minRepDistance minimum distance between the corners of the rejected candidate and the
+ * reprojected marker in order to consider it as a correspondence.
+ * @param errorCorrectionRate rate of allowed erroneous bits respect to the error correction
+ * capability of the used dictionary. -1 ignores the error correction step.
+ * @param checkAllOrders Consider the four posible corner orders in the rejectedCorners array.
+ * If it set to false, only the provided corner order is considered (default true).
+ * @param recoveredIdxs Optional array to returns the indexes of the recovered candidates in the
+ * original rejectedCorners array.
+ * @param parameters marker detection parameters
+ *
+ * This function tries to find markers that were not detected in the basic detecMarkers function.
+ * First, based on the current detected marker and the board layout, the function interpolates
+ * the position of the missing markers. Then it tries to find correspondence between the reprojected
+ * markers and the rejected candidates based on the minRepDistance and errorCorrectionRate
+ * parameters.
+ * If camera parameters and distortion coefficients are provided, missing markers are reprojected
+ * using projectPoint function. If not, missing marker projections are interpolated using global
+ * homography, and all the marker corners in the board must have the same Z coordinate.
+ */
+CV_EXPORTS_W void refineDetectedMarkers(
+	cv::InputArray image,const  cv::Ptr<Board> &board, cv::InputOutputArrayOfArrays detectedCorners,
+	cv::InputOutputArray detectedIds, cv::InputOutputArrayOfArrays rejectedCorners,
+	cv::InputArray cameraMatrix = cv::noArray(), cv::InputArray distCoeffs = cv::noArray(),
+	float minRepDistance = 10.f, float errorCorrectionRate = 3.f, bool checkAllOrders = true,
+	cv::OutputArray recoveredIdxs = cv::noArray(), const cv::Ptr<DetectorParameters> &parameters = DetectorParameters::create());
+
+
+
+/**
+ * @brief Draw detected markers in image
+ *
+ * @param image input/output image. It must have 1 or 3 channels. The number of channels is not
+ * altered.
+ * @param corners positions of marker corners on input image.
+ * (e.g std::vector<std::vector<cv::Point2f> > ). For N detected markers, the dimensions of
+ * this array should be Nx4. The order of the corners should be clockwise.
+ * @param ids vector of identifiers for markers in markersCorners .
+ * Optional, if not provided, ids are not painted.
+ * @param borderColor color of marker borders. Rest of colors (text color and first corner color)
+ * are calculated based on this one to improve visualization.
+ *
+ * Given an array of detected marker corners and its corresponding ids, this functions draws
+ * the markers in the image. The marker borders are painted and the markers identifiers if provided.
+ * Useful for debugging purposes.
+ */
+CV_EXPORTS_W void drawDetectedMarkers(cv::InputOutputArray image, cv::InputArrayOfArrays corners,
+                                      cv::InputArray ids = cv::noArray(),
+                                      cv::Scalar borderColor = cv::Scalar(0, 255, 0));
+
+
+
+/**
+ * @brief Draw coordinate system axis from pose estimation
+ *
+ * @param image input/output image. It must have 1 or 3 channels. The number of channels is not
+ * altered.
+ * @param cameraMatrix input 3x3 floating-point camera matrix
+ * \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$
+ * @param distCoeffs 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 rvec rotation vector of the coordinate system that will be drawn. (@sa Rodrigues).
+ * @param tvec translation vector of the coordinate system that will be drawn.
+ * @param length length of the painted axis in the same unit than tvec (usually in meters)
+ *
+ * Given the pose estimation of a marker or board, this function draws the axis of the world
+ * coordinate system, i.e. the system centered on the marker/board. Useful for debugging purposes.
+ */
+CV_EXPORTS_W void drawAxis(cv::InputOutputArray image, cv::InputArray cameraMatrix, cv::InputArray distCoeffs,
+                           cv::InputArray rvec, cv::InputArray tvec, float length); // editorconfig-checker-disable-line
+
+
+
+/**
+ * @brief Draw a canonical marker image
+ *
+ * @param dictionary dictionary of markers indicating the type of markers
+ * @param id identifier of the marker that will be returned. It has to be a valid id
+ * in the specified dictionary.
+ * @param sidePixels size of the image in pixels
+ * @param img output image with the marker
+ * @param borderBits width of the marker border.
+ *
+ * This function returns a marker image in its canonical form (i.e. ready to be printed)
+ */
+CV_EXPORTS_W void drawMarker(const cv::Ptr<Dictionary> &dictionary, int id, int sidePixels, cv::OutputArray img,
+                             int borderBits = 1); // editorconfig-checker-disable-line
+
+
+
+/**
+ * @brief Draw a planar board
+ * @sa _drawPlanarBoardImpl
+ *
+ * @param board layout of the board that will be drawn. The board should be planar,
+ * z coordinate is ignored
+ * @param outSize size of the output image in pixels.
+ * @param img output image with the board. The size of this image will be outSize
+ * and the board will be on the center, keeping the board proportions.
+ * @param marginSize minimum margins (in pixels) of the board in the output image
+ * @param borderBits width of the marker borders.
+ *
+ * This function return the image of a planar board, ready to be printed. It assumes
+ * the Board layout specified is planar by ignoring the z coordinates of the object points.
+ */
+CV_EXPORTS_W void drawPlanarBoard(const cv::Ptr<Board> &board, cv::Size outSize, cv::OutputArray img,
+                                  int marginSize = 0, int borderBits = 1); // editorconfig-checker-disable-line
+
+
+
+/**
+ * @brief Implementation of drawPlanarBoard that accepts a raw Board pointer.
+ */
+void _drawPlanarBoardImpl(Board *board, cv::Size outSize, cv::OutputArray img,
+                          int marginSize = 0, int borderBits = 1); // editorconfig-checker-disable-line
+
+
+
+/**
+ * @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<cv::Mat>>). 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.
+ */
+CV_EXPORTS_AS(calibrateCameraArucoExtended) double calibrateCameraAruco(
+	cv::InputArrayOfArrays corners, cv::InputArray ids, cv::InputArray counter, const cv::Ptr<Board> &board,
+	cv::Size imageSize, cv::InputOutputArray cameraMatrix, cv::InputOutputArray distCoeffs,
+	cv::OutputArrayOfArrays rvecs, cv::OutputArrayOfArrays tvecs,
+	cv::OutputArray stdDeviationsIntrinsics, cv::OutputArray stdDeviationsExtrinsics,
+	cv::OutputArray perViewErrors, int flags = 0,
+	cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, DBL_EPSILON));
+
+
+/** @brief It's the same function as #calibrateCameraAruco but without calibration error estimation.
+ */
+CV_EXPORTS_W double calibrateCameraAruco(
+	cv::InputArrayOfArrays corners, cv::InputArray ids, cv::InputArray counter, const cv::Ptr<Board> &board,
+	cv::Size imageSize, cv::InputOutputArray cameraMatrix, cv::InputOutputArray distCoeffs,
+	cv::OutputArrayOfArrays rvecs = cv::noArray(), cv::OutputArrayOfArrays tvecs = cv::noArray(), int flags = 0,
+	cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, DBL_EPSILON));
+
+
+/**
+ * @brief Given a board configuration and a set of detected markers, returns the corresponding
+ * image points and object points to call solvePnP
+ *
+ * @param board Marker board layout.
+ * @param detectedCorners List of detected marker corners of the board.
+ * @param detectedIds List of identifiers for each marker.
+ * @param objPoints Vector of vectors of board marker points in the board coordinate space.
+ * @param imgPoints Vector of vectors of the projections of board marker corner points.
+*/
+CV_EXPORTS_W void getBoardObjectAndImagePoints(const cv::Ptr<Board> &board, cv::InputArrayOfArrays detectedCorners,
+	cv::InputArray detectedIds, cv::OutputArray objPoints, cv::OutputArray imgPoints);
+
+
+//! @}
+}
+
+#endif

aruco_pose/src/aruco_lib/aruco/include/aruco/charuco.hpp

@@ -0,0 +1,341 @@
+/*
+By downloading, copying, installing or using the software you agree to this
+license. If you do not agree to this license, do not download, install,
+copy or use the software.
+
+													License Agreement
+							 For Open Source Computer Vision Library
+											 (3-clause BSD License)
+
+Copyright (C) 2013, OpenCV Foundation, all rights reserved.
+Third party copyrights are property of their respective owners.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+	* Redistributions of source code must retain the above copyright notice,
+		this list of conditions and the following disclaimer.
+
+	* Redistributions in binary form must reproduce the above copyright notice,
+		this list of conditions and the following disclaimer in the documentation
+		and/or other materials provided with the distribution.
+
+	* Neither the names of the copyright holders nor the names of the contributors
+		may be used to endorse or promote products derived from this software
+		without specific prior written permission.
+
+This software is provided by the copyright holders and contributors "as is" and
+any express or implied warranties, including, but not limited to, the implied
+warranties of merchantability and fitness for a particular purpose are
+disclaimed. In no event shall copyright holders or contributors be liable for
+any direct, indirect, incidental, special, exemplary, or consequential damages
+(including, but not limited to, procurement of substitute goods or services;
+loss of use, data, or profits; or business interruption) however caused
+and on any theory of liability, whether in contract, strict liability,
+or tort (including negligence or otherwise) arising in any way out of
+the use of this software, even if advised of the possibility of such damage.
+*/
+
+#ifndef __OPENCV_CHARUCO_HPP__
+#define __OPENCV_CHARUCO_HPP__
+
+#include <opencv2/core.hpp>
+#include <vector>
+#include <aruco.hpp>
+
+
+namespace aruco_lib {
+
+//! @addtogroup aruco
+//! @{
+
+
+/**
+ * @brief ChArUco board
+ * Specific class for ChArUco boards. A ChArUco board is a planar board where the markers are placed
+ * inside the white squares of a chessboard. The benefits of ChArUco boards is that they provide
+ * both, ArUco markers versatility and chessboard corner precision, which is important for
+ * calibration and pose estimation.
+ * This class also allows the easy creation and drawing of ChArUco boards.
+ */
+class CV_EXPORTS_W CharucoBoard : public Board {
+
+public:
+	// vector of chessboard 3D corners precalculated
+	CV_PROP std::vector< cv::Point3f > chessboardCorners;
+
+	// for each charuco corner, nearest marker id and nearest marker corner id of each marker
+	CV_PROP std::vector< std::vector< int > > nearestMarkerIdx;
+	CV_PROP std::vector< std::vector< int > > nearestMarkerCorners;
+
+	/**
+	 * @brief Draw a ChArUco board
+	 *
+	 * @param outSize size of the output image in pixels.
+	 * @param img output image with the board. The size of this image will be outSize
+	 * and the board will be on the center, keeping the board proportions.
+	 * @param marginSize minimum margins (in pixels) of the board in the output image
+	 * @param borderBits width of the marker borders.
+	 *
+	 * This function return the image of the ChArUco board, ready to be printed.
+	 */
+	CV_WRAP void draw(cv::Size outSize, cv::OutputArray img, int marginSize = 0, int borderBits = 1);
+
+
+	/**
+	 * @brief Create a CharucoBoard object
+	 *
+	 * @param squaresX number of chessboard squares in X direction
+	 * @param squaresY number of chessboard squares in Y direction
+	 * @param squareLength chessboard square side length (normally in meters)
+	 * @param markerLength marker side length (same unit than squareLength)
+	 * @param dictionary dictionary of markers indicating the type of markers.
+	 * The first markers in the dictionary are used to fill the white chessboard squares.
+	 * @return the output CharucoBoard object
+	 *
+	 * This functions creates a CharucoBoard object given the number of squares in each direction
+	 * and the size of the markers and chessboard squares.
+	 */
+	CV_WRAP static cv::Ptr<CharucoBoard> create(int squaresX, int squaresY, float squareLength,
+	                                            float markerLength, const cv::Ptr<Dictionary> &dictionary); // editorconfig-checker-disable-line
+
+	/**
+	*
+	*/
+	CV_WRAP cv::Size getChessboardSize() const { return cv::Size(_squaresX, _squaresY); }
+
+	/**
+	*
+	*/
+	CV_WRAP float getSquareLength() const { return _squareLength; }
+
+	/**
+	*
+	*/
+	CV_WRAP float getMarkerLength() const { return _markerLength; }
+
+private:
+	void _getNearestMarkerCorners();
+
+	// number of markers in X and Y directions
+	int _squaresX, _squaresY;
+
+	// size of chessboard squares side (normally in meters)
+	float _squareLength;
+
+	// marker side lenght (normally in meters)
+	float _markerLength;
+};
+
+
+
+
+/**
+ * @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<std::vector<cv::Point2f> > ). 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.
+ */
+CV_EXPORTS_W int interpolateCornersCharuco(cv::InputArrayOfArrays markerCorners, cv::InputArray markerIds,
+                                           cv::InputArray image, const cv::Ptr<CharucoBoard> &board, // editorconfig-checker-disable-line
+                                           cv::OutputArray charucoCorners, cv::OutputArray charucoIds, // editorconfig-checker-disable-line
+                                           cv::InputArray cameraMatrix = cv::noArray(), // editorconfig-checker-disable-line
+                                           cv::InputArray distCoeffs = cv::noArray(), int minMarkers = 2); // editorconfig-checker-disable-line
+
+
+
+
+/**
+ * @brief Pose estimation for a ChArUco board given some of their corners
+ * @param charucoCorners vector of detected charuco corners
+ * @param charucoIds list of identifiers for each corner in charucoCorners
+ * @param board layout of ChArUco board.
+ * @param cameraMatrix input 3x3 floating-point camera matrix
+ * \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$
+ * @param distCoeffs 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 rvec Output vector (e.g. cv::Mat) corresponding to the rotation vector of the board
+ * (see cv::Rodrigues).
+ * @param tvec Output vector (e.g. cv::Mat) corresponding to the translation vector of the board.
+ * @param useExtrinsicGuess defines whether initial guess for \b rvec and \b tvec will be used or not.
+ *
+ * This function estimates a Charuco board pose from some detected corners.
+ * The function checks if the input corners are enough and valid to perform pose estimation.
+ * If pose estimation is valid, returns true, else returns false.
+ */
+CV_EXPORTS_W bool estimatePoseCharucoBoard(cv::InputArray charucoCorners, cv::InputArray charucoIds,
+                                           const cv::Ptr<CharucoBoard> &board, cv::InputArray cameraMatrix, // editorconfig-checker-disable-line
+                                           cv::InputArray distCoeffs, cv::OutputArray rvec, cv::OutputArray tvec, // editorconfig-checker-disable-line
+                                           bool useExtrinsicGuess = false); // editorconfig-checker-disable-line
+
+
+
+
+/**
+ * @brief Draws a set of Charuco corners
+ * @param image input/output image. It must have 1 or 3 channels. The number of channels is not
+ * altered.
+ * @param charucoCorners vector of detected charuco corners
+ * @param charucoIds list of identifiers for each corner in charucoCorners
+ * @param cornerColor color of the square surrounding each corner
+ *
+ * This function draws a set of detected Charuco corners. If identifiers vector is provided, it also
+ * draws the id of each corner.
+ */
+CV_EXPORTS_W void drawDetectedCornersCharuco(cv::InputOutputArray image, cv::InputArray charucoCorners,
+                                             cv::InputArray charucoIds = cv::noArray(), // editorconfig-checker-disable-line
+                                             cv::Scalar cornerColor = cv::Scalar(255, 0, 0)); // editorconfig-checker-disable-line
+
+
+
+/**
+ * @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<cv::Mat>>). 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.
+ */
+CV_EXPORTS_AS(calibrateCameraCharucoExtended) double calibrateCameraCharuco(
+	cv::InputArrayOfArrays charucoCorners, cv::InputArrayOfArrays charucoIds, const cv::Ptr<CharucoBoard> &board,
+	cv::Size imageSize, cv::InputOutputArray cameraMatrix, cv::InputOutputArray distCoeffs,
+	cv::OutputArrayOfArrays rvecs, cv::OutputArrayOfArrays tvecs,
+	cv::OutputArray stdDeviationsIntrinsics, cv::OutputArray stdDeviationsExtrinsics,
+	cv::OutputArray perViewErrors, int flags = 0,
+	cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, DBL_EPSILON));
+
+/** @brief It's the same function as #calibrateCameraCharuco but without calibration error estimation.
+*/
+CV_EXPORTS_W double calibrateCameraCharuco(
+	cv::InputArrayOfArrays charucoCorners, cv::InputArrayOfArrays charucoIds, const cv::Ptr<CharucoBoard> &board,
+	cv::Size imageSize, cv::InputOutputArray cameraMatrix, cv::InputOutputArray distCoeffs,
+	cv::OutputArrayOfArrays rvecs = cv::noArray(), cv::OutputArrayOfArrays tvecs = cv::noArray(), int flags = 0,
+	cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, DBL_EPSILON));
+
+
+
+/**
+ * @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<std::vector<cv::Point2f> > ).
+ * @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.
+ *
+ * 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 can slightly reduce the detection rate.
+ */
+CV_EXPORTS_W void detectCharucoDiamond(cv::InputArray image, cv::InputArrayOfArrays markerCorners,
+                                       cv::InputArray markerIds, float squareMarkerLengthRate, // editorconfig-checker-disable-line
+                                       cv::OutputArrayOfArrays diamondCorners, cv::OutputArray diamondIds, // editorconfig-checker-disable-line
+                                       cv::InputArray cameraMatrix = cv::noArray(), // editorconfig-checker-disable-line
+                                       cv::InputArray distCoeffs = cv::noArray()); // editorconfig-checker-disable-line
+
+
+
+/**
+ * @brief Draw a set of detected ChArUco Diamond markers
+ *
+ * @param image input/output image. It must have 1 or 3 channels. The number of channels is not
+ * altered.
+ * @param diamondCorners positions of diamond corners in the same format returned by
+ * detectCharucoDiamond(). (e.g std::vector<std::vector<cv::Point2f> > ). For N detected markers,
+ * the dimensions of this array should be Nx4. The order of the corners should be clockwise.
+ * @param diamondIds vector of identifiers for diamonds in diamondCorners, in the same format
+ * returned by detectCharucoDiamond() (e.g. std::vector<Vec4i>).
+ * Optional, if not provided, ids are not painted.
+ * @param borderColor color of marker borders. Rest of colors (text color and first corner color)
+ * are calculated based on this one.
+ *
+ * Given an array of detected diamonds, this functions draws them in the image. The marker borders
+ * are painted and the markers identifiers if provided.
+ * Useful for debugging purposes.
+ */
+CV_EXPORTS_W void drawDetectedDiamonds(cv::InputOutputArray image, cv::InputArrayOfArrays diamondCorners,
+                                       cv::InputArray diamondIds = cv::noArray(), // editorconfig-checker-disable-line
+                                       cv::Scalar borderColor = cv::Scalar(0, 0, 255)); // editorconfig-checker-disable-line
+
+
+
+
+/**
+ * @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.
+ */
+// TODO cannot be exported yet; conversion from/to Vec4i is not wrapped in core
+CV_EXPORTS void drawCharucoDiamond(const cv::Ptr<Dictionary> &dictionary, cv::Vec4i ids, int squareLength,
+                                   int markerLength, cv::OutputArray img, int marginSize = 0, // editorconfig-checker-disable-line
+                                   int borderBits = 1); // editorconfig-checker-disable-line
+
+
+
+
+//! @}
+}
+
+#endif

aruco_pose/src/aruco_lib/aruco/include/aruco/dictionary.hpp

@@ -0,0 +1,210 @@
+/*
+By downloading, copying, installing or using the software you agree to this
+license. If you do not agree to this license, do not download, install,
+copy or use the software.
+
+													License Agreement
+							 For Open Source Computer Vision Library
+											 (3-clause BSD License)
+
+Copyright (C) 2013, OpenCV Foundation, all rights reserved.
+Third party copyrights are property of their respective owners.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+	* Redistributions of source code must retain the above copyright notice,
+		this list of conditions and the following disclaimer.
+
+	* Redistributions in binary form must reproduce the above copyright notice,
+		this list of conditions and the following disclaimer in the documentation
+		and/or other materials provided with the distribution.
+
+	* Neither the names of the copyright holders nor the names of the contributors
+		may be used to endorse or promote products derived from this software
+		without specific prior written permission.
+
+This software is provided by the copyright holders and contributors "as is" and
+any express or implied warranties, including, but not limited to, the implied
+warranties of merchantability and fitness for a particular purpose are
+disclaimed. In no event shall copyright holders or contributors be liable for
+any direct, indirect, incidental, special, exemplary, or consequential damages
+(including, but not limited to, procurement of substitute goods or services;
+loss of use, data, or profits; or business interruption) however caused
+and on any theory of liability, whether in contract, strict liability,
+or tort (including negligence or otherwise) arising in any way out of
+the use of this software, even if advised of the possibility of such damage.
+*/
+
+#ifndef __OPENCV_DICTIONARY_HPP__
+#define __OPENCV_DICTIONARY_HPP__
+
+#include <opencv2/core.hpp>
+
+namespace aruco_lib {
+
+//! @addtogroup aruco
+//! @{
+
+
+/**
+ * @brief Dictionary/Set of markers. It contains the inner codification
+ *
+ * bytesList contains the marker codewords where
+ * - bytesList.rows is the dictionary size
+ * - each marker is encoded using `nbytes = ceil(markerSize*markerSize/8.)`
+ * - each row contains all 4 rotations of the marker, so its length is `4*nbytes`
+ *
+ * `bytesList.ptr(i)[k*nbytes + j]` is then the j-th byte of i-th marker, in its k-th rotation.
+ */
+class CV_EXPORTS_W Dictionary {
+
+public:
+	CV_PROP_RW cv::Mat bytesList;         // marker code information
+	CV_PROP_RW int markerSize;        // number of bits per dimension
+	CV_PROP_RW int maxCorrectionBits; // maximum number of bits that can be corrected
+
+
+	/**
+	*/
+	Dictionary(const cv::Mat &_bytesList = cv::Mat(), int _markerSize = 0, int _maxcorr = 0);
+
+
+	/**
+	Dictionary(const Dictionary &_dictionary);
+	*/
+
+
+	/**
+	*/
+	Dictionary(const cv::Ptr<Dictionary> &_dictionary);
+
+
+	/**
+	 * @see generateCustomDictionary
+	 */
+	CV_WRAP_AS(create) static cv::Ptr<Dictionary> create(int nMarkers, int markerSize, int randomSeed=0);
+
+
+	/**
+	 * @see generateCustomDictionary
+	 */
+	CV_WRAP_AS(create_from) static cv::Ptr<Dictionary> create(int nMarkers, int markerSize,
+	                                                          const cv::Ptr<Dictionary> &baseDictionary, int randomSeed=0);
+
+	/**
+	 * @see getPredefinedDictionary
+	 */
+	CV_WRAP static cv::Ptr<Dictionary> get(int dict);
+
+	/**
+	 * @brief Given a matrix of bits. Returns whether if marker is identified or not.
+	 * It returns by reference the correct id (if any) and the correct rotation
+	 */
+	bool identify(const cv::Mat &onlyBits, int &idx, int &rotation, double maxCorrectionRate) const;
+
+	/**
+	* @brief Returns the distance of the input bits to the specific id. If allRotations is true,
+	* the four posible bits rotation are considered
+	*/
+	int getDistanceToId(cv::InputArray bits, int id, bool allRotations = true) const;
+
+
+	/**
+	 * @brief Draw a canonical marker image
+	 */
+	CV_WRAP void drawMarker(int id, int sidePixels, cv::OutputArray _img, int borderBits = 1) const;
+
+
+	/**
+	* @brief Transform matrix of bits to list of bytes in the 4 rotations
+	*/
+	CV_WRAP static cv::Mat getByteListFromBits(const cv::Mat &bits);
+
+
+	/**
+	* @brief Transform list of bytes to matrix of bits
+	*/
+	CV_WRAP static cv::Mat getBitsFromByteList(const cv::Mat &byteList, int markerSize);
+};
+
+
+
+
+/**
+ * @brief Predefined markers dictionaries/sets
+ * Each dictionary indicates the number of bits and the number of markers contained
+ * - DICT_ARUCO_ORIGINAL: standard ArUco Library Markers. 1024 markers, 5x5 bits, 0 minimum
+													distance
+ */
+enum PREDEFINED_DICTIONARY_NAME {
+	DICT_4X4_50 = 0,
+	DICT_4X4_100,
+	DICT_4X4_250,
+	DICT_4X4_1000,
+	DICT_5X5_50,
+	DICT_5X5_100,
+	DICT_5X5_250,
+	DICT_5X5_1000,
+	DICT_6X6_50,
+	DICT_6X6_100,
+	DICT_6X6_250,
+	DICT_6X6_1000,
+	DICT_7X7_50,
+	DICT_7X7_100,
+	DICT_7X7_250,
+	DICT_7X7_1000,
+	DICT_ARUCO_ORIGINAL,
+	DICT_APRILTAG_16h5,     ///< 4x4 bits, minimum hamming distance between any two codes = 5, 30 codes
+	DICT_APRILTAG_25h9,     ///< 5x5 bits, minimum hamming distance between any two codes = 9, 35 codes
+	DICT_APRILTAG_36h10,    ///< 6x6 bits, minimum hamming distance between any two codes = 10, 2320 codes
+	DICT_APRILTAG_36h11     ///< 6x6 bits, minimum hamming distance between any two codes = 11, 587 codes
+};
+
+
+/**
+* @brief Returns one of the predefined dictionaries defined in PREDEFINED_DICTIONARY_NAME
+*/
+CV_EXPORTS cv::Ptr<Dictionary> getPredefinedDictionary(PREDEFINED_DICTIONARY_NAME name);
+
+
+/**
+* @brief Returns one of the predefined dictionaries referenced by DICT_*.
+*/
+CV_EXPORTS_W cv::Ptr<Dictionary> getPredefinedDictionary(int dict);
+
+
+/**
+	* @see generateCustomDictionary
+	*/
+CV_EXPORTS_AS(custom_dictionary) cv::Ptr<Dictionary> generateCustomDictionary(
+	int nMarkers,
+	int markerSize,
+	int randomSeed=0);
+
+
+/**
+* @brief Generates a new customizable marker dictionary
+*
+* @param nMarkers number of markers in the dictionary
+* @param markerSize number of bits per dimension of each markers
+* @param baseDictionary Include the markers in this dictionary at the beginning (optional)
+* @param randomSeed a user supplied seed for theRNG()
+*
+* This function creates a new dictionary composed by nMarkers markers and each markers composed
+* by markerSize x markerSize bits. If baseDictionary is provided, its markers are directly
+* included and the rest are generated based on them. If the size of baseDictionary is higher
+* than nMarkers, only the first nMarkers in baseDictionary are taken and no new marker is added.
+*/
+CV_EXPORTS_AS(custom_dictionary_from) cv::Ptr<Dictionary> generateCustomDictionary(
+	int nMarkers,
+	int markerSize,
+	const cv::Ptr<Dictionary> &baseDictionary,
+	int randomSeed=0);
+
+
+
+//! @}
+}
+
+#endif

aruco_pose/src/aruco_lib/aruco/precomp.hpp

@@ -0,0 +1,49 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+ //
+ //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+ //
+ //  By downloading, copying, installing or using the software you agree to this license.
+ //  If you do not agree to this license, do not download, install,
+ //  copy or use the software.
+ //
+ //
+ //                           License Agreement
+ //                For Open Source Computer Vision Library
+ //
+ // Copyright (C) 2014, OpenCV Foundation, all rights reserved.
+ // Third party copyrights are property of their respective owners.
+ //
+ // Redistribution and use in source and binary forms, with or without modification,
+ // are permitted provided that the following conditions are met:
+ //
+ //   * Redistribution's of source code must retain the above copyright notice,
+ //     this list of conditions and the following disclaimer.
+ //
+ //   * Redistribution's in binary form must reproduce the above copyright notice,
+ //     this list of conditions and the following disclaimer in the documentation
+ //     and/or other materials provided with the distribution.
+ //
+ //   * The name of the copyright holders may not be used to endorse or promote products
+ //     derived from this software without specific prior written permission.
+ //
+ // This software is provided by the copyright holders and contributors "as is" and
+ // any express or implied warranties, including, but not limited to, the implied
+ // warranties of merchantability and fitness for a particular purpose are disclaimed.
+ // In no event shall the Intel Corporation or contributors be liable for any direct,
+ // indirect, incidental, special, exemplary, or consequential damages
+ // (including, but not limited to, procurement of substitute goods or services;
+ // loss of use, data, or profits; or business interruption) however caused
+ // and on any theory of liability, whether in contract, strict liability,
+ // or tort (including negligence or otherwise) arising in any way out of
+ // the use of this software, even if advised of the possibility of such damage.
+ //
+ //M*/
+
+#ifndef __OPENCV_CCALIB_PRECOMP__
+#define __OPENCV_CCALIB_PRECOMP__
+
+#include <opencv2/core.hpp>
+#include <opencv2/calib3d.hpp>
+#include <vector>
+
+#endif

aruco_pose/src/aruco_lib/aruco/unionfind.hpp

@@ -0,0 +1,134 @@
+// 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.
+//
+// Copyright (C) 2013-2016, The Regents of The University of Michigan.
+//
+// This software was developed in the APRIL Robotics Lab under the
+// direction of Edwin Olson, ebolson@umich.edu. This software may be
+// available under alternative licensing terms; contact the address above.
+//
+// The views and conclusions contained in the software and documentation are those
+// of the authors and should not be interpreted as representing official policies,
+// either expressed or implied, of the Regents of The University of Michigan.
+#ifndef _OPENCV_UNIONFIND_HPP_
+#define _OPENCV_UNIONFIND_HPP_
+
+#include <stdint.h>
+#include <stdlib.h>
+
+namespace aruco_lib {
+
+typedef struct unionfind unionfind_t;
+struct unionfind{
+	uint32_t maxid;
+	struct ufrec *data;
+};
+
+struct ufrec{
+	// the parent of this node. If a node's parent is its own index,
+	// then it is a root.
+	uint32_t parent;
+
+	// for the root of a connected component, the number of components
+	// connected to it. For intermediate values, it's not meaningful.
+	uint32_t size;
+};
+
+static inline unionfind_t *unionfind_create(uint32_t maxid){
+	unionfind_t *uf = (unionfind_t*) calloc(1, sizeof(unionfind_t));
+	uf->maxid = maxid;
+	uf->data = (struct ufrec*) malloc((maxid+1) * sizeof(struct ufrec));
+	for (unsigned int i = 0; i <= maxid; i++) {
+		uf->data[i].size = 1;
+		uf->data[i].parent = i;
+	}
+	return uf;
+}
+
+static inline void unionfind_destroy(unionfind_t *uf){
+	free(uf->data);
+	free(uf);
+}
+
+/*
+static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id)
+{
+	// base case: a node is its own parent
+	if (uf->data[id].parent == id)
+		return id;
+
+	// otherwise, recurse
+	uint32_t root = unionfind_get_representative(uf, uf->data[id].parent);
+
+	// short circuit the path. [XXX This write prevents tail recursion]
+	uf->data[id].parent = root;
+
+	return root;
+}
+ */
+
+// this one seems to be every-so-slightly faster than the recursive
+// version above.
+static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id){
+	uint32_t root = id;
+
+	// chase down the root
+	while (uf->data[root].parent != root) {
+		root = uf->data[root].parent;
+	}
+
+	// go back and collapse the tree.
+	//
+	// XXX: on some of our workloads that have very shallow trees
+	// (e.g. image segmentation), we are actually faster not doing
+	// this...
+	while (uf->data[id].parent != root) {
+		uint32_t tmp = uf->data[id].parent;
+		uf->data[id].parent = root;
+		id = tmp;
+	}
+
+	return root;
+}
+
+static inline uint32_t unionfind_get_set_size(unionfind_t *uf, uint32_t id){
+	uint32_t repid = unionfind_get_representative(uf, id);
+	return uf->data[repid].size;
+}
+
+static inline uint32_t unionfind_connect(unionfind_t *uf, uint32_t aid, uint32_t bid){
+	uint32_t aroot = unionfind_get_representative(uf, aid);
+	uint32_t broot = unionfind_get_representative(uf, bid);
+
+	if (aroot == broot)
+		return aroot;
+
+	// we don't perform "union by rank", but we perform a similar
+	// operation (but probably without the same asymptotic guarantee):
+	// We join trees based on the number of *elements* (as opposed to
+	// rank) contained within each tree. I.e., we use size as a proxy
+	// for rank.  In my testing, it's often *faster* to use size than
+	// rank, perhaps because the rank of the tree isn't that critical
+	// if there are very few nodes in it.
+	uint32_t asize = uf->data[aroot].size;
+	uint32_t bsize = uf->data[broot].size;
+
+	// optimization idea: We could shortcut some or all of the tree
+	// that is grafted onto the other tree. Pro: those nodes were just
+	// read and so are probably in cache. Con: it might end up being
+	// wasted effort -- the tree might be grafted onto another tree in
+	// a moment!
+	if (asize > bsize) {
+		uf->data[broot].parent = aroot;
+		uf->data[aroot].size += bsize;
+		return aroot;
+	} else {
+		uf->data[aroot].parent = broot;
+		uf->data[broot].size += asize;
+		return broot;
+	}
+}
+}
+
+#endif

aruco_pose/src/aruco_lib/aruco/zarray.hpp

@@ -0,0 +1,149 @@
+// 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.
+//
+// Copyright (C) 2013-2016, The Regents of The University of Michigan.
+//
+// This software was developed in the APRIL Robotics Lab under the
+// direction of Edwin Olson, ebolson@umich.edu. This software may be
+// available under alternative licensing terms; contact the address above.
+//
+// The views and conclusions contained in the software and documentation are those
+// of the authors and should not be interpreted as representing official policies,
+// either expressed or implied, of the Regents of The University of Michigan.
+#ifndef _OPENCV_ZARRAY_HPP_
+#define _OPENCV_ZARRAY_HPP_
+
+#include <stdlib.h>
+#include <string.h>
+
+namespace aruco_lib {
+
+
+struct sQuad{
+	float p[4][2]; // corners
+};
+
+/**
+ * Defines a structure which acts as a resize-able array ala Java's ArrayList.
+ */
+typedef struct zarray zarray_t;
+struct zarray{
+	size_t el_sz; // size of each element
+
+	int size; // how many elements?
+	int alloc; // we've allocated storage for how many elements?
+	char *data;
+};
+
+/**
+ * Creates and returns a variable array structure capable of holding elements of
+ * the specified size. It is the caller's responsibility to call zarray_destroy()
+ * on the returned array when it is no longer needed.
+ */
+inline static zarray_t *_zarray_create(size_t el_sz){
+	zarray_t *za = (zarray_t*) calloc(1, sizeof(zarray_t));
+	za->el_sz = el_sz;
+	return za;
+}
+
+/**
+ * Frees all resources associated with the variable array structure which was
+ * created by zarray_create(). After calling, 'za' will no longer be valid for storage.
+ */
+inline static void _zarray_destroy(zarray_t *za){
+	if (za == NULL)
+		return;
+
+	if (za->data != NULL)
+		free(za->data);
+	memset(za, 0, sizeof(zarray_t));
+	free(za);
+}
+
+/**
+ * Retrieves the number of elements currently being contained by the passed
+ * array, which may be different from its capacity. The index of the last element
+ * in the array will be one less than the returned value.
+ */
+inline static int _zarray_size(const zarray_t *za){
+	return za->size;
+}
+
+/**
+ * Allocates enough internal storage in the supplied variable array structure to
+ * guarantee that the supplied number of elements (capacity) can be safely stored.
+ */
+inline static void _zarray_ensure_capacity(zarray_t *za, int capacity){
+	if (capacity <= za->alloc)
+		return;
+
+	while (za->alloc < capacity) {
+		za->alloc *= 2;
+		if (za->alloc < 8)
+			za->alloc = 8;
+	}
+
+	za->data = (char*) realloc(za->data, za->alloc * za->el_sz);
+}
+
+/**
+ * Adds a new element to the end of the supplied array, and sets its value
+ * (by copying) from the data pointed to by the supplied pointer 'p'.
+ * Automatically ensures that enough storage space is available for the new element.
+ */
+inline static void _zarray_add(zarray_t *za, const void *p){
+	_zarray_ensure_capacity(za, za->size + 1);
+
+	memcpy(&za->data[za->size*za->el_sz], p, za->el_sz);
+	za->size++;
+}
+
+/**
+ * Retrieves the element from the supplied array located at the zero-based
+ * index of 'idx' and copies its value into the variable pointed to by the pointer
+ * 'p'.
+ */
+inline static void _zarray_get(const zarray_t *za, int idx, void *p){
+	CV_DbgAssert(idx >= 0);
+	CV_DbgAssert(idx < za->size);
+
+	memcpy(p, &za->data[idx*za->el_sz], za->el_sz);
+}
+
+/**
+ * Similar to zarray_get(), but returns a "live" pointer to the internal
+ * storage, avoiding a memcpy. This pointer is not valid across
+ * operations which might move memory around (i.e. zarray_remove_value(),
+ * zarray_remove_index(), zarray_insert(), zarray_sort(), zarray_clear()).
+ * 'p' should be a pointer to the pointer which will be set to the internal address.
+ */
+inline static void _zarray_get_volatile(const zarray_t *za, int idx, void *p){
+	CV_DbgAssert(idx >= 0);
+	CV_DbgAssert(idx < za->size);
+
+	*((void**) p) = &za->data[idx*za->el_sz];
+}
+
+inline static void _zarray_truncate(zarray_t *za, int sz){
+	za->size = sz;
+}
+
+/**
+ * Sets the value of the current element at index 'idx' by copying its value from
+ * the data pointed to by 'p'. The previous value of the changed element will be
+ * copied into the data pointed to by 'outp' if it is not null.
+ */
+static inline void _zarray_set(zarray_t *za, int idx, const void *p, void *outp){
+	CV_DbgAssert(idx >= 0);
+	CV_DbgAssert(idx < za->size);
+
+	if (outp != NULL)
+		memcpy(outp, &za->data[idx*za->el_sz], za->el_sz);
+
+	memcpy(&za->data[idx*za->el_sz], p, za->el_sz);
+}
+
+}
+
+#endif

aruco_pose/src/aruco_lib/aruco/zmaxheap.cpp

@@ -0,0 +1,208 @@
+// 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.
+//
+// Copyright (C) 2013-2016, The Regents of The University of Michigan.
+//
+// This software was developed in the APRIL Robotics Lab under the
+// direction of Edwin Olson, ebolson@umich.edu. This software may be
+// available under alternative licensing terms; contact the address above.
+//
+// The views and conclusions contained in the software and documentation are those
+// of the authors and should not be interpreted as representing official policies,
+// either expressed or implied, of the Regents of The University of Michigan.
+
+#include "precomp.hpp"
+#include "zmaxheap.hpp"
+
+
+//                 0
+//         1               2
+//      3     4        5       6
+//     7 8   9 10    11 12   13 14
+//
+// Children of node i:  2*i+1, 2*i+2
+// Parent of node i: (i-1) / 2
+//
+// Heap property: a parent is greater than (or equal to) its children.
+
+#define MIN_CAPACITY 16
+
+namespace aruco_lib {
+
+struct zmaxheap
+{
+	size_t el_sz;
+
+	int size;
+	int alloc;
+
+	float *values;
+	char *data;
+
+	void (*swap)(zmaxheap_t *heap, int a, int b);
+};
+
+static inline void _swap_default(zmaxheap_t *heap, int a, int b)
+{
+	float t = heap->values[a];
+	heap->values[a] = heap->values[b];
+	heap->values[b] = t;
+
+	cv::AutoBuffer<char> tmp(heap->el_sz);
+	memcpy(tmp, &heap->data[a*heap->el_sz], heap->el_sz);
+	memcpy(&heap->data[a*heap->el_sz], &heap->data[b*heap->el_sz], heap->el_sz);
+	memcpy(&heap->data[b*heap->el_sz], tmp, heap->el_sz);
+}
+
+static inline void _swap_pointer(zmaxheap_t *heap, int a, int b)
+{
+	float t = heap->values[a];
+	heap->values[a] = heap->values[b];
+	heap->values[b] = t;
+
+	void **pp = (void**) heap->data;
+	void *tmp = pp[a];
+	pp[a] = pp[b];
+	pp[b] = tmp;
+}
+
+
+zmaxheap_t *zmaxheap_create(size_t el_sz)
+{
+	zmaxheap_t *heap = (zmaxheap_t*)calloc(1, sizeof(zmaxheap_t));
+	heap->el_sz = el_sz;
+
+	heap->swap = _swap_default;
+
+	if (el_sz == sizeof(void*))
+		heap->swap = _swap_pointer;
+
+	return heap;
+}
+
+void zmaxheap_destroy(zmaxheap_t *heap)
+{
+	free(heap->values);
+	free(heap->data);
+	memset(heap, 0, sizeof(zmaxheap_t));
+	free(heap);
+}
+
+static void _zmaxheap_ensure_capacity(zmaxheap_t *heap, int capacity)
+{
+	if (heap->alloc >= capacity)
+		return;
+
+	int newcap = heap->alloc;
+
+	while (newcap < capacity) {
+		if (newcap < MIN_CAPACITY) {
+			newcap = MIN_CAPACITY;
+			continue;
+		}
+
+		newcap *= 2;
+	}
+
+	heap->values = (float*)realloc(heap->values, newcap * sizeof(float));
+	heap->data = (char*)realloc(heap->data, newcap * heap->el_sz);
+	heap->alloc = newcap;
+}
+
+void zmaxheap_add(zmaxheap_t *heap, void *p, float v)
+{
+	_zmaxheap_ensure_capacity(heap, heap->size + 1);
+
+	int idx = heap->size;
+
+	heap->values[idx] = v;
+	memcpy(&heap->data[idx*heap->el_sz], p, heap->el_sz);
+
+	heap->size++;
+
+	while (idx > 0) {
+
+		int parent = (idx - 1) / 2;
+
+		// we're done!
+		if (heap->values[parent] >= v)
+			break;
+
+		// else, swap and recurse upwards.
+		heap->swap(heap, idx, parent);
+		idx = parent;
+	}
+}
+
+// Removes the item in the heap at the given index.  Returns 1 if the
+// item existed. 0 Indicates an invalid idx (heap is smaller than
+// idx). This is mostly intended to be used by zmaxheap_remove_max.
+static int zmaxheap_remove_index(zmaxheap_t *heap, int idx, void *p, float *v)
+{
+	if (idx >= heap->size)
+		return 0;
+
+	// copy out the requested element from the heap.
+	if (v != NULL)
+		*v = heap->values[idx];
+	if (p != NULL)
+		memcpy(p, &heap->data[idx*heap->el_sz], heap->el_sz);
+
+	heap->size--;
+
+	// If this element is already the last one, then there's nothing
+	// for us to do.
+	if (idx == heap->size)
+		return 1;
+
+	// copy last element to first element. (which probably upsets
+	// the heap property).
+	heap->values[idx] = heap->values[heap->size];
+	memcpy(&heap->data[idx*heap->el_sz], &heap->data[heap->el_sz * heap->size], heap->el_sz);
+
+	// now fix the heap. Note, as we descend, we're "pushing down"
+	// the same node the entire time. Thus, while the index of the
+	// parent might change, the parent_score doesn't.
+	int parent = idx;
+	float parent_score = heap->values[idx];
+
+	// descend, fixing the heap.
+	while (parent < heap->size) {
+
+		int left = 2*parent + 1;
+		int right = left + 1;
+
+//            assert(parent_score == heap->values[parent]);
+
+		float left_score = (left < heap->size) ? heap->values[left] : -INFINITY;
+		float right_score = (right < heap->size) ? heap->values[right] : -INFINITY;
+
+		// put the biggest of (parent, left, right) as the parent.
+
+		// already okay?
+		if (parent_score >= left_score && parent_score >= right_score)
+			break;
+
+		// if we got here, then one of the children is bigger than the parent.
+		if (left_score >= right_score) {
+			CV_Assert(left < heap->size);
+			heap->swap(heap, parent, left);
+			parent = left;
+		} else {
+			// right_score can't be less than left_score if right_score is -INFINITY.
+			CV_Assert(right < heap->size);
+			heap->swap(heap, parent, right);
+			parent = right;
+		}
+	}
+
+	return 1;
+}
+
+int zmaxheap_remove_max(zmaxheap_t *heap, void *p, float *v)
+{
+	return zmaxheap_remove_index(heap, 0, p, v);
+}
+
+}

aruco_pose/src/aruco_lib/aruco/zmaxheap.hpp

@@ -0,0 +1,43 @@
+// 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.
+//
+// Copyright (C) 2013-2016, The Regents of The University of Michigan.
+//
+// This software was developed in the APRIL Robotics Lab under the
+// direction of Edwin Olson, ebolson@umich.edu. This software may be
+// available under alternative licensing terms; contact the address above.
+//
+// The views and conclusions contained in the software and documentation are those
+// of the authors and should not be interpreted as representing official policies,
+// either expressed or implied, of the Regents of The University of Michigan.
+#ifndef _OPENCV_ZMAXHEAP_HPP_
+#define _OPENCV_ZMAXHEAP_HPP_
+
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+namespace aruco_lib {
+
+typedef struct zmaxheap zmaxheap_t;
+
+typedef struct zmaxheap_iterator zmaxheap_iterator_t;
+struct zmaxheap_iterator {
+	zmaxheap_t *heap;
+	int in, out;
+};
+
+zmaxheap_t *zmaxheap_create(size_t el_sz);
+
+void zmaxheap_destroy(zmaxheap_t *heap);
+
+void zmaxheap_add(zmaxheap_t *heap, void *p, float v);
+
+// returns 0 if the heap is empty, so you can do
+// while (zmaxheap_remove_max(...)) { }
+int zmaxheap_remove_max(zmaxheap_t *heap, void *p, float *v);
+
+}
+
+#endif

aruco_pose/src/aruco_map.cpp

@@ -43,7 +43,7 @@
 #include <aruco_pose/Marker.h>
 
 #include <opencv2/opencv.hpp>
-#include <opencv2/aruco.hpp>
+#include <aruco.hpp>
 
 #include "draw.h"
 #include "utils.h"
@@ -65,7 +65,7 @@ private:
 	message_filters::Subscriber<CameraInfo> info_sub_;
 	message_filters::Subscriber<MarkerArray> markers_sub_;
 	boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > sync_;
-	cv::Ptr<cv::aruco::Board> board_;
+	cv::Ptr<aruco_lib::Board> board_;
 	Mat camera_matrix_, dist_coeffs_;
 	geometry_msgs::TransformStamped transform_;
 	geometry_msgs::PoseWithCovarianceStamped pose_;
@@ -92,9 +92,9 @@ public:
 		img_pub_ = nh_priv_.advertise<sensor_msgs::Image>("image", 1, true);
 		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1, true);
 
-		board_ = cv::makePtr<cv::aruco::Board>();
-		board_->dictionary = cv::aruco::getPredefinedDictionary(
-			                 static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(nh_priv_.param("dictionary", 2)));
+		board_ = cv::makePtr<aruco_lib::Board>();
+		board_->dictionary = aruco_lib::getPredefinedDictionary(
+			                 static_cast<aruco_lib::PREDEFINED_DICTIONARY_NAME>(nh_priv_.param("dictionary", 2)));
 		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
 		dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);
 
@@ -170,7 +170,7 @@ public:
 
 		if (known_tilt_.empty()) {
 			// simple estimation
-			valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
+			valid = aruco_lib::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
 			                                     rvec, tvec, false);
 			if (!valid) goto publish_debug;
 
@@ -183,7 +183,7 @@ public:
 		} else {
 			Mat obj_points, img_points;
 			// estimation with "snapping"
-			cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points);
+			aruco_lib::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points);
 			if (obj_points.empty()) goto publish_debug;
 
 			double center_x = 0, center_y = 0, center_z = 0;
@@ -228,7 +228,7 @@ publish_debug:
 		// publish debug image (even if no map detected)
 		if (debug_pub_.getNumSubscribers() > 0) {
 			Mat mat = cv_bridge::toCvCopy(image, "bgr8")->image; // copy image as we're planning to modify it
-			cv::aruco::drawDetectedMarkers(mat, corners, ids); // draw detected markers
+			aruco_lib::drawDetectedMarkers(mat, corners, ids); // draw detected markers
 			if (valid) {
 				_drawAxis(mat, camera_matrix_, dist_coeffs_, rvec, tvec, 1.0); // draw board axis
 			}

aruco_pose/src/draw.cpp

@@ -5,7 +5,7 @@
 #include <math.h>
 
 using namespace cv;
-using namespace cv::aruco;
+using namespace aruco_lib;
 
 static void _cvProjectPoints2( const CvMat* object_points, const CvMat* rotation_vector,
                        const CvMat* translation_vector, const CvMat* camera_matrix,

aruco_pose/src/draw.h

@@ -1,9 +1,11 @@
+#pragma once
+
 #include <cmath>
 #include <ros/ros.h>
 #include <opencv2/opencv.hpp>
-#include <opencv2/aruco.hpp>
+#include <aruco.hpp>
 
-void _drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img,
+void _drawPlanarBoard(aruco_lib::Board *_board, cv::Size outSize, cv::OutputArray _img,
                       int marginSize, int borderBits, bool drawAxis); // editorconfig-checker-disable-line
 void _drawAxis(cv::InputOutputArray image, cv::InputArray cameraMatrix, cv::InputArray distCoeffs,
                cv::InputArray rvec, cv::InputArray tvec, float length); // editorconfig-checker-disable-line

builder/test/tests.sh

@@ -29,7 +29,8 @@ pigpiod -v
 i2cdetect -V
 butterfly -h
 espeak --version
-mjpg_streamer --version
+# FIXME: no mjpg_streamer for buster yet
+# mjpg_streamer --version
 
 # ros stuff