dltCalibrateCameras <- function(coor.2d, nx, grid.size, c.run = FALSE, reduce.grid.dim = 3,
fit.min.break = 1, nlm.iter.max.init = 100, objective.min.init = 10,
nlm.eval.max = 350, nlm.iter.max = 250, nlm.calls.max = 100, min.views = 'max',
objective.min = 1, grid.incl.min = 2, objective.min.break = NULL, start.param = NULL,
sx = NULL, sy = NULL, print.progress = FALSE, print.tab = ''){
# objective.min.break OBJECTIVE AT WHICH TO STOP ADDING CHECKERBOARDS AND START OVER WITH NEW SET
# SET MIN NUMBER OF VIEWS TO USE IN ESTIMATING CALIBRATION COEFFICIENTS
if(min.views == 'max'){min_views <- dim(coor.2d)[4]}else{min_views <- min.views}
# FIND NUMBER OF NON-NA VIEWS FOR EACH ASPECT
aspect_non_na <- rowSums(apply(!is.na(coor.2d), c(3, 4), 'sum') > 0)
# REMOVE SETS WITH LESS THAN THE MINIMUM NUMBER OF VIEWS
coor.2d <- coor.2d[, , aspect_non_na >= min_views, ]
# FIND SECOND GRID DIMENSION
ny <- dim(coor.2d)[1]/nx
# REDUCE GRID DIMENSIONS
if(reduce.grid.dim){
# CHECK THAT reduce.grid.dim IS GREATER THAN TWO
if(reduce.grid.dim < 3) stop(paste0("reduce.grid.dim (", reduce.grid.dim, ") must be greater than 2."))
# SET REDUCED GRID DIMENSIONS
rx <- reduce.grid.dim
ry <- reduce.grid.dim
# SET REDUCED GRID SIZES
sx <- ((nx-1)*grid.size[1]) / (rx-1)
sy <- ((ny-1)*grid.size[1]) / (ry-1)
# EMPTY REDUCED GRID DIMENSION ARRAY
coor_2d_red <- array(NA, dim=c(rx*ry, 2, dim(coor.2d)[3], dim(coor.2d)[4]))
if(print.progress) cat('\n', print.tab, 'Reduce grid point density (', dim(coor.2d)[3]*dim(coor.2d)[4], ' total)\n', sep='')
for(i in 1:dim(coor.2d)[3]){
for(j in 1:dim(coor.2d)[4]){
if(print.progress) cat(print.tab, '\t', (i-1)*dim(coor.2d)[4] + j, ') Aspect ', i, ', View ', j, '; ', sep='')
coor_2d_red[, , i, j] <- resampleGridImagePoints(pts=coor.2d[, , i, j], nx=nx, rx=rx, ry=ry, fit.min.break=fit.min.break, print.progress=print.progress)$pts
}
}
}else{
# MAINTAIN FULL GRID DIMENSIONS
rx <- nx;ry <- ny;
#
if(is.null(sx)) sx <- grid.size[1]
if(is.null(sy) && length(grid.size) > 1) sy <- grid.size[2]
# COPY POINTS TO REDUCED 2D COOR ARRAY
coor_2d_red <- coor.2d
}
# SCALE INITIAL TRANSLATE PARAMETER TO REAL-WORLD UNITS (APPROX THIRD THE MAX DIMENSION OF GRID)
t_init <- (max(nx, ny)*grid.size[1])/3
# SET INITIAL TIME POINT
ptm <- proc.time()
# RUN NLM FUNCTION TO FIND TRANSFORMATION PARAMETERS THAT MINIMIZE INTERNAL RMS CALIBRATION ERROR
if(print.progress) cat('\n', print.tab, 'Full Transform RMSE Minimization\n', print.tab,'Number of parameters: ', (dim(coor.2d)[3]-1)*6, '\n', print.tab, 'Number of points: ', rx*ry*dim(coor.2d)[3], '\n', sep='')
# SET FIXED START PARAMETERS, FIX FIRST GRID AT ORIGIN
p_fix <- c()
for(num_grid in grid.incl.min:dim(coor.2d)[3]){
if(print.progress) cat('\n', print.tab, 'Running minimization with ', num_grid, ' grids...', sep='')
# ESTIMATE STARTING PARAMETERS BASED ON 2D VIEW
estimate_start_params <- dltCCEstimateStartParams(coor.2d, num_grid, nx, ny, grid.size[1], p_fix)
start_param_array <- estimate_start_params$start_param_array
max_disp_rwu <- estimate_start_params$max_disp_rwu
# GET 2D COORDINATE SUBSET MATRIX FOR TRANSFORMATION OPTIMIZATION
coor_2d_t <- apply(coor_2d_red[, , 1:num_grid, ], c(2, 4), matrix, byrow=FALSE)
lower <- rep(c(rep(-pi, 3), rep(-7*max(nx, ny)*grid.size[1], 3)), num_grid)
# SET NLM CALL VARIABLES
nlm_min <- rep(NA, nlm.calls.max)
nlm_calls <- list()
# SAVE PROCESSING TIME
nlm_start <- proc.time()
for(nlm_n in 1:nlm.calls.max){
# SET STARTING PARAMETERS
if(is.null(start.param)){
# VARY EACH ITERATION
start <- c(t(start_param_array[nlm_n, ((length(p_fix)/6)+1):(num_grid-1), ]))
if(length(p_fix)) start <- c(p_fix, start)
}else{
start <- start.param[1:((num_grid-1)*6)]
if(length(p_fix)) start[1:length(p_fix)] <- p_fix
if(((num_grid-1)*6) > length(start.param)){
start <- c(t(start_param_array[nlm_n, ((length(p_fix)/6)+1):(num_grid-1), ]))
if(length(p_fix)) start <- c(p_fix, start)
}
# VARY LAST SIX PARAMETERS AFTER FIRST ITERATION
if(nlm_n > 1){
start[(length(start)-6+1):length(start)] <- sample(seq(0.5, 1.5, length=30), size=6)*start[(length(start)-6+1):length(start)]
}
}
if(print.progress) cat(' ', nlm_n, sep='')
#if(!is.null(sink.trace)) sink(paste0(sink.trace, "/", "num grids ", num_grid, " nlm iter ", nlm_n, ".txt"))
start_full <- start
stopped <- FALSE
if(!is.null(nlm.iter.max.init)){
nlm_fit <- tryCatch(
expr={
nlminb(start=start, objective=dltTransformationParameterRMSError,
control=list(iter.max = nlm.iter.max.init, rel.tol=1e-6),
lower=lower, upper=-lower, coor.2d=coor_2d_t, nx=rx, ny=ry,
sx=sx, sy=sy, p.fixed=rep(0, 6))
},
error=function(cond){if(print.progress) cat('F');return(NULL)},
warning=function(cond) return(NULL)
)
if(is.null(nlm_fit)) next
#cat(" nlm_fit$objective: ", nlm_fit$objective, ", ", nlm_fit$message, ", ", sep="")
}else{
nlm_fit <- list('objective' = 0, 'message' = '')
}
#if(!is.null(sink.trace)) sink()
if(nlm_fit$objective > objective.min.init && nlm_fit$message == "relative convergence (4)"){
stopped <- TRUE
}else{
start_full <- nlm_fit$par
nlm_fit <- tryCatch(
expr={
nlminb(start=start_full, objective=dltTransformationParameterRMSError,
control=list(eval.max = nlm.eval.max, iter.max = nlm.iter.max),
lower=lower, upper=-lower, coor.2d=coor_2d_t, nx=rx, ny=ry,
sx=sx, sy=sy, p.fixed=rep(0, 6))
},
error=function(cond){if(print.progress) cat('N');return(NULL)},
warning=function(cond) return(NULL)
)
}
#
if(!is.null(nlm_fit)){
# GET MINIMUM
if(print.progress){
if(nlm_fit$message == 'function evaluation limit reached without convergence (9)' || nlm_fit$message == 'iteration limit reached without convergence (10)'){
cat('L')
}else if(nlm_fit$message == 'false convergence (8)'){
cat('F')
}else if(nlm_fit$message == 'relative convergence (4)'){
cat('R')
}else{
cat('C')
}
if(stopped) cat('-S')
cat('(', round(nlm_fit$objective, 2), ')', sep='')
}
nlm_calls[[nlm_n]] <- nlm_fit
nlm_min[nlm_n] <- nlm_calls[[nlm_n]]$objective
# IF OPTIMIZATION MINIMUM IS LESS THAN objective.min, STOP ITERATING
if(nlm_calls[[nlm_n]]$convergence == 0 && nlm_calls[[nlm_n]]$objective < objective.min) break
if(nlm_fit$message == 'iteration limit reached without convergence (10)' && nlm_calls[[nlm_n]]$objective < objective.min) break
if(nlm_fit$message == 'function evaluation limit reached without convergence (9)' && nlm_calls[[nlm_n]]$objective < objective.min) break
if(nlm_fit$message == 'false convergence (8)' && nlm_calls[[nlm_n]]$objective < objective.min) break
}
}
# SAVE PROCESSING TIME
nlm_elapsed <- proc.time() - nlm_start
# SAVE RUN WITH MINIMUM
nlm_res_t <- nlm_calls[[which.min(nlm_min)]]
if(print.progress){
cat('\n', print.tab, '\tNumber of nlminb() calls: ', nlm_n, ' (', nlm.calls.max, ' max)',
'\n', print.tab, '\tTermination message: ', nlm_res_t$message,
'\n', print.tab, '\tMinimum: ', nlm_res_t$objective,
'\n', print.tab, '\tIterations: ', nlm_res_t$iterations, ' (', nlm.iter.max, ' max)',
'\n', print.tab, '\tFunction evaluations: ', nlm_res_t$evaluations['function'], ' (', nlm.eval.max, ' max)',
'\n', print.tab, '\tRun-time: ', nlm_elapsed[1], ' sec',
'\n', print.tab, '\tStarting parameters: ', sep='')
cat(round(start, 5), sep=', ')
cat('\n', print.tab, '\tFinal estimated parameters: ', sep='')
cat(round(nlm_res_t$par, 5), sep=', ')
cat('\n', print.tab, '\tSum of absolute differences between initial and final parameters: ', sum(abs(start - nlm_res_t$par)), sep='')
cat('\n')
}
# SET FIXED PARAMETERS FOR NEXT ITERATION
p_fix <- nlm_res_t$par
if(!is.null(objective.min.break)) if(nlm_res_t$objective > objective.min.break) break
}
# SAVE PROCESSING TIME
run_time <- proc.time() - ptm
if(print.progress) cat('\n', print.tab, 'Total estimation processing time: ', run_time[1], ' sec', sep='')
# SAVE OPTIMIZED PARAMETERS
p_init <- matrix(c(rep(0, 6), nlm_res_t$par), nrow=6)
# GET 3D COORDINATES BASED ON OPTIMIZED PARAMETERS
if(length(grid.size) == 1){
coor_3d_coeff <- transformPlanarCalibrationCoordinates(tpar=c(p_init), nx=nx, ny=ny, sx=grid.size[1])
}else{
coor_3d_coeff <- transformPlanarCalibrationCoordinates(tpar=c(p_init), nx=nx, ny=ny, sx=grid.size[1], sy=grid.size[2])
}
# GET 2D INPUT COORDINATES
coor_2d_coeff <- apply(coor.2d[, , 1:(length(p_init)/6), ], c(2, 4), matrix, byrow=FALSE)
# GET DLT CALIBRATION COEFFICIENTS FROM OPTIMIZED 3D COORDINATE SUBSET (ALL GRID POINTS - SO RMSE CAN DIFFER FROM NLM MINIMUM)
dlt_coefficients_t <- dltCoefficients(coor.3d=coor_3d_coeff, coor.2d=coor_2d_coeff)
if(c.run){
# GET 2D COORDINATE SUBSET MATRIX FOR COEFFICIENT OPTIMIZATION
coor_2d_c <- apply(coor_2d_red, c(2, 4), matrix, byrow=FALSE)
# SET INITIAL TIME POINT AND NUMBER OF ITERATIONS
ptm <- proc.time();#c_iter <<- 0
# RUN NLM FUNCTION TO FIND COEFFICIENTS THAT MINIMIZE INTERNAL RMS CALIBRATION ERROR
if(print.progress) cat('\n', print.tab, 'Coefficient RMSE Minimization\n', print.tab, 'Number of parameters: ', length(c(dlt_coefficients_t$cal.coeff)), '\n', print.tab, 'Number of points: ', rx*ry*dim(coor.2d)[3], '\n', print.tab, 'Running minimization...\n', sep='')
nlm_res_c <- nlm(dltCoefficientRMSError, p=c(dlt_coefficients_t$cal.coeff), coor.2d=coor_2d_c)
if(print.progress) cat('\n', print.tab, 'Termination code: ', nlm_res_c$code, '\n', print.tab, 'Iterations: ', nlm_res_c$iterations, '\n', sep='')
# SAVE PROCESSING TIME
run_time_c <- proc.time() - ptm
# USE DLT COEFFICIENTS TO RECONSTRUCT ALL 2D COORDINATES
dlt_reconstruct <- dltReconstruct(cal.coeff=matrix(nlm_res_c$estimate, nrow=11, ncol=dim(coor.2d)[4]), coor.2d=apply(coor.2d, c(2, 4), matrix, byrow=FALSE))
# USE DLT COEFFICIENTS TO RECONSTRUCT ALL 2D COORDINATES
dlt_coefficients_c <- dltCoefficients(coor.3d=dlt_reconstruct$coor.3d, coor.2d=apply(coor.2d, c(2, 4), matrix, byrow=FALSE))
mean_reconstruct_rmse <- mean(dlt_reconstruct$rmse)
coor_3d <- dlt_reconstruct$coor.3d
cal_coeff <- dlt_coefficients_c$cal.coeff
coefficient_rmse <- dlt_coefficients_c$rmse
}else{
# USE DLT COEFFICIENTS TO RECONSTRUCT ALL 2D COORDINATES
dlt_reconstruct <- dltReconstruct(cal.coeff=dlt_coefficients_t$cal.coeff, coor.2d=apply(coor.2d, c(2, 4), matrix, byrow=FALSE))
mean_reconstruct_rmse <- mean(dlt_reconstruct$rmse)
coor_3d <- dlt_reconstruct$coor.3d
cal_coeff <- dlt_coefficients_t$cal.coeff
coefficient_rmse <- dlt_coefficients_t$rmse
#dlt_coefficients_c <- dltCoefficients2(coor.3d=coor_3d, coor.2d=apply(coor.2d, c(2, 4), matrix, byrow=FALSE))
#hist(dlt_coefficients_c$rmse)
nlm_res_c <- list('estimate'=NA, 'minimum'=NA)
run_time_c <- NA
}
l <- list(
cal.coeff=cal_coeff,
coor.3d=coor_3d,
mean.reconstruct.rmse=mean_reconstruct_rmse,
coefficient.rmse=coefficient_rmse,
t.param.final=p_init,
t.min=nlm_res_t$objective,
t.runtime=run_time[1],
c.param.init=c(dlt_coefficients_t$cal.coeff),
c.param.final=nlm_res_c$estimate,
c.min=nlm_res_c$minimum,
c.iter=nlm_res_c$iterations,
c.runtime=run_time_c[1]
)
class(l) <- 'dltCalibrateCameras'
l
}
summary.dltCalibrateCameras <- function(object, ...){
r <- ''
r <- c(r, '\ndltCalibrateCameras Summary\n')
r <- c(r, '\tMinimize RMS Error by transformation\n')
r <- c(r, '\t\tTotal number of parameters estimated: ', length(object$t.param.final)-6, '\n')
r <- c(r, '\t\tFinal Minimum Mean RMS Error: ', round(object$t.min, 3), '\n')
r <- c(r, '\t\tTotal Run-time: ', round(object$t.runtime, 2), ' sec\n')
if(!is.null(object$c.iter)){
r <- c(r, '\tMinimize RMSE by coefficients\n')
r <- c(r, '\t\tNumber of parameters: ', length(object$c.param.init), '\n')
r <- c(r, '\t\tSum of absolute differences between initial and final parameters: ', format(sum(abs(object$c.param.init - object$c.param.final))), '\n')
r <- c(r, '\t\tTotal function calls: ', object$c.iter, '\n')
r <- c(r, '\t\tFinal Mean RMS Error: ', format(object$c.min), '\n')
r <- c(r, '\t\tRun-time: ', round(object$c.runtime, 2), ' sec\n')
}
r <- c(r, '\n')
r <- c(r, '\tMean Reconstruct RMSE: ', round(object$mean.reconstruct.rmse, 3), '\n')
if(length(object$coefficient.rmse) == 1){
r <- c(r, '\tMean Calibration Coefficient RMS Error: ', round(object$coefficient.rmse, 3), '\n')
}else{
r <- c(r, '\tCalibration Coefficient RMS Error:\n')
for(i in 1:length(object$coefficient.rmse)){
r <- c(r, '\t\t', i, ': ', round(object$coefficient.rmse[i], 3), '\n')
}
}
r <- c(r, '\t3D Coordinate Matrix dimensions: ', dim(object$coor.3d)[1], ' x ', dim(object$coor.3d)[2], '\n')
class(r) <- "summary.dltCalibrateCameras"
r
}
print.summary.dltCalibrateCameras <- function(x, ...) cat(x, sep='')
Add the following code to your website.
For more information on customizing the embed code, read Embedding Snippets.