I have a 3D vector and a 3D face normal. How do I go along to move this vector along the given face normal using Python (with or without numpy)?
Ideally, I'd build a matrix using the face normal with the x and y and multiply it by the original vector or something like that, but I can't get my head around on how to build it. It's been a while since Linear Algebra.
EDIT:
Thanks for pointing out that my question was too broad.
My goal is to get a new point, that is x and y units away from the original point, along the face defined by its normal.
Example: If the point is (0,0,0) and the normal is (0, 0, 1), the result would be (x, y, 0).
Example 2: If the point is (1, 0, 0) and the normal is (0, 1, 0), the result would be (1+x, 0, y).
I'd need to extrapolate that to work with any point, normal, x and y.
The projection of a vector to a plane defined by its normal is:
def projection(vector, normal):
return vector - vector.dot(normal) * normal
Presumably this means you want something like:
x + projection(y, normal)
def give_me_a_new_vertex_position_along_normal(old_vertex_position, normal):
new_vertex_position = old_vertex_position + normal
return new_vertex_position
There is a difference between affine spaces (your normals) and euclidean/linear spaces (your vertices).
Vectors in linear space have coordinates associated with them, while vectors in affine space do not.
Adding an affine-spaced vector to a linear-spaced vector is called projection and that is what you are looking to do.
Related
the below code transforms a detected 2D-image point to it's 3D location on a defined plane Grid in 3D-world.
This mean Z=0, and taking into account that the Extrinsics and Intrinsics are known, we can compute the corresponding 3D_point of the detected 2D-image point:
import cv2
import numpy as np
#load extrinsics & intrinsics
with np.load('parameters_cam1.npz') as X:
mtx, dist = [X[i] for i in ('mtx','dist','rvecs','tvecs')]
with np.load('extrincic.npz') as X:
rvecs1,tvecs1 = [X[i] for i in('rvecs1','tvecs1')]
#prepare rotation matrix
R_mtx, jac=cv2.Rodrigues(rvecs1)
#prepare projection matrix
Extrincic=cv2.hconcat([R_mtx,tvecs1])
Projection_mtx=mtx.dot(Extrincic)
#delete the third column since Z=0
Projection_mtx = np.delete(Projection_mtx, 2, 1)
#finding the inverse of the matrix
Inv_Projection = np.linalg.inv(Projection_mtx)
#detected image point (extracted from a que)
img_point=np.array([pts1_blue[0]])
#adding dimension 1 in order for the math to be correct (homogeneous coordinates)
img_point=np.vstack((img_point,np.array(1)))
#calculating the 3D point which located on the 3D plane
3D_point=Inv_Projection.dot(img_point)
#show results
print('3D_pt_method1\n',3D_point)
#output
3D_pt_method1
[[0.01881387]
[0.0259416 ]
[0.04150276]]
By normalizing the point (dividing by the third value) the result is
`X_World=0.45331611680765327 # 45.3 cm from defined world point cm which is correct
Y_world=0.6250572251098481 # 62.5 cm which is also correct
By evaluating the results, it turns out that they are correct.
I now that we can't retrieve the the Z coordinate of the 3D world point since depth information is lost going from 3d to 2d. The following equation also performs the inverse projection of the 2D point onto 3D world and can be found in all literature, and the result is an equation which represents a line on which the 3D_ world point must lie on
I put the equation 3.15 into code, however without setting Z=0, meaning to say with out deleting the third column of the projection matrix like i did in the previous method (Just as it's written) by doing the following the following:
#inverting the rotation matrix
INV_R=np.linalg.inv(R_mtx)
#inverting the camera matrix
INV_k=np.linalg.inv(mtx)
#multiplying the tow matrices
kinv_Rinv=INV_k.dot(INV_R)
#calcuating the 3D_point X which expressed in eq.3.15
3D_point=kinv_Rinv.dot(img_point)+tvecs1
#print the results
print('3D_pt_method2\n',3D_point)
and the result was
3D_pt_method2 #how should one understand these coordinates ?
[[-9.12505825]
[-5.57152147]
[40.12264881]]
My question is, How should i understand or interpret this result? as it doesn't make any sense compared to the previous method where Z=0. the 3D 3x1 resulted vector seems to give an intuition that it's values represents simply the 3D X, Y and Z of the detected image_point. However, this is not true if we compare X and Y with the previous method!!
So what is literally the difference between 3D_pt_method1 and 3D_pt_method2???
I hope i could express my self and really appreciate helping me understand the difference between the two implementations!
Note: the Grid that represents my defined World-plane and can be seen in the below image in which the distance between every two yellow points is 40 cm
Thanks in advance
You miss the key variable "w" in method2.
You can get help from referring to this article: https://blog.csdn.net/zhou4411781/article/details/103876478
This article is written in Chinese, but you can just try to get the point from those formula in that article if you cannot understand Chinese.
Simply speaking:
You said right "I know that we can't retrieve the the Z coordinate of the 3D world point since depth information is lost going from 3d to 2d. "
This also means: If you know the depth (the Z axis value in world coordination), you can get 3d ordinate by 2d ordinate and the depth. As well, if you know the X or Y axis value in world coordination, you can also get the result.
I am asking this questions as a trimmed version of my previous question. Now that I have a face looking some position on screen and also gaze coordinates (pitch and yaw) of both the eye. Let us say
Left_Eye = [-0.06222888 -0.06577308]
Right_Eye = [-0.04176027 -0.44416167]
I want to identify the screen coordinates where the person probably may be looking at? Is this possible? Please help!
What you need is:
3D position and direction for each eye
you claim you got it but pitch and yaw are just Euler angles and you need also some reference frame and order of transforms to convert them back into 3D vector. Its better to leave the direction in a vector form (which I suspect you got in the first place). Along with the direction you need th position in 3D in the same coordinate system too...
3D definition of your projection plane
so you need at least start position and 2 basis vectors defining your planar rectangle. Much better is to use 4x4 homogenous transform matrix for this because that allows very easy transform from and in to its local coordinate system...
So I see it like this:
So now its just matter of finding the intersection between rays and plane
P(s) = R0 + s*R
P(t) = L0 + t*L
P(u,v) = P0 + u*U +v*V
Solving this system will lead to acquiring u,v which is also the 2D coordinate inside your plane yo are looking at. Of course because of inaccuracies this will not be solvable algebraicaly. So its better to convert the rays into plane local coordinates and just computing the point on each ray with w=0.0 (making this a simple linear equation with single unknown) and computing average position between one for left eye and the other for right eye (in case they do not align perfectly).
so If R0',R',L0',L' are the converted values in UVW local coordinates then:
R0z' + s*Rz' = 0.0
s = -R0z'/Rz'
// so...
R1 = R0' - R'*R0z'/Rz'
L1 = L0' - L'*L0z'/Lz'
P = 0.5 * (R1 + L1)
Where P is the point you are looking at in the UVW coordinates...
The conversion is done easily according to your notations you either multiply the inverse or direct matrix representing the plane by (R,1),(L,1),(R0,0)(L0,0). The forth coordinate (0,1) just tells if you are transforming vector or point.
Without knowing more about your coordinate systems, data accuracy, and what knowns and unknowns you got is hard to be more specific than this.
If your plane is the camera projection plane than U,V are the x and y axis of the image taken from camera and W is normal to it (direction is just matter of notation).
As you are using camera input which uses a perspective projection I hope your positions and vectors are corrected for it.
I have the coordinates of 6 points in an image
(170.01954650878906, 216.98866271972656)
(201.3812255859375, 109.42137145996094)
(115.70114135742188, 210.4272918701172)
(45.42426300048828, 97.89037322998047)
(167.0367889404297, 208.9329833984375)
(70.13690185546875, 140.90538024902344)
I have a point as center [89.2458, 121.0896]. I am trying to re-calculate the position of points in python using 4 rotation degree (from 0,90,-90,180) and 6 scaling factor (0.5,0.75,1,1.10,1.25,1.35,1.5).
My question is how can I rotate and scale the abovementioned points relative to the center point and get the new coordinates of those 6 points?
Your help is really appreciated.
Mathematics
A mathematical approach would be to represent this data as vectors from the center to the image-points, translate these vectors to the origin, apply the transformation and relocate them around the center point. Let's look at how this works in detail.
Representation as vectors
We can show these vectors in a grid, this will produce following image
This image provides a nice way to look at these points, so we can see our actions happening in a visual way. The center point is marked with a dot at the beginning of all the arrows, and the end of each arrow is the location of one of the points supplied in the question.
A vector can be seen as a list of the values of the coordinates of the point so
my_vector = [point[0], point[1]]
could be a representation for a vector in python, it just holds the coordinates of a point, so the format in the question could be used as is! Notice that I will use the position 0 for the x-coordinate and 1 for the y-coordinate throughout my answer.
I have only added this representation as a visual aid, we can look at any set of two points as being a vector, no calculation is needed, this is only a different way of looking at those points.
Translation to origin
The first calculations happen here. We need to translate all these vectors to the origin. We can very easily do this by subtracting the location of the center point from all the other points, for example (can be done in a simple loop):
point_origin_x = point[0] - center_point[0] # Xvalue point - Xvalue center
point_origin_y = point[1] - center_point[1] # Yvalue point - Yvalue center
The resulting points can now be rotated around the origin and scaled with respect to the origin. The new points (as vectors) look like this:
In this image, I deliberately left the scale untouched, so that it is clear that these are exactly the same vectors (arrows), in size and orientation, only shifted to be around (0, 0).
Why the origin
So why translate these points to the origin? Well, rotations and scaling actions are easy to do (mathematically) around the origin and not as easy around other points.
Also, from now on, I will only include the 1st, 2nd and 4th point in these images to save some space.
Scaling around the origin
A scaling operation is very easy around the origin. Just multiply the coordinates of the point with the factor of the scaling:
scaled_point_x = point[0] * scaling_factor
scaled_point_y = point[1] * scaling_factor
In a visual way, that looks like this (scaling all by 1.5):
Where the blue arrows are the original vectors and the red ones are the scaled vectors.
Rotating
Now for rotating. This is a little bit harder, because a rotation is most generally described by a matrix multiplication with this vector.
The matrix to multiply with is the following
(from wikipedia: Rotation Matrix)
So if V is the vector than we need to perform V_r = R(t) * V to get the rotated vector V_r. This rotation will always be counterclockwise! In order to rotate clockwise, we simply need to use R(-t).
Because only multiples of 90° are needed in the question, the matrix becomes a almost trivial. For a rotation of 90° counterclockwise, the matrix is:
Which is basically in code:
rotated_point_x = -point[1] # new x is negative of old y
rotated_point_y = point[0] # new y is old x
Again, this can be nicely shown in a visual way:
Where I have matched the colors of the vectors.
A rotation 90° clockwise will than be
rotated_counter_point_x = point[1] # x is old y
rotated_counter_point_y = -point[0] # y is negative of old x
A rotation of 180° will just be taking the negative coordinates or, you could just scale by a factor of -1, which is essentially the same.
As last point of these operations, might I add that you can scale and/or rotated as much as you want in a sequence to get the desired result.
Translating back to the center point
After the scaling actions and/or rotations the only thing left is te retranslate the vectors to the center point.
retranslated_point_x = new_point[0] + center_point_x
retranslated_point_y = new_point[1] + center_point_y
And all is done.
Just a recap
So to recap this long post:
Subtract the coordinates of the center point from the coordinates of the image-point
Scale by a factor with a simply multiplication of the coordinates
Use the idea of the matrix multiplication to think about the rotation (you can easily find these things on Google or Wikipedia).
Add the coordinates of the center point to the new coordinates of the image-point
I realize now that I could have just given this recap, but now there is at least some visual aid and a slight mathematical background in this post, which is also nice. I really believe that such problems should be looked at from a mathematical angle, the mathematical description can help a lot.
(forgive my terminology - it has been a long time since I took an advanced math class)
Let's say I have n "planes" each "perpendicular" to a single axis in m-dimensional space. No two planes are perpendicular to the same axis. I believe I can safely assume that there will be some intersection between all n planes.
I want to project point a onto the intersection and get the position vector for the result.
For example:
I have a single plane whose normal vector is (0.75, 0, 0) and a point a at position (0.25, 0, 1). I want to get the position vector of point a projected onto the plane.
Another example:
I have two planes represented by normal vectors (0.5, 0, 0) and (0, 1, 0). I have a point a at position (0.1, 0.1, 0.1). I want to get the position vector of the point projected onto the result of the intersection between my two planes (a line)
Your "planes" in m-dimensional space are (m-1)-dimensional objects. They are usually referred to as hyperplanes — a generalization of planes, 2-dimensional objects in 3-dimensional space. To define a hyperplane you need not only a normal vector but also a point (think of lines in two-dimensional space: all parallel lines share the same direction, and in order to isolate one you need to specify a point).
I suspect you mean all of your hyperplanes to pass through the origin (in which case indeed there is a point in the intersection — the origin itself), and I interpret your "being perpendicular to a single axis" as saying that the normal vectors all point along some coordinate axis (in other words, they have a single nonzero component). In that case, all you have to do to find the projection of an arbitrary point (vector, really) onto the intersection is set to zero the components of the point (again, vector, really) along the normal vectors of your hyperplanes.
Let me go through your examples:
The (hyper)plane in 3-dimensional space with normal vector (0.75, 0, 0) is the yz-plane: the projection of an arbitrary point (x, y, z) is (0, y, z) — the hyperplane has a normal vector along the first coordinate, so set to zero the first component of the point (for the last time: vector, really). In particular, (0.25, 0, 1) projects to (0, 0, 1).
The planes perpendicular to (0.5, 0, 0) and (0, 1, 0) are the yz- and xz-planes. Their intersection is the z-axis. The projection of the point (0.1, 0.1, 0.1) is (0, 0, 0.1).
The projection can be computed by solving an overdetermined system in the sense of least squares, with lstsq. The columns of the matrix of the system is formed by normal vectors, used as columns (hence, transpose on the second line below).
coeff are coefficients to be attached to these normal vectors; this linear combination of normal vectors is subtracted from the given point to obtain the projection
import numpy as np
normals = np.transpose(np.array([[0.5, 0, 0], [0, 1, 0]])) # normals
point = np.array([0.1, 0.1, 0.1]) # point
coeff = np.linalg.lstsq(normals, point)[0]
proj = point - np.dot(normals, coeff)
print(proj)
Output: [0, 0, 0.1].
Skip to Update 2 below, if you don't want to read too much background.
I'm trying to implement a model for simple orbital simulations (two body).
However, when I try to use the code I've written, the plots generated from the result look quite odd.
The program uses initial state vectors (position and velocity) to calculate the Keplerian orbital elements, which are used to then calculate the next position, and returned as the next two state vectors.
This seems to work fine, and by itself, plots correctly as long as I keep the plot on the orbital plane. But I would like to rotate the plot to the frame of reference (the parent body) so that I can see a cool 3D view of what the orbits look like (obvs).
Right now, I suspect that the bug is in how I convert from the two state vectors in the orbital plane, to rotating them to the frame of reference. I am using the equations from step 6 of this document to create the following code from (but applying individual roation matricies [copied from here]):
from numpy import sin, cos, matrix, newaxis, asarray, squeeze, dot
def Rx(theta):
"""
Return a rotation matrix for the X axis and angle *theta*
"""
return matrix([
[1, 0, 0 ],
[0, cos(theta), -sin(theta) ],
[0, sin(theta), cos(theta) ],
], dtype="float64")
def Rz(theta):
"""
Return a rotation matrix for the Z axis and angle *theta*
"""
return matrix([
[cos(theta), -sin(theta), 0],
[sin(theta), cos(theta), 0],
[0, 0, 1],
], dtype="float64")
def rotate1(vector, O, i, w):
# The starting value of *vector* is just a 1-dimensional numpy
# array.
# Transform into a column vector.
vector = vector[:, newaxis]
# Perform the rotation
R = Rz(-O) * Rx(-i) * Rz(-w)
res2 = dot(R, vector)
# Transform back into a row vector (because that's what
# the rest of the program uses)
return squeeze(asarray(res2))
(For context, this is the full class I am using for the orbit model.)
When I plot X and Y coordinates from the result, I get this:
But when I change the rotation matrix to R = Rz(-O) * Rx(-i), I get this more plausible plot (although obviously missing one rotation, and slightly off-center):
And when I reduce it further to R = Rx(-i), as one would expect, I get this:
So as I said, I am fairly sure that it is not the orbital calculation code that is behaving weirdly, but rather some error in the rotation code. But I'm not sure where to narrow this down, as I'm pretty new to both numpy and matrix math in general.
Update: Based on stochastic's answer I transposed the matricies (R = Rz(-O).T * Rx(-i).T * Rz(-w).T), but then got this plot:
which made me wonder if my conversion to screen coordinates was somehow wrong -- but it looks correct to me (and is the same code as the more-correct plots with less rotation) namely:
def recenter(v_position, viewport_width, viewport_height):
x, y, z = v_position
# the size of the viewport in meters
bounds = 20000000
# viewport_width is the screen pixels (800)
scale = viewport_width/bounds
# Perform the scaling operation
x *= scale
y *= scale
# recenter to screen X and Y measured from the top-left corner
# of the viewport
x += viewport_width/2
y = viewport_height/2 - y
# Cast to int, because we don't care about pixel fractions
return int(x), int(y)
Update 2
Although I have triple-checked my implementation of the equations, as well as the rotations with stochastic's help, I still can't get the orbits to come out right. They still appear basically the same as in the plots above.
Using data from the NASA Horizon's system, I set up an orbit with specific state vectors from the ISS (2457380.183935185 = A.D. 2015-Dec-23 16:24:52.0000 (TDB)), and checked them against the Kepler orbit elements for the same moment in time, which produces this result:
inclination :
0.900246137041
0.900246137041
true_anomaly :
0.11497063007
0.0982485984565
long_of_asc_node :
3.80727461492
3.80727461492
eccentricity :
0.000429082122137
0.000501850615905
semi_major_axis :
6778560.7037
6779057.01374
mean_anomaly :
0.114872215066
0.0981501816537
argument_of_periapsis :
0.843226618347
0.85994864996
The top values are my (calculated) values, and the bottom values are the NASA ones. Obviously some floating point precision error is to be expected, but the variations in mean_anomaly and true_anomaly did strike me as larger than I expected. (I'm currently running all of my numpy calculations using float128 numbers on a 64-bit system).
In addition, the resulting orbit still looks like the (quite) eccentric first plot, above (even though I know that this LEO ISS orbit is quite circular). So I'm a bit stumped as to what the source of the problem could be.
I believe you have at least two problems.
After looking more closely at the orbital simulation you are doing (see this additional document from the comments), I think the main problem is the initially-very-reasonable-but-yet-untrue assumption that the final plot should look like an ellipse. In general it will not, since an orbiting body will not necessarily stay in a single plane.
The other problem, I think, is that your rotation matrices are the transpose of what they should be, per the document you described (see below).
On transposed rotation matrices
The document you cited does not directly specify whether R_x and R_z should be right-handed rotations of the axes or of the vector they will multiply, though you can figure it out from equation 9 (or 10). It turns out that they should be right-handed rotations of the axes, not the vector. That means that they should be defined like this:
return matrix([
[1, 0, 0 ],
[0, cos(theta), sin(theta) ],
[0,-sin(theta), cos(theta) ],
], dtype="float64")
instead of like this:
return matrix([
[1, 0, 0 ],
[0, cos(theta),-sin(theta) ],
[0, sin(theta), cos(theta) ],
], dtype="float64")
I found this out by reproducing equation 9 by hand on paper.
In that equation, look at the first component of the vector r(t).
There are two terms: one with o_x in it and one with o_y.
Look at the thing multliplying o_y. It is: -(sin(omega)*cos(Omega)+cos(omega)*cos(i)*sin(Omega)).
That leading minus sign is the key. It comes from the minus sign in the first row of your Rz matrix.
Since the Omega, i, and omega in equation 9 are all negated, that means that the minus sign needs to be on the second row of R_z, which would mean that R_z represents a right-handed rotation of the axes, not the vector.
Similarly, we can look at the o_y component of the last term and see that the minus sign needs to be on the second row of R_x, meaning (thank goodness for sanity) the both R_z and R_x right-handed rotations of the axes.
Your Rx and Rz functions are currently defining right handed rotations of a vector, not the axes.
You can fix this by either (all three are equivalent):
Removing the minus signs on your euler angles: Rz(O) * Rx(i) * Rz(w)
transposing your rotation matrices: Rz(-O).T * Rx(-i).T * Rz(-w).T
moving the - sign in the definition of Rx and Rz to the second row sine term, as shown above
I am going to mark stochastic's answer as right, because a) he deserves the points for being so helpful, and b) his advice was fundamentally correct.
However the source of the weird plot actually ended up being these lines in the linked Orbit class:
self.v_position = self.rotate(v_position, self.long_of_asc_node, self.inclination, self.argument_of_periapsis)
self.v_velocity = self.rotate(v_velocity, self.long_of_asc_node, self.inclination, self.argument_of_periapsis)
Notice that the self.v_position property is updated before the call to rotate the velocity vector happens; one might also notice, when reading the code, that I in my cleverness decided to make all of the orbital element values methods wrapped in #property decorators to make the calculations more clear.
But of course, this also means the methods are called -- and the values recalculated -- every time a property was accessed. So the second call to self.rotate() happens with slightly different values of the orbital elements from the first call and, more importantly, with values that don't match up 100% correctly with the "current" position and velocity state vectors!
So after a few days of banging my head against this bug, I figured it out from a bit of yak-shaving I was doing in the form of a refactoring, and now it all works perfectly.