I have been trying to get the common tangents to two rotated ellipse. I was following the method given by Edward Doolittle in the following thread. The two ellipses are given as the equation given in Wiki.
In the Matrix form the ellipse can be shown as this:
First ellipse is centered at (0,0) rotated by 45 degrees with semi-major and semi-minor axes length as 2,1. Second ellipse is centered at (15,0), rotated by 120 degrees with semi-major and semi-minor axes length as 3,1
Linear combination of the adjoint matrices of the two ellipses are per dual of two ellipse combined
I am getting this value
.
Then I tried to find to find the value of t which will make the conic (above matrix) degenerate.
I found the value of t to be (-0.05,0.29,2.46). However, when I put these values back into the above matrix I am not able to reduce the matrix to two variables form. I am always dealing with 3 variables. For example, if I put t = -0.05 then I get the following:
Can someone please help me with this?
It boils down to finding an algorithm for solving a system of two quadratic equations of two variables, by interpreting it as a projective geometry pencil of conics, then finding the three degenerate conics of the pencil together with a projective transformation that simplifies these three degenerate conics to the point of your being able to read off the solutions very easily in new simplifying coordinate system, and after that transforming them back to the original coordinate system.
I sketched an algorithm in python, I think it seems to work on your example... but I was in a hurry and did not check it properly, so there may be bugs...
import numpy as np
import math
# technical module, functions, details
def homogenize(x):
return np.array([x[0], x[1], 1])
def cos_sin(angle_deg):
return math.cos(angle_deg*math.pi/180), math.sin(angle_deg*math.pi/180)
def rotation(cs_sn):
return np.array([[cs_sn[0], -cs_sn[1]],
[cs_sn[1], cs_sn[0]]])
# defining the isometry (the rotation plus translation) transformation
# between the coordinate system aligned with the conic and a general (world)
# coordinate system
def isom_inverse(angle, translation):
'''
isometry from conic-aligned coordinate system (conic attached)
to global coordinate system (world system)
'''
cos_, sin_ = cos_sin(angle)
return np.array([[cos_, -sin_, translation[0]],
[sin_, cos_, translation[1]],
[ 0, 0, 1]])
def isom(angle, translation):
'''
isometry from global coordinate system (world system)
to conic-aligned coordinate system (conic attached)
'''
cos_, sin_ = cos_sin(-angle)
tr = - rotation((cos_, sin_)).dot(translation)
return np.array([[ cos_, -sin_, tr[0]],
[ sin_, cos_, tr[1]],
[ 0, 0, 1 ]])
# calculating the coinc defined by a pair of axes' lengts,
# axes rotation angle and center of the conic
def Conic(major, minor, angle, center):
D = np.array([[minor**2, 0, 0],
[ 0, major**2, 0],
[ 0, 0, -(minor*major)**2]])
U = isom(angle, center)
return (U.T).dot(D.dot(U))
# calculating the coinc dual to the conic defined by a pair of axes' lengths,
# axes rotation angle and center of the conic
def dual_Conic(major, minor, angle, center):
D_1 = np.array([[major**2, 0, 0],
[ 0, minor**2, 0],
[ 0, 0, -1]])
U_1 = isom_inverse(angle, center)
return (U_1).dot(D_1.dot(U_1.T))
# transforming the matrix of a conic into a vector of six coefficients
# of a quadratic equation with two variables
def conic_to_equation(C):
'''
c[0]*x**2 + c[1]*x*y + c[2]*y**2 + c[3]*x + c[4]*y + c[5] = 0
'''
return np.array([C[0,0], 2*C[0,1], C[1,1], 2*C[0,2], 2*C[1,2], C[2,2]])
# transforming the vector of six coefficients
# of a quadratic equation with two variables into a matrix of
# the corresponding conic
def equation_to_conic(eq):
'''
eq[0]*x**2 + eq[1]*x*y + eq[2]*y**2 + eq[3]*x + eq[4]*y + eq[5] = 0
'''
return np.array([[2*eq[0], eq[1], eq[3]],
[ eq[1], 2*eq[2], eq[4]],
[ eq[3], eq[4], 2*eq[5]]]) / 2
# given a point (x,y) define the vector (x^2, xy, y^2, x, y, 1)
def argument(x):
return np.array([x[0]**2, x[0]*x[1], x[1]**2, x[0], x[1], 1])
# given x = (x[0],x[1]) calculate the value of the quadratic equation with
# six coefficients coeff
def quadratic_equation(x, coeff):
'''
coeff[0]*x**2 + coeff[1]*x*y + coeff[2]*y**2 + coeff[3]*x + coeff[4]*y + coeff[5] = 0
'''
return coeff.dot( argument(x) )
# given a pair of conics, as a pair of symmetric matrices,
# calculate the vector k = (k[0], k[1], k[2]) of values for each of which
# the conic c1 - k[i]*c2 from the pencil of conics c1 - t*c2
# is a degenerate conic (the anti-symmetric product of a pair of linear forms)
# and also find the matrix U
# of the projective transformation that simplifies the geometry of
# the pair of conics, the geometry of the pencil c1 - t*c2 in general,
# as well as the geometry of the three degenerate conics in particular
def transform(c1, c2):
'''
c1 and c2 are 3 by 3 symmetric matrices of the two conics
'''
c21 = np.linalg.inv(c2).dot(c1)
k, U = np.linalg.eig(c21)
return k, U
# the same as before, but for a pair of equations instead of matrices of conics
def eq_transform(eq1, eq2):
'''
eq1 and eq2 = np.array([eq[0], eq[1], eq[2], eq[3], eq[4], eq[5]])
'''
C1 = equation_to_conic(eq1)
C2 = equation_to_conic(eq2)
return transform(C1, C2)
# realizing the matrix U as a projective transformation
def proj(U, x):
if len(x) == 2:
x = homogenize(x)
y = U.dot(x)
y = y / y[2]
return y[0:2]
# find the common points, i.e. points of intersection of a pair of conics
# represented by a pair of symmetric matrices
def find_common_points(c1, c2):
k, U = transform(c1, c2)
L1 = (U.T).dot((c1 - k[0]*c2).dot(U))
L2 = (U.T).dot((c1 - k[1]*c2).dot(U))
sol = np.empty((4,3), dtype=float)
for i in range(2):
for j in range(2):
sol[i+2*j,0:2] = np.array([math.sqrt(abs(L2[2,2] / L2[0,0]))*(-1)**i, math.sqrt(abs(L1[2,2] / L1[1,1]))*(-1)**j])
sol[i+2*j,0:2] = proj(U, sol[i+2*j,0:2])
sol[:,2] = np.ones(4)
return sol
# find the solutions, i.e. the points x=(x[0],x[1]) saisfying the pair
# of quadratic equations
# represented by a pair of vectors eq1 and eq2 of 6 coefficients
def solve_eq(eq1, eq2):
conic1 = equation_to_conic(eq1)
conic2 = equation_to_conic(eq2)
return find_common_points(conic1, conic2)
'''
Esample of finding the common tangents of a pair of conics:
conic 1: major axis = 2, minor axis = 1, angle = 45, center = (0,0)
conic 2: major axis = 3, minor axis = 1, angle = 120, center = (15,0)
'''
a = 2
b = 1
cntr = np.array([0,0])
w = 45
Q1 = Conic(a, b, w, cntr)
dQ1 = dual_Conic(a, b, w, cntr)
a = 3
b = 1
cntr = np.array([15,0])
w = 120
Q2 = Conic(a, b, w, cntr)
dQ2 = dual_Conic(a, b, w, cntr)
R = find_common_points(dQ1, dQ2)
print('')
print(R)
print('')
print('checking that the output forms common tangent lines: ')
print('')
print('conic 1: ')
print(np.diagonal(R.dot(dQ1.dot(R.T))) )
print('')
print('conic 2: ')
print(np.diagonal(R.dot(dQ2.dot(R.T))) )
#conic_to_equation(dQ1)
Some Explanations: Assume you want to find the intersection points of two conics C1 and C2. Let us assume for simplicity that they are real ellipses that intersect in four different points (to avoid complex numbers)
In the case of finding the common tangents to a pair of conics,
simply convert the two conics two corresponding duals and then find the intersection points
of the duals. These intersection points are the equation coefficients of the tangents of the original conics.
There are possibly several different geometric interpretations of this problem, but let us go with the pencil of conics. The two conics C1 and C2 are represented by 3 by 3 symmetric matrices with non-zero determinants, which I have denoted C1 and C2. The linear combination, called pencil of conics generated by C1 and C2, is the t-parametrized family of conics C1 - t*C2 , where t is just a number. What is crucial is that every conic C1 - tC2 passes through the intersection points of C1 and C2 and these are the only four points they all have in common. You can prove this by observing that if x.T * C1 * x = x.T * C1 * x = 0 then
x.T * (C1 - t*C2) * x = x.T * C1 * x - t * x.T * C2 * x = 0 - t*0 = 0. Furthermore, if you take an intersection point of C1 and C1 - t*C2, then C2 = C1 - t*C2 + s*C2 you can apply the same argument when s = t.
In this family of conics, there are three degenerate conics, that are geometrically three pairs of lines. They occur exactly when t is such that det( C1 - t*C2 ) = 0. This is a polynomial equation of degree 3 with respect to t, so there are three, say different solutions k[0], k[1], k[2], due to the niceness of the C1 and C2. Projectively speaking, each degenerate conic C1 - k[j]*C2 is a pair of lines and they have a common intersection point u[:,j] = [ u[0,j] : u[1,j] : u[2,j] ]. Moreover, rank(C1 - k[j]*C2) = 2, so ker(C1 - k[j]*C2) = 1. This point u[:,j] is characterized as a solution to the equation
(C1 - k[j]*C2) * u[:,j] = 0.
Since C2 is invertible (non-degenerate), multiply both sides of the equation by inverse(C2) and obtain the equivalent equation ( (inverse(C2) * C1) - k[j]*Identity ) * u[:,j] = 0 which is an eigenvalue equation, with k[j] as eigenvalue and u[:,j] as eigenvector. The output of the function transform() is the 1 by 3 array k of eigenvalues and the 3 by 3 matrix U = [ u[:,0], u[:,1], u[:,2] ] of eigenvectors.
Conjugating C1 - k[j]*C2 by U, i.e. (U.T)*(C1 - k[j]*C2)*U, is geometrically equivalent to performing a projective transformation that sends u[:,0] and u[:,1] to infinity and u[:,2] to the origin. Thus, U changes the coordinate system from a general one to a special coordinate system, in which two of the degenerate conics are given by pairs of parallel lines and combined they intersect in a rectangle. The third conic is represeneted by the diagnols of the rectangle.
In this new picture, the intersection problem can be easily solved, just by reading off one solution from the entries of the matrices (U.T)*(C1 - k[j]*C2)*U (the intersection points are the vertices of the rectangle, so if you find one of them, the others are simply mirror symmetric of each other).
I know all coordinates of tetrahedron and the point I would like to determine. So does anyone know how to do it? I've tried to determine the point's belonging to each triangle of tetrahedron, and if it's true to all triangles then the point is in the tetrahedron. But it's absolutely wrong.
For each plane of the tetrahedron, check if the point is on the same side as the remaining vertex:
bool SameSide(v1, v2, v3, v4, p)
{
normal := cross(v2 - v1, v3 - v1)
dotV4 := dot(normal, v4 - v1)
dotP := dot(normal, p - v1)
return Math.Sign(dotV4) == Math.Sign(dotP);
}
And you need to check this for each plane:
bool PointInTetrahedron(v1, v2, v3, v4, p)
{
return SameSide(v1, v2, v3, v4, p) &&
SameSide(v2, v3, v4, v1, p) &&
SameSide(v3, v4, v1, v2, p) &&
SameSide(v4, v1, v2, v3, p);
}
You define a tetrahedron by four vertices, A B C and D.
Therefore you also can have the 4 triangles defining the surface of the tetrahedron.
You now just check if a point P is on the other side of the plane. The normal of each plane is pointing away from the center of the tetrahedron.
So you just have to test against 4 planes.
Your plane equation looks like this: a*x+b*y+c*z+d=0 Just fill in the point values (x y z). If the sign of the result is >0 the point is of the same side as the normal, result == 0, point lies in the plane, and in your case you want the third option: <0 means it is on the backside of the plane.
If this is fulfilled for all 4 planes, your point lies inside the tetrahedron.
Starting from Hugues' solution, here is a simpler and (even) more efficient one:
import numpy as np
def tetraCoord(A,B,C,D):
# Almost the same as Hugues' function,
# except it does not involve the homogeneous coordinates.
v1 = B-A ; v2 = C-A ; v3 = D-A
mat = np.array((v1,v2,v3)).T
# mat is 3x3 here
M1 = np.linalg.inv(mat)
return(M1)
def pointInside(v1,v2,v3,v4,p):
# Find the transform matrix from orthogonal to tetrahedron system
M1=tetraCoord(v1,v2,v3,v4)
# apply the transform to P (v1 is the origin)
newp = M1.dot(p-v1)
# perform test
return (np.all(newp>=0) and np.all(newp <=1) and np.sum(newp)<=1)
In the coordinate system associated to the tetrahedron, the opposite face from the origin (denoted v1 here) is characterized by x+y+z=1. Thus, the half space on the same side as v1 from this face satisfies x+y+z<1.
As a comparison, here is the full code for comparing the methods proposed by Nico, Hugues and me:
import numpy as np
import time
def sameside(v1,v2,v3,v4,p):
normal = np.cross(v2-v1, v3-v1)
return (np.dot(normal, v4-v1) * np.dot(normal, p-v1) > 0)
# Nico's solution
def pointInside_Nico(v1,v2,v3,v4,p):
return sameside(v1, v2, v3, v4, p) and sameside(v2, v3, v4, v1, p) and sameside(v3, v4, v1, v2, p) and sameside(v4, v1, v2, v3, p)
# Hugues' solution
def tetraCoord(A,B,C,D):
v1 = B-A ; v2 = C-A ; v3 = D-A
# mat defines an affine transform from the tetrahedron to the orthogonal system
mat = np.concatenate((np.array((v1,v2,v3,A)).T, np.array([[0,0,0,1]])))
# The inverse matrix does the opposite (from orthogonal to tetrahedron)
M1 = np.linalg.inv(mat)
return(M1)
def pointInside_Hugues(v1,v2,v3,v4,p):
# Find the transform matrix from orthogonal to tetrahedron system
M1=tetraCoord(v1,v2,v3,v4)
# apply the transform to P
p1 = np.append(p,1)
newp = M1.dot(p1)
# perform test
return(np.all(newp>=0) and np.all(newp <=1) and sameside(v2,v3,v4,v1,p))
# Proposed solution
def tetraCoord_Dorian(A,B,C,D):
v1 = B-A ; v2 = C-A ; v3 = D-A
# mat defines an affine transform from the tetrahedron to the orthogonal system
mat = np.array((v1,v2,v3)).T
# The inverse matrix does the opposite (from orthogonal to tetrahedron)
M1 = np.linalg.inv(mat)
return(M1)
def pointInside_Dorian(v1,v2,v3,v4,p):
# Find the transform matrix from orthogonal to tetrahedron system
M1=tetraCoord_Dorian(v1,v2,v3,v4)
# apply the transform to P
newp = M1.dot(p-v1)
# perform test
return (np.all(newp>=0) and np.all(newp <=1) and np.sum(newp)<=1)
npt=100000
Pt=np.random.rand(npt,3)
A=np.array([0.1, 0.1, 0.1])
B=np.array([0.9, 0.2, 0.1])
C=np.array([0.1, 0.9, 0.2])
D=np.array([0.3, 0.3, 0.9])
inTet_Nico=np.zeros(shape=(npt,1),dtype=bool)
inTet_Hugues=inTet_Nico
inTet_Dorian=inTet_Nico
start_time = time.time()
for i in range(0,npt):
inTet_Nico[i]=pointInside_Nico(A,B,C,D,Pt[i,:])
print("--- %s seconds ---" % (time.time() - start_time)) # https://stackoverflow.com/questions/1557571/how-do-i-get-time-of-a-python-programs-execution
start_time = time.time()
for i in range(0,npt):
inTet_Hugues[i]=pointInside_Hugues(A,B,C,D,Pt[i,:])
print("--- %s seconds ---" % (time.time() - start_time))
start_time = time.time()
for i in range(0,npt):
inTet_Dorian[i]=pointInside_Dorian(A,B,C,D,Pt[i,:])
print("--- %s seconds ---" % (time.time() - start_time))
And here are the results, in terms of running time:
--- 15.621951341629028 seconds ---
--- 8.97989797592163 seconds ---
--- 4.597853660583496 seconds ---
[EDIT]
Based on the Tom's idea of vectorizing the process, if one wants to find which element of a mesh contains a given point, here is a somehow highly vectorized solution:
Input data:
node_coordinates: (n_nodes,3) array containing the coordinates of each node
node_ids: (n_tet, 4) array, where the i-th row gives the vertex indices of the i-th tetrahedron.
def where(node_coordinates, node_ids, p):
ori=node_coordinates[node_ids[:,0],:]
v1=node_coordinates[node_ids[:,1],:]-ori
v2=node_coordinates[node_ids[:,2],:]-ori
v3=node_coordinates[node_ids[:,3],:]-ori
n_tet=len(node_ids)
v1r=v1.T.reshape((3,1,n_tet))
v2r=v2.T.reshape((3,1,n_tet))
v3r=v3.T.reshape((3,1,n_tet))
mat = np.concatenate((v1r,v2r,v3r), axis=1)
inv_mat = np.linalg.inv(mat.T).T # https://stackoverflow.com/a/41851137/12056867
if p.size==3:
p=p.reshape((1,3))
n_p=p.shape[0]
orir=np.repeat(ori[:,:,np.newaxis], n_p, axis=2)
newp=np.einsum('imk,kmj->kij',inv_mat,p.T-orir)
val=np.all(newp>=0, axis=1) & np.all(newp <=1, axis=1) & (np.sum(newp, axis=1)<=1)
id_tet, id_p = np.nonzero(val)
res = -np.ones(n_p, dtype=id_tet.dtype) # Sentinel value
res[id_p]=id_tet
return res
The hack here is to do the matrix product with multidimensional arrays.
The where function takes the point(s) coordinates as the 3rd argument. Indeed, this function can be run on multiple coordinates at once; the output argument is of same length as p. It returns -1 if the corresponding coordinate is not in the mesh.
On a mesh consisting in 1235 tetrahedrons, this method is 170-180 times faster than looping over each tetrahedron. Such a mesh is very small, so this gap may increase for larger meshes.
Given 4 points A,B,C,D defining a non-degenerate tetrahedron, and a point P to test, one way would be to transform the coordinates of P into the tetrahedron coordinate system, for example taking A as the origin, and the vectors B-A, C-A, D-A as the unit vectors.
In this coordinate system, the coordinates of P are all between 0 and 1 if it is inside P, but it could also be in anywhere in the transformed cube defined by the origin and the 3 unit vectors.
One way to assert that P is inside (A,B,C,D) is by taking in turn as origin the points (A, B, C, and D) and the other three points to define a new coordinate system. This test repeated 4 times is effective but can be improved.
It is most efficient to transform the coordinates only once and reuse the SameSide function as proposed earlier, for example taking A as the origin, transforming into the (A,B,C,D) coordinates system, P and A must lie on the same side of the (B,C,D) plane.
Following is a numpy/python implementation of that test. Tests indicate this method is 2-3 times faster than the Planes method.
import numpy as np
def sameside(v1,v2,v3,v4,p):
normal = np.cross(v2-v1, v3-v1)
return ((np.dot(normal, v4-v1)*p.dot(normal, p-v1) > 0)
def tetraCoord(A,B,C,D):
v1 = B-A ; v2 = C-A ; v3 = D-A
# mat defines an affine transform from the tetrahedron to the orthogonal system
mat = np.concatenate((np.array((v1,v2,v3,A)).T, np.array([[0,0,0,1]])))
# The inverse matrix does the opposite (from orthogonal to tetrahedron)
M1 = np.linalg.inv(mat)
return(M1)
def pointInsideT(v1,v2,v3,v4,p):
# Find the transform matrix from orthogonal to tetrahedron system
M1=tetraCoord(v1,v2,v3,v4)
# apply the transform to P
p1 = np.append(p,1)
newp = M1.dot(p1)
# perform test
return(np.all(newp>=0) and np.all(newp <=1) and sameside(v2,v3,v4,v1,p))
I've vectorized Dorian and Hughes solutions to take the entire array of points as input. I also moved the tetraCoord function outside of the pointsInside function and renamed both, since there was no point in calling it for every point.
On my computer, #Dorian's solution and example runs in 2.5 seconds. On the same data, mine runs nearly a thousand times faster at 0.003 seconds. If one for some reason needs even more speed, importing the GPU cupy package as "np" pushes it into the 100 microsecond range.
import time
# alternatively, import cupy as np if len(points)>1e7 and GPU
import numpy as np
def Tetrahedron(vertices):
"""
Given a list of the xyz coordinates of the vertices of a tetrahedron,
return tetrahedron coordinate system
"""
origin, *rest = vertices
mat = (np.array(rest) - origin).T
tetra = np.linalg.inv(mat)
return tetra, origin
def pointInside(point, tetra, origin):
"""
Takes a single point or array of points, as well as tetra and origin objects returned by
the Tetrahedron function.
Returns a boolean or boolean array indicating whether the point is inside the tetrahedron.
"""
newp = np.matmul(tetra, (point-origin).T).T
return np.all(newp>=0, axis=-1) & np.all(newp <=1, axis=-1) & (np.sum(newp, axis=-1) <=1)
npt=10000000
points = np.random.rand(npt,3)
# Coordinates of vertices A, B, C and D
A=np.array([0.1, 0.1, 0.1])
B=np.array([0.9, 0.2, 0.1])
C=np.array([0.1, 0.9, 0.2])
D=np.array([0.3, 0.3, 0.9])
start_time = time.time()
vertices = [A, B, C, D]
tetra, origin = Tetrahedron(vertices)
inTet = pointInside(points, tetra, origin)
print("--- %s seconds ---" % (time.time() - start_time))
Thanks to Dorian's test case script i could work on yet another solution and compare it quickly to the ones so far.
the intuition
for a triangle ABC and a point P if one connects P to the corners to get the vectors PA, PB, PC and compares the two triangles X and Y that are spanned by PA,PC and PB,PC, then the point P lies within the triangle ABC if X and Y overlap.
Or in other words, it is impossible to construct the vector PA by linearly combining PC and PB with only positive coefficients if P is in the triangle ABC.
From there on i tried to transfer it to the tetrahedron case and read here that it is possible to check whether vectors are linearly independent by checking the determinant of the matrix constructed out of the vectors as columns to be non-zero.
I tried out various approaches using determinants and i stumbled across this one:
Let PA, PB, PC, PD be the connections of P to the tetrahedron points ABCD (i.e. PA = A - P, etc.). calculate the determinants detA = det(PB PC PD), detB, detC and detD (like detA).
Then the point P lies within the tetrahedron spanned by ABCD if:
detA > 0 and detB < 0 and detC > 0 and detD < 0
or
detA < 0 and detB > 0 and detC < 0 and detD > 0
so the determinants switch signs, starting from negative, or starting from positive.
Does it work ? Apparently. Why does it work ? I don't know, or at least, i can't proof it. Maybe someone else with better math skills can help us here.
(EDIT: actually barycentric coordinates can be defined using those determinants and in the end, the barycentric coordinates need to sum up to one. It is like comparing the volumes of the tetrahedra that are spanned by the combinations of P with the points A,B,C,D with the volume of the tetrahedron ABCD itself. The explained case with observing the determinant signs is still unclear whether it works in general and i don't recommend it)
i changed the test case to not check n points Pi against one tetrahedron T, but to check n points Pi against n tetrahedrons Ti. All of the answers still give correct results. I think the reason why this approach is faster is that it doesn't need a matrix inversion.
I left TomNorway's approach implemented with one tetrahedron, and i leave the vectorization of this new approach to others since i am not so familiar with python and numpy.
import numpy as np
import time
def sameside(v1,v2,v3,v4,p):
normal = np.cross(v2-v1, v3-v1)
return (np.dot(normal, v4-v1) * np.dot(normal, p-v1) > 0)
# Nico's solution
def pointInside_Nico(v1,v2,v3,v4,p):
return sameside(v1, v2, v3, v4, p) and sameside(v2, v3, v4, v1, p) and sameside(v3, v4, v1, v2, p) and sameside(v4, v1, v2, v3, p)
# Hugues' solution
def tetraCoord(A,B,C,D):
v1 = B-A ; v2 = C-A ; v3 = D-A
# mat defines an affine transform from the tetrahedron to the orthogonal system
mat = np.concatenate((np.array((v1,v2,v3,A)).T, np.array([[0,0,0,1]])))
# The inverse matrix does the opposite (from orthogonal to tetrahedron)
M1 = np.linalg.inv(mat)
return(M1)
def pointInside_Hugues(v1,v2,v3,v4,p):
# Find the transform matrix from orthogonal to tetrahedron system
M1=tetraCoord(v1,v2,v3,v4)
# apply the transform to P
p1 = np.append(p,1)
newp = M1.dot(p1)
# perform test
return(np.all(newp>=0) and np.all(newp <=1) and sameside(v2,v3,v4,v1,p))
#Dorian's solution
def tetraCoord_Dorian(A,B,C,D):
v1 = B-A ; v2 = C-A ; v3 = D-A
# mat defines an affine transform from the tetrahedron to the orthogonal system
mat = np.array((v1,v2,v3)).T
# The inverse matrix does the opposite (from orthogonal to tetrahedron)
M1 = np.linalg.inv(mat)
return(M1)
def pointInside_Dorian(v1,v2,v3,v4,p):
# Find the transform matrix from orthogonal to tetrahedron system
M1=tetraCoord_Dorian(v1,v2,v3,v4)
# apply the transform to P
newp = M1.dot(p-v1)
# perform test
return (np.all(newp>=0) and np.all(newp <=1) and np.sum(newp)<=1)
#TomNorway's solution adapted to cope with n tetrahedrons
def Tetrahedron(vertices):
"""
Given a list of the xyz coordinates of the vertices of a tetrahedron,
return tetrahedron coordinate system
"""
origin, *rest = vertices
mat = (np.array(rest) - origin).T
tetra = np.linalg.inv(mat)
return tetra, origin
def pointInside(point, tetra, origin):
"""
Takes a single point or array of points, as well as tetra and origin objects returned by
the Tetrahedron function.
Returns a boolean or boolean array indicating whether the point is inside the tetrahedron.
"""
newp = np.matmul(tetra, (point-origin).T).T
return np.all(newp>=0, axis=-1) & np.all(newp <=1, axis=-1) & (np.sum(newp, axis=-1) <=1)
# Proposed solution
def det3x3_Philipp(b,c,d):
return b[0]*c[1]*d[2] + c[0]*d[1]*b[2] + d[0]*b[1]*c[2] - d[0]*c[1]*b[2] - c[0]*b[1]*d[2] - b[0]*d[1]*c[2]
def pointInside_Philipp(v0,v1,v2,v3,p):
a = v0 - p
b = v1 - p
c = v2 - p
d = v3 - p
detA = det3x3_Philipp(b,c,d)
detB = det3x3_Philipp(a,c,d)
detC = det3x3_Philipp(a,b,d)
detD = det3x3_Philipp(a,b,c)
ret0 = detA > 0.0 and detB < 0.0 and detC > 0.0 and detD < 0.0
ret1 = detA < 0.0 and detB > 0.0 and detC < 0.0 and detD > 0.0
return ret0 or ret1
npt=100000
Pt= np.array([ np.array([p[0]-0.5,p[1]-0.5,p[2]-0.5]) for p in np.random.rand(npt,3)])
A=np.array([ np.array([p[0]-0.5,p[1]-0.5,p[2]-0.5]) for p in np.random.rand(npt,3)])
B=np.array([ np.array([p[0]-0.5,p[1]-0.5,p[2]-0.5]) for p in np.random.rand(npt,3)])
C=np.array([ np.array([p[0]-0.5,p[1]-0.5,p[2]-0.5]) for p in np.random.rand(npt,3)])
D=np.array([ np.array([p[0]-0.5,p[1]-0.5,p[2]-0.5]) for p in np.random.rand(npt,3)])
inTet_Nico=np.zeros(shape=(npt,1),dtype=bool)
inTet_Hugues=np.copy(inTet_Nico)
inTet_Dorian=np.copy(inTet_Nico)
inTet_Philipp=np.copy(inTet_Nico)
print("non vectorized, n points, different tetrahedrons:")
start_time = time.time()
for i in range(0,npt):
inTet_Nico[i]=pointInside_Nico(A[i,:],B[i,:],C[i,:],D[i,:],Pt[i,:])
print("Nico's: --- %s seconds ---" % (time.time() - start_time)) # https://stackoverflow.com/questions/1557571/how-do-i-get-time-of-a-python-programs-execution
start_time = time.time()
for i in range(0,npt):
inTet_Hugues[i]=pointInside_Hugues(A[i,:],B[i,:],C[i,:],D[i,:],Pt[i,:])
print("Hugues': --- %s seconds ---" % (time.time() - start_time))
start_time = time.time()
for i in range(0,npt):
inTet_Dorian[i]=pointInside_Dorian(A[i,:],B[i,:],C[i,:],D[i,:],Pt[i,:])
print("Dorian's: --- %s seconds ---" % (time.time() - start_time))
start_time = time.time()
for i in range(0,npt):
inTet_Philipp[i]=pointInside_Philipp(A[i,:],B[i,:],C[i,:],D[i,:],Pt[i,:])
print("Philipp's:--- %s seconds ---" % (time.time() - start_time))
print("vectorized, n points, 1 tetrahedron:")
start_time = time.time()
vertices = [A[0], B[0], C[0], D[0]]
tetra, origin = Tetrahedron(vertices)
inTet_Tom = pointInside(Pt, tetra, origin)
print("TomNorway's: --- %s seconds ---" % (time.time() - start_time))
for i in range(0,npt):
assert inTet_Hugues[i] == inTet_Nico[i]
assert inTet_Dorian[i] == inTet_Hugues[i]
#assert inTet_Tom[i] == inTet_Dorian[i] can not compare because Tom implements 1 tetra instead of n
assert inTet_Philipp[i] == inTet_Dorian[i]
'''errors = 0
for i in range(0,npt):
if ( inTet_Philipp[i] != inTet_Dorian[i]):
errors = errors + 1
print("errors " + str(errors))'''
Results:
non vectorized, n points, different tetrahedrons:
Nico's: --- 25.439453125 seconds ---
Hugues': --- 28.724457263946533 seconds ---
Dorian's: --- 15.006574153900146 seconds ---
Philipp's:--- 4.389788389205933 seconds ---
vectorized, n points, 1 tetrahedron:
TomNorway's: --- 0.008165121078491211 seconds ---
I am detecting a printed Aruco marker using opencv 3.2:
aruco::estimatePoseSingleMarkers(corners, markerLength, camMatrix, distCoeffs, rvecs,tvecs);
this returns a translation and rotation vector for the marker. What I need, is the 3d coordinates for each corner of the marker.
As i know the marker length, i could do something like
corner1 = tvecs[0] - markerlength /2;
corner2 = tvecs[0] + markerlength /2;
....
But is there an better way? Or an existing function?
To sum up, I have:
a 3d point in the center of a 2d square.
the length of the sides of that square.
the rotation value of the square.
How can I find the 3d coordinates of the corners?
First, let's assume that we only have one marker given with side = 2 * half_side.
Second, aruco::detectMarker returns the relative position of the camera in the marker's world. Thus, I assume that you are looking for the coordinates of the corners in camera's world.
Then, in marker's space:
[ half_side ] [ 0 ]
E = [ 0 ], F = [ half_side ]
[ 0 ] [ 0 ]
where the center O of the square has coordinate tvec (in camera's world) and rotation mat of the marker rot_mat is computed by cv::Rodrigues(rvec,rot_mat).
Now, using the pinhole camera model, the relation between coordinates of a point P in cam's world and marker's world is:
[P_x_cam] [P_x_marker]
[P_y_cam] = rot_mat * [P_y_marker] + tvec
[P_z_cam] [P_z_marker]
for example, the center O, which is [0,0,0] in marker's world, is tvec in cam's world.
So, the coordinates of E in cam's world are:
[E_x_cam] [half_side]
|E_y_cam| = rot_mat * | 0 | + tvec
[E_z_cam] [ 0 ]
Magically, it is the sum of rot_mat's first column multiplied by half_size and tvec. Similarly,
the coodinates of F is rot_mat's second column multiplied by half_size and tvec.
Now, the corners can be computed, for example
C - O = (E - O) + (F - O), B - O = (E - O) - (F - O)
where E-O is exactly rot_mat's first column multiplied by half_size.
With all that in mind, we can compose the function:
vector<Point3f> getCornersInCameraWorld(double side, Vec3d rvec, Vec3d tvec){
double half_side = side/2;
// compute rot_mat
Mat rot_mat;
Rodrigues(rvec, rot_mat);
// transpose of rot_mat for easy columns extraction
Mat rot_mat_t = rot_mat.t();
// the two E-O and F-O vectors
double * tmp = rot_mat_t.ptr<double>(0);
Point3f camWorldE(tmp[0]*half_side,
tmp[1]*half_side,
tmp[2]*half_side);
tmp = rot_mat_t.ptr<double>(1);
Point3f camWorldF(tmp[0]*half_side,
tmp[1]*half_side,
tmp[2]*half_side);
// convert tvec to point
Point3f tvec_3f(tvec[0], tvec[1], tvec[2]);
// return vector:
vector<Point3f> ret(4,tvec_3f);
ret[0] += camWorldE + camWorldF;
ret[1] += -camWorldE + camWorldF;
ret[2] += -camWorldE - camWorldF;
ret[3] += camWorldE - camWorldF;
return ret;
}
Note 1: I hate that SO doesn't have MathJax
Note 2: there must be some faster implementation which I don't know of.
A python implementation I wrote for the above described rotation of marker corners using rvec and tvec as returned from cv2.aruco.estimatePoseSingleMarkers(). Thanks #Quang Hoang for the detailed explanation.
import numpy as np
# rotate a markers corners by rvec and translate by tvec if given
# input is the size of a marker.
# In the markerworld the 4 markercorners are at (x,y) = (+- markersize/2, +- markersize/2)
# returns the rotated and translated corners and the rotation matrix
def rotate_marker_corners(rvec, markersize, tvec = None):
mhalf = markersize / 2.0
# convert rot vector to rot matrix both do: markerworld -> cam-world
mrv, jacobian = cv2.Rodrigues(rvec)
#in markerworld the corners are all in the xy-plane so z is zero at first
X = mhalf * mrv[:,0] #rotate the x = mhalf
Y = mhalf * mrv[:,1] #rotate the y = mhalf
minusX = X * (-1)
minusY = Y * (-1)
# calculate 4 corners of the marker in camworld. corners are enumerated clockwise
markercorners = []
markercorners.append(np.add(minusX, Y)) #was upper left in markerworld
markercorners.append(np.add(X, Y)) #was upper right in markerworld
markercorners.append(np.add( X, minusY)) #was lower right in markerworld
markercorners.append(np.add(minusX, minusY)) #was lower left in markerworld
# if tvec given, move all by tvec
if tvec is not None:
C = tvec #center of marker in camworld
for i, mc in enumerate(markercorners):
makercorners[i] = np.add(C,mc) #add tvec to each corner
#print('Vec X, Y, C, dot(X,Y)', X,Y,C, np.dot(X,Y)) # just for debug
markercorners = np.array(markercorners,dtype=np.float32) # type needed when used as input to cv2
return markercorners, mrv
'''
Copyright 2019 Marco Noll, Garmin International Inc. Licensed under the Apache
License, Version 2.0 (the "License"); you may not use this file except in compliance
with the License. You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software distributed
under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
CONDITIONS OF ANY KIND, either express or implied. See the License for the specific
language governing permissions and limitations under the License.
'''
Building upon #Quang's answer, C# code for transforming any point to camera coordinates. Of course it needs R and t vectors, so you're going to need a marker in order to get them.
private Point3d GetWorldPoint(Point3d input, Vec3d rvec, Vec3d tvec)
{
var rot_mat = new Mat();
Cv2.Rodrigues(MatOfDouble.FromArray(rvec.Item0, rvec.Item1, rvec.Item2), rot_mat);
var pointProject = (rot_mat * MatOfDouble.FromArray(input.X, input.Y, input.Z)).ToMat();
return tvec + new Point3d(pointProject.Get<double>(0, 0), pointProject.Get<double>(0, 1), pointProject.Get<double>(0, 2));
}
As known, in OpenCV I can get affine or perspective transformation between 2 images:
M - affine transformation - by using estimateRigidTransform()
H - perspective (homography) transformation - by using FeatureDetector (SIFT, SURF, BRISK, FREAK, ...), then FlannBasedMatcher and findHomography()
Then I can do:
affine transformation - by using warpAffine(img_src, img_dst, M)
perspective transformation - by using warpPerspective(img_src, img_dst, H)
But if I have 3 or more images, and I already found:
affine: M1 (img1 -> img2), M2 (img2 -> img3)
perspective: H1 (img1 -> img2), H2 (img2 -> img3)
then can I get matix of transformation (img1 -> img3) by simply add two matrix?
of an affine transform: M3 = M1 + M2;
of an perspective transform: H3 = H1 + H2;
Or which of functions should I use for this?
No, you need to multiply the matrices to get the cascaded effect. I won't go into the math, but applying a transformation to coordinates is a matter of performing a matrix multiplication. If you are however curious as to know why that is, I refer you to this good Wikipedia article on cascading matrix transformations. Given a coordinate X and a transformation matrix M, you get the output coordinate Y by:
Y = M*X
Here I use * to refer to matrix multiplication as opposed to element-wise multiplication. What you have is a pair of transformation matrices which go from img1 to img2 then img2 to img3. You'll need to do the operation twice. So to go from img1 to img2 where X belongs to the coordinate space of img1, we have (assuming we're using the affine matrices):
Y1 = M1*X
Next, to go from img2 to img3, we have:
Y2 = M2*Y1 --> Y2 = M2*M1*X --> Y2 = M3*X --> M3 = M2*M1
Therefore, to get the desired chain effect, you need to create a new matrix such that M2 is multiplied by M1. Same as H2 and H1.
So define a new matrix such that:
cv::Mat M3 = M2*M1;
Similarly for your projective matrices, you can do:
cv::Mat H3 = H2*H1;
However, estimateRigidTransform (the output is M in your case) gives you a 2 x 3 matrix. One trick is to augment this matrix so that it becomes 3 x 3 where we add an additional row where it is all 0 except for the last element, which is set to 1. Therefore, you would have the last row such that it becomes [0 0 1]. You would do this for both matrices, multiply them, then extract just the first two rows into a new matrix to pipe into warpAffine. Therefore, do something like this:
// Create padded matrix for M1
cv::Mat M1new = cv::Mat(3,3,M1.type());
M1new.at<double>(0,0) = M1.at<double>(0,0);
M1new.at<double>(0,1) = M1.at<double>(0,1);
M1new.at<double>(0,2) = M1.at<double>(0,2);
M1new.at<double>(1,0) = M1.at<double>(1,0);
M1new.at<double>(1,1) = M1.at<double>(1,1);
M1new.at<double>(1,2) = M1.at<double>(1,2);
M1new.at<double>(2,0) = 0.0;
M1new.at<double>(2,1) = 0.0;
M1new.at<double>(2,2) = 1.0;
// Create padded matrix for M2
cv::Mat M2new = cv::Mat(3,3,M2.type());
M2new.at<double>(0,0) = M2.at<double>(0,0);
M2new.at<double>(0,1) = M2.at<double>(0,1);
M2new.at<double>(0,2) = M2.at<double>(0,2);
M2new.at<double>(1,0) = M2.at<double>(1,0);
M2new.at<double>(1,1) = M2.at<double>(1,1);
M2new.at<double>(1,2) = M2.at<double>(1,2);
M2new.at<double>(2,0) = 0.0;
M2new.at<double>(2,1) = 0.0;
M2new.at<double>(2,2) = 1.0;
// Multiply the two matrices together
cv::Mat M3temp = M2new*M1new;
// Extract out relevant rows and place into M3
cv::Mat M3 = cv::Mat(2, 3, M3temp.type());
M3.at<double>(0,0) = M3temp.at<double>(0,0);
M3.at<double>(0,1) = M3temp.at<double>(0,1);
M3.at<double>(0,2) = M3temp.at<double>(0,2);
M3.at<double>(1,0) = M3temp.at<double>(1,0);
M3.at<double>(1,1) = M3temp.at<double>(1,1);
M3.at<double>(1,2) = M3temp.at<double>(1,2);
When dealing with cv::Mat and the * operator, it is overloaded to specifically perform matrix multiplication.
You can then use M3 and H3 into warpAffine and warpPerspective respectively.
Hope this helps!