### Fisher integral on SO(3) ### ### 2010/05/08 sei ### ### 2011/03/28 Takayama ### ### Preliminaries (about 10 seconds) ### dm <- 20 ZR <- 2 fct <- array(0,ZR+dm) dfct <- array(0,ZR+4*dm) Etens <- array(0,rep(ZR+dm,3)) # Calculate factorials fct[ZR] <- 1 for(i in 1:dm){ fct[ZR+i] <- fct[ZR+i-1]*i } # Calculate double factorials dfct[ZR-1] <- dfct[ZR] <- 1 for(i in 1:(4*dm)){ dfct[ZR+i] <- dfct[ZR+i-2]*i } # Calculate expectation of monomials for(k in 0:dm){ for(l in 0:dm){ for(m in 0:dm){ if((k-l)%%2==0 && (l-m)%%2==0){ e <- 0 for(i in 0:(l%/%2)){ # l-n = 2i a0 <- choose(l,2*i) a1 <- dfct[ZR+k+m]*dfct[ZR+2*i-1]/dfct[ZR+k+m+2*i+1] a2 <- dfct[ZR+k+l-2*i-1]*dfct[ZR+2*i-1]/dfct[ZR+k+l] a3 <- dfct[ZR+m+l-2*i-1]*dfct[ZR+2*i-1]/dfct[ZR+m+l] e <- e + a0*a1*a2*a3 } Etens[ZR+k,ZR+l,ZR+m] <- e }else{ Etens[ZR+k,ZR+l,ZR+m] <- 0 } } } } rm(k,l,m,i,e,a0,a1,a2,a3) ## verify (see shibata\10_02_17\10_02_17\monomialexpectation.pdf) #cat(Etens[ZR+2,ZR,ZR],"\t",1/3,"\n") #cat(Etens[ZR+4,ZR,ZR],"\t",1/5,"\n") #cat(Etens[ZR+2,ZR+2,ZR],"\t",2/15,"\n") #cat(Etens[ZR+3,ZR+3,ZR+1],"\t",9/140,"\n") #cat(Etens[ZR+3,ZR+3,ZR+2],"\t",0,"\n") ### functions ### SO.svd <- function(X){ # sign-preserved SVD S <- svd(X) sigu <- det(S$u); sigv <- det(S$v) S$u[,1] <- S$u[,1] * sigu S$v[,1] <- S$v[,1] * sigv S$d[1] <- S$d[1] / sigu / sigv S } SO.int <- function(X){ # Fisher integral by Taylor expansion (only for SO(3)) S <- SO.svd(X) phi <- S$d f <- 0 df1 <- df2 <- df3 <- 0 kmax <- lmax <- mmax <- dm-1 for(k in 0:kmax){ for(l in 0:lmax){ for(m in 0:mmax){ f <- f + (phi[1]^k)*(phi[2]^l)*(phi[3]^m)*Etens[ZR+k,ZR+l,ZR+m]/fct[ZR+k]/fct[ZR+l]/fct[ZR+m] df1 <- df1 + (phi[1]^k)*(phi[2]^l)*(phi[3]^m)*Etens[ZR+k+1,ZR+l,ZR+m]/fct[ZR+k]/fct[ZR+l]/fct[ZR+m] df2 <- df2 + (phi[1]^k)*(phi[2]^l)*(phi[3]^m)*Etens[ZR+k,ZR+l+1,ZR+m]/fct[ZR+k]/fct[ZR+l]/fct[ZR+m] df3 <- df3 + (phi[1]^k)*(phi[2]^l)*(phi[3]^m)*Etens[ZR+k,ZR+l,ZR+m+1]/fct[ZR+k]/fct[ZR+l]/fct[ZR+m] }}} df <- S$u %*% diag(c(df1,df2,df3)) %*% t(S$v) list(F=f,dF=df) } SO.int.diag <- function(phi){ f <- 0 kmax <- lmax <- mmax <- dm-1 for(k in 0:kmax){ for(l in 0:lmax){ for(m in 0:mmax){ f <- f + (phi[1]^k)*(phi[2]^l)*(phi[3]^m)*Etens[ZR+k,ZR+l,ZR+m]/fct[ZR+k]/fct[ZR+l]/fct[ZR+m] }}} f } dSO.int.diag <- function(phi){ df1 <- df2 <- df3 <- 0 kmax <- lmax <- mmax <- dm-1 for(k in 0:kmax){ for(l in 0:lmax){ for(m in 0:mmax){ df1 <- df1 + (phi[1]^k)*(phi[2]^l)*(phi[3]^m)*Etens[ZR+k+1,ZR+l,ZR+m]/fct[ZR+k]/fct[ZR+l]/fct[ZR+m] df2 <- df2 + (phi[1]^k)*(phi[2]^l)*(phi[3]^m)*Etens[ZR+k,ZR+l+1,ZR+m]/fct[ZR+k]/fct[ZR+l]/fct[ZR+m] df3 <- df3 + (phi[1]^k)*(phi[2]^l)*(phi[3]^m)*Etens[ZR+k,ZR+l,ZR+m+1]/fct[ZR+k]/fct[ZR+l]/fct[ZR+m] }}} c(df1,df2,df3) } rSO <- function(p=3){ # random sample from SO(p) Z <- matrix(rnorm(p*p),p,p) if(det(Z) < 0) Z[,p] <- -Z[,p] S <- svd(Z) S$u%*%t(S$v) } SO.MC <- function(X,LOOP=100000){ # Fisher integral by Monte Carlo method p <- nrow(X) a <- 0 M <- matrix(0,p,p) for(Li in (1:LOOP)){ t <- rSO(p) a <- a + exp(sum(X*t)) M <- M + t*exp(sum(X*t)) } list(F=a/LOOP,dF=M/LOOP) } ## by N.T. SO.int2 <- function(X){ V <- SO.int(X) f <- V$F df <- V$dF H <- 0.001 X11p <- X X11p[1,1] <- X11p[1,1]+H X11m <- X X11m[1,1] <- X11m[1,1]-H V11p <- SO.int(X11p) V11m <- SO.int(X11m) ddf <- matrix(0,3,3) ddf[1,1] = (V11p$F - 2*V$F +V11m$F)/(H*H) list(F=f,dF=df,ddF=ddf) } # Commets Xm <- matrix(c( 0.257,0.158,0.079,0.044,-0.052,0.765,0.189,-0.146,0.004 ), 3,3) # grep 0.115 *.rr #Xm <- matrix(c( # 0.115, 0.001, 0.140, # 0.113, -0.102, -0.233, # 0.022, 0.038, -0.091 # 3,3,byrow=TRUE)) Ts <- matrix(c( 0.689, 0.394, 0.496, 0.341, -0.229, 4.273, 0, 0, 0),3,3) #Ts <- matrix(c( # 0.35, 0.346, 0.066, # 0.1, -0.31, 0.113, # 0, 0, 0),3,3) Ts0 <- matrix(c( 0.35, 0.346, 0.066, 0.0, -0.31, 0.113, 0, 0, 0),3,3) # a test starting point Tsp <- matrix(c( 0.5, 0.5, 0.5, 0.5, -0.5, 3, 0, 0, 0),3,3) # next starting point Tsp2 <- matrix(c( 0.72,0.263 ,0, 0.423 ,-0.164 ,0, 0.396 , 3, 0), 3,3,byrow=TRUE) # Asteroids XmAst <- matrix(c( 0.074, 0.012, 0.016, 0.018, 0.003, -0.070, -0.0, 0.949, 0.002 ), 3,3,byrow=TRUE) TsAst <- matrix(c( 0.157, 0.038, 0.005, 0.254, 0.060, 19.568, 0, 0, 0),3,3) #SO.init(Ts,Xm) -> sf2.rr, test2() SO.init<-function(T,X) { e<- exp(-sum(diag(t(T) %*% X))) f<- SO.int2(T) t11<- T[1,1] t12<- T[1,2] c0<- f$F c11<- (f$dF)[1,1] c12<- (f$dF)[1,2] c1111<- (f$ddF)[1,1] F <- c(c0, t11*c11, t12*c12, (t11*t11*c1111+t11*c11)) print(F) ans <- e*F write("[",file="tmp-SO-init.txt"); write(ans,file="tmp-SO-init.txt",append=TRUE,sep=","); write("];",file="tmp-SO-init.txt",append=TRUE); # write("end$",file="tmp-SO-init.txt",append=TRUE); return(ans); } ### test ### Xa <- matrix(c( 0.93, 0.76, 0.72, 0.33, 0.58, 0.63, 0.85, 0.88, 0.54),3,3) Xb <- Xa Xb[1,1] <- Xb[1,1] + 0.1 Xb[2,2] <- Xb[2,2] + 0.1 #> SO.int(Xa) #$F #[1] 1.948187 # #$dF # [,1] [,2] [,3] #[1,] 0.4352104 0.1873025 0.4631452 #[2,] 0.4649201 0.2803339 0.3789145 #[3,] 0.3222974 0.3181602 0.3255542 # #> SO.int(Xb) #$F #[1] 2.028714 # #$dF # [,1] [,2] [,3] #[1,] 0.5270154 0.1814459 0.4556040 #[2,] 0.4698139 0.3708984 0.3747549 #[3,] 0.3076376 0.3033725 0.3797748