Aligning a Point Cloud with the Floor (plane) Using Open3D - python

Community,
I am trying to align a point cloud with the detected floor using Open3D. So far I implemented the following steps (partly of this answer):
Detecting the floor using Open3D's plane segmentation
Translating the plane to the coordinate center
Calculating rotation angle between plane normal & z-axis
Calculating the axis of rotation
Rotating the point cloud using Open3Ds get_rotation_matrix_from_axis_angle function (see 3)
The results are OK, but I have to use an optimization-factor at the end for better results. Is there a mistake or a simpler/ more precise way for the alignment?
# See functions below
# Get the plane equation of the floor → ax+by+cz+d = 0
floor = get_floor_plane(pcd)
a, b, c, d = floor
# Translate plane to coordinate center
pcd.translate((0,-d/c,0))
# Calculate rotation angle between plane normal & z-axis
plane_normal = tuple(floor[:3])
z_axis = (0,0,1)
rotation_angle = vector_angle(plane_normal, z_axis)
# Calculate rotation axis
plane_normal_length = math.sqrt(a**2 + b**2 + c**2)
u1 = b / plane_normal_length
u2 = -a / plane_normal_length
rotation_axis = (u1, u2, 0)
# Generate axis-angle representation
optimization_factor = 1.4
axis_angle = tuple([x * rotation_angle * optimization_factor for x in rotation_axis])
# Rotate point cloud
R = pcd.get_rotation_matrix_from_axis_angle(axis_angle)
pcd.rotate(R, center=(0,0,0))
# FUNCTIONS
def vector_angle(u, v):
return np.arccos(np.dot(u,v) / (np.linalg.norm(u)* np.linalg.norm(v)))
def get_floor_plane(pcd, dist_threshold=0.02, visualize=False):
plane_model, inliers = pcd.segment_plane(distance_threshold=dist_threshold,
ransac_n=3,
num_iterations=1000)
[a, b, c, d] = plane_model
return plane_model

Related

Is it possible to make a elipse on folium?

i saw on the internet images that folium generate a elipse form on map, is it possible, i tried creating manually but is inviable point per point, is there any way to do that ?
elipse on folium
Yes, by using folium's Polygon (and some other stuff)
I copied this answer for the polygon construction
import math
from shapely.geometry import Point
from shapely.affinity import scale, rotate
#input parameters
A = Point(-95.5, 41.25)
B = Point(-96.5, 41.25)
R = .25
d = A.distance(B)
#first, rotate B to B' around A so that |AB'| = |AB| and B'.y = A.y
#and then take S as midpoint of AB'
S = Point(A.x + d/2, A.y)
#alpha represents the angle of this rotation
alpha = math.atan2(B.y - A.y, B.x - A.x)
#create a circle with center at S passing through A and B'
C = S.buffer(d/2)
#rescale this circle in y-direction so that the corresponding
#axis is R units long
C = scale(C, 1, R/(d/2))
#rotate the ellipse obtained in previous step around A into the
#original position (positive angles represent counter-clockwise rotation)
C = rotate(C, alpha, origin = A, use_radians = True)
Where C is a shapely polygon
print(type(C))
<class 'shapely.geometry.polygon.Polygon'>
I flip the coordinates from X,Y to Y,X to make it folium friendly
folium_poly = [[y,x] for x,y in C.exterior.coords]
then folium for the rest
import folium
m = folium.Map([C.centroid.y, C.centroid.x])
folium.Polygon(folium_poly).add_to(m)
m

Apply rotation defined by Euler angles to 3D image, in python

I'm working with 3D images and have to rotate them according to Euler angles (phi,psi,theta) in 'zxz' convention (these Euler angles are part of a dataset, so I have to use that convention). I found the function scipy.ndimage.rotate that seems useful in that regard.
arrayR = scipy.ndimage.rotate(array , phi, axes=(0,1), reshape=False)
arrayR = scipy.ndimage.rotate(arrayR, psi, axes=(1,2), reshape=False)
arrayR = scipy.ndimage.rotate(arrayR, the, axes=(0,1), reshape=False)
Sadly, this does not do what intended. This is why:
Definition:
In the z-x-z convention, the x-y-z frame is rotated three times: first
about the z-axis by an angle phi; then about the new x-axis by an
angle psi; then about the newest z-axis by an angle theta.
However with above code, the rotations are always with respect to the original axes. Which is why obtained rotations are not correct. Anyone has a suggestion to obtain correct rotations, as explained in the definition?
In other words, in the present 'zxz' convention the rotations are intrinsic (rotations about the axes of the rotating coordinate system XYZ, solidary with the moving body, which changes its orientation after each elemental rotation). If I use the above code, the rotations are extrinsic (rotations about the axes xyz of the original coordinate system, which is assumed to remain motionless). I need a way for doing extrinsic rotations, in python.
I found a satisfying solution following this link: https://nbviewer.jupyter.org/gist/lhk/f05ee20b5a826e4c8b9bb3e528348688
This method uses np.meshgrid, scipy.ndimage.map_coordinates. The above link uses some third party library for generating the rotation matrix, however I use scipy.spatial.transform.Rotation. This function allows to define both intrinsic and extrinsic rotations: see description of scipy.spatial.transform.Rotation.from_euler.
Here is my function:
import numpy as np
from scipy.spatial.transform import Rotation as R
from scipy.ndimage import map_coordinates
# Rotates 3D image around image center
# INPUTS
# array: 3D numpy array
# orient: list of Euler angles (phi,psi,the)
# OUTPUT
# arrayR: rotated 3D numpy array
# by E. Moebel, 2020
def rotate_array(array, orient):
phi = orient[0]
psi = orient[1]
the = orient[2]
# create meshgrid
dim = array.shape
ax = np.arange(dim[0])
ay = np.arange(dim[1])
az = np.arange(dim[2])
coords = np.meshgrid(ax, ay, az)
# stack the meshgrid to position vectors, center them around 0 by substracting dim/2
xyz = np.vstack([coords[0].reshape(-1) - float(dim[0]) / 2, # x coordinate, centered
coords[1].reshape(-1) - float(dim[1]) / 2, # y coordinate, centered
coords[2].reshape(-1) - float(dim[2]) / 2]) # z coordinate, centered
# create transformation matrix
r = R.from_euler('zxz', [phi, psi, the], degrees=True)
mat = r.as_matrix()
# apply transformation
transformed_xyz = np.dot(mat, xyz)
# extract coordinates
x = transformed_xyz[0, :] + float(dim[0]) / 2
y = transformed_xyz[1, :] + float(dim[1]) / 2
z = transformed_xyz[2, :] + float(dim[2]) / 2
x = x.reshape((dim[1],dim[0],dim[2]))
y = y.reshape((dim[1],dim[0],dim[2]))
z = z.reshape((dim[1],dim[0],dim[2])) # reason for strange ordering: see next line
# the coordinate system seems to be strange, it has to be ordered like this
new_xyz = [y, x, z]
# sample
arrayR = map_coordinates(array, new_xyz, order=1)
Note:
You can also use this function for intrinsic rotations, simply adapt the first argument of 'from_euler' to your Euler convention. In this case, you obtain equivalent result than in my 1st post (using scipy.ndimage.rotate). However I noticed that the present code is 3x faster (0.01s for 40^3 volume) than when using scipy.ndimage.rotate (0.03s for 40^3 volume).
Hope this will help someone!
There seem to be a bit confusion about the "axes" parameter in your first post. To do a rotation about the x axis, the plane of rotation would be the yz plane which means your "axes" parameter should be set to (1,2). Also the first and the third rotations are, presumably about the x and z axes. But, both your rotations are in the xy plane. Could these be possibly the reasons behind the discrepancies in your answers? I am not convinced by your explanations about the new and original axes. The independent calls to the "rotate" function does not have access to the old data in any form or shape. It only sees the new axes and rotated array.
I check the code https://nbviewer.jupyter.org/gist/lhk/f05ee20b5a826e4c8b9bb3e528348688
There is a minor bug. The tested image is square, but if doing rectangular image, it will encounter some problems. below are correct ones for 2D and 3D rotations (noted that the Euler angle sequence used in my example is 'ZYZ', you should define this before using it):
def rotate_array_2D(array, orient):
# create a transformation matrix
angle=orient/180.*np.pi
c=np.cos(angle)
s=np.sin(angle)
mat=np.array([[c,s],[-s,c]])
# create meshgrid
dim = array.shape
ax = np.arange(dim[0])
ay = np.arange(dim[1])
coords = np.meshgrid(ax, ay)
# stack the meshgrid to position vectors, center them around 0 by substracting dim/2
xy = np.vstack([coords[0].reshape(-1) - float(dim[0]) / 2, # x coordinate, centered
coords[1].reshape(-1) - float(dim[1]) / 2]) # y coordinate, centered
# apply transformation
transformed_xy = np.dot(mat, xy)
# extract coordinates
x = transformed_xy[0, :] + float(dim[0]) / 2
y = transformed_xy[1, :] + float(dim[1]) / 2
x = x.reshape((dim[1],dim[0]))
y = y.reshape((dim[1],dim[0]))
new_xy = [x,y]
# sample
arrayR = map_coordinates(array, new_xy, order=1).T
return arrayR
def rotate_array_3D(array, orient):
rot = orient[0]
tilt = orient[1]
phi = orient[2]
# create meshgrid
dim = array.shape
ax = np.arange(dim[0])
ay = np.arange(dim[1])
az = np.arange(dim[2])
coords = np.meshgrid(ax, ay, az)
# stack the meshgrid to position vectors, center them around 0 by substracting dim/2
xyz = np.vstack([coords[0].reshape(-1) - float(dim[0]) / 2, # x coordinate, centered
coords[1].reshape(-1) - float(dim[1]) / 2, # y coordinate, centered
coords[2].reshape(-1) - float(dim[2]) / 2]) # z coordinate, centered
# create transformation matrix
r = R.from_euler('ZYZ', [rot, tilt, phi], degrees=True)
mat = r.as_matrix()
# apply transformation
transformed_xyz = np.dot(mat, xyz)
# extract coordinates
x = transformed_xyz[0, :] + float(dim[0]) / 2
y = transformed_xyz[1, :] + float(dim[1]) / 2
z = transformed_xyz[2, :] + float(dim[2]) / 2
x = x.reshape((dim[1],dim[0],dim[2]))
y = y.reshape((dim[1],dim[0],dim[2]))
z = z.reshape((dim[1],dim[0],dim[2])) # I test the rotation in 2D and this strange thing can be explained
new_xyz = [x,y,z]
arrayR = map_coordinates(array, new_xyz, order=1).T
return arrayR

How to convert a 2D point on defined plane in planar uv coordinates back to 3D xyz coordinates?

I have a defined plane (one which happens to be orthogonal to the vector defined by two xyz points in 3D space). I can project any xyz point onto the plane and represent that projection uv coordinate space. I would like to take an arbitrary point in uv coordinate space and find out what its coordinates are in xyz space.
a = x2 - x1
b = y2 - y1
c = z2 - z1
d = -1*(a*x1 + b*y1 + c*z1)
magnitude = (a**2 + b**2 + c**2)**.5
u_magnitude = (b**2 + a**2)**.5
normal = [a/magnitude, b/magnitude, c/magnitude]
u = [b/u_magnitude, -a/u_magnitude, 0]
v = np.cross(normal, u)
p_u = np.dot(u,[x1,y1,z1])
p_v = np.dot(v,[x1,y1,z1])
This code I believe accurately produces the plane I want and will assign the x1,y1,z1 point in uv coordinates to p_u,p_v. My sense is that I have everything I need to do the reverse operation, but I don't know how. If I have a point u0,v0 how can I find x0,y0,z0 that describes its location in 3D space?
From the definition in the text (not reading the code), the problem is not well defined - as there is an infinite number of planes orthogonal to a given vector (think of all the options as planes at different "offsets" along the line from the first point to the second). What you need is first to pick some point through which the plane has to go.
Secondly, when we convert a (U, V) pair to 3D point, I assume you mean a 3D point on the plane.
Trying to be more concrete though, here is your code, with documentation on how I understand it, and how to do the reverse:
# ### The original computation of the plane equation ###
# Given points p1 and p2, the vector through them is W = (p2 - p1)
# We want the plane equation Ax + By + Cz + d = 0, and to make
# the plane prepandicular to the vector, we set (A, B, C) = W
p1 = np.array([x1, y1, z1])
p2 = np.array([x2, y2, z2])
A, B, C = W = p2 - p1
# Now we can solve D in the plane equation. This solution assumes that
# the plane goes through p1.
D = -1 * np.dot(W, p1)
# ### Normalizing W ###
magnitude = np.linalg.norm(W)
normal = W / magnitude
# Now that we have the plane, we want to define
# three things:
# 1. The reference point in the plane (the "origin"). Given the
# above computation of D, that is p1.
# 2. The vectors U and V that are prepandicular to W
# (and therefore spanning the plane)
# We take a vector U that we know that is perpendicular to
# W, but we also need to make sure it's not zero.
if A != 0:
u_not_normalized = np.array([B, -A, 0])
else:
# If A is 0, then either B or C have to be nonzero
u_not_normalized = np.array([0, B, -C])
u_magnitude = np.linalg.norm(u_not_normalized)
# ### Normalizing W ###
U = u_not_normalized / u_magnitude
V = np.cross(normal, U)
# Now, for a point p3 = (x3, y3, z3) it's (u, v) coordinates would be
# computed relative to our reference point (p1)
p3 = np.array([x3, y3, z3])
p3_u = np.dot(U, p3 - p1)
p3_v = np.dot(V, p3 - p1)
# And to convert the point back to 3D, we just use the same reference point
# and multiply U and V by the coordinates
p3_again = p1 + p3_u * U + p3_v * V

Verification of 3D Fractal Slice-Rendering Method

Ray Casting Algorithm
MandelBulb Ray Casting Algorithm Python Example
So, if I understand correctly, the ray casting algorithm requires that an observer be located external to the 3D fractal at which point vectors are drawn from the observer toward a point on the plane normal to the vector and intersecting the origin.
It would seem to me that this would either severely limit the rendered view of the fractal or require stereoscopic 3D reconstruction of the fractal using multiple observer positions (which seems difficult to me). Additionally, no information can be gathered regarding the internal structure of the fractal.
Other Algorithms
Alternatively, Direct Volume Rendering seems intuitive enough however, computationally expensive and potentially inefficient in and of itself. Indirect Volume Rendering using an algorithm such as marching cubes might also employ a bit of a learning curve it seems.
Somewhere in the pdf of the 2nd link it talks about cut plane views in order to see slices of the fractal.
Question:
Why not use cut planes as a rendering method?
1) Using a modified ray tracing algorithm, say we put the observer at point Q at the origin (0, 0, 0).
2) Let us then emit rays from the origin toward the incident plane spanned by y & z point combinations that is slicing the fractal.
3) Calculate the distance to the fractal surface using the algorithm in the 1st link. If the x component of computed distance is within a certain tolerance, dx of the slicing plane, then the y & z coordinates along with the x value of the slicing plane are stored as the x, y, z coordinates. These coordinates are now representative of the surface at that specific slice in x.
4) Let us say that the slicing plane has one degree of freedom in the x direction. By moving the plane in its degree of freedom, we can receive yet another set of x, y, z coordinates for a given slice.
5) The final result is a calculable surface generated by the point cloud created in the previous steps.
6) Additionally, the degree of freedom of the slicing plane can be altered to create an another point cloud which can then be verified against the previous as a means of post-processing.
Please see the image below as a visual aid (the sphere represents the MandelBulb).
Below is my Python code so far, adapted from the first link. I successfully generate the plane of points and am able to get the directions from the origin to the points on the plane. There must be something fundamentally flawed in the distance estimator function because thats where everything breaks down and I get nans for the total distances
def get_plane_points(x, y_res=500, z_res=500, y_min=-10, y_max=10, z_min=-10, z_max=10):
y = np.linspace(y_min, y_max, y_res)
z = np.linspace(z_min, z_max, z_res)
x, y, z = np.meshgrid(x, y, z)
x, y, z = x.reshape(-1), y.reshape(-1) , z.reshape(-1)
P = np.vstack((x, y, z)).T
return P
def get_directions(P):
v = np.array(P - 0)
v = v/np.linalg.norm(v, axis=1)[:, np.newaxis]
return v
#jit
def DistanceEstimator(positions, plane_loc, iterations, degree):
m = positions.shape[0]
x, y, z = np.zeros(m), np.zeros(m), np.zeros(m)
x0, y0, z0 = positions[:, 0], positions[:, 1], positions[:, 2]
dr = np.zeros(m) + 1
r = np.zeros(m)
theta = np.zeros(m)
phi = np.zeros(m)
zr = np.zeros(m)
for _ in range(iterations):
r = np.sqrt(x * x + y * y + z * z)
dx = .01
x_loc = plane_loc
idx = (x < x_loc + dx) & (x > x_loc - dx)
dr[idx] = np.power(r[idx], degree - 1) * degree * dr[idx] + 1.0
theta[idx] = np.arctan2(np.sqrt(x[idx] * x[idx] + y[idx] * y[idx]), z[idx])
phi[idx] = np.arctan2(y[idx], x[idx])
zr[idx] = r[idx] ** degree
theta[idx] = theta[idx] * degree
phi[idx] = phi[idx] * degree
x[idx] = zr[idx] * np.sin(theta[idx]) * np.cos(phi[idx]) + x0[idx]
y[idx] = zr[idx] * np.sin(theta[idx]) * np.sin(phi[idx]) + y0[idx]
z[idx] = zr[idx] * np.cos(theta[idx]) + z0[idx]
return 0.5 * np.log(r) * r / dr
def trace(directions, plane_location, max_steps=50, iterations=50, degree=8):
total_distance = np.zeros(directions.shape[0])
keep_iterations = np.ones_like(total_distance)
steps = np.zeros_like(total_distance)
for _ in range(max_steps):
positions = total_distance[:, np.newaxis] * directions
distance = DistanceEstimator(positions, plane_location, iterations, degree)
total_distance += distance * keep_iterations
steps += keep_iterations
# return 1 - (steps / max_steps) ** power
return total_distance
def run():
plane_location = 2
plane_points = get_plane_points(x=plane_location)
directions = get_directions(plane_points)
distance = trace(directions, plane_location)
return distance
I am eager to hear thoughts on this and what limitations/issues I may encounter. Thanks in advance for the help!
If I am not mistaken, it is not impossible for this algorithm to work. There is inherent potential for problems with any assumptions made about the internal structure of the MandelBulb and what positions an observer is allowed to occupy. That is, if the observer is known to initially be in a zone of convergence then the ray tracing algorithm with return nothing meaningful since the furthest distance that could be measured is 0. This is due to the fact that the current ray tracing algorithm terminates upon first contact with the surface. It is likely this could be altered, however.
Rather than slicing the fractal with plane P, it might make more sense to prevent the termination of the ray upon first contact and instead, terminate based on a distance thats known to exist past the surface of the MandelBulb.

distance measurement using disparity map

I was working on 3D reconstruction and distance measurement using OpenCP and Python. I generate the disparity map for the left camera and then I used this formula to get the distance:
D=(f*b/disp)
Where f is the focal length, b is the distance between the 2 cameras and disp is the matrix of the disparity map.
My questions are:
The numbers that I get, are they supposed to be the distance of each point in the picture?
What is the max distance that I can get with this method (for example in my project the max number i get is 110)?
img_L = cv2.pyrDown( cv2.imread(Li) )
img_R = cv2.pyrDown( cv2.imread(Ri) )
'''h, w = img_L.shape[:2]
diff=(6,6,6)
mask=np.zeros((h+2,w+2),np.uint8)
window_size = 3
min_disp = 16
num_disp = 112-min_disp
stereo = cv2.StereoSGBM(minDisparity = min_disp,
numDisparities = num_disp,
SADWindowSize = window_size,
uniquenessRatio = 10,
speckleWindowSize = 100,
speckleRange = 32,
disp12MaxDiff = 1,
P1 = 8*3*window_size**2,
P2 = 32*3*window_size**2,
fullDP = False
)
print "computing disparity..."
disp = stereo.compute(img_L, img_R).astype(np.float32) / 16.0
print "generating 3d point cloud..."
h, w = img_L.shape[:2]
f = 0.8*w # guess for focal length
points = cv2.reprojectImageTo3D(disp, Mat)
colors = cv2.cvtColor(img_L, cv2.COLOR_BGR2RGB)
mask = disp > disp.min()
cv2.imshow('left', img_L)
disparity=(disp-min_disp)/num_disp
cv2.imshow('disparity',disparity )
b=6.50
D=b*f/disp
cv2.waitKey()
cv.DestroyAllWindows()
return D
The values D that you get using this formula are the depths of each point for which you provided a disparity.
The depth and the distance are two slightly different things. If you use the standard coordinate system for a camera (i.e. Z axis along the optical axis, X and Y axis in the directions of the image X and Y axis), then a 3D point M = (X, Y, Z) has a distance of sqrt(X²+Y²+Z²) from the optical center and a depth of Z. The D in the formula is the depth, not the distance.
If you want to retrieve the 3D point M = (X, Y, Z) from the depth value, you need to know the camera matrix K: M = D * inv(K) * [u; v; 1], where (u, v) are the image coordinates of the point.
Edit: Concerning your second question, the maximum depth that you can get with this method is linked to the minimum disparity (not the maximum, since disp is on the denominator). And since disparity estimation is quantified (done pixel by pixel), you can't estimate depth up to infinity.

Categories