Related
I want to draw a line on an image. I have only to give the angle and the end point of the line. How can I do this with Python?
I think it is easy by identifying the vertical line passing through that given point and ploting the line according to the angle. The line should ends with the given point.
I tried it with this code. But didn't work.
import math
def get_coords(x, y, angle, imwidth, imheight):
#img = cv2.imread('contours_none_image2.jpg', 1)
x1_length = (x-imwidth) / math.cos(angle)
y1_length = (y-imheight) / math.sin(angle)
length = max(abs(x1_length), abs(y1_length))
endx1 = x + length * math.cos(math.radians(angle))
endy1 = y + length * math.sin(math.radians(angle))
x2_length = (x-imwidth) / math.cos(angle+45)
y2_length = (y-imheight) / math.sin(angle+45)
length = max(abs(x2_length), abs(y2_length))
endx2 = x + length * math.cos(math.radians(angle+45))
endy2 = y + length * math.sin(math.radians(angle+45))
cv2.line(img, (int(endx1),int(endy1)), (int(endx2),int(endy2)), (0, 255, 255), 3)
cv2.imshow("contours_none_image2.jpg", img)
#cv2.imshow("contours_none_image2.jpg", result)
cv2.waitKey(0)
cv2.destroyAllWindows()
return endx1, endy1, endx2, endy2
An interesting way for finding the intersection point between the Y axis and the line is by using three cross products with homogeneous coordinates.
Ways for finding lines intersections are described in Wikipedia.
The cross products solution using homogeneous coordinates is described here.
Start by finding a very "far" origin point (x, y) - outside the image:
length = cv2.norm(np.array([imwidth, imheight])) # Apply maximum possible length: length = sqrt(imwidth**2 + imheight**2)
x0 = x - length * math.cos(math.radians(angle))
y0 = y + length * math.sin(math.radians(angle)) # Reverse sings because y axis in image goes down
Finding intersection with the Y axis:
The Y axis may be described as a line from (0,0) to (0, imheight-1).
We may find the line representation in homogeneous coordinates using cross product:
p0 = np.array([0, 0, 1])
p1 = np.array([0, imheight-1, 1])
l0 = np.cross(p0, p1) # [-107, 0, 0]
In the same way we may find the representation of the line from (x0, y0) to (x, y):
p0 = np.array([x0, y0, 1])
p1 = np.array([x, y, 1])
l1 = np.cross(p0, p1)
Finding the intersection point using cross product between the lines, and "normalizing" the homogeneous coordinate:
p = np.cross(l0, l1)
p = p / p[2]
Code sample:
import math
import cv2
import numpy as np
img = np.zeros((108, 192, 3), np.uint8)
x, y, angle = 150, 20, 80
imheight, imwidth = img.shape[0], img.shape[1]
angle = 90 - angle # Usualy the angle is relative to the horizontal axis - use 90 - angle for swaping axes
length = cv2.norm(np.array([imwidth, imheight])) # Apply maximum possible length: length = sqrt(imwidth**2 + imheight**2)
x0 = x - length * math.cos(math.radians(angle))
y0 = y + length * math.sin(math.radians(angle)) # Reverse sings because y axis in image goes down
# http://robotics.stanford.edu/~birch/projective/node4.html
# Find lines in homogeneous coordinates (using cross product):
# l0 represents a line of Y axis.
p0 = np.array([0, 0, 1])
p1 = np.array([0, imheight-1, 1])
l0 = np.cross(p0, p1) # [-107, 0, 0]
# l1 represents
p0 = np.array([x0, y0, 1])
p1 = np.array([x, y, 1])
l1 = np.cross(p0, p1)
# https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
# Lines intersection in homogeneous coordinates (using cross product):
p = np.cross(l0, l1)
p = p / p[2]
x0, y0 = p[0], p[1]
# Convert from homogeneous coordinate to euclidean coordinate (divide by last element).
cv2.line(img, (int(x0),int(y0)), (int(x),int(y)), (0, 255, 255), 3)
cv2.imshow("img", img)
cv2.waitKey(0)
cv2.destroyAllWindows()
Sample output:
More conventional solution:
We may simply assign x0 = 0, and find length:
x0 = x - length * cos(alpha)
y0 = y + length * sin(alpha)
Assign x0 = 0:
x - length * cos(alpha) = 0
=> x = length * cos(alpha)
=> length = x/cos(alpha)
Code:
length = x / math.cos(math.radians(angle)) # We better verify that math.cos(math.radians(angle)) != 0
x0 = 0
y0 = y + length * math.sin(math.radians(angle))
cv2.line(img, (int(x0),int(y0)), (int(x),int(y)), (255, 0, 0), 3)
cv2.imshow("img", img)
cv2.waitKey(0)
cv2.destroyAllWindows()
Output:
I have 4 points marked in an equirectangular image. [Red dots]
I also have the 4 corresponding points marked in an overhead image [ Red dots ]
How do I calculate where on the overhead image the camera was positioned?
So far I see there are 4 rays (R1, R2, R3, R4) extending from the unknown camera center C = (Cx, Cy, Cz) through the points in the equirectangular image and ending at the pixel coordinates of the overhead image (P1, P2, P3, P4). So 4 vector equations of the form:
[Cx, Cy, Cz] + [Rx, Ry, Rz]*t = [x, y, 0]
for each correspondence. So
C + R1*t1 = P1 = [x1, y1, 0]
C + R2*t2 = P2 = [x2, y2, 0]
C + R3*t3 = P3 = [x3, y3, 0]
C + R4*t4 = P4 = [x4, y4, 0]
So 7 unknowns and 12 equations? This was my attempt but doesn't seem to give a reasonable answer:
import numpy as np
def equi2sphere(x, y):
width = 2000
height = 1000
theta = 2 * np.pi * x / width - np.pi
phi = np.pi * y / height
return theta, phi
HEIGHT = 1000
MAP_HEIGHT = 788
#
# HEIGHT = 0
# MAP_HEIGHT = 0
# Point in equirectangular image, bottom left = (0, 0)
xs = [1190, 1325, 1178, 1333]
ys = [HEIGHT - 730, HEIGHT - 730, HEIGHT - 756, HEIGHT - 760]
# import cv2
# img = cv2.imread('equirectangular.jpg')
# for x, y in zip(xs, ys):
# img = cv2.circle(img, (x, y), 15, (255, 0, 0), -1)
# cv2.imwrite("debug_equirectangular.png", img)
# Corresponding points in overhead map, bottom left = (0, 0)
px = [269, 382, 269, 383]
py = [778, 778, 736, 737]
# import cv2
# img = cv2.imread('map.png')
# for x, y in zip(px, py):
# img = cv2.circle(img, (x, y), 15, (255, 0, 0), -1)
# cv2.imwrite("debug_map.png", img)
As = []
bs = []
for i in range(4):
x, y = xs[i], ys[i]
theta, phi = equi2sphere(x, y)
# convert to spherical
p = 1
sx = p * np.sin(phi) * np.cos(theta)
sy = p * np.sin(phi) * np.sin(theta)
sz = p * np.cos(phi)
print(x, y, '->', np.degrees(theta), np.degrees(phi), '->', round(sx, 2), round(sy, 2), round(sz, 2))
block = np.array([
[1, 0, 0, sx],
[0, 1, 0, sy],
[1, 0, 1, sz],
])
y = np.array([px[i], py[i], 0])
As.append(block)
bs.append(y)
A = np.vstack(As)
b = np.hstack(bs).T
solution = np.linalg.lstsq(A, b)
Cx, Cy, Cz, t = solution[0]
import cv2
img = cv2.imread('map_overhead.png')
for i in range(4):
x, y = xs[i], ys[i]
theta, phi = equi2sphere(x, y)
# convert to spherical
p = 1
sx = p * np.sin(phi) * np.cos(theta)
sy = p * np.sin(phi) * np.sin(theta)
sz = p * np.cos(phi)
pixel_x = Cx + sx * t
pixel_y = Cy + sy * t
pixel_z = Cz + sz * t
print(pixel_x, pixel_y, pixel_z)
img = cv2.circle(img, (int(pixel_x), img.shape[0] - int(pixel_y)), 15, (255,255, 0), -1)
img = cv2.circle(img, (int(Cx), img.shape[0] - int(Cy)), 15, (0,255, 0), -1)
cv2.imwrite("solution.png", img)
# print(A.dot(solution[0]))
# print(b)
Resulting camera position (Green) and projected points (Teal)
EDIT: One bug fixed is that the longitude offset in the equirectangular images in PI/4 which fixes the rotation issue but the scale is still off somehow.
EDIT: using the MAP picture width/length for spherical conversion gives way better results for camera center. Points positions are still a bit messy.
Map with a better solution for camera center: , points are somewhat flattened
I took the liberty of rewriting a bit of the code, adding points identification using variables and colors (In your original code, some points were in different order in the various lists).
This is preferable if one wants to work with more data points. yeah, I chose a dict for debug purposes, but a list of N points would indeed be preferrable, provided that theyare correctly index paired between the different projections.
I also adapted the coordinates to match the pictures I had. And the x,y variables usage/naming for my understanding.
It is still incorrect, but there is some sort of consistency between each found position.
Possible cause
OpenCV images put the [0,0] in the TOPLEFT corner. The code below is consistent with that convention for points coordinates, but I did not change any math formula.
Maybe there is an error or inconsistencies in some of the formulas.
You may want to check again your conventions : signs, [0,0] location etc.
I don't see any input related to camera location and altitude in the formulas, which may be a source of error.
You may have a look to this project that performs equirectangular projections: https://github.com/NitishMutha/equirectangular-toolbox
from typing import Dict
import cv2
import numpy as np
def equi2sphere(x, y, width, height):
theta = (2 * np.pi * x / width) - np.pi
phi = (np.pi * y / height)
return theta, phi
WIDTH = 805
HEIGHT = 374 # using stackoverflow PNG
MAP_WIDTH = 662
MAP_HEIGHT = 1056 # using stackoverflow PNG
BLUE = (255, 0, 0)
GREEN = (0, 255, 0)
RED = (0, 0, 255)
CYAN = (255, 255, 0)
points_colors = [BLUE, GREEN, RED, CYAN]
TOP_LEFT = "TOP_LEFT"
TOP_RIGHT = "TOP_RIGHT"
BOTTOM_LEFT = "BOTTOM_LEFT"
BOTTOM_RIGHT = "BOTTOM_RIGHT"
class Point:
def __init__(self, x, y, color):
self.x = x
self.y = y
self.c = color
#property
def coords(self):
return (self.x, self.y)
# coords using GIMP which uses upperleft [0,0]
POINTS_ON_SPHERICAL_MAP: Dict[str, Point] = {TOP_LEFT : Point(480, 263, BLUE),
TOP_RIGHT : Point(532, 265, GREEN),
BOTTOM_LEFT : Point(473, 274, RED),
BOTTOM_RIGHT: Point(535, 275, CYAN),
}
# xs = [480, 532, 473, 535, ]
# ys = [263, 265, 274, 275, ]
img = cv2.imread('equirectangular.png')
for p in POINTS_ON_SPHERICAL_MAP.values():
img = cv2.circle(img, p.coords, 5, p.c, -1)
cv2.imwrite("debug_equirectangular.png", img)
# coords using GIMP which uses upperleft [0,0]
# px = [269, 382, 269, 383]
# py = [278, 278, 320, 319]
POINTS_ON_OVERHEAD_MAP: Dict[str, Point] = {TOP_LEFT : Point(269, 278, BLUE),
TOP_RIGHT : Point(382, 278, GREEN),
BOTTOM_LEFT : Point(269, 320, RED),
BOTTOM_RIGHT: Point(383, 319, CYAN),
}
img = cv2.imread('map.png')
for p in POINTS_ON_OVERHEAD_MAP.values():
img = cv2.circle(img, p.coords, 5, p.c, -1)
cv2.imwrite("debug_map.png", img)
As = []
bs = []
for point_location in [TOP_LEFT, TOP_RIGHT, BOTTOM_LEFT, BOTTOM_RIGHT]:
x_spherical, y_spherical = POINTS_ON_SPHERICAL_MAP[point_location].coords
theta, phi = equi2sphere(x=x_spherical, y=y_spherical, width=MAP_WIDTH, height=MAP_HEIGHT) # using the overhead map data for conversions
# convert to spherical
p = 1
sx = p * np.sin(phi) * np.cos(theta)
sy = p * np.sin(phi) * np.sin(theta)
sz = p * np.cos(phi)
print(f"{x_spherical}, {y_spherical} -> {np.degrees(theta):+.3f}, {np.degrees(phi):+.3f} -> {sx:+.3f}, {sy:+.3f}, {sz:+.3f}")
block = np.array([[1, 0, 0, sx],
[0, 1, 0, sy],
[1, 0, 1, sz], ])
x_map, y_map = POINTS_ON_OVERHEAD_MAP[point_location].coords
vector = np.array([x_map, y_map, 0])
As.append(block)
bs.append(vector)
A = np.vstack(As)
b = np.hstack(bs).T
solution = np.linalg.lstsq(A, b)
Cx, Cy, Cz, t = solution[0]
img = cv2.imread("debug_map.png")
for point_location in [TOP_LEFT, TOP_RIGHT, BOTTOM_LEFT, BOTTOM_RIGHT]:
x_spherical, y_spherical = POINTS_ON_SPHERICAL_MAP[point_location].coords
theta, phi = equi2sphere(x=x_spherical, y=y_spherical, width=MAP_WIDTH, height=MAP_HEIGHT) # using the overhead map data for conversions
# convert to spherical
p = 1
sx = p * np.sin(phi) * np.cos(theta)
sy = p * np.sin(phi) * np.sin(theta)
sz = p * np.cos(phi)
pixel_x = Cx + sx * t
pixel_y = Cy + sy * t
pixel_z = Cz + sz * t
print(f"{pixel_x:+0.0f}, {pixel_y:+0.0f}, {pixel_z:+0.0f}")
img = cv2.circle(img, (int(pixel_x), int(pixel_y)), 5, POINTS_ON_SPHERICAL_MAP[point_location].c, -1)
img = cv2.circle(img, (int(Cx), int(Cy)), 4, (200, 200, 127), 3)
cv2.imwrite("solution.png", img)
Map with my initial solution:
Debug map:
Equirectangular image:
Debug equirectangular:
To expand on my comment, here's the method I use to first calculate Cx and Cy. Cz will be determined afterwards using Cx and Cy.
On this overhead view, the circle is the cylinder that unrolls into the equirectangular image; A' , B' , C' and D' are the points that represent A, B, C, D on this image; the horizontal distances between A' and B', ... are proportional to the angles A-Camera-B, ... . Hence A'B'/ circle-perimeter = A-Camera-B / 2pi
and thus A-Camera-B = A'B'/ circle-perimeter * 2pi (the circle's perimeter being the width of the equirectangular image). Let's call this angle alpha.
This figure illustrates how we can determine the possible positions of the camera from the angle alpha, using the properties of angles in circles : the 3 marked angles are equal to alpha, thus tan(alpha) = AH/O1H, hence O1H = AH / tan(alpha) . We now have the coordinates of O1 (AB/2 , AB/(2 tan(alpha)) . (in a cartesian coordinate system with A as origin).
By doing the same for segment [AD], we get a 2nd circle of possible positions for the camera. The intersection points of the 2 circles are A and the actual camera position.
Of course the precision of the determined position is dependent on the precision of the coordinates of A', B'... on the equirectangular picture; here A' and D' are (horizontally) only 6-7 pixels apart, so there's some fluctuation.
Now to calculate Cz : on this side view, the half-circle unfolds into the pixel column containing A' in the equirectangular image ; similar to the calculation of alpha earlier, the ratio of A'I / length of the half-circle (which is the height of the image) is equal to tilt angle / pi, so tilt = A'I / height * pi ; on the equirectangular image, A'I is the vertical pixel coordinate of A'.
Basic trigonometry yields : tan(tilt) = -AH/OH, so Cz = OH = -AH/tan(tilt).
AH is calculated from the coordinates of H computed before.
---------------------------------------------------
Here's the Python code for the calculations; for the intersections of the circles, I've used the code from this post ; note that since we know that A is one of the intersections, the code could be simplified (CamPos is actually the symmetrical reflection of A in relation to (O1 O2)).
The results are (Cx, Cy) relative to A, in pixels, then Cz, also in pixels.
Note that the calculations only make sense if the overhead picture's dimensions are proportional to the real dimensions (since calculating distances only make sense in an orthonormal coordinate system).
import math
# Equirectangular info
A_eq = (472,274)
B_eq = (542,274)
C_eq = (535,260)
D_eq = (479,260)
width = 805
height = 374
# Overhead info
A = (267,321)
B = (377,321)
C = (377,274)
D = (267,274)
Rect_width = C[0] - A[0]
Rect_height = A[1] - C[1]
# Angle of view of edge [AB]
alpha = (B_eq[0] - A_eq[0]) / width * 2 * math.pi
# Center and squared radius of the circle of camera positions related to edge [AB]
x0 = Rect_width / 2
y0 = Rect_width / (2* math.tan(alpha))
r02 = x0**2 + y0**2
# Angle of view of edge [AD]
beta = (D_eq[0] - A_eq[0]) / width * 2 * math.pi
# Center and squared radius of the circle of camera positions related to edge [AD]
x1 = Rect_height / (2* math.tan(beta))
y1 = -Rect_height / 2
r12 = x1**2 + y1**2
def get_intersections(x0, y0, r02, x1, y1, r12):
# circle 1: (x0, y0), sq_radius r02
# circle 2: (x1, y1), sq_radius r12
d=math.sqrt((x1-x0)**2 + (y1-y0)**2)
a=(r02-r12+d**2)/(2*d)
h=math.sqrt(r02-a**2)
x2=x0+a*(x1-x0)/d
y2=y0+a*(y1-y0)/d
x3=x2+h*(y1-y0)/d
y3=y2-h*(x1-x0)/d
x4=x2-h*(y1-y0)/d
y4=y2+h*(x1-x0)/d
return (round(x3,2), round(y3,2), round(x4,2), round(y4,2))
# The intersection of these 2 circles are A and Camera_Base_Position (noted H)
inters = get_intersections(x0, y0, r02, x1, y1, r12)
H = (Cx, Cy) = (inters[2], inters[3])
print(H)
def get_elevation(camera_base, overhead_point, equirect_point):
tilt = (equirect_point[1])/height * math.pi
x , y = overhead_point[0] - A[0] , overhead_point[1] - A[1]
base_distance = math.sqrt((camera_base[0] - x)**2 + (camera_base[1] - y)**2 )
Cz = -base_distance / math.tan(tilt)
return Cz
print(get_elevation(H, A, A_eq))
print(get_elevation(H, B, B_eq))
print(get_elevation(H, C, C_eq))
print(get_elevation(H, D, D_eq))
# (59.66, 196.19) # These are (Cx, Cy) relative to point A
# 185.36640516274633 # These are the values of the elevation Cz
# 183.09278981601847 # when using A and A', B and B' ...
# 176.32257112738986
# 177.7819910650333
I am trying to use scipy.odr to get a best fit plane for some x, y, z points.
I define the plane equation implicitly as ax + by + cz + d = 0 and I perform a least squares (with scipy.linalg.lstsq) to provide the odr with an initial estimation.
The components of the beta vector (where beta = [a, b, c, d]) returned by the odr are of a magnitude between 1e167 and 1e172... Is such a result trustworthy? I find the numbers to be absurd...
Note that the points come from 3D scanning of a relatively flat face which is almost parallel to the xz plane (nearly vertical).
Here is the pprint() of the odr result:
'
Beta: [ 3.14570111e-170 3.21821458e-169 4.49232028e-172 4.49374557e-167]
Beta Std Error: [ 0. 0. 0. 0.]
Beta Covariance: [[ 6.37459471e-10 -8.57690019e-09 -2.18092934e-11 -1.13009384e-06]
[ -8.57690019e-09 5.11732570e-07 1.30123070e-09 6.74263262e-05]
[ -2.18092934e-11 1.30123070e-09 5.22674068e-12 1.70799469e-07]
[ -1.13009384e-06 6.74263262e-05 1.70799469e-07 8.88444676e-03]]
Residual Variance: 0.0
Inverse Condition #: 0.0010484041422201213
Reason(s) for Halting:
Sum of squares convergence
None
'
The code I am using :
import numpy as np
import scipy.linalg
from scipy import odr
import pickle
def planar_fit(points):
# best-fit linear plane
a = np.c_[points[:, 0], points[:, 1], np.ones(points.shape[0])]
c, _, _, _ = scipy.linalg.lstsq(a, points[:, 2]) # coefficients
# The coefficients are returned as an array beta=[a, b, c, d] from the implicit form 'a*x + b*y + c*z + d = 0'.
beta = np.r_[c[0], c[1], -1, c[2]] / c[2]
return beta
def odr_planar_fit(points):
def f_3(beta, xyz):
""" implicit definition of the plane"""
return beta[0] * xyz[0] + beta[1] * xyz[1] + beta[2] * xyz[2] + beta[3]
# # Coordinates of the 2D points
x = points[:, 0]
y = points[:, 1]
z = points[:, 2]
# Use least squares for initial estimate.
beta0 = planar_fit(points)
# Create the data object for the odr. The equation is given in the implicit form 'a*x + b*y + c*z + d = 0' and
# beta=[a, b, c, d] (beta is the vector to be fitted). The positional argument y=1 means that the dimensionality
# of the fitting is 1.
lsc_data = odr.Data(np.row_stack([x, y, z]), y=1)
# Create the odr model
lsc_model = odr.Model(f_3, implicit=True)
# Create the odr object based on the data, the model and the first estimation vector.
lsc_odr = odr.ODR(lsc_data, lsc_model, beta0)
# run the regression.
lsc_out = lsc_odr.run()
return lsc_out, beta0
def main():
#import from pickle.
with open('./points.pkl', 'rb') as f:
points = np.array(pickle.load(f))
# Perform the ODR
odr_out, lstsq = odr_planar_fit(points)
print(lstsq)
print(odr_out.pprint())
main()
The pickle containing my points.
ODR is completely fine with multidimensional data, you were going the correct direction.
You just chose bad to use the implicit version of ODR with your f_3 definition. The problem is you have a function A*X=0 which you try to minimize without any additional constraints. Of course, the best the optimizer can do is to minimize the magnitude of A towards zero - that minimizes the error the best! For the implicit optimization to work, you need to somehow introduce a constraint on magnitude of A, e.g. by dividing by the last number:
def f_3(beta, xyz):
""" implicit definition of the plane"""
return beta[0]/beta[3] * xyz[0] + beta[1]/beta[3] * xyz[1] + beta[2]/beta[3] * xyz[2] + 1.0
This way, the optimizer has no other option than to do what you wanted it to do :)
Alternatively, you can also convert your model to the explicit form y = ax + cz + d, which doesn't suffer from the magnitude problems (as b == 1 all the time).
Of course, you could get additional precision by shifting your points to origin and scaling them to have a unit variance in distance.
Since I'm also about to use ODR, I was curious about its properties, so I played around to find out how precise and sensitive it is, and here's the result: https://gist.github.com/peci1/fb1cea77c41fe8ace6c0db8ef82539a3 .
I tested both implicit and exlicit ODR, with and without normalization, and with initial guess either from LSQ or a bad one (to see how sensitive to the guess it is). It looked like this on your data:
Basically, the yellow and grey planes are the implicit fits without normalization, which came out pretty bad, and the rest of ODR fits is more or less the same. You can see the ODR fits differ a bit from the (faint blue) LSQ fit (which is expected).
As far as I understand the odr it is not made for 3D data, but I might be wrong here. As this is a simple plane fit, I suggest to use simple leastsq. Moreover, note that you do not really have 4 free parameters as you can divide a * x + b * y + c * z + d = 0 e.g. by d providing a' * x + b' * y + c' * z + 1 = 0 ( if d is not zero ).
If instead we write the plane in the form: all points P for which(P - p0) * n = 0 we already have the odr function for free. One can simplify by assuming that the plane offset vector p0 = s * n is the scaled normal vector. Like this there are 3 free parameters, the scale s and the direction angles of the normal vector (theta, phi).
The according fit looks as follows
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
from scipy.optimize import leastsq
from random import random
# for rotating test data
def y_mx( theta ):
out = np.array( [ np.cos( theta ),0, np.sin( theta ), 0, 1, 0, -np.sin( theta ),0, np.cos( theta ) ] )
return out.reshape( 3, 3)
# for rotating test data
def z_mx( theta ):
out = np.array( [ np.cos( theta ), np.sin( theta ), 0, -np.sin( theta ), np.cos( theta ), 0, 0, 0, 1 ] )
return out.reshape( 3, 3)
# for test data
def make_plane( theta, phi, px, py, pz, n=100 ):
points=[]
for i in range( n ):
x = 1 - 2 * random( )
y = 1 - 2 * random( )
z = 0.15 * ( 1 - 2 * random() )
points += [ np.array( [ x, y, z] ) ]
points = np.array( points)
points = [ np.array( [px, py, pz ] ) + np.dot( z_mx( phi ), np.dot( y_mx( theta ) , p ) ) for p in points ]
return np.array( points )
# residual function for leastsq
# note the plane equation is (P - p0) n = 0 if P is member of plane
# and n is normal vector of plane directly provides the normal distance function
# moreover p0 can be chosen to be s * n
def residuals( params, points ):
scale, theta, phi = params
nVector = np.array( [ np.sin( theta ) * np.cos( phi ), np.sin( theta ) * np.sin( phi ), np.cos( theta ) ] )
p0 = scale * nVector
diff = [ np.dot( p - p0, nVector ) for p in points]
return diff
# some test data
pnts = make_plane( 1.5, 1.49, .15, .2, .33)
#and the fit
guess=[ 0, 0, 0 ]
bestfit, err = leastsq( residuals, guess, pnts )
#the resulting normal vectot and offset
nVectorFit = np.array( [ np.sin( bestfit[1] ) * np.cos( bestfit[2] ), np.sin( bestfit[1] ) * np.sin( bestfit[2] ), np.cos( bestfit[1] ) ] )
p0Fit = bestfit[0] * nVectorFit
# converting to standard plane equation
a = nVectorFit[0] / nVectorFit[1]
c = nVectorFit[2] / nVectorFit[1]
d = bestfit[0] / nVectorFit[1]
# plane equation data
X = np.linspace( -.6, .6, 20 )
Z = np.linspace( -.6, .6, 20 )
XX, ZZ = np.meshgrid( X, Z )
YY = -a * XX - c * ZZ + d
#plotting
fig = plt.figure()
ax = fig.add_subplot( 1, 1, 1, projection='3d')
# original data
ax.scatter( pnts[:,0], pnts[:,1] , pnts[:,2])
# offset vector
ax.plot( [0, p0Fit[0] ], [0, p0Fit[1] ], [0, p0Fit[2] ], color = 'r')
# fitted plane
ax.plot_wireframe(XX, YY, ZZ , color = '#9900bb')
ax.set_xlim( [-1,1] )
ax.set_ylim( [-1,1] )
ax.set_zlim( [-1,1] )
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_zlabel("z")
plt.show()
Providing
Blue points is noisy data, purple is the fitted plane, and red the offset vector.
It is easy to see that the for the case here y = a * x + c * z + d, a, c, d are calculated straight forward from the fit result.
My problem
I want to rotate a mayavi.mlab.imshow object with a 3x3 rotation matrix. The only method I could find for rotating this object is through setting the object's actor.orientation to [pitch, roll, yaw] (this order is inherited from vtk). My only problem is that I cannot find a way to convert a rotation matrix to the parameters requested by mayavi.
How do I rotate an object in mayavi with a rotation matrix or what transformation should I use to obtain the correct (pitch, roll and yaw) used by Mayavi/Vtk?
A near solution
I have found some code over here to transform a rotation matrix to different types of Euler angles (according to the order of rotation). Correct me at this point if I am wrong in assuming Euler angles is equivalent to pitch, roll, yaw. I have tried all the different conversions, but failed to find a correct one.
Trying every combination
I tested all the different transformations by rotating x, y and z vectors with my rotation matrix and testing the paramaters on the mayavi.mlab.imshow object. I used all the available transforms on both R and transpose(R) along with using the Euler the parameters in all 9 available orders, but could not find a correct combination:
import pylab as pl
import cameraTools #my own lib
from mayavi import mlab
im = pl.imread('dice.png', format='png')[:,:,0]*255 #1 color channel
rot = pl.r_[30, 80, 230]
R_orig = cameraTools.composeRotation(*(rot*pl.pi/180))
RList = [R_orig, R_orig.T]
for ii, inOrder in enumerate(['sxyz','sxzx','syxz','szxz','rzyx','rxzx','rzxy','rzxz','sxyx','syzx','syxy','szyx','rxyx','rxzy','ryxy','rxyz','sxzy','syzy','szxy','szyz','ryzx','ryzy','ryxz','rzyz']):
tries = 0
for outOrder in [[0,1,2], [0,2,1], [1, 0, 2], [1, 2, 0], [2, 0, 1], [2, 1, 0]]:
for R in RList:
for vector, color in zip([[800, 0, 0], [0, 800, 0], [0, 0, 800]],
[(1., 0., 0.), (0., 1., 0.), (0., 0., 1.)]):
c = pl.c_[[0, tries*1000, ii*1000]]
if ii == 0 and tries == 0: vector = pl.r_[vector]*5 #point out the first vector
lin = R_orig.dot(pl.c_[[0,0,0], vector]) + c
mlab.plot3d(*lin,
color = color,
tube_radius=5)
lin3D = mlab.imshow(im, colormap="gray")
rxyz = pl.array(cameraTools.euler_from_matrix(R, inOrder))*180/pi
i,j,k = outOrder
lin3D.actor.orientation = [rxyz[i], rxyz[j], rxyz[k]]
lin3D.actor.position = c.flatten()
tries +=1
mlab.draw()
mlab.show()
Output from Mayavi, the top left item is the starting point.
Sorry, it seems I did not concentrate hard enough. The answer is in row 3, column 5 with syxz input order and '[1,0,2]' output order. I now use the following function to convert a Rotation matrix to the required Euler angles:
def rotationToVtk(R):
'''
Concert a rotation matrix into the Mayavi/Vtk rotation paramaters (pitch, roll, yaw)
'''
def euler_from_matrix(matrix):
"""Return Euler angles (syxz) from rotation matrix for specified axis sequence.
:Author:
`Christoph Gohlke <http://www.lfd.uci.edu/~gohlke/>`_
full library with coplete set of euler triplets (combinations of s/r x-y-z) at
http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
Note that many Euler angle triplets can describe one matrix.
"""
# epsilon for testing whether a number is close to zero
_EPS = np.finfo(float).eps * 5.0
# axis sequences for Euler angles
_NEXT_AXIS = [1, 2, 0, 1]
firstaxis, parity, repetition, frame = (1, 1, 0, 0) # ''
i = firstaxis
j = _NEXT_AXIS[i+parity]
k = _NEXT_AXIS[i-parity+1]
M = np.array(matrix, dtype='float', copy=False)[:3, :3]
if repetition:
sy = np.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k])
if sy > _EPS:
ax = np.arctan2( M[i, j], M[i, k])
ay = np.arctan2( sy, M[i, i])
az = np.arctan2( M[j, i], -M[k, i])
else:
ax = np.arctan2(-M[j, k], M[j, j])
ay = np.arctan2( sy, M[i, i])
az = 0.0
else:
cy = np.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i])
if cy > _EPS:
ax = np.arctan2( M[k, j], M[k, k])
ay = np.arctan2(-M[k, i], cy)
az = np.arctan2( M[j, i], M[i, i])
else:
ax = np.arctan2(-M[j, k], M[j, j])
ay = np.arctan2(-M[k, i], cy)
az = 0.0
if parity:
ax, ay, az = -ax, -ay, -az
if frame:
ax, az = az, ax
return ax, ay, az
r_yxz = pl.array(euler_from_matrix(R))*180/pi
r_xyz = r_yxz[[1, 0, 2]]
return r_xyz
If I use matplotlib to generate a delaunay triangulation for a group of points, what is the most appropraite way of getting the circumcentres of the triangles that have been geenrated? I haven't yet managed to find an obvious method in the Triangulation library to do this.
You should be able to calculate it using matplotlib.delaunay.triangulate.Triangulation:
Triangulation(x, y)
x, y -- the coordinates of the points as 1-D arrays of floats
.
.
.
Attributes: (all should be treated as
read-only to maintain consistency)
x, y -- the coordinates of the points as 1-D arrays of floats.
circumcenters -- (ntriangles, 2) array of floats giving the (x,y)
coordinates of the circumcenters of each triangle (indexed by a triangle_id).
Adapted from one of the matplotlib examples (there is probably a cleaner way to do this, but it should work):
import matplotlib.pyplot as plt
import matplotlib.delaunay
import matplotlib.tri as tri
import numpy as np
import math
# Creating a Triangulation without specifying the triangles results in the
# Delaunay triangulation of the points.
# First create the x and y coordinates of the points.
n_angles = 36
n_radii = 8
min_radius = 0.25
radii = np.linspace(min_radius, 0.95, n_radii)
angles = np.linspace(0, 2*math.pi, n_angles, endpoint=False)
angles = np.repeat(angles[...,np.newaxis], n_radii, axis=1)
angles[:,1::2] += math.pi/n_angles
x = (radii*np.cos(angles)).flatten()
y = (radii*np.sin(angles)).flatten()
tt = matplotlib.delaunay.triangulate.Triangulation(x,y)
triang = tri.Triangulation(x, y)
# Plot the triangulation.
plt.figure()
plt.gca().set_aspect('equal')
plt.triplot(triang, 'bo-')
plt.plot(tt.circumcenters[:,0],tt.circumcenters[:,1],'r.')
plt.show()
Here is a function that computes them. It can also be used on other triangulation structures, e.g. scipy's Delaunay triangulation (see below).
def compute_triangle_circumcenters(xy_pts, tri_arr):
"""
Compute the centers of the circumscribing circle of each triangle in a triangulation.
:param np.array xy_pts : points array of shape (n, 2)
:param np.array tri_arr : triangles array of shape (m, 3), each row is a triple of indices in the xy_pts array
:return: circumcenter points array of shape (m, 2)
"""
tri_pts = xy_pts[tri_arr] # (m, 3, 2) - triangles as points (not indices)
# finding the circumcenter (x, y) of a triangle defined by three points:
# (x-x0)**2 + (y-y0)**2 = (x-x1)**2 + (y-y1)**2
# (x-x0)**2 + (y-y0)**2 = (x-x2)**2 + (y-y2)**2
#
# becomes two linear equations (squares are canceled):
# 2(x1-x0)*x + 2(y1-y0)*y = (x1**2 + y1**2) - (x0**2 + y0**2)
# 2(x2-x0)*x + 2(y2-y0)*y = (x2**2 + y2**2) - (x0**2 + y0**2)
a = 2 * (tri_pts[:, 1, 0] - tri_pts[:, 0, 0])
b = 2 * (tri_pts[:, 1, 1] - tri_pts[:, 0, 1])
c = 2 * (tri_pts[:, 2, 0] - tri_pts[:, 0, 0])
d = 2 * (tri_pts[:, 2, 1] - tri_pts[:, 0, 1])
v1 = (tri_pts[:, 1, 0] ** 2 + tri_pts[:, 1, 1] ** 2) - (tri_pts[:, 0, 0] ** 2 + tri_pts[:, 0, 1] ** 2)
v2 = (tri_pts[:, 2, 0] ** 2 + tri_pts[:, 2, 1] ** 2) - (tri_pts[:, 0, 0] ** 2 + tri_pts[:, 0, 1] ** 2)
# solve 2x2 system (see https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_2_%C3%97_2_matrices)
det = (a * d - b * c)
detx = (v1 * d - v2 * b)
dety = (a * v2 - c * v1)
x = detx / det
y = dety / det
return (np.vstack((x, y))).T
On the data from #JoshAdel's answer above, adding the following code:
cc = compute_triangle_circumcenters(np.vstack([tt.x, tt.y]).T, tt.triangle_nodes)
plt.plot(cc[:, 0], cc[:, 1], ".k")
I get the following figure:
It can also be used on scipy.spatial.Delaunay like this:
from scipy.spatial import Delaunay
xy_pts = np.vstack([x, y]).T
dt = Delaunay(xy_pts)
cc = compute_triangle_circumcenters(dt.points, dt.simplices)