ArUco Axis Swap при рисовании 3dAxis

Question

В настоящее время я пытаюсь разработать кубический детектор ArUco для проекта. Цель состоит в том, чтобы иметь более стабильную и точную оценку позы без использования большой платы ArUco. Для этого, однако, мне нужно знать ориентацию каждого из маркеров. Используя метод draw3dAxis, я обнаружил, что оси X и Y не отображаются последовательно в одном месте. Вот видео, демонстрирующее проблему: https://youtu.be/gS7BWKm2nmg

Кажется, проблема с обнаружением Rvec. Существует явный сдвиг в первых двух значениях Rvec, который останется довольно последовательным до тех пор, пока ось не обменивается. Когда происходит эта замена оси, значения могут меняться на величину от 2 до 6. Библиотека ARuco действительно пытается справиться с поворотами, как показано на 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

Я нашел проблему. Оказывается, класс Marker имеет переменную вращения, которая может использоваться для поворота оси для согласования с ориентацией маркера. Я написал следующий метод в классе 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());