java - 绘制 3dAxis 时 ArUco 轴交换

Question

我目前正在尝试为一个项目开发 ArUco 立方体检测器。目标是在不使用大型 ArUco 板的情况下进行更稳定和准确的姿势估计。然而，为了让它起作用，我需要知道每个标记的方向。使用 draw3dAxis 方法，我发现 X 轴和 Y 轴并没有始终出现在同一位置。这是演示该问题的视频:https://youtu.be/gS7BWKm2nmg

好像是Rvec检测的问题。 Rvec 的前两个值有明显的变化，在轴交换之前将保持相当一致。当这种轴交换发生时，值可以改变 2-6 之间的任何幅度。 ARu​​co 库确实尝试处理旋转，如 Marker.calculateMarkerId() 方法所示:

/**
 * Return the id read in the code inside a marker. Each marker is divided into 7x7 regions
 * of which the inner 5x5 contain info, the border should always be black. This function
 * assumes that the code has been extracted previously.
 * @return the id of the marker
 */
protected int calculateMarkerId(){
    // check all the rotations of code
    Code[] rotations = new Code[4];
    rotations[0] = code;
    int[] dists = new int[4];
    dists[0] = hammDist(rotations[0]);
    int[] minDist = {dists[0],0};
    for(int i=1;i<4;i++){
        // rotate
        rotations[i] = Code.rotate(rotations[i-1]);
        dists[i] = hammDist(rotations[i]);
        if(dists[i] < minDist[0]){
            minDist[0] = dists[i];
            minDist[1] = i;
        }
    }
    this.rotations = minDist[1];
    if(minDist[0] != 0){
        return -1; // matching id not found
    }
    else{
        this.id = mat2id(rotations[minDist[1]]);
    }
    return id;
}

并且 MarkerDetector.detect() 调用该方法并使用 getRotations() 方法:

    // identify the markers
    for(int i=0;i<nCandidates;i++){
        if(toRemove.get(i) == 0){
            Marker marker = candidateMarkers.get(i);
            Mat canonicalMarker = new Mat();
            warp(in, canonicalMarker, new Size(50,50), marker.toList());
            marker.setMat(canonicalMarker);
            marker.extractCode();
            if(marker.checkBorder()){
                int id = marker.calculateMarkerId();
                if(id != -1){
                    // rotate the points of the marker so they are always in the same order no matter the camera orientation
                    Collections.rotate(marker.toList(), 4-marker.getRotations());

                    newMarkers.add(marker);

                }
            }
        }
    }

ArUco 库的完整源代码在这里:https://github.com/sidberg/aruco-android/blob/master/Aruco/src/es/ava/aruco/MarkerDetector.java

如果有人有任何建议或解决方案，我将不胜感激。如果您有任何问题，请与我联系。

Answer 1

我确实找到了问题。事实证明，标记类有一个旋转变量，可用于旋转轴以与标记的方向对齐。我在Utils类中写了如下方法:

protected static void alignToId(Mat rotation, int codeRotation) {
    //get the matrix corresponding to the rotation vector
    Mat R = new Mat(3, 3, CvType.CV_64FC1);
    Calib3d.Rodrigues(rotation, R);

    codeRotation += 1;
    //create the matrix to rotate around Z Axis
    double[] rot = {
            Math.cos(Math.toRadians(90) * codeRotation), -Math.sin(Math.toRadians(90) * codeRotation), 0,
            Math.sin(Math.toRadians(90) * codeRotation), Math.cos(Math.toRadians(90) * codeRotation), 0,
            0, 0, 1
    };

    // multiply both matrix
    Mat res = new Mat(3, 3, CvType.CV_64FC1);
    double[] prod = new double[9];
    double[] a = new double[9];
    R.get(0, 0, a);
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++) {
            prod[3 * i + j] = 0;
            for (int k = 0; k < 3; k++) {
                prod[3 * i + j] += a[3 * i + k] * rot[3 * k + j];
            }
        }
    // convert the matrix to a vector with rodrigues back
    res.put(0, 0, prod);
    Calib3d.Rodrigues(res, rotation);
}

我从 Marker.calculateExtrinsics 方法调用它:

        Utils.alignToId(Rvec, this.getRotations());