Bielefeld Augmented Reality Tracker

# Dies ist die Datei "fit.r".

# Sie benutzt einen Levenberg-Marquardt-Algorithmus

# um Eulerwinkel zu bestimmen, die der gegebenen

# Rotationsmatrix möglicht gut entsprechen.

# Dadurch wird der Fehler aller Matritzeneinträge

# in den Eulerwinkeln abgebildet. 

# Berechnet die Abweichung dreier angenäherter Winkel an eine Rotationsmatrix.

# Die Winkel werden in XYZ-Euler angenommen.

# a ist ein Zeilenvektor der Rotationsmatrix

# x ist ein Vektor der Rotationswinkel in XYZ-Euler

# Das Ergebnis ist ein Vektor, der die Differenz der aus den Winkeln berechneten Rotationsmatrix von der vorgegebenen Matrix angibt.

DiffXYZeulerRotmFIT <- function(x,a){

    ret <- numeric(10)

    #cat("Diff",x,a,"\n")

    #Die unten zugrundeliegende Berechnung der Rotationsmatrix entspricht der Darstellung in Weisstein (k.D.), wurde jedoch auf des verwendete Koordinatensystem (rechtshändig) und die Koordinatensysteme der verwendeten Bibliotheken umgerechnet

    #cat(class(as.numeric(x[2])))

    ret[1]<- (cos(as.numeric(x[2]))*cos(as.numeric(x[3]))-as.numeric(a[3]))

    ret[2]<- ((sin(as.numeric(x[1]))*sin(as.numeric(x[2]))*cos(as.numeric(x[3]))-sin(as.numeric(x[3]))*cos(as.numeric(x[1])))-as.numeric(a[4]))

    ret[3]<- (cos(as.numeric(x[1]))*sin(as.numeric(x[2]))*cos(as.numeric(x[3]))+sin(as.numeric(x[1]))*sin(as.numeric(x[3]))-as.numeric(a[5]))

    ret[4]<- (cos(as.numeric(x[2]))*sin(as.numeric(x[3]))-as.numeric(a[6]))

    ret[5]<- (sin(as.numeric(x[1]))*sin(as.numeric(x[2]))*sin(as.numeric(x[3]))+cos(as.numeric(x[1]))*cos(as.numeric(x[3]))-as.numeric(a[7]))

    ret[6]<- (cos(as.numeric(x[1]))*sin(as.numeric(x[2]))*sin(as.numeric(x[3]))-sin(as.numeric(x[1]))*cos(as.numeric(x[3]))-as.numeric(a[8]))

    ret[7]<- (-sin(as.numeric(x[2])))-as.numeric(a[9])

    ret[8]<- (cos(as.numeric(x[2]))*sin(as.numeric(x[1]))-a[10])

    ret[9]<- (cos(as.numeric(x[1]))*cos(as.numeric(x[2]))-a[11])

    #Dieser Eintrag kann für Anpassungen der Gewichtsfunktion verwendet werden  

    ret[10]<- 0

    return(ret)

}

# Die obenstehende Methode läuft oft in lokale Minima und liefert dann falsche Werte.

# Um dies zu verhindern, werden die Werte für Blender aus 3 Gleichungen "analytisch" berechnet. (Das ist natürlich nicht wirklich eine analytische Rechnung, weil die trigonometrischen Funktionen nummerisch implementiert sein dürften).

startXYZeulerFIT <- function(a){

    toleranz = 1e-10

    ret <- c(-1,-1,-1)

    #Bester Ansatz nach Slabaugh (2015) (Wesentliche Abweichungen sind angegeben)

    if (abs(abs(a[7])-1) > toleranz) #Betrachte auch Werte sehr nahe an +-1 als +-1

    {

        ret[2] <- asin(-a[7])

        #cat("Y-Euler:",ret[2]," ")

        temp <- cos(ret[2])

        ret[1] <- atan2(a[8]/temp,a[9]/temp)

        #cat("X-Euler:",ret[1]," ")

        ret[3] <- atan2(a[4]/temp,a[1]/temp)

        #cat("Z-Euler:",ret[3]," ",a[4]," ",a[1]," ",temp,"\n")

    }

    else

    {

        ret[3] <- (pi/2) # Die im Pseudocode von Slabaugh vorgeschlagene 0 ist für uns numerisch ungünstig

        if(abs(a[7]+1) <= toleranz) #Betrachte auch Werte sehr nahe an -1 als -1

        {

            ret[2] = pi/2

            ret[1] = (pi/2)+atan2(a[2],a[3])

        }

        else

        {

            ret[2] = 3*pi/2 # Entspricht -pi/2

            ret[1] = atan2(-a[2],-a[3])-(pi/2)

        }

    }

    #Sorgt für positive Winkel

    for(i in 1:3){

        while(ret[i] < 0){

            ret[i] <- ret[i]+2*pi

        }

    }

    return(ret)

}

#Wrapper für acos

#min, max behebt Probleme mit der Maschienengenauigkeit im Grenzbereich und verhindert Nans, weil Werte die Definitionsgrenze auf hinteren Nachkommastellen überschreiten (vgl. Bolker (2012)).

myacos <- function(x){

    return (acos(max(min(x,1.0),-1.0)))

}

# Diese Funktion führt alle notwendigen Schritte aus.

# Leider hat lsqnonlin ein Problem damit, wenn der perfekte Wert bereits im ersten Durchgang übergeben wird. Desshalb müssen wir vorher prüfen, ob wir bereits die optimale Lösung haben.

# a ist wieder die Rotationsmatrix

# x0 ist der Startwert

# init_rotm_inv ist die Inverse der für die Bibliothek benötigten Anpassungsmatrix.

Rot2XYZeulerFIT <- function(a,x0,init_rotm_inv) {

    rotm <- as.numeric(as.character(a))

    ret <- numeric(5)

    # Setze Frame und Markerid

    ret[1] <- a[1]

    ret[2] <- a[2]

    # Korrigiere Eingabematrix

    matA <- matrix(rotm[3:11],ncol=3,byrow=TRUE)

    matA <- matA %*% init_rotm_inv

    for(i in 0:2){

        rotm[(i*3+3):(i*3+5)] <- unlist(matA[(i+1),])

    }

    #Berechne aktuellen Fehler

    akterr <- pracma::rmserr(as.double(DiffXYZeulerRotmFIT(x=x0,a=rotm)),rep(0,10))

    #Sind wir nicht perfekt (mse = gemittelter quadratischer Fehler)?

    if(akterr$mse > 0){

        #winkel2 <- pracma::gaussNewton(as.double(winkel),DiffXYZeulerRotmFIT,JacobiFIT,maxiter=250,a=as.double(rotm))

        winkel2 <- pracma::lsqnonlin(DiffXYZeulerRotmFIT,as.double(x0),options = list(tau=1e-9,maxeval=250),a=rotm)

        newerr <- pracma::rmserr(as.double(DiffXYZeulerRotmFIT(x=winkel2$x,rotm)),rep(0,10))

    }

    if(newerr$mse <= akterr$mse){

        #print("Verbesserung erfolgreich!")

        #print(x0)

        #print(winkel2$x)

        ret[3:5] <- winkel2$x

        return(ret)

    }

    else{

        #Verbesserung war nicht möglich

        print("Warnung: Verbesserung gescheitert!")

        print(akterr$mse)

        print(newerr$mse)

        print(x0)

        print(winkel2$x)

        ret[3:5] <- x0

        return(ret)

    }

}

Rot2XYZeulerBART <- function(a,x0) {

    #print("BART")

    x0 <- unlist(x0)

    #Drehrichtung um die X-Achse umdrehen

    x0[1]<- (2*pi)-x0[1]

    init_mat_table <- read.table(file="Bestimmung_Umformungen_Rotm/bart_init_rotm_inv.table")

    init_rotm_inv <- matrix(unlist(init_mat_table), ncol=3, nrow=3, byrow=TRUE)

    ret <- Rot2XYZeulerFIT(a,x0,init_rotm_inv)

    #Drehrichtung um die X-Achse umdrehen

    ret[[3]]<- (2*pi)-ret[[3]]

    return(ret)

}

Rot2XYZeulerALVAR <- function(a,x0) {

    #print("ALVAR")

    x0 <- unlist(x0)

    #Drehrichtung um die X-Achse umdrehen

    x0[1]<- (2*pi)-x0[1]

    init_mat_table <- read.table(file="Bestimmung_Umformungen_Rotm/alvar_init_rotm_inv.table")

    init_rotm_inv <- matrix(unlist(init_mat_table), ncol=3, nrow=3, byrow=TRUE)

    ret <- Rot2XYZeulerFIT(a,x0,init_rotm_inv)

    #Drehrichtung um die X-Achse umdrehen

    ret[[3]]<- (2*pi)-ret[[3]]

    return(ret)

}

Rot2XYZeulerARUCO <- function(a,x0) {

    #print("ARUCO")

    #print(x0)

    #Tausche die ersten beiden Zeilen

    a[3:8] <- a[c(6:8,3:5)]

    x0 <- unlist(x0)

    #Drehrichtung um die Z-Achse umdrehen

    x0[3] <- 2*pi-x0[3]

    init_mat_table <- read.table(file="Bestimmung_Umformungen_Rotm/aruco_init_rotm_inv.table")

    init_rotm_inv <- matrix(unlist(init_mat_table), ncol=3, nrow=3, byrow=TRUE)

    ret <- Rot2XYZeulerFIT(a,x0,init_rotm_inv)

    #Drehrichtung um die Z-Achse umdrehen

    ret[[5]]<- (2*pi)-ret[[5]]

    return(ret)

}

Rot2XYZeulerBLENDER <- function(a) {

    rotm <- as.numeric(as.character(a))

    ret <- numeric(5)

    # Setze Frame und Markerid

    ret[1] <- a[1]

    ret[2] <- a[2]

    #Skalierung auf der Z-Achse ausgleichen

    a[9:11] <- a[9:11]*200

    ret[3:5] <- startXYZeulerFIT(unlist(a[3:11]))

    #Drehrichtung um die Y-Achse umdrehen

    ret[4] <- 2*pi-unlist(ret[4])

    return(ret)

}

#Quellen:

#Bolker, B. (2012, 24. Dezember) acos(1) returns NaN for some values, not others Antwort auf http://stackoverflow.com/questions/18806944/about-arccos-function-in-r-nan-produced

#Slabaugh, G. G. (2015) Computing Euler angles from a rotation matrix http://staff.city.ac.uk/~sbbh653/publications/euler.pdf