3次元回転　ーパラメータ計算とリー代数による最適化ー　金谷健一先生

```{r}


#p.164 Gauss法
#p.12 3次元回転行列でのデータサンプルを予想

m=3;r=3

N=5
obs=array(0,dim=c(m,N))
train=array(0,dim=c(m,N))
for(j in 1:N){
vec=sample(1:9,m,replace=T)  
train[,j]=vec
roll=2*pi/3
R=matrix(c(cos(roll),sin(roll),0,-sin(roll),cos(roll),0,0,0,1),nrow=m)*5
obs[,j]=R%*%vec 
}

f=function(x){
ss=x[1];r=x[2];vec=x[3:5]
mat=matrix(c(cos(ss),sin(ss),0,-sin(ss),cos(ss),0,0,0,1),nrow=m)
return(c((r*(mat))%*%train+vec))
}

Y=c(obs)
h=0.001
lam=1
ite=10^4
sita=sample(1:9,5,replace=T)
eta=10^(-2)

for(l in 1:ite){
sita_pre=sita
Z=array(0,dim=c(length(Y),length(sita)))
vec=sita
for(j in 1:ncol(Z)){
vec_sub=vec;vec_sub[j]=vec_sub[j]+h
Z_sub=(f(vec_sub)-f(vec))/h
Z[,j]=Z_sub
}
beta=solve(t(Z)%*%Z+diag(lam,ncol(Z)))%*%t(Z)%*%(Y-f(sita))
sita=c(sita+eta*beta)
print(sum((Y-f(sita))^2))
}


```

```{r}


#p.164 Gauss法
#p.15 ロール・ピッチ・ヨーで回転行列のデータサンプルを予想

m=3;r=3

N=5
obs=array(0,dim=c(m,N))
train=array(0,dim=c(m,N))
for(j in 1:N){
vec=sample(1:9,m,replace=T)  
train[,j]=vec
roll=2*pi/3
R=matrix(c(cos(roll),sin(roll),0,-sin(roll),cos(roll),0,0,0,1),nrow=m)*5
obs[,j]=R%*%vec 
}

f=function(x){
ss=x[1];ll=x[2];mm=x[3];r=x[4];vec=x[5:7]
Rx=matrix(c(1,0,0,0,cos(ss),sin(ss),0,-sin(ss),cos(ss)),nrow=m)
Ry=matrix(c(cos(ll),0,-sin(ll),0,1,0,sin(ll),0,cos(ll)),nrow=m)
Rz=matrix(c(cos(mm),sin(mm),0,-sin(mm),cos(mm),0,0,0,1),nrow=m)
mat=Rx%*%Ry%*%Rz
return(c((r*(mat))%*%train+vec))
}

Y=c(obs)
h=0.001
lam=1
ite=10^3
sita=sample(1:9,7,replace=T)
eta=10^(-1)

for(l in 1:ite){
sita_pre=sita
Z=array(0,dim=c(length(Y),length(sita)))
vec=sita
for(j in 1:ncol(Z)){
vec_sub=vec;vec_sub[j]=vec_sub[j]+h
Z_sub=(f(vec_sub)-f(vec))/h
Z[,j]=Z_sub
}
beta=solve(t(Z)%*%Z+diag(lam,ncol(Z)))%*%t(Z)%*%(Y-f(sita))
sita=c(sita+eta*beta)
print(sum((Y-f(sita))^2))
}




```


```{r}


#p.164 Gauss法
#p.21 オイラー角で回転行列のデータサンプルを予想

m=3;r=3

N=5
obs=array(0,dim=c(m,N))
train=array(0,dim=c(m,N))
for(j in 1:N){
vec=sample(1:9,m,replace=T)  
train[,j]=vec
roll=2*pi/3
R=matrix(c(cos(roll),sin(roll),0,-sin(roll),cos(roll),0,0,0,1),nrow=m)*5
obs[,j]=R%*%vec 
}

f=function(x){
ss=x[1];ll=x[2];mm=x[3];r=x[4];vec=x[5:7]
mat=matrix(c(
cos(ss)*cos(ll)-sin(ss)*cos(mm)*sin(ll),-cos(ss)*sin(ll)-sin(ss)*cos(mm)*cos(ll),sin(ss)*sin(mm),
sin(ss)*cos(ll)+cos(ss)*cos(mm)*sin(ll),-sin(ss)*sin(ll)+cos(ss)*cos(mm)*cos(ll),-cos(ss)*sin(mm),             
sin(mm)*sin(ll),sin(mm)*cos(ll),cos(mm)             
             ),nrow=m)
return(c((r*t(mat))%*%train+vec))
}

Y=c(obs)
h=0.001
lam=1
ite=10^4
sita=sample(1:9,7,replace=T)
eta=10^(-2)

for(l in 1:ite){
sita_pre=sita
Z=array(0,dim=c(length(Y),length(sita)))
vec=sita
for(j in 1:ncol(Z)){
vec_sub=vec;vec_sub[j]=vec_sub[j]+h
Z_sub=(f(vec_sub)-f(vec))/h
Z[,j]=Z_sub
}
beta=solve(t(Z)%*%Z+diag(lam,ncol(Z)))%*%t(Z)%*%(Y-f(sita))
sita=c(sita+eta*beta)
print(sum((Y-f(sita))^2))
}





```

```{r}


#p.164 Gauss法
#p.26 ロドリーグの式で回転行列のデータサンプルを予想

m=3;r=3

N=5
obs=array(0,dim=c(m,N))
train=array(0,dim=c(m,N))
for(j in 1:N){
vec=sample(1:9,m,replace=T)  
train[,j]=vec
roll=2*pi/3
R=matrix(c(cos(roll),sin(roll),0,-sin(roll),cos(roll),0,0,0,1),nrow=m)
obs[,j]=R%*%vec 
}

f=function(x){
omega=x[1];r=x[2];l=x[3:5];vec=x[6:8]
mat=matrix(c(
cos(omega)+(1-cos(omega))*l[1]^2,
l[1]*l[2]*(1-cos(omega))-l[3]*sin(omega),
l[1]*l[3]*(1-cos(omega))+l[2]*sin(omega),

l[1]*l[2]*(1-cos(omega))+l[3]*sin(omega),
cos(omega)+(1-cos(omega))*l[2]^2,
l[2]*l[3]*(1-cos(omega))-l[1]*sin(omega),  

l[1]*l[3]*(1-cos(omega))-l[2]*sin(omega),
l[2]*l[3]*(1-cos(omega))+l[1]*sin(omega),
cos(omega)+(1-cos(omega))*l[3]^2
),nrow=m)
return(c((r*t(mat))%*%train+vec))
}

Y=c(obs)
h=0.001
lam=1
ite=10^3
sita=sample(1:9,8,replace=T)
eta=10^(-1)

for(l in 1:ite){
sita_pre=sita
Z=array(0,dim=c(length(Y),length(sita)))
vec=sita
for(j in 1:ncol(Z)){
vec_sub=vec;vec_sub[j]=vec_sub[j]+h
Z_sub=(f(vec_sub)-f(vec))/h
Z[,j]=Z_sub
}
beta=solve(t(Z)%*%Z+diag(lam,ncol(Z)))%*%t(Z)%*%(Y-f(sita))
sita=c(sita+eta*beta)
print(sum((Y-f(sita))^2))
}




```


```{r}

#地点の移動と行列の写像による最小二乗法
#p.164 Gauss法

m=3;r=3

N=5
obs=array(0,dim=c(m,N))
train=array(0,dim=c(m,N))
for(j in 1:N){
vec=sample(1:9,m,replace=T)  
train[,j]=vec
roll=2*pi/sample(1:9,1)
R=matrix(c(cos(roll),sin(roll),0,-sin(roll),cos(roll),0,0,0,1),nrow=m)*sample(1:9,1)
obs[,j]=R%*%vec+sample(1:9,m,replace=T)  
}

f=function(x){
X=matrix(x[c(1:(m*r))],ncol=r)
v=c(x[-c(1:(m*r))])
return(c(X%*%train+v))
}

Y=c(obs)
h=0.001
lam=1
ite=10^4
sita=sample(1:9,m*r+m,replace=T)
eta=10^(-2)

for(l in 1:ite){
sita_pre=sita
Z=array(0,dim=c(length(Y),length(sita)))
vec=sita
for(j in 1:ncol(Z)){
vec_sub=vec;vec_sub[j]=vec_sub[j]+h
Z_sub=(f(vec_sub)-f(vec))/h
Z[,j]=Z_sub
}
beta=solve(t(Z)%*%Z+diag(lam,ncol(Z)))%*%t(Z)%*%(Y-f(sita))
sita=c(sita+eta*beta)
print(sum((Y-f(sita))^2))
}

#写像
R=matrix(sita[c(1:(m*r))],ncol=r)

#地点の移動
t=sita[-c(1:(m*r))]

```

```{r}


#p.59 FNS法

m=3;r=3

N=5
obs=array(0,dim=c(m,N))
train=array(0,dim=c(m,N))
for(j in 1:N){
vec=sample(1:9,m,replace=T)  
train[,j]=vec
roll=2*pi/3
R=matrix(c(cos(roll),sin(roll),0,-sin(roll),cos(roll),0,0,0,1),nrow=m)
obs[,j]=R%*%vec 
}


t1=matrix(c(-1,0,0,0,0,0,0,1,0,0,-1,0,1,0,0,0,0,0,0,1,0,0,-1,0),nrow=4)
t2=matrix(c(0,0,0,-1,-1,0,0,0,0,1,0,0,0,0,0,-1,1,0,0,0,0,1,0,0),nrow=4)
t3=matrix(c(0,0,1,0,0,-1,0,0,-1,0,0,0,0,-1,1,0,0,0,0,0,1,1,0,0),nrow=4)
t=list(t1,t2,t3)

guzai=function(a1,a2){
guzai1=c(a1[1]-a2[1],0,-(a1[3]+a2[3]),a1[2]+a2[2])
guzai2=c(a1[2]-a2[2],a1[3]+a2[3],0,-a1[1]-a2[1])
guzai3=c(a1[3]-a2[3],-a1[2]-a2[2],a1[1]+a2[1],0)
return(cbind(guzai1,guzai2,guzai3))
}


q=rep(1,4)
ite=10

for(l in 1:ite){
Va=array(0,dim=c(3,3))
for(i in 1:3){
for(j in 1:3){
Va[i,j]=c(q)%*%(t[[i]]%*%diag(1,6)%*%t(t[[j]]))%*%c(q) 
}}
Wa=solve(Va)
M=array(0,dim=c(4,4))
L=array(0,dim=c(4,4))
J=0
for(k in 1:N){
guzaivec=guzai(obs[,k],train[,k])  
vak=c(Wa%*%c(t(guzaivec)%*%q))
ep=c(t(guzaivec)%*%q)
J=J+sum(t(t(c(ep)))%*%t(c(ep))*Wa)
for(i in 1:3){
for(j in 1:3){
L=L+vak[i]*vak[j]*(t[[i]]%*%diag(1,6)%*%t(t[[j]]))
M=M+Wa[i,j]*t(t(c(guzaivec[,i])))%*%t(c(guzaivec[,j]))
}}
}
X=M-L
q=eigen(X)$vectors[,4]
print(J)
}


```

```{r}

#p.85

m=3;r=3
N=5
obs=array(0,dim=c(m,N))
train=array(0,dim=c(m,N))
for(j in 1:N){
vec=sample(1:9,m,replace=T)  
train[,j]=vec
roll=2*pi/3
R=matrix(c(cos(roll),sin(roll),0,-sin(roll),cos(roll),0,0,0,1),nrow=m)*5
obs[,j]=R%*%vec 
}

A=function(w){
A1=array(0,dim=c(3,3));A1[3,2]=1;A1[2,3]=-1
A2=array(0,dim=c(3,3));A2[1,3]=1;A2[3,1]=-1
A3=array(0,dim=c(3,3));A3[1,2]=-1;A3[2,1]=1
return(w[1]*A1+w[2]*A2+w[3]*A3)}

eA=function(s){
U=eigen(A(s))$vectors
lam=diag(exp(eigen(A(s))$values))
return(Re(U%*%lam%*%t(U)))
}



V0a1=diag(1,3)
V0a2=diag(1,3)


h=0.001
lam=0.00001
ite=10^5
sita=sample(1:9,3,replace=T)
r=diag(rep(1,3))
J=sum((obs-eA(sita)%*%train)^2)

for(l in 1:ite){

Jpre=J  
rpre=r

Wa=solve(eA(sita)%*%V0a1%*%t(eA(sita))+V0a2)
H=t(t(eA(sita)%*%train)%*%Wa)%*%t(eA(sita)%*%train)

g=c()
for(j in 1:length(sita)){
vec=sita;vecsub=vec;vecsub[j]=vecsub[j]+h
v1=sum((obs-eA(vecsub)%*%train)^2)
v2=sum((obs-eA(vec)%*%train)^2)
g=c(g,(v1-v2)/h)
}

dw=solve(H+diag(lam,length(sita)))%*%(-g)
sita=sita+lam*dw
u=eigen(A(dw))$vectors
lambda=diag(exp(eigen(A(dw))$values))
eAdw=Re(u%*%lambda%*%t(u))
r=eAdw%*%r

J=sum((obs-eA(sita)%*%train)^2)
print(sum((obs-eA(sita)%*%train)^2))

if(Jpre<J){
lam=lam*10  
}else{
if(sqrt(sum(abs(dw)^2))>10^(-3)){
 r=rpre 
}else{
lam=lam/10
}}

}



```