Nothing
R/conicfit.R
In conicfit: Algorithms for Fitting Circles, Ellipses and Conics Based on the Work by Prof. Nikolai Chernov
Residuals.parabola <- function(XY,ParG)
{
# Projecting a given set of points onto a parabola and computing the distances from the points to the parabola
Vertex <- ParG[1:2]
p= ParG[3]
Angle <- ParG[4]
n <- dim(XY)[1]
XYproj <- matrix(0,n,2)
Iter <- matrix(0,n,1)
tolerance <- 1e-9
tol.p <- tolerance * p/2
pp <- p*p
# Matrix Q for rotating the points and the parabola
s <- sin(Angle)
cA <- cos(Angle)
Q <- matrix(c(cA, -s,s, cA),2,2,byrow=TRUE)
# data points in canonical coordinates
XY0 <- cbind(XY[,1]-Vertex[1], XY[,2]-Vertex[2]) %*% Q
XYA <- cbind(XY0[,1], abs(XY0[,2]))
# main loop over the data points
for (i in 1:n){
u <- XYA[i,1]
v <- XYA[i,2]
uu <- u*u
vv <- v*v
pu=p*u
if (v == 0) z2 <- 1 else z2 <- sign(XY0[i,2])
# does the point lie on the x-axis?
if (v<tol.p){
if (u>p) XYproj[i,] <- cbind(u-p, z2*sqrt(max(2*p*(u-p),0))) else XYproj[i,] <- c(0, 0)
next
}
# pick the initial point T starting from -1/2 until F(t)>0
for (j in 1:20){
Tp <- 1/2^j-1
Fp <- vv/(Tp+1)^2 - 2*pu-2*pp*Tp
if (Fp>0) break
}
# generic case: start the iterative procedure
for (iter in 1:100){
F0= vv/(Tp+1)^2
Fp <- F0-2*(pu+pp*Tp)
if (Fp<0) break
Fder <- 2*(F0/(Tp+1) + pp)
Ratio <- Fp/Fder
if (Ratio<tol.p){
Iter[i]=iter
break
}
Tp <- Tp + Ratio
}
# compute the projection of the point onto the parabola
XYproj[i,] <- cbind(u+Tp*p, XY0[i,2]/(Tp+1))
} # end the main loop
XYproj <- XYproj %*% t(Q)
XYproj <- cbind(XYproj[,1]+Vertex[1], XYproj[,2]+Vertex[2])
RSS <- norm(XY-XYproj,'F')^2
list(RSS=RSS, XYproj=XYproj)
} # Residuals.parabola
Residuals.hyperbola <- function(XY,ParG)
{# Projecting a given set of points onto a hyperbola and computing the distances from the points to the hyperbola
Center <- ParG[1:2]
Axes <- ParG[3:4]
Angle <- ParG[5]
n <- dim(XY)[1]
XYproj <- matrix(0,n,2)
tolerance <- 1e-9
a <- Axes[1]
b=Axes[2]
aa <- a^2
bb <- b^2
at=sqrt(a)
bt=sqrt(b)
tol.a <- tolerance*a
tol.b <- tolerance*b
tol.aa <- tolerance*aa
# Matrix Q for rotating the points and the hyperbola
s <- sin(Angle)
cA <- cos(Angle)
Q <- matrix(c(cA, -s,s, cA),2,2,byrow=TRUE)
# data points in canonical coordinates
XY0 <- (cbind(XY[,1]-Center[1], XY[,2]-Center[2])) %*% Q
XYA <- abs(XY0)
# find the inflection points TInf for all given pairs
XYS <- sqrt(XYA)
XYS1 <- matrix(bb*at*XYS[,1]-aa*bt*XYS[,2],ncol=1)
XYS2 <- matrix(at*XYS[,1]+bt*XYS[,2],ncol=1)
TInf <- matrix(XYS1/XYS2,ncol=1) #inflection points
# main loop over the data points
for (i in 1:n){
u <- XYA[i,1]
v <- XYA[i,2]
ua <- u*a
vb <- v*b
if (u == 0) z1 <- 1 else z1 <- sign(XY0[i,1])
if (v == 0) z2 <- 1 else z2 <- sign(XY0[i,2])
# does the point lie on the major axis?
if (v<tol.b){
if (u>a+bb/a){
xproj <- aa*u/(aa+bb)
XYproj[i,] <- cbind(z1*xproj, z2*b*sqrt(max((xproj/a)^2-1,0)))
} else XYproj[i,] <- cbind(z1*a, 0)
next
} # end if
# does the point lie on the minor axis?
if (u<tol.a){
yproj <- bb*v/(aa+bb)
XYproj[i,] <- cbind(z1*a*sqrt(1+(yproj/b)^2), z2*yproj )
next
} # } if
# generic case: start the iterative procedure
T0 <- TInf[i] # inflection point t
F0 <- (ua/(T0+aa))^2 - (vb/(-T0+bb))^2 - 1 # the corresponding F(t)
# if F0>0 then pick the initial point to the right of the root
# if F0<0 then pick the initial point to the left of the root
if (F0 > 0){ # case1: pick the initial point T until F(T)<0
for (j in 1:20){
Tp<-bb-(-T0+bb)/2^j
Fp<-(ua/(Tp+aa))^2 - (vb/(-Tp+bb))^2 - 1
if (Fp<0) break
} # end for j
for (iter in 1:100){
Taa <- Tp + aa
Tbb <- -Tp + bb
PP1 <- (ua/Taa)^2
PP2 <- (vb/Tbb)^2
Fp <- PP1 - PP2 - 1
if (Fp>0) break
Fder <- 2*(PP1/Taa + PP2/Tbb)
Ratio <- Fp/Fder
if (abs(Ratio)<tol.aa) break
Tp <- Tp + Ratio
} } else { for (j in 1:20) { # case2: pick the initial point T until F(T)>0
Tp=-aa+(T0+aa)/2^j
Fp=(ua/(Tp+aa))^2 - (vb/(-Tp+bb))^2 - 1
if (Fp>0) break
} # end for j
for (iter in 1:100){
Taa <- Tp + aa
Tbb <- -Tp + bb
PP1 <- (ua/Taa)^2
PP2 <- (vb/Tbb)^2
Fp <- PP1 - PP2 - 1
if (Fp<0) break
Fder <- 2*(PP1/Taa + PP2/Tbb)
Ratio <- Fp/Fder
if (abs(Ratio)<tol.aa) break
Tp <- Tp + Ratio
}
} # end if
# compute the projection of the point onto the hyperbola
if (Taa < 1e-6){
yproj <- XY0[i,2]*bb/Tbb
XYproj[i,] <- cbind(sign(XY0[i,1])*a*sqrt(1+(yproj/b)^2), yproj)
} else { if (Tbb < 1e-6){
xproj=XY0[i,1]*aa/Taa
yproj <- sign(XY0[i,2])*b*sqrt(max((xproj/a)^2-1,0))
XYproj[i,] <- cbind(xproj, yproj)
} else XYproj[i,] <- cbind(XY0[i,1]*aa/Taa, XY0[i,2]*bb/Tbb)
} # end if
} # } the main for-loop
XYproj <- XYproj %*% t(Q)
XYproj <- cbind(XYproj[,1]+Center[1], XYproj[,2]+Center[2])
RSS <- norm(XY-XYproj,'F')^2
list(RSS=RSS, XYproj=XYproj)
} # Residuals.hyperbola
fit.conicLMA <- function(XY,ParAini,LambdaIni=1,epsilonP=0.0000000001,epsilonF=0.0000000000001,IterMAX=2000000)
{
# Fitting a conic to a given set of points (Implicit method) using algebraic parameter
#epsilonP <- tolerance (small threshold)
#epsilonF <- tolerance (small threshold)
#IterMAX <- maximal number of (main) iterations usually 10-20 suffice
lambda.sqrt <- sqrt(LambdaIni) # sqrt(Lambda) is actually used by the code
tmp <- AtoG(ParAini)
ParGini <- tmp$ParG
exitCode <- tmp$exitCode
if (exitCode == 1){
tmp <- Residuals.ellipse(XY,ParGini)
Fp <- tmp$RSS
XYproj <- tmp$XYproj
} else { if (exitCode == 2){
tmp <- Residuals.hyperbola(XY,ParGini)
Fp <- tmp$RSS
XYproj <- tmp$J
} else stop('invalid initial parameter')
}
tmp <- JmatrixLMA(XY,ParAini,XYproj) # calculate the Jacobian matrix
Res <- tmp$Res
J <- tmp$J
# tmp <- HessianLMA(XY,ParAini,XYproj) # calculate the Hessian matrix
# H <- tmp$H
# ev <- tmp$ev
ParA <- ParAini
ParG <- ParGini
codeTemp <- exitCode
ParGTemp <- ParG
for (iter in 1:IterMAX) # main loop, each run is one (main) iteration
{
while (TRUE) # secondary loop - adjusting Lambda (no limit on cycles)
{
DelPar <- mldivide(rbind(J, lambda.sqrt*diag(6)), rbind(-Res, matrix(0,6,1))) # step candidate
ParTemp <- ParA + DelPar
ParTemp <- ParTemp/norm(ParTemp,'F')
tmp <- AtoG(ParTemp)
ParGTemp <- tmp$ParG
codeTemp <- tmp$exitCode
if (codeTemp != exitCode) progress <- 1 else progress <- norm(DelPar,'F')
if (progress < epsilonP) break # stopping rule
if (codeTemp == 1){
tmp <- Residuals.ellipse(XY,ParGTemp)
FTemp <- tmp$RSS
XYprojTemp <- tmp$XYproj
} else {if (codeTemp == 2){
tmp <- Residuals.hyperbola(XY,ParGTemp)
FTemp <- tmp$RSS
XYprojTemp <- tmp$XYproj
}else{
lambda.sqrt <- lambda.sqrt*2 #if it's degenerate, increase lambda, recompute the step
next
}
}
if (FTemp < Fp*(1-epsilonF/lambda.sqrt)){ # yes, improvement
lambda.sqrt <- lambda.sqrt/2 # reduce lambda, move to next iteration
break
}else { # no improvement
lambda.sqrt <- lambda.sqrt*2 # increase lambda, recompute the step
next
}
} # while (TRUE), the end of the secondary loop
if (progress < epsilonP) break # stopping rule
tmp <- JmatrixLMA(XY,ParTemp,XYprojTemp)
Res <- tmp$Res
J <- tmp$J
ParA <- ParTemp
Fp <- FTemp # update the iteration
ParG <- ParGTemp
exitCode <- codeTemp
} # main loop
RSS <- Fp
iters <- iter
list(ParA=ParA,RSS=RSS,iters=iters,exitCode=exitCode)
}
GtoA<-function(ParG)
{# Conversion of geometric parameters of an ellipse
# ParG <- [Center(1:2), Axes(1:2), Angle]'
# to algebraic parameters
k <- cos(ParG[5])
s <- sin(ParG[5])
a <- ParG[3]
b <- ParG[4]
Xc <- ParG[1]
Yc <- ParG[2]
P <- (k/a)^2 + (s/b)^2
Q <- (s/a)^2 + (k/b)^2
R <- 2*k*s*(1/a^2 - 1/b^2)
cbind(A=P, B=R, C=Q, D=-2*P*Xc-R*Yc, E=-2*Q*Yc-R*Xc, F=P*Xc^2+Q*Yc^2+R*Xc*Yc-1)
}
AtoG <- function(ParA)
{
# Conversion of algebraic parameters to geometric parameters
tolerance1 <- 1.e-10
tolerance2 <- 1.e-20
if (abs(ParA[1]-ParA[3]) > tolerance1) Angle <- atan(ParA[2]/(ParA[1]-ParA[3]))/2 else Angle <- pi/4
cA <- cos(Angle)
s <- sin(Angle)
Q <- matrix(c(cA, s,-s, cA),2,2,byrow=TRUE)
M <- matrix(c(ParA[1], ParA[2]/2, ParA[2]/2, ParA[3]),2,2,byrow=TRUE)
D <- Q %*% M %*% t(Q)
N <- Q %*% matrix(c(ParA[4], ParA[5]),2,1,byrow=TRUE)
O <- ParA[6]
if ((D[1,1] < 0) && (D[2,2] < 0)){
D <- -D
N <- -N
O <- -O
}
UVcenter <- matrix(c(-N[1,1]/2/D[1,1], -N[2,1]/2/D[2,2]),2,1,byrow=TRUE)
free <- O - UVcenter[1,1]*UVcenter[1,1]*D[1,1] - UVcenter[2,1]*UVcenter[2,1]*D[2,2]
# if the determinant of [A B/2 D/2 B/2 C E/2 D/2 E/2 F]is zero
# And if K>0,then it's a empty set
# otherwise the conic is degenerate
Deg <- matrix(c(ParA[1],ParA[2]/2,ParA[4]/2, ParA[2]/2,ParA[3],ParA[5]/2, ParA[4]/2,ParA[5]/2,ParA[6]),3,3,byrow=TRUE)
K1 <- matrix(c(ParA[1],ParA[4]/2, ParA[4]/2, ParA[6]),2,2,byrow=TRUE)
K2 <- matrix(c(ParA[3],ParA[5]/2, ParA[5]/2, ParA[6]),2,2,byrow=TRUE)
K <- det(K1)+det(K2)
if ((abs(det(Deg)) < tolerance2)) if ((abs(det(M))<tolerance2) && (K > tolerance2)) { exitCode <- 4
# empty set(imaginary parellel lines)
} else {
exitCode <- -1
# degenerate cases
} else {
if (D[1,1]*D[2,2] > tolerance1) if (free < 0) { exitCode <- 1
# ellipse
} else {
exitCode <- 0
# empty set(imaginary ellipse)
} else { if (D[1,1]*D[2,2] < - tolerance1){
exitCode <- 2
# hyperbola
} else {
exitCode <- 3
# parabola
}
} }
XYcenter <- t(Q) %*% UVcenter
Axes <- matrix(c(sqrt(abs(free/D[1,1])), sqrt(abs(free/D[2,2]))),2,1)
if (exitCode == 1 && Axes[1]<Axes[2]) {
AA <- Axes[1]
Axes[1] <- Axes[2]
Axes[2] <- AA
Angle <- Angle + pi/2
}
if (exitCode == 2 && free*D[1,1]>0) {
AA <- Axes[1]
Axes[1] <- Axes[2]
Axes[2] <- AA
Angle <- Angle + pi/2
}
while (Angle > pi) Angle <- Angle - pi
while (Angle < 0) Angle <- Angle + pi
ParG <- rbind(XYcenter, Axes, Angle)
dimnames(ParG) <- NULL
list(ParG=ParG, exitCode=exitCode)
}
Residuals.ellipse <- function(XY,ParG)
{
Center <- ParG[1:2]
Axes <- ParG[3:4]
Angle <- ParG[5]
n <- dim(XY)[1]
XYproj <- matrix(0,n,2)
tolerance <- 1e-9
# First handling the circle case
if (abs((Axes[1]-Axes[2])/Axes[1])<tolerance){
phiall <- angle(XY[,1]-Center[1] + sqrt(-1) * (XY[,2]-Center[2]))
XYproj <- matrix(c(Axes[1] * cos(phiall)+Center[1], Axes[2] * sin(phiall)+Center[2]),1,2)
RSS <- norm(XY-XYproj,'F')^2
list(RSS=RSS, XYproj=XYproj)
}
# Now dealing with proper ellipses
a <- Axes[1]
b <- Axes[2]
aa <- a^2
bb <- b^2
tol.a <- tolerance * a
tol.b <- tolerance * b
tol.aa <- tolerance * aa
# Matrix Q for rotating the points and the ellipse to the canonical system
s <- sin(Angle)
cA <- cos(Angle)
Q <- matrix(c(cA, -s,s, cA),2,2,byrow=TRUE)
# data points in canonical coordinates
XY0 <- cbind(XY[,1]-Center[1], XY[,2]-Center[2]) %*% Q
XYA <- abs(XY0)
Tini <- apply(cbind(a * (XYA[,1]-a),b * (XYA[,2]-b)),1,max)
# main loop over the data points
for (i in 1:n){
u <- XYA[i,1]
v <- XYA[i,2]
ua <- u * a
vb <- v * b
if (u == 0) z1 <- 1 else z1 <- sign(XY0[i,1])
if (v == 0) z2 <- 1 else z2 <- sign(XY0[i,2])
# does the point lie on the minor axis?
if (u<tol.a){
if (XY0[i,2]<0) XYproj[i,] <- cbind(0, -b) else XYproj[i,] <- cbind(0, b)
next
}
# does the point lie on the major axis?
if (v<tol.b){
if (u < (a-bb/a)){
xproj <- aa * u/(aa-bb)
XYproj[i,] <- matrix(c(z1 * xproj, z2 * b * sqrt(max(1-(xproj/a)^2,0))),1,2)
} else {
XYproj[i,] <- cbind(z1 * a, 0)
}
next
}
# generic case: start the iterative procedure
T <- Tini[i]
for (iter in 1:100){
Taa <- T + aa
Tbb <- T + bb
PP1 <- (ua/Taa)^2
PP2 <- (vb/Tbb)^2
Fp <- PP1 + PP2 - 1
if (Fp<0) break
Fder <- 2 * (PP1/Taa + PP2/Tbb)
Ratio <- Fp/Fder
if (Ratio<tol.aa) break
T <- T + Ratio
}
# compute the projection of the point onto the ellipse
xproj <- XY0[i,1] * aa/Taa
yproj <- sign(XY0[i,2]) * b * sqrt(max(1-(xproj/a)^2,0))
XYproj[i,] <- cbind(xproj, yproj)
} # end the main loop
# rotate back to the original system
XYproj <- XYproj %*% t(Q)
XYproj <- cbind(XYproj[,1]+Center[1], XYproj[,2]+Center[2])
RSS <- norm(XY-XYproj,'F')^2
list(RSS=RSS,XYproj=XYproj)
} # Residuals.ellipse
JmatrixLMA<-function(XY,ParA,XYproj){
#Compute the Jacobian matrix(Implicit method)using algebraic parameter
n <- dim(XY)[1]
Res <- matrix(0,n,1)
X <- matrix(XY[,1],ncol=1)
Y <- matrix(XY[,2],ncol=1)
Z <- cbind(X^2, X*Y, Y^2, X, Y, matrix(1,n,ncol=1) ) %*% ParA
DD <- XY-XYproj
for (i in 1:n) Res[i] <- sign(Z[i])*norm(matrix(DD[i,]),'F')
D2 <- matrix(c(XYproj,matrix(1,n,1)),ncol=3,byrow=FALSE)
x <- matrix(XYproj[,1],ncol=1)
y <- matrix(XYproj[,2],ncol=1)
xx <- matrix(x^2,ncol=1)
yy <- matrix(y^2,ncol=1)
xy <- matrix(x*y,ncol=1)
# dPar <- matrix(c(xx,xy,yy,x,y,ones(n,1)),1) #partial derivative of P wrt ParA
du <- D2 %*% matrix(c(2*ParA[1],ParA[2],ParA[4]),3) #partial derivative of P wrt x-coordinate
dv <- D2 %*% matrix(c(ParA[2],2*ParA[3],ParA[5]),3) #partial derivative of P wrt y-coordinate
eA <- sqrt(du^2+dv^2)
J <- cbind(xx/eA, xy/eA, yy/eA, x/eA, y/eA,matrix(1,n,1)/eA)
list(Res = Res,J = J)
}
ResidualsG<-function(XY,ParG)
{
# Projecting a given set of points onto an ellipse and computing the distances from the points to the ellipse
Center <- ParG[1:2]
Axes <- ParG[3:4]
Angle <- ParG[5]
n <- dim(XY)[1]
XYproj <- matrix(0,n,2)
tolerance <- 1e-9
# First handling the circle case
if (abs((Axes[1]-Axes[2])/Axes[1])<tolerance){
phiall <- angle(XY[,1]-Center[1] + sqrt(-1)*(XY[,2]-Center[2]))
XYproj <- cbind(Axes[1]*cos(phiall)+Center[1], Axes[2]*sin(phiall)+Center[2])
RSS <- norm(XY-XYproj,'F')^2
list(RSS=RSS, XYproj=XYproj)
}
# Now dealing with proper ellipses
a <- Axes[1]
b <- Axes[2]
aa <- a^2
bb <- b^2
tol_a <- tolerance*a
tol_b <- tolerance*b
tol_aa <- tolerance*aa
# Matrix Q for rotating the points and the ellipse
s <- sin(Angle)
cA <- cos(Angle)
Q <- matrix(c(cA, -s,s, cA),2,2,byrow=TRUE)
XY0 <- cbind(XY[,1]-Center[1], XY[,2]-Center[2]) %*% Q
# data points in canonical coordinates
XYA <- abs(XY0)
Tini <- matrix( apply(cbind(a*(XYA[,1]-a),b*(XYA[,2]-b)),1,max),ncol=1)
# main loop over the data points
for (i in 1:n){
u <- XYA[i,1]
v <- XYA[i,2]
ua <- u * a
vb <- v * b
# does the point lie on the minor axis?
if (u<tol_a){
if (XY0[i,2]<0) XYproj[i,] <- cbind(0, -b) else XYproj[i,] <- cbind(0, b)
next
}
# does the point lie on the major axis?
if (v<tol_b){
if (u<a-bb/a){
xproj <- aa*u/(aa-bb)
XYproj[i,] <- cbind(sign(XY0[i,1])*xproj, sign(XY0[i,2])*b*sqrt(1-(xproj/a)^2))
} else {
XYproj[i,] <- cbind(sign(XY0[i,1])*a, 0)
}
next
}
# generic case: start the iterative procedure
T <- Tini[i]
for (iter in 1:100){
Taa <- T + aa
Tbb <- T + bb
PP1 <- (ua/Taa)^2
PP2 <- (vb/Tbb)^2
Fp <- PP1 + PP2 - 1
if (Fp<0) break
Fder <- 2*(PP1/Taa + PP2/Tbb)
Ratio <- Fp/Fder
if (Ratio<tol_aa) break
T <- T + Ratio
}
# compute the projection of the point onto the ellipse
XYproj[i,] <- cbind(XY0[i,1]*aa/Taa, XY0[i,2]*bb/Tbb)
}
XYproj <- XYproj %*% t(Q)
XYproj <- cbind(XYproj[,1]+Center[1], XYproj[,2]+Center[2])
RSS <- norm(XY-XYproj,'F')^2
list(RSS=RSS, XYproj=XYproj)
} # ResidualsG
fit.ellipseLMG <- function(XY,ParGini,LambdaIni=1,epsilon=0.000001,IterMAX = 200,L = 200)
{# fitting ellipse using Implicit method
#epsilon <- tolerance (small threshold)
#IterMAX <- maximal number of (main) iterations usually 10-20 suffice
#L <- boundary for major/minor axis
lambda.sqrt <- sqrt(LambdaIni)# sqrt(Lambda) is actually used by the code
TF <- FALSE
tmp <- ResidualsG(XY,ParGini)
Fp <- tmp$RSS
XYproj <- tmp$XYproj
tmp <- JmatrixLMG(XY,ParGini,XYproj)
Res <- tmp$Res
J <- tmp$J
ParG <- ParGini
for (iter in 1:IterMAX) # main loop, each run is one (main) iteration
{
while (TRUE) # secondary loop - adjusting Lambda (no limit on cycles)
{
DelPar <- mldivide(rbind(J, lambda.sqrt*diag(5)), rbind(-Res, matrix(0,5,1))) # step candidate
progress <- norm(DelPar,'F')/(norm(ParG,'F')+epsilon)
if (progress < epsilon) break # stopping rule
ParTemp <- ParG + DelPar
if (ParTemp[3]< ParTemp[4]){ # out of range
Temp=ParTemp[3]
ParTemp[3]=ParTemp[4]
ParTemp[4]=Temp
ParTemp[5]=ParTemp[5]-sign(ParTemp[5])*pi/2
}
tmp <- ResidualsG(XY,ParTemp)
FTemp <- tmp$RSS
XYprojTemp <- tmp$XYproj
if (FTemp < Fp && ParTemp[3]>0 && ParTemp[4]>0) { # yes, improvement
lambda.sqrt <- lambda.sqrt/2 # reduce lambda, move to next iteration
break
} else { # no improvement
lambda.sqrt <- lambda.sqrt*2 # increase lambda, recompute the step
next
}
} # while TRUE, the end of the secondary loop
if (ParTemp[3] > L){ # diverge
TF <- TRUE
break
}
if (progress < epsilon) break
tmp <- JmatrixLMG(XY,ParTemp,XYprojTemp)
Res <- tmp$Res
J <- tmp$J
ParG <- ParTemp
Fp <- FTemp # update the iteration
}
RSS <- Fp
iters <- iter
# make the angle parameter between 0 and pi
while(ParG[5] >= pi) ParG[5] <- ParG[5] - pi
while(ParG[5] < 0) ParG[5] <- ParG[5] + pi
if (iters >= IterMAX) TF <- TRUE
list(ParG=ParG,RSS=RSS,iters=iters,TF=TF)
} #fit.ellipseLMG
fit.ellipseLMG.H <- function(XY,ParGini,LambdaIni=1,epsilon=0.000001,IterMAX = 200,L = 200)
{# fitting ellipse using Implicit method
#epsilon <- tolerance (small threshold)
#IterMAX <- maximal number of (main) iterations usually 10-20 suffice
#L <- boundary for major/minor axis
lambda.sqrt <- sqrt(LambdaIni)# sqrt(Lambda) is actually used by the code
TF <- FALSE
tmp <- ResidualsG(XY,ParGini)
Fp <- tmp$RSS
XYproj <- tmp$XYproj
tmp <- JmatrixLMG(XY,ParGini,XYproj)
Res <- tmp$Res
J <- tmp$J
ParG <- ParGini
for (iter in 1:IterMAX) # main loop, each run is one (main) iteration
{
while (TRUE) # secondary loop - adjusting Lambda (no limit on cycles)
{
DelPar <- mldivide(rbind(J, lambda.sqrt*diag(5)), rbind(-Res, matrix(0,5,1))) # step candidate
progress <- norm(DelPar,'F')/(norm(ParG,'F')+epsilon)
if (progress < epsilon) break # stopping rule
ParTemp <- ParG + DelPar
if (ParTemp[3]< ParTemp[4]){ # out of range
Temp=ParTemp[3]
ParTemp[3]=ParTemp[4]
ParTemp[4]=Temp
ParTemp[5]=ParTemp[5]-sign(ParTemp[5])*pi/2
}
tmp <- ResidualsG(XY,ParTemp)
FTemp <- tmp$RSS
XYprojTemp <- tmp$XYproj
if (FTemp < Fp && ParTemp[3]>0 && ParTemp[4]>0) { # yes, improvement
lambda.sqrt <- lambda.sqrt/2 # reduce lambda, move to next iteration
break
} else { # no improvement
lambda.sqrt <- lambda.sqrt*2 # increase lambda, recompute the step
next
}
} # while TRUE, the end of the secondary loop
if (ParTemp[3] > L){ # diverge
TF <- TRUE
break
}
if (progress < epsilon) break
tmp <- JmatrixLMG(XY,ParTemp,XYprojTemp)
Res <- tmp$Res
J <- tmp$J
ParG <- ParTemp
Fp <- FTemp # update the iteration
}
RSS <- Fp
iters <- iter
Jg <- t(J) %*% Res
H <- t(J) %*% J
# make the angle parameter between 0 and pi
while(ParG[5] >= pi) ParG[5] <- ParG[5] - pi
while(ParG[5] < 0) ParG[5] <- ParG[5] + pi
if (iters >= IterMAX) TF <- TRUE
list(ParG=ParG,RSS=RSS,iters=iters,TF=TF,Jg=Jg,H=H)
} #fit.ellipseLMG.H
JmatrixLMG <- function(XY,A,XYproj){
#Implicit method
n <- dim(XY)[1]
Res <- matrix(0,n,1)
s <- sin(A[5])
C <- cos(A[5])
ss <- s*s
cc <- C*C
cs <- C*s
a <- A[3]
b <- A[4]
aa <- a^2
bb <- b^2
ba <- bb-aa
DD <- XY-XYproj
D1 <- cbind(XYproj[,1]-A[1], XYproj[,2]-A[2])
D2 <- cbind(D1[,2], D1[,1])
for (i in 1:n) Res[i] <- sign(DD[i,] %*% matrix(D1[i,],2)) * norm(matrix(DD[i,]),'F')
du <- D1 %*% rbind(bb*cc+aa*ss, cs*ba)
dv <- D2 %*% rbind(bb*ss+aa*cc, cs*ba)
D3 <- cbind(D1[,1]^2, D1[,2]^2, D1[,1]*D1[,2],matrix(1,n,1))
d3 <- D3 %*% rbind(ss,cc,-2*cs,-bb) * a
d4 <- D3 %*% rbind(cc,ss,2*cs,-aa)*b
d5 <- D3 %*% rbind(-ba*cs,ba*cs,ba*(cc-ss),0)
e <- sqrt(du^2+dv^2)
J <- cbind(-du/e, -dv/e, d3/e, d4/e, d5/e)
list(Res=Res,J=J)
}
CircleFitByLandau<-function(XY,ParIni=NA,epsilon=0.000001,IterMAX = 50)
{
if (length(ParIni) != 3) ParIni <- NA
if (!is.numeric(ParIni)) ParIni <- NA
if (any(is.na(ParIni))) ParIni <- estimateInitialGuessCircle(XY)
centroid <- apply(XY,2,mean)
X <- XY[,1] - centroid[1]
Y <- XY[,2] - centroid[2]
# centering the initial guess
ParNew <- matrix(c(ParIni - c(centroid,0)),1)
for (iter in 1:IterMAX) # main loop, each run is one iteration
{
ParOld <- ParNew
Dx <- X - ParOld[1]
Dy <- Y - ParOld[2]
D <- sqrt(Dx * Dx + Dy * Dy)
ParNew <- cbind(-mean(Dx/D) , -mean(Dy/D) , 1) * mean(D)
progress <- (norm(ParNew-ParOld,'F'))/(norm(ParOld,'F')+epsilon)
if (progress < epsilon) break # stopping rule
} # the end of the main loop (over iterations)
# decentering the parameter vector for output
Par <- ParOld + c(centroid,0)
Par
}
CircleFitBySpath<-function(XY,ParIni=NA,epsilon=0.000001,IterMAX = 50)
{
if (length(ParIni) != 3) ParIni <- NA
if (!is.numeric(ParIni)) ParIni <- NA
if (any(is.na(ParIni))) ParIni <- estimateInitialGuessCircle(XY)
centroid <- apply(XY,2,mean)
X <- XY[,1] - centroid[1]
Y <- XY[,2] - centroid[2]
# centering the initial guess
ParNew <- matrix(c(ParIni - c(centroid,0)),1)
for (iter in 1:IterMAX) # main loop, each run is one iteration
{
ParOld <- ParNew
Dx <- X - ParOld[1]
Dy <- Y - ParOld[2]
D <- sqrt(Dx * Dx + Dy * Dy)
Mu <- mean(Dx/D)
Mv <- mean(Dy/D)
Mr <- mean(D)
Radius <- (Mu * ParOld[1] + Mv * ParOld[2] + Mr)/(1.0 - Mu * Mu - Mv * Mv)
ParNew <- cbind(-Mu , -Mv , 1) * Radius
progress <- (norm(ParNew-ParOld,'F'))/(norm(ParOld,'F')+epsilon)
if (progress < epsilon) break # stopping rule
} # the end of the main loop (over iterations)
# decentering the parameter vector for output
Par <- ParOld + c(centroid,0)
Par
}
estimateInitialGuessCircle <- function(XY)
{# estimate initial guess for circle LM
x0 <- mean(XY[,1])
y0 <- mean(XY[,2])
c(x0,y0,mean(sqrt((XY[,1]^2+x0^2)+(XY[,2]^2+y0^2))))
}
CurrentIteration<-function(Par,XY){
# computes the objective function F and its derivatives at the current point Par
Dx = matrix(XY[,1] - Par[1],ncol=1)
Dy = matrix(XY[,2] - Par[2],ncol=1)
D = sqrt(Dx^2 + Dy^2)
J = cbind(-Dx / D, -Dy / D, -1 )
g = matrix(D - Par[3],ncol=1)
F = sqrt(sum(g^2))^2
Radius = mean(D)
list(J=J,g=g,F=F,Radius = Radius)
}
LMcircleFit<-function(XY,ParIni=NA,LambdaIni=1,epsilon=0.000001,IterMAX = 50){
#epsilon = tolerance (small threshold)
#IterMAX = maximal number of (main) iterations; usually 10-20 suffice
if (length(ParIni) != 3) ParIni <- NA
if (!is.numeric(ParIni)) ParIni <- NA
if (is.na(ParIni)) ParIni <- estimateInitialGuessCircle(XY)
lambda_sqrt = sqrt(LambdaIni)# sqrt(Lambda) is actually used by the code
Par = ParIni# starting with the given initial guess
Jtmp = CurrentIteration(Par,XY)# compute objective function and its derivatives
J <- Jtmp$J
g <- Jtmp$g
F <- Jtmp$F
for (iter in 1:IterMAX){ # main loop, each run is one (main) iteration
while (TRUE){ # secondary loop - adjusting Lambda (no limit on cycles)
DelPar = mldivide(rbind(J, lambda_sqrt * diag(3)), rbind(g, 0,0,0)) # step candidate
progress = sqrt(sum(DelPar^2)) / (sqrt(sum(Par^2))+epsilon)
if (progress < epsilon) break # stopping rule
ParTemp = Par - t(DelPar)
Jtmp = CurrentIteration(ParTemp,XY); # objective function + derivatives
JTemp<- Jtmp$J
gTemp<- Jtmp$g
FTemp<- Jtmp$F
if (FTemp < F && ParTemp[3]>0){ # yes, improvement
lambda_sqrt = lambda_sqrt/2 # reduce lambda, move to next iteration
break
} else { # no improvement
lambda_sqrt = lambda_sqrt*2 # increase lambda, recompute the step
next
}
} # while (1), the end of the secondary loop
if (progress < epsilon) break # stopping rule
Par = ParTemp; J = JTemp; g = gTemp; F = FTemp # update the iteration
}
Par
}
CurrentIterationReduced<-function(Par,XY){
# computes the objective function F and its derivatives at the current point Par
Dx = matrix(XY[,1] - Par[1],ncol=1)
Dy = matrix(XY[,2] - Par[2],ncol=1)
D = sqrt(Dx^2 + Dy^2)
J = cbind(-Dx + mean(Dx), -Dy + mean(Dy) )
Radius = mean(D)
g = matrix(D - Radius,ncol=1)
F = sqrt(sum(g^2))^2
list(J=J,g=g,F=F,Radius = Radius)
}
LMreducedCircleFit<-function(XY,ParIni=NA,LambdaIni=1,epsilon=0.000001,IterMAX = 50){
#epsilon = tolerance (small threshold)
#IterMAX = maximal number of (main) iterations; usually 10-20 suffice
if (length(ParIni) != 3) ParIni <- NA
if (!is.numeric(ParIni)) ParIni <- NA
if (any(is.na(ParIni))) ParIni <- estimateInitialGuessCircle(XY)
lambda_sqrt = sqrt(LambdaIni)# sqrt(Lambda) is actually used by the code
Par = ParIni[1:2]# starting with the given initial guess
Jtmp = CurrentIterationReduced(Par,XY)# compute objective function and its derivatives
J <- Jtmp$J[,1:2]
g <- Jtmp$g
F <- Jtmp$F
Radius <- Jtmp$Radius
for (iter in 1:IterMAX){ # main loop, each run is one (main) iteration
while (TRUE){ # secondary loop - adjusting Lambda (no limit on cycles)
DelPar = mldivide(rbind(J, lambda_sqrt * diag(2)), rbind(g, 0,0)) # step candidate
progress = norm(matrix(DelPar,1),'F')/(Radius+norm(matrix(Par,1),'F')+epsilon)
if (progress < epsilon) break # stopping rule
ParTemp = Par - t(DelPar)
Jtmp = CurrentIterationReduced(ParTemp,XY) # objective function + derivatives
JTemp<- Jtmp$J
gTemp<- Jtmp$g
FTemp<- Jtmp$F
RadiusTemp<- Jtmp$Radius
if (FTemp < F){ # yes, improvement
lambda_sqrt = lambda_sqrt/2 # reduce lambda, move to next iteration
break
} else { # no improvement
lambda_sqrt = lambda_sqrt*2 # increase lambda, recompute the step
next
}
} # while (1), the end of the secondary loop
if (progress < epsilon) break # stopping rule
Par = ParTemp; J = JTemp; g = gTemp; F = FTemp; Radius = RadiusTemp # update the iteration
}
c(Par, Radius) # assembling the full parameter vector "Par" for output
}
calculateCircle<-function(x, y, r, steps=50,sector=c(0,360),randomDist=FALSE, randomFun=runif,noiseFun=NA,...)
{
points = matrix(0,steps,2)
if (randomDist) n<-sector[1]+randomFun(steps,...)*(sector[2]-sector[1]) else n<-seq(sector[1],sector[2],length.out=steps)
if (randomDist) repeat {
n[which(!(n>=sector[1] & n<=sector[2]))]<-sector[1]+randomFun(sum(!(n>=sector[1] & n<=sector[2])),...)*(sector[2]-sector[1])
if (all(n>=sector[1] & n<=sector[2])) break
}
alpha = n * (pi / 180)
sinalpha = sin(alpha)
cosalpha = cos(alpha)
points[,1]<- x + (r * cosalpha)
points[,2]<- y + (r * sinalpha)
if (is.function(noiseFun)) points<-apply(points,1:2,noiseFun)
points
}
calculateEllipse<-function(x, y, a, b, angle=0, steps=50,sector=c(0,360),randomDist=FALSE, randomFun=runif,noiseFun=NA,...)
{# http://en.wikipedia.org/w/index.php?title=Ellipse&oldid=456212176#Ellipses_in_computer_graphics
points = matrix(0,steps,2)
beta = angle * (pi / 180)
sinbeta = sin(beta)
cosbeta = cos(beta)
if (randomDist) n<-sector[1]+randomFun(steps,...)*(sector[2]-sector[1]) else n<-seq(sector[1],sector[2],length.out=steps)
if (randomDist) repeat {
n[which(!(n>=sector[1] & n<=sector[2]))]<-sector[1]+randomFun(sum(!(n>=sector[1] & n<=sector[2])),...)*(sector[2]-sector[1])
if (all(n>=sector[1] & n<=sector[2])) break
}
alpha = n * (pi / 180)
sinalpha = sin(alpha)
cosalpha = cos(alpha)
points[,1]<- x + (a * cosalpha * cosbeta - b * sinalpha * sinbeta)
points[,2]<- y + (a * cosalpha * sinbeta + b * sinalpha * cosbeta)
if (is.function(noiseFun)) points<-apply(points,1:2,noiseFun)
points
}
EllipseDirectFit<-function(XY){
# translated to R by Jose Gama 2014
# Original code by: Nikolai Chernov http://www.mathworks.com/matlabcentral/fileexchange/22684-ellipse-fit-direct-method
# A. W. Fitzgibbon, M. Pilu, R. B. Fisher, "Direct Least Squares Fitting of Ellipses", IEEE Trans. PAMI, Vol. 21, pages 476-480 (1999)
# Halir R, Flusser J (1998) Proceedings of the 6th International Conference in Central Europe on Computer Graphics and Visualization,
# Numerically stable direct least squares fitting of ellipses (WSCG, Plzen, Czech Republic), pp 125–132.
centroid <- apply(XY,2,mean)
D1 <- cbind((XY[,1]-centroid[1])^2, (XY[,1]-centroid[1])*(XY[,2]-centroid[2]), (XY[,2]-centroid[2])^2)
D2 <- cbind(XY[,1]-centroid[1], XY[,2]-centroid[2], matrix(1,dim(XY)[1]))
S1 <- t(D1) %*% D1
S2 <- t(D1) %*% D2
S3 <- t(D2) %*% D2
T <- -solve(S3) %*% t(S2)
M <- S1 + S2 %*% T
M <- rbind(M[3,]/2, -M[2,], M[1,]/2)
evec<-eigen(M)$vectors
cond <- 4*evec[1,]*evec[3,]-evec[2,]^2
A1 <- matrix(evec[,which(cond>0)[1]],3)
A <- rbind(A1, T %*% A1)
A4 <- A[4]-2*A[1]*centroid[1]-A[2]*centroid[2]
A5 <- A[5]-2*A[3]*centroid[2]-A[2]*centroid[1]
A6 <- A[6]+A[1]*centroid[1]^2+A[3]*centroid[2]^2+ A[2]*centroid[1]*centroid[2]-A[4]*centroid[1]-A[5]*centroid[2]
A[4] <- A4; A[5] <- A5; A[6] <- A6
A <- A / norm(A,'F')
# # general-form conic equation ax^2 + bxy + cy^2 +dx + ey + f = 0
# a <- A[1];b <- A[2]/2;C <- A[3];d <- A[4]/2;E <- A[5]/2;f <- A[6]
# x0 <- (C*d-b*E)/(b*b-a*C)
# y0 <- (a*E-b*d)/(b*b-a*C)
# semiaxis.a <- sqrt(2*(a*E^2+C*d^2+f*b^2-2*b*d*E-a*C*f)/((b^2-a*C)*(sqrt((a-C)^2+4*b^2)-(a+C))))
# semiaxis.b <- sqrt(2*(a*E^2+C*d^2+f*b^2-2*b*d*E-a*C*f)/((b^2-a*C)*(-sqrt((a-C)^2+4*b^2)-(a+C))))
# #, x0=x0,y0=y0,semiaxis.a=semiaxis.a,semiaxis.b=semiaxis.b
A
}
EllipseFitByTaubin<-function(XY){
# translated to R by Jose Gama 2014
# Original code by: Nikolai Chernov
centroid <- apply(XY,2,mean)
Z <- cbind((XY[,1]-centroid[1])^2, (XY[,1]-centroid[1])*(XY[,2]-centroid[2]), (XY[,2]-centroid[2])^2, XY[,1]-centroid[1], XY[,2]-centroid[2], matrix(1,dim(XY)[1]))
M <- t(Z) %*% Z/dim(XY)[1]
P <- rbind(cbind(M[1,1]-M[1,6]^2, M[1,2]-M[1,6]*M[2,6], M[1,3]-M[1,6]*M[3,6], M[1,4], M[1,5]),
cbind(M[1,2]-M[1,6]*M[2,6], M[2,2]-M[2,6]^2, M[2,3]-M[2,6]*M[3,6], M[2,4], M[2,5]),
cbind(M[1,3]-M[1,6]*M[3,6], M[2,3]-M[2,6]*M[3,6], M[3,3]-M[3,6]^2, M[3,4], M[3,5]),
cbind(M[1,4], M[2,4], M[3,4], M[4,4], M[4,5]),
cbind(M[1,5], M[2,5], M[3,5], M[4,5], M[5,5]))
Q <- rbind(cbind(4*M[1,6], 2*M[2,6], 0, 0, 0),
cbind(2*M[2,6], M[1,6]+M[3,6], 2*M[2,6], 0, 0),
cbind(0, 2*M[2,6], 4*M[3,6], 0, 0),
cbind(0, 0, 0, 1, 0),
cbind(0, 0, 0, 0, 1))
V2 <- geigen(P,Q)
V <- V2$vectors
diagD <- V2$values
Dsort <- matrix(sort(diagD),ncol=1)
ID <- matrix(order(diagD),ncol=1)
A <- matrix(V[,ID[1]],ncol=1)
A <- rbind(A, -t(A[1:3]) %*% M[1:3,6])
A4 <- A[4]-2*A[1]*centroid[1]-A[2]*centroid[2]
A5 <- A[5]-2*A[3]*centroid[2]-A[2]*centroid[1]
A6 <- A[6]+A[1]*centroid[1]^2+A[3]*centroid[2]^2+ A[2]*centroid[1]*centroid[2]-A[4]*centroid[1]-A[5]*centroid[2]
A[4] <- A4; A[5] <- A5; A[6] <- A6
A <- A / norm(A,'F')
A
}
CircleFitByTaubin<-function(XY){
# translated to R by Jose Gama 2014
# Original code by: Nikolai Chernov
n <- dim(XY)[1] # number of data points
centroid <- apply(XY,2,mean)
Mxx <- 0; Myy <- 0; Mxy <- 0; Mxz <- 0; Myz <- 0; Mzz <- 0
for (x in 1:n) {
Xi <- XY[x,1] - centroid[1]
Yi <- XY[x,2] - centroid[2]
Zi <- Xi*Xi + Yi*Yi
Mxy <- Mxy + Xi*Yi
Mxx <- Mxx + Xi*Xi
Myy <- Myy + Yi*Yi
Mxz <- Mxz + Xi*Zi
Myz <- Myz + Yi*Zi
Mzz <- Mzz + Zi*Zi }
Mxx <- Mxx/n
Myy <- Myy/n
Mxy <- Mxy/n
Mxz <- Mxz/n
Myz <- Myz/n
Mzz <- Mzz/n
Mz <- Mxx + Myy
Cov_xy <- Mxx * Myy - Mxy * Mxy
A3 <- 4 * Mz
A2 <- -3 * Mz * Mz - Mzz
A1 <- Mzz * Mz + 4 * Cov_xy * Mz - Mxz * Mxz - Myz * Myz - Mz * Mz * Mz
A0 <- Mxz * Mxz * Myy + Myz * Myz * Mxx - Mzz * Cov_xy - 2 * Mxz * Myz * Mxy + Mz * Mz * Cov_xy
A22 <- A2 + A2
A33 <- A3 + A3 + A3
xnew <- 0
ynew <- 1e+20
epsilon <- 1e-12
IterMax <- 20
for (iter in 1:IterMax){
yold <- ynew
ynew <- A0 + xnew*(A1 + xnew*(A2 + xnew*A3))
if (abs(ynew) > abs(yold)) {
cat('Newton-Taubin goes wrong direction: |ynew| > |yold|\n')
xnew <- 0
break
}
Dy <- A1 + xnew*(A22 + xnew*A33)
xold <- xnew
xnew <- xold - ynew/Dy
if (abs((xnew-xold)/xnew) < epsilon) break
if (iter >= IterMax){
cat('Newton-Taubin will not converge\n')
xnew <- 0
}
if (xnew<0){
cat(1,'Newton-Taubin negative root: x=%f\n',xnew,'\n')
xnew <- 0
}
}
DET <- xnew*xnew - xnew*Mz + Cov_xy
Center <- cbind(Mxz*(Myy-xnew)-Myz*Mxy , Myz*(Mxx-xnew)-Mxz*Mxy)/DET/2
P <- cbind(Center+centroid , sqrt(Center %*% t(Center)+Mz))
P
}
CircleFitByPratt<-function(XY){
# translated to R by Jose Gama 2014
# Original code by: Nikolai Chernov
n <- dim(XY)[1] # number of data points
centroid <- apply(XY,2,mean)
Mxx <- 0; Myy <- 0; Mxy <- 0; Mxz <- 0; Myz <- 0; Mzz <- 0
for (x in 1:n) {
Xi <- XY[x,1] - centroid[1]
Yi <- XY[x,2] - centroid[2]
Zi <- Xi*Xi + Yi*Yi
Mxy <- Mxy + Xi*Yi
Mxx <- Mxx + Xi*Xi
Myy <- Myy + Yi*Yi
Mxz <- Mxz + Xi*Zi
Myz <- Myz + Yi*Zi
Mzz <- Mzz + Zi*Zi }
Mxx <- Mxx/n
Myy <- Myy/n
Mxy <- Mxy/n
Mxz <- Mxz/n
Myz <- Myz/n
Mzz <- Mzz/n
Mz <- Mxx + Myy
Cov_xy <- Mxx * Myy - Mxy * Mxy
Mxz2 <- Mxz * Mxz
Myz2 <- Myz * Myz
A2 <- 4 * Cov_xy - 3 * Mz * Mz - Mzz
A1 <- Mzz * Mz + 4 * Cov_xy * Mz - Mxz2 - Myz2 - Mz * Mz * Mz
A0 <- Mxz2 * Myy + Myz2 * Mxx - Mzz * Cov_xy - 2 * Mxz * Myz * Mxy + Mz * Mz * Cov_xy
A22 <- A2 + A2
epsilon <- 1e-12
ynew <- 1e+20
IterMax <- 20
xnew <- 0
for (iter in 1:IterMax){
yold <- ynew
ynew <- A0 + xnew*(A1 + xnew*(A2 + xnew*xnew*4))
if (abs(ynew) > abs(yold)) {
cat('Newton-Pratt goes wrong direction: |ynew| > |yold|\n')
xnew <- 0
break
}
Dy <- A1 + xnew*(A22 + 16*xnew*xnew)
xold <- xnew
xnew <- xold - ynew/Dy
if (abs((xnew-xold)/xnew) < epsilon) break
if (iter >= IterMax){
cat('Newton-Pratt will not converge\n')
xnew <- 0
}
if (xnew<0){
cat(1,'Newton-Pratt negative root: x=%f\n',xnew,'\n')
xnew <- 0
}
}
DET <- xnew*xnew - xnew*Mz + Cov_xy
Center <- cbind(Mxz*(Myy-xnew)-Myz*Mxy , Myz*(Mxx-xnew)-Mxz*Mxy)/DET/2
P <- cbind(Center+centroid , sqrt(Center %*% t(Center)+Mz+2*xnew))
P
}
CircleFitByKasa<-function(XY){
# translated to R by Jose Gama 2014
# Original code by: Nikolai Chernov
P <- mldivide(cbind(XY,1), matrix(XY[,1]^2 + XY[,2]^2,ncol=1))
Pout = cbind(P[1]/2 , P[2]/2 , sqrt((P[1]^2+P[2]^2)/4+P[3]))
Pout
}
# http://en.wikipedia.org/wiki/Ellipse#Mathematical_definitions_and_properties
ellipticity <- function(minorAxis,majorAxis) 1 - (minorAxis/majorAxis) # ellipticity = flattening factor
ellipseEccentricity <- function(minorAxis,majorAxis) (sqrt (1 - (minorAxis/majorAxis)^2)) # eccentricity of the ellipse
ellipseFocus <- function(minorAxis,majorAxis) sqrt(minorAxis-majorAxis)^2 # focus of the ellipse
ellipseRa <- function(minorAxis,majorAxis) (1+ellipseEccentricity(minorAxis,majorAxis))*majorAxis # radius at apoapsis (the farthest distance)
ellipseRp <- function(minorAxis,majorAxis) (1-ellipseEccentricity(minorAxis,majorAxis))*majorAxis # radius at periapsis (the closest distance)
ellipse.l <- function(minorAxis,majorAxis)
{# semi-latus rectum l
ra <- ellipseRa(minorAxis,majorAxis)
rp <- ellipseRp(minorAxis,majorAxis)
2*ra*rp/(ra+rp)
}
conic2parametric<-function(A, bv, cv){
# Diagonalise A - find Q, D such at A = Q' * D * Q
# Copyright Richard Brown, this code can be freely used and modified so
# long as this line is retained
# FITELLIPSE : Least squares ellipse fitting demonstration
# Richard Brown, May 28, 2007
# http://www.mathworks.com/matlabcentral/fileexchange/15125-fitellipse-m/content/demo/html/ellipsedemo.html
eTMP<-eigen(A)
D<-diag(eTMP$values)
Q<-eTMP$vectors
Q<-t(Q)
# If the determinant < 0, it's not an ellipse
if (prod(diag(D)) <= 0 ) stop('Linear fit did not produce an ellipse')
# We have b_h' = 2 * t' * A + b'
tV = -0.5 * mldivide(A , bv)
c_h = matrix(tV,1) %*% A %*% tV + t(bv) %*% tV + cv
list(z = tV,a = sqrt(-c_h / D[1,1]),b = sqrt(-c_h / D[2,2]),alpha = atan2(Q[1,2], Q[1,1]))
}
fitbookstein<-function(x){
#FITBOOKSTEIN Linear ellipse fit using bookstein constraint
# lambda_1^2 + lambda_2^2 = 1, where lambda_i are the eigenvalues of A
# Copyright Richard Brown, this code can be freely used and modified so
# long as this line is retained
# FITELLIPSE : Least squares ellipse fitting demonstration
# Richard Brown, May 28, 2007
# http://www.mathworks.com/matlabcentral/fileexchange/15125-fitellipse-m/content/demo/html/ellipsedemo.html
# W. Gander, G. H. Golub, R. Strebel, 1994
# Least-Squares Fitting of Circles and Ellipses
# BIT Numerical Mathematics, Springer
# Convenience variables
m = dim(x)[1]
x1 = x[, 1]
x2 = x[, 2]
# Define the coefficient matrix B, such that we solve the system
# B *[v; w] = 0, with the constraint norm(w) == 1
B = cbind(x1, x2, rep(1,m), x1^2, sqrt(2) * x1 * x2, x2^2)
# To enforce the constraint, we need to take the QR decomposition
qTMP<-qr(B)
R<-qr.R(qTMP)
Q<-qr.Q(qTMP)
# Decompose R into blocks
R11 = R[1:3, 1:3]
R12 = R[1:3, 4:6]
R22 = R[4:6, 4:6]
# Solve R22 * w = 0 subject to norm(w) == 1
svdTMP<-svd(R22)
U<-svdTMP[["u"]]
S<-diag(svdTMP[["d"]])
V<-svdTMP[["v"]]
w = matrix(V[, 3],3,1)
# Solve for the remaining variables
v = mldivide(-R11 , R12) %*% w
# Fill in the quadratic form
A = matrix(0,2,2)
A[1] = w[1]
A[2:3] = 1 / sqrt(2) * w[2]
A[4] = w[3]
bv = v[1:2]
c1 = v[3]
# Find the parameters
cTMP<-conic2parametric(A, bv, c1)
list(z=cTMP$z, a=cTMP$a, b=cTMP$b, alpha=cTMP$alpha)
}
fitggk<-function(x){
# Linear least squares with the Euclidean-invariant constraint Trace(A) = 1
# Copyright Richard Brown, this code can be freely used and modified so
# long as this line is retained
# FITELLIPSE : Least squares ellipse fitting demonstration
# Richard Brown, May 28, 2007
# http://www.mathworks.com/matlabcentral/fileexchange/15125-fitellipse-m/content/demo/html/ellipsedemo.html
# W. Gander, G. H. Golub, R. Strebel, 1994
# Least-Squares Fitting of Circles and Ellipses
# BIT Numerical Mathematics, Springer
# Convenience variables
m = dim(x)[1]
x1 = x[, 1]
x2 = x[, 2]
# Coefficient matrix
B = cbind(2 * x1 * x2, x2^2 - x1^2, x1, x2, rep(1,m))
v = mldivide(B , -x1^2)
# For clarity, fill in the quadratic form variables
A = matrix(0,2,2)
A[1] = 1-v[2]
A[2:3] = v[1]
A[2,2] = v[2]
bv = v[3:4]
c1 = v[5]
# Find the parameters
cTMP<-conic2parametric(A, bv, c1)
list(z=cTMP$z, a=cTMP$a, b=cTMP$b, alpha=cTMP$alpha)
}
Try the conicfit package in your browser
Any scripts or data that you put into this service are public.
conicfit documentation built on May 2, 2019, 3:11 a.m.
R Package Documentation
Browse R Packages
We want your feedback!
Note that we can't provide technical support on individual packages. You should contact the package authors for that.
Residuals.parabola <- function(XY,ParG)
{
# Projecting a given set of points onto a parabola and computing the distances from the points to the parabola
Vertex <- ParG[1:2]
p= ParG[3]
Angle <- ParG[4]
n <- dim(XY)[1]
XYproj <- matrix(0,n,2)
Iter <- matrix(0,n,1)
tolerance <- 1e-9
tol.p <- tolerance * p/2
pp <- p*p
# Matrix Q for rotating the points and the parabola
s <- sin(Angle)
cA <- cos(Angle)
Q <- matrix(c(cA, -s,s, cA),2,2,byrow=TRUE)
# data points in canonical coordinates
XY0 <- cbind(XY[,1]-Vertex[1], XY[,2]-Vertex[2]) %*% Q
XYA <- cbind(XY0[,1], abs(XY0[,2]))
# main loop over the data points
for (i in 1:n){
u <- XYA[i,1]
v <- XYA[i,2]
uu <- u*u
vv <- v*v
pu=p*u
if (v == 0) z2 <- 1 else z2 <- sign(XY0[i,2])
# does the point lie on the x-axis?
if (v<tol.p){
if (u>p) XYproj[i,] <- cbind(u-p, z2*sqrt(max(2*p*(u-p),0))) else XYproj[i,] <- c(0, 0)
next
}
# pick the initial point T starting from -1/2 until F(t)>0
for (j in 1:20){
Tp <- 1/2^j-1
Fp <- vv/(Tp+1)^2 - 2*pu-2*pp*Tp
if (Fp>0) break
}
# generic case: start the iterative procedure
for (iter in 1:100){
F0= vv/(Tp+1)^2
Fp <- F0-2*(pu+pp*Tp)
if (Fp<0) break
Fder <- 2*(F0/(Tp+1) + pp)
Ratio <- Fp/Fder
if (Ratio<tol.p){
Iter[i]=iter
break
}
Tp <- Tp + Ratio
}
# compute the projection of the point onto the parabola
XYproj[i,] <- cbind(u+Tp*p, XY0[i,2]/(Tp+1))
} # end the main loop
XYproj <- XYproj %*% t(Q)
XYproj <- cbind(XYproj[,1]+Vertex[1], XYproj[,2]+Vertex[2])
RSS <- norm(XY-XYproj,'F')^2
list(RSS=RSS, XYproj=XYproj)
} # Residuals.parabola
Residuals.hyperbola <- function(XY,ParG)
{# Projecting a given set of points onto a hyperbola and computing the distances from the points to the hyperbola
Center <- ParG[1:2]
Axes <- ParG[3:4]
Angle <- ParG[5]
n <- dim(XY)[1]
XYproj <- matrix(0,n,2)
tolerance <- 1e-9
a <- Axes[1]
b=Axes[2]
aa <- a^2
bb <- b^2
at=sqrt(a)
bt=sqrt(b)
tol.a <- tolerance*a
tol.b <- tolerance*b
tol.aa <- tolerance*aa
# Matrix Q for rotating the points and the hyperbola
s <- sin(Angle)
cA <- cos(Angle)
Q <- matrix(c(cA, -s,s, cA),2,2,byrow=TRUE)
# data points in canonical coordinates
XY0 <- (cbind(XY[,1]-Center[1], XY[,2]-Center[2])) %*% Q
XYA <- abs(XY0)
# find the inflection points TInf for all given pairs
XYS <- sqrt(XYA)
XYS1 <- matrix(bb*at*XYS[,1]-aa*bt*XYS[,2],ncol=1)
XYS2 <- matrix(at*XYS[,1]+bt*XYS[,2],ncol=1)
TInf <- matrix(XYS1/XYS2,ncol=1) #inflection points
# main loop over the data points
for (i in 1:n){
u <- XYA[i,1]
v <- XYA[i,2]
ua <- u*a
vb <- v*b
if (u == 0) z1 <- 1 else z1 <- sign(XY0[i,1])
if (v == 0) z2 <- 1 else z2 <- sign(XY0[i,2])
# does the point lie on the major axis?
if (v<tol.b){
if (u>a+bb/a){
xproj <- aa*u/(aa+bb)
XYproj[i,] <- cbind(z1*xproj, z2*b*sqrt(max((xproj/a)^2-1,0)))
} else XYproj[i,] <- cbind(z1*a, 0)
next
} # end if
# does the point lie on the minor axis?
if (u<tol.a){
yproj <- bb*v/(aa+bb)
XYproj[i,] <- cbind(z1*a*sqrt(1+(yproj/b)^2), z2*yproj )
next
} # } if
# generic case: start the iterative procedure
T0 <- TInf[i] # inflection point t
F0 <- (ua/(T0+aa))^2 - (vb/(-T0+bb))^2 - 1 # the corresponding F(t)
# if F0>0 then pick the initial point to the right of the root
# if F0<0 then pick the initial point to the left of the root
if (F0 > 0){ # case1: pick the initial point T until F(T)<0
for (j in 1:20){
Tp<-bb-(-T0+bb)/2^j
Fp<-(ua/(Tp+aa))^2 - (vb/(-Tp+bb))^2 - 1
if (Fp<0) break
} # end for j
for (iter in 1:100){
Taa <- Tp + aa
Tbb <- -Tp + bb
PP1 <- (ua/Taa)^2
PP2 <- (vb/Tbb)^2
Fp <- PP1 - PP2 - 1
if (Fp>0) break
Fder <- 2*(PP1/Taa + PP2/Tbb)
Ratio <- Fp/Fder
if (abs(Ratio)<tol.aa) break
Tp <- Tp + Ratio
} } else { for (j in 1:20) { # case2: pick the initial point T until F(T)>0
Tp=-aa+(T0+aa)/2^j
Fp=(ua/(Tp+aa))^2 - (vb/(-Tp+bb))^2 - 1
if (Fp>0) break
} # end for j
for (iter in 1:100){
Taa <- Tp + aa
Tbb <- -Tp + bb
PP1 <- (ua/Taa)^2
PP2 <- (vb/Tbb)^2
Fp <- PP1 - PP2 - 1
if (Fp<0) break
Fder <- 2*(PP1/Taa + PP2/Tbb)
Ratio <- Fp/Fder
if (abs(Ratio)<tol.aa) break
Tp <- Tp + Ratio
}
} # end if
# compute the projection of the point onto the hyperbola
if (Taa < 1e-6){
yproj <- XY0[i,2]*bb/Tbb
XYproj[i,] <- cbind(sign(XY0[i,1])*a*sqrt(1+(yproj/b)^2), yproj)
} else { if (Tbb < 1e-6){
xproj=XY0[i,1]*aa/Taa
yproj <- sign(XY0[i,2])*b*sqrt(max((xproj/a)^2-1,0))
XYproj[i,] <- cbind(xproj, yproj)
} else XYproj[i,] <- cbind(XY0[i,1]*aa/Taa, XY0[i,2]*bb/Tbb)
} # end if
} # } the main for-loop
XYproj <- XYproj %*% t(Q)
XYproj <- cbind(XYproj[,1]+Center[1], XYproj[,2]+Center[2])
RSS <- norm(XY-XYproj,'F')^2
list(RSS=RSS, XYproj=XYproj)
} # Residuals.hyperbola
fit.conicLMA <- function(XY,ParAini,LambdaIni=1,epsilonP=0.0000000001,epsilonF=0.0000000000001,IterMAX=2000000)
{
# Fitting a conic to a given set of points (Implicit method) using algebraic parameter
#epsilonP <- tolerance (small threshold)
#epsilonF <- tolerance (small threshold)
#IterMAX <- maximal number of (main) iterations usually 10-20 suffice
lambda.sqrt <- sqrt(LambdaIni) # sqrt(Lambda) is actually used by the code
tmp <- AtoG(ParAini)
ParGini <- tmp$ParG
exitCode <- tmp$exitCode
if (exitCode == 1){
tmp <- Residuals.ellipse(XY,ParGini)
Fp <- tmp$RSS
XYproj <- tmp$XYproj
} else { if (exitCode == 2){
tmp <- Residuals.hyperbola(XY,ParGini)
Fp <- tmp$RSS
XYproj <- tmp$J
} else stop('invalid initial parameter')
}
tmp <- JmatrixLMA(XY,ParAini,XYproj) # calculate the Jacobian matrix
Res <- tmp$Res
J <- tmp$J
# tmp <- HessianLMA(XY,ParAini,XYproj) # calculate the Hessian matrix
# H <- tmp$H
# ev <- tmp$ev
ParA <- ParAini
ParG <- ParGini
codeTemp <- exitCode
ParGTemp <- ParG
for (iter in 1:IterMAX) # main loop, each run is one (main) iteration
{
while (TRUE) # secondary loop - adjusting Lambda (no limit on cycles)
{
DelPar <- mldivide(rbind(J, lambda.sqrt*diag(6)), rbind(-Res, matrix(0,6,1))) # step candidate
ParTemp <- ParA + DelPar
ParTemp <- ParTemp/norm(ParTemp,'F')
tmp <- AtoG(ParTemp)
ParGTemp <- tmp$ParG
codeTemp <- tmp$exitCode
if (codeTemp != exitCode) progress <- 1 else progress <- norm(DelPar,'F')
if (progress < epsilonP) break # stopping rule
if (codeTemp == 1){
tmp <- Residuals.ellipse(XY,ParGTemp)
FTemp <- tmp$RSS
XYprojTemp <- tmp$XYproj
} else {if (codeTemp == 2){
tmp <- Residuals.hyperbola(XY,ParGTemp)
FTemp <- tmp$RSS
XYprojTemp <- tmp$XYproj
}else{
lambda.sqrt <- lambda.sqrt*2 #if it's degenerate, increase lambda, recompute the step
next
}
}
if (FTemp < Fp*(1-epsilonF/lambda.sqrt)){ # yes, improvement
lambda.sqrt <- lambda.sqrt/2 # reduce lambda, move to next iteration
break
}else { # no improvement
lambda.sqrt <- lambda.sqrt*2 # increase lambda, recompute the step
next
}
} # while (TRUE), the end of the secondary loop
if (progress < epsilonP) break # stopping rule
tmp <- JmatrixLMA(XY,ParTemp,XYprojTemp)
Res <- tmp$Res
J <- tmp$J
ParA <- ParTemp
Fp <- FTemp # update the iteration
ParG <- ParGTemp
exitCode <- codeTemp
} # main loop
RSS <- Fp
iters <- iter
list(ParA=ParA,RSS=RSS,iters=iters,exitCode=exitCode)
}
GtoA<-function(ParG)
{# Conversion of geometric parameters of an ellipse
# ParG <- [Center(1:2), Axes(1:2), Angle]'
# to algebraic parameters
k <- cos(ParG[5])
s <- sin(ParG[5])
a <- ParG[3]
b <- ParG[4]
Xc <- ParG[1]
Yc <- ParG[2]
P <- (k/a)^2 + (s/b)^2
Q <- (s/a)^2 + (k/b)^2
R <- 2*k*s*(1/a^2 - 1/b^2)
cbind(A=P, B=R, C=Q, D=-2*P*Xc-R*Yc, E=-2*Q*Yc-R*Xc, F=P*Xc^2+Q*Yc^2+R*Xc*Yc-1)
}
AtoG <- function(ParA)
{
# Conversion of algebraic parameters to geometric parameters
tolerance1 <- 1.e-10
tolerance2 <- 1.e-20
if (abs(ParA[1]-ParA[3]) > tolerance1) Angle <- atan(ParA[2]/(ParA[1]-ParA[3]))/2 else Angle <- pi/4
cA <- cos(Angle)
s <- sin(Angle)
Q <- matrix(c(cA, s,-s, cA),2,2,byrow=TRUE)
M <- matrix(c(ParA[1], ParA[2]/2, ParA[2]/2, ParA[3]),2,2,byrow=TRUE)
D <- Q %*% M %*% t(Q)
N <- Q %*% matrix(c(ParA[4], ParA[5]),2,1,byrow=TRUE)
O <- ParA[6]
if ((D[1,1] < 0) && (D[2,2] < 0)){
D <- -D
N <- -N
O <- -O
}
UVcenter <- matrix(c(-N[1,1]/2/D[1,1], -N[2,1]/2/D[2,2]),2,1,byrow=TRUE)
free <- O - UVcenter[1,1]*UVcenter[1,1]*D[1,1] - UVcenter[2,1]*UVcenter[2,1]*D[2,2]
# if the determinant of [A B/2 D/2 B/2 C E/2 D/2 E/2 F]is zero
# And if K>0,then it's a empty set
# otherwise the conic is degenerate
Deg <- matrix(c(ParA[1],ParA[2]/2,ParA[4]/2, ParA[2]/2,ParA[3],ParA[5]/2, ParA[4]/2,ParA[5]/2,ParA[6]),3,3,byrow=TRUE)
K1 <- matrix(c(ParA[1],ParA[4]/2, ParA[4]/2, ParA[6]),2,2,byrow=TRUE)
K2 <- matrix(c(ParA[3],ParA[5]/2, ParA[5]/2, ParA[6]),2,2,byrow=TRUE)
K <- det(K1)+det(K2)
if ((abs(det(Deg)) < tolerance2)) if ((abs(det(M))<tolerance2) && (K > tolerance2)) { exitCode <- 4
# empty set(imaginary parellel lines)
} else {
exitCode <- -1
# degenerate cases
} else {
if (D[1,1]*D[2,2] > tolerance1) if (free < 0) { exitCode <- 1
# ellipse
} else {
exitCode <- 0
# empty set(imaginary ellipse)
} else { if (D[1,1]*D[2,2] < - tolerance1){
exitCode <- 2
# hyperbola
} else {
exitCode <- 3
# parabola
}
} }
XYcenter <- t(Q) %*% UVcenter
Axes <- matrix(c(sqrt(abs(free/D[1,1])), sqrt(abs(free/D[2,2]))),2,1)
if (exitCode == 1 && Axes[1]<Axes[2]) {
AA <- Axes[1]
Axes[1] <- Axes[2]
Axes[2] <- AA
Angle <- Angle + pi/2
}
if (exitCode == 2 && free*D[1,1]>0) {
AA <- Axes[1]
Axes[1] <- Axes[2]
Axes[2] <- AA
Angle <- Angle + pi/2
}
while (Angle > pi) Angle <- Angle - pi
while (Angle < 0) Angle <- Angle + pi
ParG <- rbind(XYcenter, Axes, Angle)
dimnames(ParG) <- NULL
list(ParG=ParG, exitCode=exitCode)
}
Residuals.ellipse <- function(XY,ParG)
{
Center <- ParG[1:2]
Axes <- ParG[3:4]
Angle <- ParG[5]
n <- dim(XY)[1]
XYproj <- matrix(0,n,2)
tolerance <- 1e-9
# First handling the circle case
if (abs((Axes[1]-Axes[2])/Axes[1])<tolerance){
phiall <- angle(XY[,1]-Center[1] + sqrt(-1) * (XY[,2]-Center[2]))
XYproj <- matrix(c(Axes[1] * cos(phiall)+Center[1], Axes[2] * sin(phiall)+Center[2]),1,2)
RSS <- norm(XY-XYproj,'F')^2
list(RSS=RSS, XYproj=XYproj)
}
# Now dealing with proper ellipses
a <- Axes[1]
b <- Axes[2]
aa <- a^2
bb <- b^2
tol.a <- tolerance * a
tol.b <- tolerance * b
tol.aa <- tolerance * aa
# Matrix Q for rotating the points and the ellipse to the canonical system
s <- sin(Angle)
cA <- cos(Angle)
Q <- matrix(c(cA, -s,s, cA),2,2,byrow=TRUE)
# data points in canonical coordinates
XY0 <- cbind(XY[,1]-Center[1], XY[,2]-Center[2]) %*% Q
XYA <- abs(XY0)
Tini <- apply(cbind(a * (XYA[,1]-a),b * (XYA[,2]-b)),1,max)
# main loop over the data points
for (i in 1:n){
u <- XYA[i,1]
v <- XYA[i,2]
ua <- u * a
vb <- v * b
if (u == 0) z1 <- 1 else z1 <- sign(XY0[i,1])
if (v == 0) z2 <- 1 else z2 <- sign(XY0[i,2])
# does the point lie on the minor axis?
if (u<tol.a){
if (XY0[i,2]<0) XYproj[i,] <- cbind(0, -b) else XYproj[i,] <- cbind(0, b)
next
}
# does the point lie on the major axis?
if (v<tol.b){
if (u < (a-bb/a)){
xproj <- aa * u/(aa-bb)
XYproj[i,] <- matrix(c(z1 * xproj, z2 * b * sqrt(max(1-(xproj/a)^2,0))),1,2)
} else {
XYproj[i,] <- cbind(z1 * a, 0)
}
next
}
# generic case: start the iterative procedure
T <- Tini[i]
for (iter in 1:100){
Taa <- T + aa
Tbb <- T + bb
PP1 <- (ua/Taa)^2
PP2 <- (vb/Tbb)^2
Fp <- PP1 + PP2 - 1
if (Fp<0) break
Fder <- 2 * (PP1/Taa + PP2/Tbb)
Ratio <- Fp/Fder
if (Ratio<tol.aa) break
T <- T + Ratio
}
# compute the projection of the point onto the ellipse
xproj <- XY0[i,1] * aa/Taa
yproj <- sign(XY0[i,2]) * b * sqrt(max(1-(xproj/a)^2,0))
XYproj[i,] <- cbind(xproj, yproj)
} # end the main loop
# rotate back to the original system
XYproj <- XYproj %*% t(Q)
XYproj <- cbind(XYproj[,1]+Center[1], XYproj[,2]+Center[2])
RSS <- norm(XY-XYproj,'F')^2
list(RSS=RSS,XYproj=XYproj)
} # Residuals.ellipse
JmatrixLMA<-function(XY,ParA,XYproj){
#Compute the Jacobian matrix(Implicit method)using algebraic parameter
n <- dim(XY)[1]
Res <- matrix(0,n,1)
X <- matrix(XY[,1],ncol=1)
Y <- matrix(XY[,2],ncol=1)
Z <- cbind(X^2, X*Y, Y^2, X, Y, matrix(1,n,ncol=1) ) %*% ParA
DD <- XY-XYproj
for (i in 1:n) Res[i] <- sign(Z[i])*norm(matrix(DD[i,]),'F')
D2 <- matrix(c(XYproj,matrix(1,n,1)),ncol=3,byrow=FALSE)
x <- matrix(XYproj[,1],ncol=1)
y <- matrix(XYproj[,2],ncol=1)
xx <- matrix(x^2,ncol=1)
yy <- matrix(y^2,ncol=1)
xy <- matrix(x*y,ncol=1)
# dPar <- matrix(c(xx,xy,yy,x,y,ones(n,1)),1) #partial derivative of P wrt ParA
du <- D2 %*% matrix(c(2*ParA[1],ParA[2],ParA[4]),3) #partial derivative of P wrt x-coordinate
dv <- D2 %*% matrix(c(ParA[2],2*ParA[3],ParA[5]),3) #partial derivative of P wrt y-coordinate
eA <- sqrt(du^2+dv^2)
J <- cbind(xx/eA, xy/eA, yy/eA, x/eA, y/eA,matrix(1,n,1)/eA)
list(Res = Res,J = J)
}
ResidualsG<-function(XY,ParG)
{
# Projecting a given set of points onto an ellipse and computing the distances from the points to the ellipse
Center <- ParG[1:2]
Axes <- ParG[3:4]
Angle <- ParG[5]
n <- dim(XY)[1]
XYproj <- matrix(0,n,2)
tolerance <- 1e-9
# First handling the circle case
if (abs((Axes[1]-Axes[2])/Axes[1])<tolerance){
phiall <- angle(XY[,1]-Center[1] + sqrt(-1)*(XY[,2]-Center[2]))
XYproj <- cbind(Axes[1]*cos(phiall)+Center[1], Axes[2]*sin(phiall)+Center[2])
RSS <- norm(XY-XYproj,'F')^2
list(RSS=RSS, XYproj=XYproj)
}
# Now dealing with proper ellipses
a <- Axes[1]
b <- Axes[2]
aa <- a^2
bb <- b^2
tol_a <- tolerance*a
tol_b <- tolerance*b
tol_aa <- tolerance*aa
# Matrix Q for rotating the points and the ellipse
s <- sin(Angle)
cA <- cos(Angle)
Q <- matrix(c(cA, -s,s, cA),2,2,byrow=TRUE)
XY0 <- cbind(XY[,1]-Center[1], XY[,2]-Center[2]) %*% Q
# data points in canonical coordinates
XYA <- abs(XY0)
Tini <- matrix( apply(cbind(a*(XYA[,1]-a),b*(XYA[,2]-b)),1,max),ncol=1)
# main loop over the data points
for (i in 1:n){
u <- XYA[i,1]
v <- XYA[i,2]
ua <- u * a
vb <- v * b
# does the point lie on the minor axis?
if (u<tol_a){
if (XY0[i,2]<0) XYproj[i,] <- cbind(0, -b) else XYproj[i,] <- cbind(0, b)
next
}
# does the point lie on the major axis?
if (v<tol_b){
if (u<a-bb/a){
xproj <- aa*u/(aa-bb)
XYproj[i,] <- cbind(sign(XY0[i,1])*xproj, sign(XY0[i,2])*b*sqrt(1-(xproj/a)^2))
} else {
XYproj[i,] <- cbind(sign(XY0[i,1])*a, 0)
}
next
}
# generic case: start the iterative procedure
T <- Tini[i]
for (iter in 1:100){
Taa <- T + aa
Tbb <- T + bb
PP1 <- (ua/Taa)^2
PP2 <- (vb/Tbb)^2
Fp <- PP1 + PP2 - 1
if (Fp<0) break
Fder <- 2*(PP1/Taa + PP2/Tbb)
Ratio <- Fp/Fder
if (Ratio<tol_aa) break
T <- T + Ratio
}
# compute the projection of the point onto the ellipse
XYproj[i,] <- cbind(XY0[i,1]*aa/Taa, XY0[i,2]*bb/Tbb)
}
XYproj <- XYproj %*% t(Q)
XYproj <- cbind(XYproj[,1]+Center[1], XYproj[,2]+Center[2])
RSS <- norm(XY-XYproj,'F')^2
list(RSS=RSS, XYproj=XYproj)
} # ResidualsG
fit.ellipseLMG <- function(XY,ParGini,LambdaIni=1,epsilon=0.000001,IterMAX = 200,L = 200)
{# fitting ellipse using Implicit method
#epsilon <- tolerance (small threshold)
#IterMAX <- maximal number of (main) iterations usually 10-20 suffice
#L <- boundary for major/minor axis
lambda.sqrt <- sqrt(LambdaIni)# sqrt(Lambda) is actually used by the code
TF <- FALSE
tmp <- ResidualsG(XY,ParGini)
Fp <- tmp$RSS
XYproj <- tmp$XYproj
tmp <- JmatrixLMG(XY,ParGini,XYproj)
Res <- tmp$Res
J <- tmp$J
ParG <- ParGini
for (iter in 1:IterMAX) # main loop, each run is one (main) iteration
{
while (TRUE) # secondary loop - adjusting Lambda (no limit on cycles)
{
DelPar <- mldivide(rbind(J, lambda.sqrt*diag(5)), rbind(-Res, matrix(0,5,1))) # step candidate
progress <- norm(DelPar,'F')/(norm(ParG,'F')+epsilon)
if (progress < epsilon) break # stopping rule
ParTemp <- ParG + DelPar
if (ParTemp[3]< ParTemp[4]){ # out of range
Temp=ParTemp[3]
ParTemp[3]=ParTemp[4]
ParTemp[4]=Temp
ParTemp[5]=ParTemp[5]-sign(ParTemp[5])*pi/2
}
tmp <- ResidualsG(XY,ParTemp)
FTemp <- tmp$RSS
XYprojTemp <- tmp$XYproj
if (FTemp < Fp && ParTemp[3]>0 && ParTemp[4]>0) { # yes, improvement
lambda.sqrt <- lambda.sqrt/2 # reduce lambda, move to next iteration
break
} else { # no improvement
lambda.sqrt <- lambda.sqrt*2 # increase lambda, recompute the step
next
}
} # while TRUE, the end of the secondary loop
if (ParTemp[3] > L){ # diverge
TF <- TRUE
break
}
if (progress < epsilon) break
tmp <- JmatrixLMG(XY,ParTemp,XYprojTemp)
Res <- tmp$Res
J <- tmp$J
ParG <- ParTemp
Fp <- FTemp # update the iteration
}
RSS <- Fp
iters <- iter
# make the angle parameter between 0 and pi
while(ParG[5] >= pi) ParG[5] <- ParG[5] - pi
while(ParG[5] < 0) ParG[5] <- ParG[5] + pi
if (iters >= IterMAX) TF <- TRUE
list(ParG=ParG,RSS=RSS,iters=iters,TF=TF)
} #fit.ellipseLMG
fit.ellipseLMG.H <- function(XY,ParGini,LambdaIni=1,epsilon=0.000001,IterMAX = 200,L = 200)
{# fitting ellipse using Implicit method
#epsilon <- tolerance (small threshold)
#IterMAX <- maximal number of (main) iterations usually 10-20 suffice
#L <- boundary for major/minor axis
lambda.sqrt <- sqrt(LambdaIni)# sqrt(Lambda) is actually used by the code
TF <- FALSE
tmp <- ResidualsG(XY,ParGini)
Fp <- tmp$RSS
XYproj <- tmp$XYproj
tmp <- JmatrixLMG(XY,ParGini,XYproj)
Res <- tmp$Res
J <- tmp$J
ParG <- ParGini
for (iter in 1:IterMAX) # main loop, each run is one (main) iteration
{
while (TRUE) # secondary loop - adjusting Lambda (no limit on cycles)
{
DelPar <- mldivide(rbind(J, lambda.sqrt*diag(5)), rbind(-Res, matrix(0,5,1))) # step candidate
progress <- norm(DelPar,'F')/(norm(ParG,'F')+epsilon)
if (progress < epsilon) break # stopping rule
ParTemp <- ParG + DelPar
if (ParTemp[3]< ParTemp[4]){ # out of range
Temp=ParTemp[3]
ParTemp[3]=ParTemp[4]
ParTemp[4]=Temp
ParTemp[5]=ParTemp[5]-sign(ParTemp[5])*pi/2
}
tmp <- ResidualsG(XY,ParTemp)
FTemp <- tmp$RSS
XYprojTemp <- tmp$XYproj
if (FTemp < Fp && ParTemp[3]>0 && ParTemp[4]>0) { # yes, improvement
lambda.sqrt <- lambda.sqrt/2 # reduce lambda, move to next iteration
break
} else { # no improvement
lambda.sqrt <- lambda.sqrt*2 # increase lambda, recompute the step
next
}
} # while TRUE, the end of the secondary loop
if (ParTemp[3] > L){ # diverge
TF <- TRUE
break
}
if (progress < epsilon) break
tmp <- JmatrixLMG(XY,ParTemp,XYprojTemp)
Res <- tmp$Res
J <- tmp$J
ParG <- ParTemp
Fp <- FTemp # update the iteration
}
RSS <- Fp
iters <- iter
Jg <- t(J) %*% Res
H <- t(J) %*% J
# make the angle parameter between 0 and pi
while(ParG[5] >= pi) ParG[5] <- ParG[5] - pi
while(ParG[5] < 0) ParG[5] <- ParG[5] + pi
if (iters >= IterMAX) TF <- TRUE
list(ParG=ParG,RSS=RSS,iters=iters,TF=TF,Jg=Jg,H=H)
} #fit.ellipseLMG.H
JmatrixLMG <- function(XY,A,XYproj){
#Implicit method
n <- dim(XY)[1]
Res <- matrix(0,n,1)
s <- sin(A[5])
C <- cos(A[5])
ss <- s*s
cc <- C*C
cs <- C*s
a <- A[3]
b <- A[4]
aa <- a^2
bb <- b^2
ba <- bb-aa
DD <- XY-XYproj
D1 <- cbind(XYproj[,1]-A[1], XYproj[,2]-A[2])
D2 <- cbind(D1[,2], D1[,1])
for (i in 1:n) Res[i] <- sign(DD[i,] %*% matrix(D1[i,],2)) * norm(matrix(DD[i,]),'F')
du <- D1 %*% rbind(bb*cc+aa*ss, cs*ba)
dv <- D2 %*% rbind(bb*ss+aa*cc, cs*ba)
D3 <- cbind(D1[,1]^2, D1[,2]^2, D1[,1]*D1[,2],matrix(1,n,1))
d3 <- D3 %*% rbind(ss,cc,-2*cs,-bb) * a
d4 <- D3 %*% rbind(cc,ss,2*cs,-aa)*b
d5 <- D3 %*% rbind(-ba*cs,ba*cs,ba*(cc-ss),0)
e <- sqrt(du^2+dv^2)
J <- cbind(-du/e, -dv/e, d3/e, d4/e, d5/e)
list(Res=Res,J=J)
}
CircleFitByLandau<-function(XY,ParIni=NA,epsilon=0.000001,IterMAX = 50)
{
if (length(ParIni) != 3) ParIni <- NA
if (!is.numeric(ParIni)) ParIni <- NA
if (any(is.na(ParIni))) ParIni <- estimateInitialGuessCircle(XY)
centroid <- apply(XY,2,mean)
X <- XY[,1] - centroid[1]
Y <- XY[,2] - centroid[2]
# centering the initial guess
ParNew <- matrix(c(ParIni - c(centroid,0)),1)
for (iter in 1:IterMAX) # main loop, each run is one iteration
{
ParOld <- ParNew
Dx <- X - ParOld[1]
Dy <- Y - ParOld[2]
D <- sqrt(Dx * Dx + Dy * Dy)
ParNew <- cbind(-mean(Dx/D) , -mean(Dy/D) , 1) * mean(D)
progress <- (norm(ParNew-ParOld,'F'))/(norm(ParOld,'F')+epsilon)
if (progress < epsilon) break # stopping rule
} # the end of the main loop (over iterations)
# decentering the parameter vector for output
Par <- ParOld + c(centroid,0)
Par
}
CircleFitBySpath<-function(XY,ParIni=NA,epsilon=0.000001,IterMAX = 50)
{
if (length(ParIni) != 3) ParIni <- NA
if (!is.numeric(ParIni)) ParIni <- NA
if (any(is.na(ParIni))) ParIni <- estimateInitialGuessCircle(XY)
centroid <- apply(XY,2,mean)
X <- XY[,1] - centroid[1]
Y <- XY[,2] - centroid[2]
# centering the initial guess
ParNew <- matrix(c(ParIni - c(centroid,0)),1)
for (iter in 1:IterMAX) # main loop, each run is one iteration
{
ParOld <- ParNew
Dx <- X - ParOld[1]
Dy <- Y - ParOld[2]
D <- sqrt(Dx * Dx + Dy * Dy)
Mu <- mean(Dx/D)
Mv <- mean(Dy/D)
Mr <- mean(D)
Radius <- (Mu * ParOld[1] + Mv * ParOld[2] + Mr)/(1.0 - Mu * Mu - Mv * Mv)
ParNew <- cbind(-Mu , -Mv , 1) * Radius
progress <- (norm(ParNew-ParOld,'F'))/(norm(ParOld,'F')+epsilon)
if (progress < epsilon) break # stopping rule
} # the end of the main loop (over iterations)
# decentering the parameter vector for output
Par <- ParOld + c(centroid,0)
Par
}
estimateInitialGuessCircle <- function(XY)
{# estimate initial guess for circle LM
x0 <- mean(XY[,1])
y0 <- mean(XY[,2])
c(x0,y0,mean(sqrt((XY[,1]^2+x0^2)+(XY[,2]^2+y0^2))))
}
CurrentIteration<-function(Par,XY){
# computes the objective function F and its derivatives at the current point Par
Dx = matrix(XY[,1] - Par[1],ncol=1)
Dy = matrix(XY[,2] - Par[2],ncol=1)
D = sqrt(Dx^2 + Dy^2)
J = cbind(-Dx / D, -Dy / D, -1 )
g = matrix(D - Par[3],ncol=1)
F = sqrt(sum(g^2))^2
Radius = mean(D)
list(J=J,g=g,F=F,Radius = Radius)
}
LMcircleFit<-function(XY,ParIni=NA,LambdaIni=1,epsilon=0.000001,IterMAX = 50){
#epsilon = tolerance (small threshold)
#IterMAX = maximal number of (main) iterations; usually 10-20 suffice
if (length(ParIni) != 3) ParIni <- NA
if (!is.numeric(ParIni)) ParIni <- NA
if (is.na(ParIni)) ParIni <- estimateInitialGuessCircle(XY)
lambda_sqrt = sqrt(LambdaIni)# sqrt(Lambda) is actually used by the code
Par = ParIni# starting with the given initial guess
Jtmp = CurrentIteration(Par,XY)# compute objective function and its derivatives
J <- Jtmp$J
g <- Jtmp$g
F <- Jtmp$F
for (iter in 1:IterMAX){ # main loop, each run is one (main) iteration
while (TRUE){ # secondary loop - adjusting Lambda (no limit on cycles)
DelPar = mldivide(rbind(J, lambda_sqrt * diag(3)), rbind(g, 0,0,0)) # step candidate
progress = sqrt(sum(DelPar^2)) / (sqrt(sum(Par^2))+epsilon)
if (progress < epsilon) break # stopping rule
ParTemp = Par - t(DelPar)
Jtmp = CurrentIteration(ParTemp,XY); # objective function + derivatives
JTemp<- Jtmp$J
gTemp<- Jtmp$g
FTemp<- Jtmp$F
if (FTemp < F && ParTemp[3]>0){ # yes, improvement
lambda_sqrt = lambda_sqrt/2 # reduce lambda, move to next iteration
break
} else { # no improvement
lambda_sqrt = lambda_sqrt*2 # increase lambda, recompute the step
next
}
} # while (1), the end of the secondary loop
if (progress < epsilon) break # stopping rule
Par = ParTemp; J = JTemp; g = gTemp; F = FTemp # update the iteration
}
Par
}
CurrentIterationReduced<-function(Par,XY){
# computes the objective function F and its derivatives at the current point Par
Dx = matrix(XY[,1] - Par[1],ncol=1)
Dy = matrix(XY[,2] - Par[2],ncol=1)
D = sqrt(Dx^2 + Dy^2)
J = cbind(-Dx + mean(Dx), -Dy + mean(Dy) )
Radius = mean(D)
g = matrix(D - Radius,ncol=1)
F = sqrt(sum(g^2))^2
list(J=J,g=g,F=F,Radius = Radius)
}
LMreducedCircleFit<-function(XY,ParIni=NA,LambdaIni=1,epsilon=0.000001,IterMAX = 50){
#epsilon = tolerance (small threshold)
#IterMAX = maximal number of (main) iterations; usually 10-20 suffice
if (length(ParIni) != 3) ParIni <- NA
if (!is.numeric(ParIni)) ParIni <- NA
if (any(is.na(ParIni))) ParIni <- estimateInitialGuessCircle(XY)
lambda_sqrt = sqrt(LambdaIni)# sqrt(Lambda) is actually used by the code
Par = ParIni[1:2]# starting with the given initial guess
Jtmp = CurrentIterationReduced(Par,XY)# compute objective function and its derivatives
J <- Jtmp$J[,1:2]
g <- Jtmp$g
F <- Jtmp$F
Radius <- Jtmp$Radius
for (iter in 1:IterMAX){ # main loop, each run is one (main) iteration
while (TRUE){ # secondary loop - adjusting Lambda (no limit on cycles)
DelPar = mldivide(rbind(J, lambda_sqrt * diag(2)), rbind(g, 0,0)) # step candidate
progress = norm(matrix(DelPar,1),'F')/(Radius+norm(matrix(Par,1),'F')+epsilon)
if (progress < epsilon) break # stopping rule
ParTemp = Par - t(DelPar)
Jtmp = CurrentIterationReduced(ParTemp,XY) # objective function + derivatives
JTemp<- Jtmp$J
gTemp<- Jtmp$g
FTemp<- Jtmp$F
RadiusTemp<- Jtmp$Radius
if (FTemp < F){ # yes, improvement
lambda_sqrt = lambda_sqrt/2 # reduce lambda, move to next iteration
break
} else { # no improvement
lambda_sqrt = lambda_sqrt*2 # increase lambda, recompute the step
next
}
} # while (1), the end of the secondary loop
if (progress < epsilon) break # stopping rule
Par = ParTemp; J = JTemp; g = gTemp; F = FTemp; Radius = RadiusTemp # update the iteration
}
c(Par, Radius) # assembling the full parameter vector "Par" for output
}
calculateCircle<-function(x, y, r, steps=50,sector=c(0,360),randomDist=FALSE, randomFun=runif,noiseFun=NA,...)
{
points = matrix(0,steps,2)
if (randomDist) n<-sector[1]+randomFun(steps,...)*(sector[2]-sector[1]) else n<-seq(sector[1],sector[2],length.out=steps)
if (randomDist) repeat {
n[which(!(n>=sector[1] & n<=sector[2]))]<-sector[1]+randomFun(sum(!(n>=sector[1] & n<=sector[2])),...)*(sector[2]-sector[1])
if (all(n>=sector[1] & n<=sector[2])) break
}
alpha = n * (pi / 180)
sinalpha = sin(alpha)
cosalpha = cos(alpha)
points[,1]<- x + (r * cosalpha)
points[,2]<- y + (r * sinalpha)
if (is.function(noiseFun)) points<-apply(points,1:2,noiseFun)
points
}
calculateEllipse<-function(x, y, a, b, angle=0, steps=50,sector=c(0,360),randomDist=FALSE, randomFun=runif,noiseFun=NA,...)
{# http://en.wikipedia.org/w/index.php?title=Ellipse&oldid=456212176#Ellipses_in_computer_graphics
points = matrix(0,steps,2)
beta = angle * (pi / 180)
sinbeta = sin(beta)
cosbeta = cos(beta)
if (randomDist) n<-sector[1]+randomFun(steps,...)*(sector[2]-sector[1]) else n<-seq(sector[1],sector[2],length.out=steps)
if (randomDist) repeat {
n[which(!(n>=sector[1] & n<=sector[2]))]<-sector[1]+randomFun(sum(!(n>=sector[1] & n<=sector[2])),...)*(sector[2]-sector[1])
if (all(n>=sector[1] & n<=sector[2])) break
}
alpha = n * (pi / 180)
sinalpha = sin(alpha)
cosalpha = cos(alpha)
points[,1]<- x + (a * cosalpha * cosbeta - b * sinalpha * sinbeta)
points[,2]<- y + (a * cosalpha * sinbeta + b * sinalpha * cosbeta)
if (is.function(noiseFun)) points<-apply(points,1:2,noiseFun)
points
}
EllipseDirectFit<-function(XY){
# translated to R by Jose Gama 2014
# Original code by: Nikolai Chernov http://www.mathworks.com/matlabcentral/fileexchange/22684-ellipse-fit-direct-method
# A. W. Fitzgibbon, M. Pilu, R. B. Fisher, "Direct Least Squares Fitting of Ellipses", IEEE Trans. PAMI, Vol. 21, pages 476-480 (1999)
# Halir R, Flusser J (1998) Proceedings of the 6th International Conference in Central Europe on Computer Graphics and Visualization,
# Numerically stable direct least squares fitting of ellipses (WSCG, Plzen, Czech Republic), pp 125–132.
centroid <- apply(XY,2,mean)
D1 <- cbind((XY[,1]-centroid[1])^2, (XY[,1]-centroid[1])*(XY[,2]-centroid[2]), (XY[,2]-centroid[2])^2)
D2 <- cbind(XY[,1]-centroid[1], XY[,2]-centroid[2], matrix(1,dim(XY)[1]))
S1 <- t(D1) %*% D1
S2 <- t(D1) %*% D2
S3 <- t(D2) %*% D2
T <- -solve(S3) %*% t(S2)
M <- S1 + S2 %*% T
M <- rbind(M[3,]/2, -M[2,], M[1,]/2)
evec<-eigen(M)$vectors
cond <- 4*evec[1,]*evec[3,]-evec[2,]^2
A1 <- matrix(evec[,which(cond>0)[1]],3)
A <- rbind(A1, T %*% A1)
A4 <- A[4]-2*A[1]*centroid[1]-A[2]*centroid[2]
A5 <- A[5]-2*A[3]*centroid[2]-A[2]*centroid[1]
A6 <- A[6]+A[1]*centroid[1]^2+A[3]*centroid[2]^2+ A[2]*centroid[1]*centroid[2]-A[4]*centroid[1]-A[5]*centroid[2]
A[4] <- A4; A[5] <- A5; A[6] <- A6
A <- A / norm(A,'F')
# # general-form conic equation ax^2 + bxy + cy^2 +dx + ey + f = 0
# a <- A[1];b <- A[2]/2;C <- A[3];d <- A[4]/2;E <- A[5]/2;f <- A[6]
# x0 <- (C*d-b*E)/(b*b-a*C)
# y0 <- (a*E-b*d)/(b*b-a*C)
# semiaxis.a <- sqrt(2*(a*E^2+C*d^2+f*b^2-2*b*d*E-a*C*f)/((b^2-a*C)*(sqrt((a-C)^2+4*b^2)-(a+C))))
# semiaxis.b <- sqrt(2*(a*E^2+C*d^2+f*b^2-2*b*d*E-a*C*f)/((b^2-a*C)*(-sqrt((a-C)^2+4*b^2)-(a+C))))
# #, x0=x0,y0=y0,semiaxis.a=semiaxis.a,semiaxis.b=semiaxis.b
A
}
EllipseFitByTaubin<-function(XY){
# translated to R by Jose Gama 2014
# Original code by: Nikolai Chernov
centroid <- apply(XY,2,mean)
Z <- cbind((XY[,1]-centroid[1])^2, (XY[,1]-centroid[1])*(XY[,2]-centroid[2]), (XY[,2]-centroid[2])^2, XY[,1]-centroid[1], XY[,2]-centroid[2], matrix(1,dim(XY)[1]))
M <- t(Z) %*% Z/dim(XY)[1]
P <- rbind(cbind(M[1,1]-M[1,6]^2, M[1,2]-M[1,6]*M[2,6], M[1,3]-M[1,6]*M[3,6], M[1,4], M[1,5]),
cbind(M[1,2]-M[1,6]*M[2,6], M[2,2]-M[2,6]^2, M[2,3]-M[2,6]*M[3,6], M[2,4], M[2,5]),
cbind(M[1,3]-M[1,6]*M[3,6], M[2,3]-M[2,6]*M[3,6], M[3,3]-M[3,6]^2, M[3,4], M[3,5]),
cbind(M[1,4], M[2,4], M[3,4], M[4,4], M[4,5]),
cbind(M[1,5], M[2,5], M[3,5], M[4,5], M[5,5]))
Q <- rbind(cbind(4*M[1,6], 2*M[2,6], 0, 0, 0),
cbind(2*M[2,6], M[1,6]+M[3,6], 2*M[2,6], 0, 0),
cbind(0, 2*M[2,6], 4*M[3,6], 0, 0),
cbind(0, 0, 0, 1, 0),
cbind(0, 0, 0, 0, 1))
V2 <- geigen(P,Q)
V <- V2$vectors
diagD <- V2$values
Dsort <- matrix(sort(diagD),ncol=1)
ID <- matrix(order(diagD),ncol=1)
A <- matrix(V[,ID[1]],ncol=1)
A <- rbind(A, -t(A[1:3]) %*% M[1:3,6])
A4 <- A[4]-2*A[1]*centroid[1]-A[2]*centroid[2]
A5 <- A[5]-2*A[3]*centroid[2]-A[2]*centroid[1]
A6 <- A[6]+A[1]*centroid[1]^2+A[3]*centroid[2]^2+ A[2]*centroid[1]*centroid[2]-A[4]*centroid[1]-A[5]*centroid[2]
A[4] <- A4; A[5] <- A5; A[6] <- A6
A <- A / norm(A,'F')
A
}
CircleFitByTaubin<-function(XY){
# translated to R by Jose Gama 2014
# Original code by: Nikolai Chernov
n <- dim(XY)[1] # number of data points
centroid <- apply(XY,2,mean)
Mxx <- 0; Myy <- 0; Mxy <- 0; Mxz <- 0; Myz <- 0; Mzz <- 0
for (x in 1:n) {
Xi <- XY[x,1] - centroid[1]
Yi <- XY[x,2] - centroid[2]
Zi <- Xi*Xi + Yi*Yi
Mxy <- Mxy + Xi*Yi
Mxx <- Mxx + Xi*Xi
Myy <- Myy + Yi*Yi
Mxz <- Mxz + Xi*Zi
Myz <- Myz + Yi*Zi
Mzz <- Mzz + Zi*Zi }
Mxx <- Mxx/n
Myy <- Myy/n
Mxy <- Mxy/n
Mxz <- Mxz/n
Myz <- Myz/n
Mzz <- Mzz/n
Mz <- Mxx + Myy
Cov_xy <- Mxx * Myy - Mxy * Mxy
A3 <- 4 * Mz
A2 <- -3 * Mz * Mz - Mzz
A1 <- Mzz * Mz + 4 * Cov_xy * Mz - Mxz * Mxz - Myz * Myz - Mz * Mz * Mz
A0 <- Mxz * Mxz * Myy + Myz * Myz * Mxx - Mzz * Cov_xy - 2 * Mxz * Myz * Mxy + Mz * Mz * Cov_xy
A22 <- A2 + A2
A33 <- A3 + A3 + A3
xnew <- 0
ynew <- 1e+20
epsilon <- 1e-12
IterMax <- 20
for (iter in 1:IterMax){
yold <- ynew
ynew <- A0 + xnew*(A1 + xnew*(A2 + xnew*A3))
if (abs(ynew) > abs(yold)) {
cat('Newton-Taubin goes wrong direction: |ynew| > |yold|\n')
xnew <- 0
break
}
Dy <- A1 + xnew*(A22 + xnew*A33)
xold <- xnew
xnew <- xold - ynew/Dy
if (abs((xnew-xold)/xnew) < epsilon) break
if (iter >= IterMax){
cat('Newton-Taubin will not converge\n')
xnew <- 0
}
if (xnew<0){
cat(1,'Newton-Taubin negative root: x=%f\n',xnew,'\n')
xnew <- 0
}
}
DET <- xnew*xnew - xnew*Mz + Cov_xy
Center <- cbind(Mxz*(Myy-xnew)-Myz*Mxy , Myz*(Mxx-xnew)-Mxz*Mxy)/DET/2
P <- cbind(Center+centroid , sqrt(Center %*% t(Center)+Mz))
P
}
CircleFitByPratt<-function(XY){
# translated to R by Jose Gama 2014
# Original code by: Nikolai Chernov
n <- dim(XY)[1] # number of data points
centroid <- apply(XY,2,mean)
Mxx <- 0; Myy <- 0; Mxy <- 0; Mxz <- 0; Myz <- 0; Mzz <- 0
for (x in 1:n) {
Xi <- XY[x,1] - centroid[1]
Yi <- XY[x,2] - centroid[2]
Zi <- Xi*Xi + Yi*Yi
Mxy <- Mxy + Xi*Yi
Mxx <- Mxx + Xi*Xi
Myy <- Myy + Yi*Yi
Mxz <- Mxz + Xi*Zi
Myz <- Myz + Yi*Zi
Mzz <- Mzz + Zi*Zi }
Mxx <- Mxx/n
Myy <- Myy/n
Mxy <- Mxy/n
Mxz <- Mxz/n
Myz <- Myz/n
Mzz <- Mzz/n
Mz <- Mxx + Myy
Cov_xy <- Mxx * Myy - Mxy * Mxy
Mxz2 <- Mxz * Mxz
Myz2 <- Myz * Myz
A2 <- 4 * Cov_xy - 3 * Mz * Mz - Mzz
A1 <- Mzz * Mz + 4 * Cov_xy * Mz - Mxz2 - Myz2 - Mz * Mz * Mz
A0 <- Mxz2 * Myy + Myz2 * Mxx - Mzz * Cov_xy - 2 * Mxz * Myz * Mxy + Mz * Mz * Cov_xy
A22 <- A2 + A2
epsilon <- 1e-12
ynew <- 1e+20
IterMax <- 20
xnew <- 0
for (iter in 1:IterMax){
yold <- ynew
ynew <- A0 + xnew*(A1 + xnew*(A2 + xnew*xnew*4))
if (abs(ynew) > abs(yold)) {
cat('Newton-Pratt goes wrong direction: |ynew| > |yold|\n')
xnew <- 0
break
}
Dy <- A1 + xnew*(A22 + 16*xnew*xnew)
xold <- xnew
xnew <- xold - ynew/Dy
if (abs((xnew-xold)/xnew) < epsilon) break
if (iter >= IterMax){
cat('Newton-Pratt will not converge\n')
xnew <- 0
}
if (xnew<0){
cat(1,'Newton-Pratt negative root: x=%f\n',xnew,'\n')
xnew <- 0
}
}
DET <- xnew*xnew - xnew*Mz + Cov_xy
Center <- cbind(Mxz*(Myy-xnew)-Myz*Mxy , Myz*(Mxx-xnew)-Mxz*Mxy)/DET/2
P <- cbind(Center+centroid , sqrt(Center %*% t(Center)+Mz+2*xnew))
P
}
CircleFitByKasa<-function(XY){
# translated to R by Jose Gama 2014
# Original code by: Nikolai Chernov
P <- mldivide(cbind(XY,1), matrix(XY[,1]^2 + XY[,2]^2,ncol=1))
Pout = cbind(P[1]/2 , P[2]/2 , sqrt((P[1]^2+P[2]^2)/4+P[3]))
Pout
}
# http://en.wikipedia.org/wiki/Ellipse#Mathematical_definitions_and_properties
ellipticity <- function(minorAxis,majorAxis) 1 - (minorAxis/majorAxis) # ellipticity = flattening factor
ellipseEccentricity <- function(minorAxis,majorAxis) (sqrt (1 - (minorAxis/majorAxis)^2)) # eccentricity of the ellipse
ellipseFocus <- function(minorAxis,majorAxis) sqrt(minorAxis-majorAxis)^2 # focus of the ellipse
ellipseRa <- function(minorAxis,majorAxis) (1+ellipseEccentricity(minorAxis,majorAxis))*majorAxis # radius at apoapsis (the farthest distance)
ellipseRp <- function(minorAxis,majorAxis) (1-ellipseEccentricity(minorAxis,majorAxis))*majorAxis # radius at periapsis (the closest distance)
ellipse.l <- function(minorAxis,majorAxis)
{# semi-latus rectum l
ra <- ellipseRa(minorAxis,majorAxis)
rp <- ellipseRp(minorAxis,majorAxis)
2*ra*rp/(ra+rp)
}
conic2parametric<-function(A, bv, cv){
# Diagonalise A - find Q, D such at A = Q' * D * Q
# Copyright Richard Brown, this code can be freely used and modified so
# long as this line is retained
# FITELLIPSE : Least squares ellipse fitting demonstration
# Richard Brown, May 28, 2007
# http://www.mathworks.com/matlabcentral/fileexchange/15125-fitellipse-m/content/demo/html/ellipsedemo.html
eTMP<-eigen(A)
D<-diag(eTMP$values)
Q<-eTMP$vectors
Q<-t(Q)
# If the determinant < 0, it's not an ellipse
if (prod(diag(D)) <= 0 ) stop('Linear fit did not produce an ellipse')
# We have b_h' = 2 * t' * A + b'
tV = -0.5 * mldivide(A , bv)
c_h = matrix(tV,1) %*% A %*% tV + t(bv) %*% tV + cv
list(z = tV,a = sqrt(-c_h / D[1,1]),b = sqrt(-c_h / D[2,2]),alpha = atan2(Q[1,2], Q[1,1]))
}
fitbookstein<-function(x){
#FITBOOKSTEIN Linear ellipse fit using bookstein constraint
# lambda_1^2 + lambda_2^2 = 1, where lambda_i are the eigenvalues of A
# Copyright Richard Brown, this code can be freely used and modified so
# long as this line is retained
# FITELLIPSE : Least squares ellipse fitting demonstration
# Richard Brown, May 28, 2007
# http://www.mathworks.com/matlabcentral/fileexchange/15125-fitellipse-m/content/demo/html/ellipsedemo.html
# W. Gander, G. H. Golub, R. Strebel, 1994
# Least-Squares Fitting of Circles and Ellipses
# BIT Numerical Mathematics, Springer
# Convenience variables
m = dim(x)[1]
x1 = x[, 1]
x2 = x[, 2]
# Define the coefficient matrix B, such that we solve the system
# B *[v; w] = 0, with the constraint norm(w) == 1
B = cbind(x1, x2, rep(1,m), x1^2, sqrt(2) * x1 * x2, x2^2)
# To enforce the constraint, we need to take the QR decomposition
qTMP<-qr(B)
R<-qr.R(qTMP)
Q<-qr.Q(qTMP)
# Decompose R into blocks
R11 = R[1:3, 1:3]
R12 = R[1:3, 4:6]
R22 = R[4:6, 4:6]
# Solve R22 * w = 0 subject to norm(w) == 1
svdTMP<-svd(R22)
U<-svdTMP[["u"]]
S<-diag(svdTMP[["d"]])
V<-svdTMP[["v"]]
w = matrix(V[, 3],3,1)
# Solve for the remaining variables
v = mldivide(-R11 , R12) %*% w
# Fill in the quadratic form
A = matrix(0,2,2)
A[1] = w[1]
A[2:3] = 1 / sqrt(2) * w[2]
A[4] = w[3]
bv = v[1:2]
c1 = v[3]
# Find the parameters
cTMP<-conic2parametric(A, bv, c1)
list(z=cTMP$z, a=cTMP$a, b=cTMP$b, alpha=cTMP$alpha)
}
fitggk<-function(x){
# Linear least squares with the Euclidean-invariant constraint Trace(A) = 1
# Copyright Richard Brown, this code can be freely used and modified so
# long as this line is retained
# FITELLIPSE : Least squares ellipse fitting demonstration
# Richard Brown, May 28, 2007
# http://www.mathworks.com/matlabcentral/fileexchange/15125-fitellipse-m/content/demo/html/ellipsedemo.html
# W. Gander, G. H. Golub, R. Strebel, 1994
# Least-Squares Fitting of Circles and Ellipses
# BIT Numerical Mathematics, Springer
# Convenience variables
m = dim(x)[1]
x1 = x[, 1]
x2 = x[, 2]
# Coefficient matrix
B = cbind(2 * x1 * x2, x2^2 - x1^2, x1, x2, rep(1,m))
v = mldivide(B , -x1^2)
# For clarity, fill in the quadratic form variables
A = matrix(0,2,2)
A[1] = 1-v[2]
A[2:3] = v[1]
A[2,2] = v[2]
bv = v[3:4]
c1 = v[5]
# Find the parameters
cTMP<-conic2parametric(A, bv, c1)
list(z=cTMP$z, a=cTMP$a, b=cTMP$b, alpha=cTMP$alpha)
}
Try the conicfit package in your browser
Any scripts or data that you put into this service are public.
R Package Documentation
Browse R Packages
We want your feedback!
Note that we can't provide technical support on individual packages. You should contact the package authors for that.
Embedding an R snippet on your website
Add the following code to your website.
For more information on customizing the embed code, read Embedding Snippets.