python+numpy实现三维点云拟合圆
参考:
https://meshlogic.github.io/posts/jupyter/curve-fitting/fitting-a-circle-to-cluster-of-3d-points/
还有:
空间3点求三点所在空间圆的圆心空间坐标(这个好像有点问题)
https://blog.csdn.net/YanMY2012/article/details/8111600
# -------------------------------------------------------------------------------
# RODRIGUES ROTATION
# - Rotate given points based on a starting and ending vector
# - Axis k and angle of rotation theta given by vectors n0,n1
# P_rot = P*cos(theta) + (k x P)*sin(theta) + k**(1-cos(theta))
# -------------------------------------------------------------------------------
def rodrigues_rot(P, n0, n1):
# If P is only 1d array (coords of single point), fix it to be matrix
if P.ndim == 1:
P = P[np.newaxis, :]
# Get vector of rotation k and angle theta
n0 = n0 / np.linalg.norm(n0)
n1 = n1 / np.linalg.norm(n1)
k = np.cross(n0, n1)
k = k / np.linalg.norm(k)
theta = np.arccos(np.dot(n0, n1))
# Compute rotated points
P_rot = np.zeros((len(P), 3))
for i in range(len(P)):
P_rot[i] = P[i] * np.cos(theta) + np.cross(k, P[i]) * np.sin(theta) + k * np.dot(k, P[i]) * (1 - np.cos(theta))
return P_rot
# -------------------------------------------------------------------------------
# FIT CIRCLE 2D
# - Find center [xc, yc] and radius r of circle fitting to set of 2D points
# - Optionally specify weights for points
#
# - Implicit circle function:
# (x-xc)^2 + (y-yc)^2 = r^2
# (2*xc)*x + (2*yc)*y + (r^2-xc^2-yc^2) = x^2+y^2
# c[0]*x + c[1]*y + c[2] = x^2+y^2
#
# - Solution by method of least squares:
# A*c = b, c' = argmin(||A*c - b||^2)
# A = [x y 1], b = [x^2+y^2]
# -------------------------------------------------------------------------------
def fit_circle_2d(x, y, w=[]):
A = np.array([x, y, np.ones(len(x))]).T
b = x ** 2 + y ** 2
# Modify A,b for weighted least squares
if len(w) == len(x):
W = np.diag(w)
A = np.dot(W, A)
b = np.dot(W, b)
# Solve by method of least squares
c = np.linalg.lstsq(A, b, rcond=None)[0]
# Get circle parameters from solution c
xc = c[0] / 2
yc = c[1] / 2
r = np.sqrt(c[2] + xc ** 2 + yc ** 2)
return xc, yc, r
# -------------------------------------------------------------------------------
# Generate points on circle
# P(t) = r*cos(t)*u + r*sin(t)*(n x u) + C
# -------------------------------------------------------------------------------
def generate_circle_by_vectors(t, C, r, n, u):
n = n / np.linalg.norm(n)
u = u / np.linalg.norm(u)
P_circle = r * np.cos(t)[:, np.newaxis] * u + r * np.sin(t)[:, np.newaxis] * np.cross(n, u) + C
return P_circle
#https://meshlogic.github.io/posts/jupyter/curve-fitting/fitting-a-circle-to-cluster-of-3d-points/
def circle_segmentation(cloud):
# -------------------------------------------------------------------------------
# (1) Fitting plane by SVD for the mean-centered data
# Eq. of plane is + d = 0, where p is a point on plane and n is normal vector
# -------------------------------------------------------------------------------
P_mean = cloud.mean(axis=0)
P_centered = cloud - P_mean
U, s, V = np.linalg.svd(P_centered)
# Normal vector of fitting plane is given by 3rd column in V
# Note linalg.svd returns V^T, so we need to select 3rd row from V^T
normal = V[2, :]
d = -np.dot(P_mean, normal) # d = -
# -------------------------------------------------------------------------------
# (2) Project points to coords X-Y in 2D plane
# -------------------------------------------------------------------------------
P_xy = rodrigues_rot(P_centered, normal, [0, 0, 1])
# -------------------------------------------------------------------------------
# (3) Fit circle in new 2D coords
# -------------------------------------------------------------------------------
xc, yc, r = fit_circle_2d(P_xy[:, 0], P_xy[:, 1])
# --- Generate circle points in 2D
t = np.linspace(0, 2 * np.pi, 100)
xx = xc + r * np.cos(t)
yy = yc + r * np.sin(t)
# -------------------------------------------------------------------------------
# (4) Transform circle center back to 3D coords
# -------------------------------------------------------------------------------
C = rodrigues_rot(np.array([xc, yc, 0]), [0, 0, 1], normal) + P_mean
C = C.flatten()
#--- Generate points for fitting circle
t = np.linspace(0, 2*np.pi, 1000)
u = cloud[0] - C
P_fitcircle = generate_circle_by_vectors(t, C, r, normal, u)
return P_fitcircle, C, r 使用:
import numpy as np
import pcl
import pcl.pcl_visualization
import random
import datetime
import colorsys
def get_n_hls_colors(num):
hls_colors = []
i = 0
step = 360.0 / num
while i < 360:
h = i
s = 90 + random.random() * 10
l = 50 + random.random() * 10
_hlsc = [h / 360.0, l / 100.0, s / 100.0]
hls_colors.append(_hlsc)
i += step
return hls_colors
def ncolors(num):
rgb_colors = []
if num < 1:
return rgb_colors
hls_colors = get_n_hls_colors(num)
for hlsc in hls_colors:
_r, _g, _b = colorsys.hls_to_rgb(hlsc[0], hlsc[1], hlsc[2])
r, g, b = [int(x * 255.0) for x in (_r, _g, _b)]
rgb_colors.append([r, g, b])
return rgb_colors
def vis_draw_point_cloud(geoms, color_num=15):
colorbar = ncolors(color_num)
vis = pcl.pcl_visualization.PCLVisualizering
vis_create_window = pcl.pcl_visualization.PCLVisualizering()
title_str = datetime.datetime.now().strftime('%Y-%m-%d_%H-%M-%S')
for index, geom in enumerate(geoms):
point_color = pcl.pcl_visualization.PointCloudColorHandleringCustom(geom, colorbar[index][0], colorbar[index][1], colorbar[index][2])
vis.AddPointCloud_ColorHandler(vis_create_window, geom, point_color, id=title_str + '_' +'point_cloud_id{:0>3d}'.format(index), viewport=0)
while not vis.WasStopped(vis_create_window):
vis.Spin(vis_create_window)
def main():
circle, circle_center, radius = circle_segmentation(np.array(cloud))
vis_draw_point_cloud([pcl.PointCloud(circle.astype(np.float32))])