R/fit_shapes.R

In r-lidar/lidR: Airborne LiDAR Data Manipulation and Visualization for Forestry Applications

fit_circle_on_3_points <- function(points_subset)
{
  stopifnot(nrow(points_subset) == 3L, ncol(points_subset) > 2L)

  # Extract the coordinates
  x1 <- points_subset[1, 1]
  y1 <- points_subset[1, 2]
  x2 <- points_subset[2, 1]
  y2 <- points_subset[2, 2]
  x3 <- points_subset[3, 1]
  y3 <- points_subset[3, 2]

  # Calculate the coefficients for the linear system
  A <- 2 * (x2 - x1)
  B <- 2 * (y2 - y1)
  C <- x2^2 + y2^2 - x1^2 - y1^2
  D <- 2 * (x3 - x1)
  E <- 2 * (y3 - y1)
  G <- x3^2 + y3^2 - x1^2 - y1^2

  # Solve for a and b using Cramer's rule
  denominator <- A * E - B * D
  if (denominator == 0)
  {
    return(c(0, 0, 0))
  }
  a <- (C * E - B * G) / denominator
  b <- (A * G - C * D) / denominator

  # Calculate the radius
  r <- sqrt((x1 - a)^2 + (y1 - b)^2)

  # Return the center and radius
  return(c(a, b, r))
}

# fit_circle_on_3_points <- function(points_subset)
# {
#   stopifnot(nrow(points_subset) == 3L, ncol(points_subset) > 2L)
#
#   # Initial guesses
#   x_mean <- mean(points_subset[, 1])
#   y_mean <- mean(points_subset[, 2])
#   r_guess <- sqrt(mean((points_subset[, 1] - x_mean)^2 + (points_subset[, 2] - y_mean)^2))
#   initial_guess <- c(x_mean, y_mean, r_guess)
#
#   # Optimize circle parameters
#   fit <- optim(par = initial_guess, fn = residuals_circle, points = points_subset)
#   return(fit$par)
# }

#' Fits 2D Circles on a Point Cloud
#'
#' Fits a 2D horizontally-aligned circle to a set of 3D points. The method uses RANSAC-based fitting
#' for robust estimation. The function returns a list with the circle parameters and additional data
#' to help determine whether the points form a circular shape. While it is always possible to fit a
#' circle to any dataset, the additional information assists in evaluating if the data genuinely
#' represent a circular pattern. The root mean square error (RMSE) may not always be a reliable metric
#' for assessing the quality of the fit due to non-circular elements (such as branches) that can arbitrarily
#' increase the RMSE of an otherwise well-fitted circle, as shown in the example below. Therefore, the
#' function also returns the angular range of the data, which indicates the spread of the inlier points:
#' 360 degrees suggests a full circle, 180 degrees indicates a half-circle, or a smaller range may
#' suggest a partial arc.
#'
#' @param points A LAS object representing a clustered slice, or an nx3 matrix of point coordinates.
#' @param num_iterations The number of iterations for the RANSAC fitting algorithm.
#' @param inlier_threshold A threshold value; points are considered inliers if their residuals are
#' below this value.
#'
#' @return A list containing the circle parameters and additional information for determining whether
#' the data fit a circular shape:
#' - `center_x`, `center_y`, `radius`: parameters of the fitted circle.
#' - `z`: average elevation of the circle.
#' - `rmse`: root mean square error between the circle and all points.
#' - `angle_range`: angular sector covered by inlier points.
#' - `inliers`: IDs of the inlier points.
#' @md
#' @export
#' @examples
#' LASfile <- system.file("extdata", "dbh.laz", package="lidR")
#' las <- readTLS(LASfile, select = "xyz")
#' xyz = sf::st_coordinates(las)
#' circle = fit_circle(xyz)
#' plot(xyz, asp = 1, pch = 19, cex = 0.1)
#' symbols(circle$center_x, circle$center_y, circles = circle$radius,
#'   add = TRUE, fg = "red", inches = FALSE)
#'
#' trunk = las[circle$inliers]
#'
#' # Fitting a half-circle
#' half = xyz[xyz[,1] > 101.45,]
#' circle = fit_circle(half)
#' plot(half, asp = 1, pch = 19, cex = 0.1)
#' symbols(circle$center_x, circle$center_y, circles = circle$radius,
#'   add = TRUE, fg = "red", inches = FALSE)
#' circle$angle_range
fit_circle <- function(points, num_iterations = 100, inlier_threshold = 0.01)
{
  best_circle <- NULL
  max_inliers <- 0

  if (is(points, "LAS")) points = st_coordinates(points)

  stopifnot(is.matrix(points), ncol(points) == 3L, nrow(points) > 3L)

  z = points[, 3]

  for (i in 1:num_iterations)
  {
    # Randomly sample points
    sample_indices <- sample(1:nrow(points), 3L)
    points_subset <- points[sample_indices, ]

    params <- fit_circle_on_3_points(points_subset)

    # Compute residuals for all points
    distances <- sqrt((points[, 1] - params[1])^2 + (points[, 2] - params[2])^2)
    residuals <- abs(distances - params[3])

    # Count inliers (points whose residuals are below the threshold)
    inliers <- sum(residuals < inlier_threshold)

    # Update best model if more inliers are found
    if (inliers > max_inliers)
    {
      max_inliers <- inliers
      best_circle <- params
    }
  }

  center_x <- best_circle[1]
  center_y <- best_circle[2]
  radius <- best_circle[3]

  # Goodness of fit
  distances <- sqrt((points[, 1] - center_x)^2 + (points[, 2] - center_y)^2)
  residuals <- abs(distances - radius)
  inliers <- residuals < inlier_threshold
  rmse <- sqrt(mean((residuals)^2))

  # Angular range
  angle_res = 3
  angles <- atan2(points[inliers, 2] - center_y, points[inliers, 1] - center_x)
  angles <- ifelse(angles < 0, angles + 2 * pi, angles)
  angles <- sort(angles*180/pi)
  rangles <- unique(round(angles/angle_res)*angle_res)
  angle_range_degrees = sum(diff(rangles) <= angle_res)*angle_res

  return(list(center_x = center_x, center_y = center_y, radius = radius, z = mean(z), rmse = rmse, angle_range = angle_range_degrees, inliers = which(inliers)))
}