ArUco Axis Swap while drawing 3dAxis

Question

I'm currently trying to develop a ArUco cube detector for a project. The goal is to have a more stable and accurate pose estimation without using a large ArUco board. For this to work however, I need to know the orientation of each of the markers. Using the draw3dAxis method, I discovered that the X and Y axis did not consistently appear in the same location. Here is a video demonstrating the issue: https://youtu.be/gS7BWKm2nmg

It seems to be a problem with the Rvec detection. There is a clear shift in the first two values of the Rvec, which will stay fairly consistent until the axis swaps. When this axis swap happens the values can change by a magnitude anywhere from 2-6. The ARuco library does try to deal with rotations as shown in the Marker.calculateMarkerId() method:

/**
 * 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;
}

and the MarkerDetector.detect() does call that method and uses the getRotations() Method:

    // 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);

                }
            }
        }
    }

The full source code for the ArUco library is here: https://github.com/sidberg/aruco-android/blob/master/Aruco/src/es/ava/aruco/MarkerDetector.java

If anyone has any advice or solutions I'd be very gracious. Please contact me if you have any questions.

score 3 · Answer 1 · answered Jun 22 '16 at 21:36

I did find the problem. It turns out that the Marker Class has a rotation variable that can be used to rotate the axis to align with the orientation of the marker. I wrote the following method in the Utils class:

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);
}

and I called it from the Marker.calculateExtrinsics Method:

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

ArUco Axis Swap while drawing 3dAxis

1 Answers1