How to fit a circle with a given radius to sample data points

Question

I have measurement points from a laser scanner coming from the inside of a tube. Now I want to fit a circle with the known radius of the tube. In principle similar to this post. But I work with R and look for a solution in R.

The data comes from a LIDAR scanner and consists of angle information and distance. I measure the inside of a tube of which I know the diameter. I need the position in the tube, which is the center of the circle. With the function fitCircle based on qr.solve I can fit a circle to the data. However, the calculated diameter differs from the actual diameter for each measurement. This is due to the measurement inaccuracy of the sensor and small interfering contours in the tube. I would now like to fit a circle with a given radius to the data to increase the accuracy.

I know the calculation can be time consuming, but I want to test how much the accuracy improves.

This picture shows my results for a fit without a diameter restriction:

# fitCircle, returns:
#   xf,yf = centre of the fitted circle
#   Rf = radius of the fitted circle
# based on:
#   x, y = raW datapoints
fitCircle <- function(x,y){
  a = qr.solve(cbind(x,y,rep(1,length(x))),cbind(-(x^2+y^2)))
  xf = -.5*a[1]
  yf = -.5*a[2]
  Rf  =  sqrt((a[1]^2+a[2]^2)/4-a[3])
  m <- cbind(xf,yf,Rf)
  return(m)
}

My example is based on this data:

# some sample data from the LIDAR scanner
LIDARData <- read.table(header = TRUE, text = "
angle distance
0.1094 460.0
1.2344 460.0
2.4844 463.0
3.9844 463.0
5.2188 463.0
15.1719 460.0
16.4219 458.0
17.4063 462.0
18.9063 463.0
43.7656 463.0
44.9063 461.0
46.4063 460.0
47.5313 460.0
48.6719 461.0
55.0625 458.0
56.5781 459.0
57.4531 459.0
58.8438 458.0
60.0938 458.0
160.7656 435.0
162.0156 435.0
163.6406 433.0
168.2656 435.0
169.5000 436.0
170.8750 435.0
172.1250 435.0
173.6250 436.0
174.6250 436.0
176.1250 435.0
177.1250 421.0
178.7500 411.0
179.9844 419.0
180.7344 434.0
182.3594 429.0
183.3594 431.0
184.8594 432.0
185.8594 433.0
187.3594 435.0
188.6094 435.0
189.5938 435.0
191.0938 437.0
195.9531 438.0
196.9531 438.0
198.4531 436.0
")

Here are the calculation steps:

# calculateCircle, returns:
#   xy = 2000 datapoints of a circle 
# based on:
#   x,y = centre of the circle
#   r = radius of the circle
calculateCircle <- function(x,y,r){
  x1 = (((0:2000)/1000)-1)
  x2 = (-1)*x1
  y1 = sqrt(1 - x1^2)
  y2 = (-1)*y1
  xr = ((c(x1,x2))*r)+x
  yr = ((c(y1,y2))*r)+y
  xy <- cbind(xr,yr)
  return(xy)
}

# calculate x, y coordinates based on angle and distance
LIDAR_x <- cos((LIDARData[[1]]*pi)/180)*LIDARData[[2]]
LIDAR_y <- sin((LIDARData[[1]]*pi)/180)*LIDARData[[2]]

plot(LIDAR_x,LIDAR_y,xlim=c(-500,500),ylim=c(-500,500), xlab="", ylab="", cex = 1.5, main="fit circle to raw data");par(new=TRUE)

# fit a circle to the data
fitdata <- fitCircle(LIDAR_x,LIDAR_y)

# calculate circle points to plot the caclutated circle
circledata <- calculateCircle(fitdata[1],fitdata[2],fitdata[3])
plot(circledata[,1],circledata[,2],xlim=c(-500,500),ylim=c(-500,500), xlab="", ylab="",col='red',type='l')

# draw the center point
points(fitdata[1],fitdata[2],pch=4,col="red",cex = 2);par(new=TRUE)
text(fitdata[1]+30,fitdata[2], sprintf("x=%.1fmm, y=%.1fmm", fitdata[1],fitdata[2]), adj = c(0,0), cex=1.0, col="red")

Many thanks for your help!

Answer 1

This code below works using a nested for. A little hacky but it should do the job.

# Convert data points to xy
LIDARData$x <- LIDARData$distance*cos(LIDARData$angle)
LIDARData$y <- LIDARData$distance*sin(LIDARData$angle)
plot(LIDARData$x, LIDARData$y)

# Create an equivalent theoretical circle using the average radius
circle_center_x = 0
circle_center_y = 0
circle_radius = mean(LIDARData$distance)
LIDARData$circle_x <- circle_radius*cos(LIDARData$angle) + circle_center_x
LIDARData$circle_y <- circle_radius*sin(LIDARData$angle) + circle_center_y
points(LIDARData$circle_x , LIDARData$circle_y, col = "red")
library(plotrix)
draw.circle(0,0,circle_radius, border="red")

# create dataframe to hold results
results <- data.frame(circle_center_x = c(NA),circle_center_y = c(NA),dist_sum = c(NA))

# Vary the center point ±50 and calculate distances from red point to black point.  
# Where the sum of those distances are minimized, we have found our ideal center point.
for (i in c(-50:50)){
for (j in c(-50:50)){
    print(paste(i, ":", j))
    circle_center_x = 0 + i
    circle_center_y = 0 + j
    LIDARData$circle_x <- circle_radius*cos(LIDARData$angle) + circle_center_x
    LIDARData$circle_y <- circle_radius*sin(LIDARData$angle) + circle_center_y    
    LIDARData$dist_to_circle <- sqrt( (LIDARData$x - LIDARData$circle_x)^2 + (LIDARData$y - LIDARData$circle_y)^2)
    # Save results to a dataframe
    results <- rbind(results, data.frame(circle_center_x = c(circle_center_x),circle_center_y = c(circle_center_y), dist_sum = c(sum(LIDARData$dist_to_circle))))
}
}

# Show the minimized center point
subset(results, dist_sum == min(results$dist_sum, na.rm=TRUE))

# circle_center_x circle_center_y dist_sum
#               0              -1 601.6528
# Here we see that our true center is 0, -1 and our ideal circle can be drawn from there.
plot(LIDARData$x, LIDARData$y)
draw.circle(0,-1,400, border="red")

---

Using this data:

# Create sample dataframe
LIDARData <- read.table(header = TRUE, text = "
angle distance
0.1094 460.0
1.2344 460.0
2.4844 463.0
3.9844 463.0
5.2188 463.0
15.1719 460.0
16.4219 458.0
17.4063 462.0
18.9063 463.0
43.7656 463.0
44.9063 461.0
46.4063 460.0
47.5313 460.0
48.6719 461.0
55.0625 458.0
56.5781 459.0
57.4531 459.0
58.8438 458.0
60.0938 458.0
160.7656 435.0
162.0156 435.0
163.6406 433.0
168.2656 435.0
169.5000 436.0
170.8750 435.0
172.1250 435.0
173.6250 436.0
174.6250 436.0
176.1250 435.0
177.1250 421.0
178.7500 411.0
179.9844 419.0
180.7344 434.0
182.3594 429.0
183.3594 431.0
184.8594 432.0
185.8594 433.0
187.3594 435.0
188.6094 435.0
189.5938 435.0
191.0938 437.0
195.9531 438.0
196.9531 438.0
198.4531 436.0
")