DataAnalysisFinal.R

library(readr)
library(tidyverse)
library(fields)
library(rgl)
library(FNN)
#library(lidR)

path <- "~/Documents/github/gpRegistration/"
source(paste0(path, 'R/functions.R'))

### test generate.data
# prm = c('x','y','z','phi')
# param.list <- list(gen.p=c(1, 3, 2, 0.0001), # smoothness, range, sill, nugget
#                    #gen.p2=c(3, 0.2, 0.15, 0.0001),
#                    #gen.p3=c(8, 0.03, 0.02, 0.0001),
#                    dereg.type='nonrigid',
#                    #dereg.p=c(2, 6, 0.01, 0.00001),
#                    beta =c(0.01,0.02), beta2=c(0.0005,0.0005), # for x
#                    #beta =c(-0.005,0.01), beta2=c(0.001,0.001), # for phi
#                    param=c(prm)) # 'x'
# data.gend <- generate.data(2000, param.list)
# plot.data(data.gend$d1, 100, 100, zlim=c(-5, 5))#, add=TRUE)
# plot.data(data.gend$d2, 100, 100, add=TRUE, zlim=c(-5,5))


path <- "~/Documents/github/gpRegistration/data/"
path1 <- paste0(path, 'chalk_fan_flight_1_20171016_10m.txt')
path2 <- paste0(path, 'chalk_fan_flight_2_20171016_10m_registered.txt')
#
# path1 <- paste0(path, 'Chalk_2018-07-30_RuPaRe_dense_high_aggressive_epsg32613.laz')
# f1 <- readLAS(path1)
# plot(f1)
#
# path1 <- paste0(path, 'data1.ICPd.txt')
# path2 <- paste0(path, 'data2.ICPd.txt')



d1 <- read_delim(paste0(path, 'chalk_fan_flight_1_20171016_10m.txt'), delim=' ', col_names = FALSE)
d2 <- read_delim(paste0(path, 'chalk_fan_flight_2_20171016_10m_registered.txt'), delim=' ', col_names=FALSE)

d1 <- data.frame(d1[,1:3])
d2 <- data.frame(d2[,1:3])
colnames(d1) <- colnames(d2) <- c('x', 'y', 'z')
d1 <- d1 %>% mutate(x= x - mean(x), y=y-mean(y), z=z-mean(z))
d2 <- d2 %>% mutate(x= x - mean(x), y=y-mean(y), z=z-mean(z))

d1 <- d1 %>% filter(x < 15, x > -13, y > -40)
d2 <- d2 %>% filter(x < 16, x > -12, y > -40, z<6)

# plot.data(d1, 100, 100)#, add=TRUE)
# points(-16, 15)
# points(-2, 39)
## 39-15/-2+16 = m = 12/7
## y = 12/7x + b => 39 = 12/7 * -2 + b => 39+24/7 = 42 3/7 = 7*42 + 3 / 7
## 7y = 12x + (7*42+3)
# plot.data(d2, 100, 100)#, add=TRUE)
# points(-15, 15)
# points(-1, 39)
## m = 12/7
## y = 12/7x + b
## 39 + 12/7 = b
## 7 * y - 12 * x = 39*7 + 12

d1 <- d1 %>% filter(7*y - 12*x <= (7*42+3))
d2 <- d2 %>% filter(7*y - 12*x <= (7*39+12))

# d1 <- d1 %>% mutate(x= x - mean(x), y=y-mean(y), z=z-mean(z))
# d2 <- d2 %>% mutate(x= x - mean(x), y=y-mean(y), z=z-mean(z))

# plot.data.3d(d1)
# plot.data.3d(d2, add=TRUE, col='red')
#

# d1 <- d1 %>% filter(z < 3.5) # 4.2
# d2 <- d2 %>% filter(z < 3.5)
# d1 <- d1 %>% mutate(z=z-mean(z))
# d2 <- d2 %>% mutate(z=z-mean(z))

# plot.data(d1, 100, 100)#, add=TRUE)
# plot.data(d2, 100, 100, add=TRUE)

# d1o <- d1
# d2o <- d2

set.seed(924)
indx <- sample_frac(data.frame(1:nrow(d1)), 0.0001) # 0.00002
d1cv <- d1[c(indx$X1.nrow.d1.),]
d1 <- d1[-c(indx$X1.nrow.d1.),]

write.csv(d1, file="~/Documents/github/gpRegistration/forccicp/d1.csv")
write.csv(d2, file="~/Documents/github/gpRegistration/forccicp/d2.csv")
write.csv(gpnl.obj$data.list$data2.trf, file="~/Documents/github/gpRegistration/forccicp/d2.trf.csv")












#load('~/Documents/github/gpRegistration/chalkOutput/gpResults.n250w4.8o1.5theta50sn1lambda5kappa100.rda')

#setup.pars <- list(nxx=8, nyy=8, ovrlp=2, sqz=10) #1.9
setup.pars <- list(nxx=5, nyy=15, ovrlp=1.5, sqz=1) #1.9

trns.bwd <- 1.2
angl.bwd <- pi/4
est.pars <- list(n.subsamples=250,
                 smoothness=1,
                 lambd=5,
                 kappa=100,# c(50,50,50,25)
                 penalty.type='L2',
                 lower.bounds=c(-Inf,-Inf,-Inf, -trns.bwd, -trns.bwd, -trns.bwd, -angl.bwd+0.1),
                 upper.bounds=c(4, Inf, Inf, trns.bwd, trns.bwd, trns.bwd, angl.bwd),
                 est.type='serial',
                 tps.lambda=1,
                 krig.theta=50, fixed=FALSE) # 'serial', 'Rmpi', 'parallel'
gpnl.obj <- gpnlreg.setup(d1, d2, setup.pars)

rm(d1); rm(d2)
length(gpnl.obj$data.subsets$D1_i)
plot.windows.i(gpnl.obj, c(1,4))

gpnl.obj <- gpnlreg.estim.def(gpnl.obj, est.pars)

gpnl.obj <- gpnlreg(gpnl.obj, setup.pars=setup.pars, est.pars=est.pars)

gpnl.obj$data.list$d1cv <- d1cv
gpnl.obj <- gpnlreg.krig.cov.pars(gpnl.obj, 100, lbd=0.2)

prs.nonrigid <- exp( gpnl.obj$cov.fit.results$trnsf.That )
pred.nonrigid <- local.krig( data=rbind(gpnl.obj$data.list$data1,
                                        gpnl.obj$data.list$data2.trf),
                             x = d1cv[,1:2],
                             pars=list(prs.nonrigid))
rmse.nonrigid <- apply(pred.nonrigid$pred, 2, function(x) sqrt(mean((x- c(d1cv[,3]))^2)) )
crps.nonrigid <- crps.normal(pred.nonrigid$pred, pred.nonrigid$pred.var, c(d1cv[,3]))
logscore.nonrigid <- log.score(pred.nonrigid$pred, pred.nonrigid$pred.var, c(d1cv[,3]))

# rmse.rigid = 0.0834867535263707
# pars = c(-0.962422680,  0.116094205,  0.193228912,  0.003968053)
# rmse.icp = 0.08874475
# pars = c(-1.01084936, -0.01962812, 0.22183046, 0)

plot.data.3d(gpnl.obj$data.list$data1, col='black')
plot.data.3d(gpnl.obj$data.list$data2, add=TRUE, col='red')
plot.data.3d(gpnl.obj$data.list$data2.trf, add=TRUE, col='green')

layout(matrix(c(1:8), ncol=4, nrow=2, byrow = FALSE))
par(mfrow=c(2,4))
plot.param(gpnl.obj, 'x', zlim=c(-1.2, 1.2))
plot.param(gpnl.obj, 'y', zlim=c(-1, 0.9))
plot.param(gpnl.obj, 'z', zlim=c(-0.5, 0.5))
plot.param(gpnl.obj, 'phi',zlim=c( -.1, .1) )


save(gpnl.obj, pred.nonrigid, rmse.nonrigid,
     file=paste0('~/Documents/github/gpRegistration/chalkOutput/gpResults.n',
                 gpnl.obj$est.pars$n.subsamples,'w',
                gpnl.obj$setup.pars$nxx, '.', gpnl.obj$setup.pars$nyy,
                'o', gpnl.obj$setup.pars$ovrlp, 'theta', gpnl.obj$est.pars$krig.theta,
                'sn', gpnl.obj$est.pars$tps.lambda,
                'lambda', gpnl.obj$est.pars$lambd, 'kappa', gpnl.obj$est.pars$kappa,'.rda') )

#load("~/Documents/github/gpRegistration/chalkOutput/gpResults.n250w4.12o1.5theta50sn1lambda5kappa100.rda")



## rigid align
#pars = c-(0,0,0,0.962422680,  0.116094205,  0.193228912,  0.003968053)
pars <- rrmatave # gpr.obj$opt.res$par
pars <- c(0,0,0,-1.01084936, -0.01962812, 0.22183046, 0)
y2 <- d2[,3] + pars[6]
phi <- pars[7]
rotat <- matrix(c(cos(phi), sin(phi), -sin(phi), cos(phi)), nrow=2, ncol=2 )
g2 <- t( rotat %*% t(cbind( d2[,1] + pars[4],
                            d2[,2] + pars[5] )) )
d2.trf <- cbind(g2, y2)
rm(y2); rm(phi); rm(rotat); rm(g2); rm(pars)
d2.trf <- data.frame(d2.trf)
names(d2.trf) <- c('x', 'y', 'z')
#plot.data.3d(d1)
plot.data.3d(d2.trf, col='blue', add=TRUE)
rm(d2.trf)




########################################################
### rigid registration
rrlist <- list()
trns.bwd <- 2
angl.bwd <- pi/4
lower.bounds=c(-Inf,-Inf,-Inf, -trns.bwd, -trns.bwd, -trns.bwd, -angl.bwd+0.1)
upper.bounds=c(4, Inf, Inf, trns.bwd, trns.bwd, trns.bwd, angl.bwd)
est.pars <- list(smoothness=1, lambda=15, penalty.type='L2',
                 kappa = 100,
                 lower.bounds=lower.bounds, upper.bounds=upper.bounds)
set.seed(908)
d1s <- sample_n(d1, 500, replace = FALSE)
d2s <- sample_n(d2, 500, replace = FALSE)
gpr.obj <- rigid.registration(d1s, d2s, est.pars = est.pars)
rrlist[[1]] <- gpr.obj$opt.res$par
for(i in 1:10){
  d1s <- sample_n(d1, 500, replace = FALSE)
  d2s <- sample_n(d2, 500, replace = FALSE)
  print('sampled')
  gpr.obj <- rigid.registration(d1s, d2s, est.pars = est.pars,
                                init = gpr.obj$opt.res$par)
  rrlist[[length(rrlist)+1]] <- gpr.obj$opt.res$par
}

rrmat <- matrix(NA, ncol=7, nrow=length(rrlist))
for(i in 1:length(rrlist)){
  rrmat[i,] <- rrlist[[i]]
}
rrmatave <- apply(rrmat, 2, mean)


# apply transformation
pars <- rrmatave # gpr.obj$opt.res$par
y2 <- d2[,3] + pars[6]
phi <- pars[7]
rotat <- matrix(c(cos(phi), sin(phi), -sin(phi), cos(phi)), nrow=2, ncol=2 )
g2 <- t( rotat %*% t(cbind( d2[,1] + pars[4],
                            d2[,2] + pars[5] )) )
d2.trf <- cbind(g2, y2)
rm(y2); rm(phi); rm(rotat); rm(g2)
d2.trf <- data.frame(d2.trf)
names(d2.trf) <- c('x', 'y', 'z')
# local krig to cv data
data4rigid <- rbind(d1, d2.trf)
prs.rigid <- data.frame( matrix( rep(exp(pars)[1:3], each=nrow(d1cv)),
                     nrow=nrow(d1cv), ncol=3) )
pred.rigid <- local.krig(data=data, x=d1cv[,1:2],
                               pars = list( prs.rigid) )

# calculate rmse
rmse.rigid <- sqrt(mean((pred.rigid - c(d1cv[,3]))^2))
###
########################################################











save(path1, path2, gpnl.obj,
     file='~/Documents/github/gpRegistration/chalkOutput/NEWRESseed924n250w8o2theta40sn0.1lambd15k100.rda')
#load('~/Documents/github/gpRegistration/chalkOutput/NEWRESn100w8o2theta20lambd5.rda')
#load('~/Documents/github/gpRegistration/chalkOutput/NEWRESn200w8o2theta20lambd25k500.rda')
load('~/Documents/github/gpRegistration/chalkOutput/NEWRESn200w8o2theta20lambd25k100.rda')
load('~/Documents/github/gpRegistration/chalkOutput/NEWRESn200w8o2theta20lambd10k100.rda')


#write.csv(gpnl.obj$data.list$data1, file= '~/Documents/github/gpRegistration/ccOutput/d1.csv')
#write.csv(gpnl.obj$data.list$data2, file='~/Documents/github/gpRegistration/ccOutput/d21.csv')
#write.csv(gpnl.obj$data.list$data2.trf, file= '~/Documents/github/gpRegistration/ccOutput/d22.csv')

#write.csv(rbind(gpnl.obj$data.list$data1, gpnl.obj$data.list$data2), file='~/Documents/github/gpRegistration/ccOutput/dbad0.csv')
#write.csv(rbind(gpnl.obj$data.list$data1, gpnl.obj$data.list$data2.trf), file= '~/Documents/github/gpRegistration/ccOutput/dboth0.csv')


gpnl.obj$fit.results <- NULL
gpnl.obj$est.pars <- est.pars

plot.data.subset(gpnl.obj, 1)
plot.windows.i(gpnl.obj, c(23:25))
par(mfrow=c(4,2))
plot.param(gpnl.obj, 'x', zlim=c(-1.1, 1))
plot.param(gpnl.obj, 'y', zlim=c(-1, 0.9))
plot.param(gpnl.obj, 'z', zlim=c(-0.5, 0.5))
plot.param(gpnl.obj, 'phi',zlim=c( -.1, .1) )

par(mfrow=c(1,2))
plot.data(gpnl.obj$data.list$data1, 100, 100, zlim=c(-5, 5))#, add=TRUE)
plot.data(gpnl.obj$data.list$data1, 100, 100, zlim=c(-5, 5), add=TRUE)#, add=TRUE)
plot.data(gpnl.obj$data.list$data2, 100, 100, add=TRUE)
plot.data(gpnl.obj$data.list$data2.trf, 100, 100, add=TRUE, zlim=c(-5, 5))#, add=TRUE, zlim=c(4, 12))
plot.data.3d(gpnl.obj$data.list$data1)
plot.data.3d(gpnl.obj$data.list$data2, add=TRUE, col='blue')
plot.data.3d(gpnl.obj$data.list$data2.trf, add=TRUE, col='green')


# plot.data.3d(smallCliffs$data1, col=myColorRamp(tim.colors(), smallCliffs$data1$z))

rs <- apply(gpnl.obj$fit.results$trnsf.That, 1, function(x){sum(abs(x))})
total.change <- cbind(gpnl.obj$data.list$data2[,1:2], rs)
quilt.plot(total.change[,1:2], total.change[,3])

quilt.plot(total.change[,1:2], abs(gpnl.obj$fit.results$trnsf.That[,1]))
quilt.plot(total.change[,1:2], abs(gpnl.obj$fit.results$trnsf.That[,2]))
quilt.plot(total.change[,1:2], abs(gpnl.obj$fit.results$trnsf.That[,3]))
quilt.plot(total.change[,1:2], abs(gpnl.obj$fit.results$trnsf.That[,4]))
ashtonwiens/gp.nonrigid.registration documentation built on April 11, 2020, 12:28 a.m.