Point cloud using depth at each pixel location - point-cloud-library

Is there any way to get Point cloud from the depth data at each pixel. I have zed stereo camera which give the depth information at each pixel location but i want x and y too.
Zed camera directly give Point cloud also but it is Unorganized and i want the organized one.

Yes Dexter, i have the resolution means the image rows and columns and at each pixel location z coordinate (depth) is known.
Actually now i have got the answer how to find X and Y coordinates.
X = x1 * Z / f
Y = y1 * Z / f
where x1 and y1 are columns and rows.
For more you can look at this link.
http://www.dis.uniroma1.it/~iocchi/stereo/triang.html

Related

Compute 3D angle given 2D vectors and their magnitude ratio

I have been trying to figure out whether the following problem has a solution. Almost having given up, I would like to ask whether someone can confirm that there is no solution, or maybe give me a hint.
We have two vectors v and w in 3D space and know that the ratio of their magnitudes is ||v|| / ||w|| = 0.8019.
in 3D space an observer would see that they form an angle of 27.017 degrees.
on the other side, an observer in 2D (only seeing the x and z axis), observes an angle of 7.125 degrees between the vectors.
From their view, the vector coordinates are v = (x: 2, z: 1) and w = (x: 3, z: 2).
Is there somehow a way that the 2D observer can calculate the actual angle between these vectors in 3D space?
I would be more than happy for any input. All my tries have failed so far and I just want to know whether there could be a possible solution.
I have solved this problem and get that the values of y1 and y2 are given by this function:
eq1: 0.6439*y2^(2)-y1^(2)=9.785.
Therefore real angle can practically any value, the factor that would narrow this problem down to an actual solution would be the information about where the observer is in the 3d space so that he sees the angle of 27.017º, however, if this is the whole problem, then I can share my solution and process.
Some graphs that I created from my calculations:
The side view of the vectors is directly from the point of view of the x and z axis of the graph, therefore the coordinates of the (x1,z1) and (x2,z2) points(terminal points of the vectors), appear authentic, and not augmented, hence you can use them in your calculations to calculate the coordinates of z1 and z2, which you need to calculate the angle.
V = (x1, y1, z1) V = (2, y1, 1)
W = (x2, y2, z2) W = (3, y2, 2)
Since ||v|| / ||w|| = 0.8019
∴Then sqrt((x1^2)+(y1^2)+(z1^2))/sqrt((x2^2)+(y2^2)+(z2^2)) = 0.8019
∴(x1^2)+(y1^2)+(z1^2)/(x2^2)+(y2^2)+(z2^2) = 0.6430
∴4+(y1^2)+1/9+(y2^2)+4 = 0.6430
∴5+(y1^2) = 8.359 + 0.6430(y2^2)
∴13.359 = 0.6430(y2^2)-(y1^2)
This gives you therefore a function that calculates the other value of y given the some input y.
You can then graph this function using Geogebra.
For all the pairs of values on the curve, together with the fixed values of x and z for both of the vectors you can calculate that the ratio between the magnitudes of the two vectors is equal to 0.8019.
This problem has therefore infinitely many solutions for the angle as there are infinitely many values of z1 and z2 that satisfy the ratio; ||v|| / ||w|| = 0.8019.
Therefore the answer to this problem can be expressed as:
∀Θº∈R:Θº≥0

I have a dart board. How can I assign points for the different sections?

PLEASE do not just post a solution to my problem. For me, this is all about understanding how to do this and be able to explain to myself and others how this and that makes it all work!
I have a dart board I created with turtle. I can post it if someone really wants to see it.
Now, I need to create a function that will create a random spot on the board to hit, then incorporate the point value for that spot. The random point is simple. But is there a way that I can assign the correct value to an AREA without having to name EVERY coordinate one by one?
Say your dart board is centered at (x0, y0), and you have a dart at (x, y). You need to translate your dart into polar coordinates (phi, r):
r = sqrt((x - x0) ** 2, (y - y0) ** 2)
phi = math.atan2(y, x)
Then figure out whether r makes your dart is in center, inner, mid or outer ring, and in which section of the circle your phi lies.
What you need is a reverse transform, one that given x,y coordinates can identify which area it belongs to.
The best way to deal with this problem is to think in terms of coordinate systems. The area of a dart board is specified by its angle and radius. You must convert your x,y coordinates to an angle and radius, then determining the area it falls within will be simple.
Determining the angle is best done with an arctan2 function, which can directly convert an x,y offset into an angle. The radius is a simple sqrt(x**2 + y**2) once you have subtracted the center point from x,y.
good ol' Pythagoras
Let the point (x1, y1) be the center of the dart board that is used from the constructor. and let (x2, y2) be the random point on the board you find.
Use this formula to find d the distance from the center. Then you just need a few if statements
if 0 <= d || d <= 2:
# area 0
elif d < 2 || 4 <= d:
# area 1
elif d < 4 || 6 <= d:
# area 2

In 3D space, how do I calculate a point at a certain height, that is linear with 2 other known points?

background
I am working on a 3d game, where I need to transfer my mouse-input to a certain height in the 3d world, so I can move a unit across a surface.
This is done from the perspective of a tilted camera (45 degress tilted). the playcanvas API supplies me with a function on my camera, which can translate my mouse input on the 2d screen, into the 3d world using a set depth.
when I set my depth to 0, I get the exact coordinates of my camera (which in this case is (0,80,80), and if I set the depth to 100, I get ~(0.09, 11.52, 7.06)...
I need a way to figure out either the coordinates where the line between the two points hits the height (y-axis of 2), OR I could find a way to calculate the depth I need to supply the camera in order to get back the coordinates I need
which leads me to:
I have 2 coordinates in 3d space: let's say p1(0, 80, 80) and p2(0.09, 11.52, 7.06).
I want to figure out how to calculate a 3rd point, where y = 2. This point should be aligned with the 2 other points.
Any suggestions to how I can achieve this? I am looking for a solution with as few calculations as possible.
One way to do it is to write a family of equations that describes all of the points in space that are collinear with p1 and p2.
x = x1*t + x2*(1-t)
y = y1*t + y2*(1-t)
z = z1*t + z2*(1-t)
... Where x1,y1,z1 are the coordinates of p1, ditto for p2, and t is any real number.
We can find the coordinates of our particular desired point by solving for t. We know that y = 2, so we'll rearrange that equation.
y = y1*t +y2 - y2*t
y - y2 = y1*t - y2*t
y - y2 = (y1 - y2)*t
(y - y2) / (y1 - y2) = t
t = (y - y2) / (y1 - y2)
Now that you know t, you can plug it into the two remaining equations to get your x and z values.

Triangulating coordinates with an equation

Ok, I know this sounds really daft to be asking here, but it is programming related.
I'm working on a game, and I'm thinking of implementing a system that allows users to triangulate their 3D coordinates to locate something (eg for a task).
I also want to be able to let the user make the coordinates of the points they are using for triangulation have user-determined coordinates (so the location's coordinate is relative, probably by setting up a beacon or something).
I have a method in place for calculating the distance between the points, so essentially I can calculate the lengths of the sides of the triangle/pyramid as well as all but the coordinate I am after.
It has been a long time since I have done any trigonometry and I am rusty with the sin, cos and tan functions, I have a feeling they are required but have no clue how to implement them.
Can anyone give me a demonstration as to how I would go about doing this in a mathematical/programatical way?
extra info:
My function returns the exact distance between the two points, so say you set two points to 0,0,0 and 4,4,0 respectively, and those points are set to scale(the game world is divided into a very large 3d grid, with each 'block' area being represented by a 3d coordinate) then it would give back a value at around 5.6.
The key point about it varying is that the user can set the points, so say they set a point to read 0,0,0, the actual location could be something like 52, 85, 93. However, providing they then count the blocks and set their other points correctly (eg, set a point 4,4,0 at the real point 56, 89, 93) then the final result will return the relative position (eg the object they are trying to locate is at real point 152, 185, 93, it will return the relative value 100,100,0). I need to be able to calculate it knowing every point but the one it's trying to locate, as well as the distances between all points.
Also, please don't ask why I can't just calculate it by using the real coordinates, I'm hoping to show the equation up on screen as it calculates the result.7
Example:
Here is a diagram
Imagine these are points in my game on a flat plain.
I want to know the point f.
I know the values of points d and e, and the sides A,B and C.
Using only the data I know, I need to find out how to do this.
Answered Edit:
After many days of working on this, Sean Kenny has provided me with his time, patience and intellect, and thus I have now got a working implementation of a triangulation method.
I hope to place the different language equivalents of the code as I test them so that future coders may use this code and not have the same problem I have had.
I spent a bit of time working on a solution but I think the implementer, i.e you, should know what it's doing, so any errors encountered can be tackled later on. As such, I'll give my answer in the form of strong hints.
First off, we have a vector from d to e which we can work out: if we consider the coordinates as position vectors rather than absolute coordinates, how can we determine what the vector pointing from d to e is? Think about how you would determine the displacement you had moved if you only knew where you started and where you ended up? Displacement is a straight line, point A to B, no deviation, not: I had to walk around that house so I walked further. A straight line. If you started at the point (0,0) it would be easy.
Secondly, the cosine rule. Do you know what it is? If not, read up on it. How can we rearrange the form given in the link to find the angle d between vectors DE and DF? Remember you need the angle, not a function of the angle (cos is a function remember).
Next we can use a vector 'trick' called the scalar product. Notice there is a cos function in there. Now, you may be thinking, we've just found the angle, why are we doing it again?
Define DQ = [1,0]. DQ is a vector of length 1, a unit vector, along the x-axis. Which other vector do we know? Do we know of two position vectors?
Once we have two vectors (I hope you worked out the other one) we can use the scalar product to find the angle; again, just the angle, not a function of it.
Now, hopefully, we have 2 angles. Could we take one from the other to get yet another angle to our desired coordinate DF? The choice of using a unit vector earlier was not arbitrary.
The scalar product, after some cancelling, gives us this : cos(theta) = x / r
Where x is the x ordinate for F and r is the length of side A.
The end result being:
theta = arccos( xe / B ) - arccos( ( (A^2) + (B^2) - (C^2) ) / ( 2*A*B ) )
Where theta is the angle formed between a unit vector along the line y = 0 where the origin is at point d.
With this information we can find the x and y coordinates of point f relative to d. How?
Again, with the scalar product. The rest is fairly easy, so I'll give it to you.
x = r.cos(theta)
y = r.sin(theta)
From basic trigonometry.
I wouldn't advise trying to code this into one value.
Instead, try this:
//pseudo code
dx = 0
dy = 0 //initialise coordinates somehow
ex = ex
ey = ey
A = A
B = B
C = C
cosd = ex / B
cosfi = ((A^2) + (B^2) - (C^2)) / ( 2*A*B)
d = acos(cosd) //acos is a method in java.math
fi = acos(cosfi) //you will have to find an equivalent in your chosen language
//look for a method of inverse cos
theta = fi - d
x = A cos(theta)
y = A sin(theta)
Initialise all variables as those which can take decimals. e.g float or double in Java.
The green along the x-axis represents the x ordinate of f, and the purple the y ordinate.
The blue angle is the one we are trying to find because, hopefully you can see, we can then use simple trig to work out x and y, given that we know the length of the hypotenuse.
This yellow line up to 1 is the unit vector for which scalar products are taken, this runs along the x-axis.
We need to find the black and red angles so we can deduce the blue angle by simple subtraction.
Hope this helps. Extensions can be made to 3D, all the vector functions work basically the same for 3D.
If you have the displacements from an origin, regardless of whether this is another user defined coordinate or not, the coordinate for that 3D point are simply (x, y, z).
If you are defining these lengths from a point, which also has a coordinate to take into account, you can simply write (x, y, z) + (x1, y1, z1) = (x2, y2, z2) where x2, y2 and z2 are the displacements from the (0, 0, 0) origin.
If you wish to find the length of this vector, i.e if you defined the line from A to B to be the x axis, what would the x displacement be, you can use Pythagoras for 3D vectors, it works just the same as with 2D:
Length l = sqrt((x^2) + (y^2) + (z^2))
EDIT:
Say you have a user defined point A (x1, y1, z1) and you want to define this as the origin (0,0,0). You have another user chosen point B (x2, y2, z2) and you know the distance from A to B in the x, y and z plane. If you want to work out what this point is, in relation to the new origin, you can simply do
B relative to A = (x2, y2, z2) - (x1, y1, z1) = (x2-x1, y2-y1, z2-z1) = C
C is the vector A>B, a vector is a quantity which has a magnitude (the length of the lines) and a direction (the angle from A which points to B).
If you want to work out the position of B relative to the origin O, you can do the opposite:
B relative to O = (x2, y2, z2) + (x1, y1, z1) = (x1+x2, y1+y2, z1+z2) = D
D is the vector O>B.
Edit 2:
//pseudo code
userx = x;
usery = y;
userz = z;
//move origin
for (every block i){
xi = xi-x;
yi = yi - y;
zi = zi -z;
}

Projecting to a 2D Plane for Barycentric Calculations

I have three vertices which make up a plane/polygon in 3D Space, v0, v1 & v2.
To calculate barycentric co-ordinates for a 3D point upon this plane I must first project both the plane and point into 2D space.
After trawling the web I have a good understanding of how to calculate barycentric co-ordinates in 2D space, but I am stuck at finding the best way to project my 3D points into a suitable 2D plane.
It was suggested to me that the best way to achieve this was to "drop the axis with the smallest projection". Without testing the area of the polygon formed when projected on each world axis (xy, yz, xz) how can I determine which projection is best (has the largest area), and therefore is most suitable for calculating the most accurate barycentric co-ordinate?
Example of computation of barycentric coordinates in 3D space as requested by the OP. Given:
3D points v0, v1, v2 that define the triangle
3D point p that lies on the plane defined by v0, v1 and v2 and inside the triangle spanned by the same points.
"x" denotes the cross product between two 3D vectors.
"len" denotes the length of a 3D vector.
"u", "v", "w" are the barycentric coordinates belonging to v0, v1 and v2 respectively.
triArea = len((v1 - v0) x (v2 - v0)) * 0.5
u = ( len((v1 - p ) x (v2 - p )) * 0.5 ) / triArea
v = ( len((v0 - p ) x (v2 - p )) * 0.5 ) / triArea
w = ( len((v0 - p ) x (v1 - p )) * 0.5 ) / triArea
=> p == u * v0 + v * v1 + w * v2
The cross product is defined like this:
v0 x v1 := { v0.y * v1.z - v0.z * v1.y,
v0.z * v1.x - v0.x * v1.z,
v0.x * v1.y - v0.y * v1.x }
WARNING - Almost every thing I know about using barycentric coordinates, and using matrices to solve linear equations, was learned last night because I found this question so interesting. So the following may be wrong, wrong, wrong - but some test values I have put in do seem to work.
Guys and girls, please feel free to rip this apart if I screwed up completely - but here goes.
Finding barycentric coords in 3D space (with a little help from Wikipedia)
Given:
v0 = (x0, y0, z0)
v1 = (x1, y1, z1)
v2 = (x2, y2, z2)
p = (xp, yp, zp)
Find the barycentric coordinates:
b0, b1, b2 of point p relative to the triangle defined by v0, v1 and v2
Knowing that:
xp = b0*x0 + b1*x1 + b2*x2
yp = b0*y0 + b1*y1 + b2*y2
zp = b0*z0 + b1*z1 + b2*z2
Which can be written as
[xp] [x0] [x1] [x2]
[yp] = b0*[y0] + b1*[y1] + b2*[y2]
[zp] [z0] [z1] [z2]
or
[xp] [x0 x1 x2] [b0]
[yp] = [y0 y1 y2] . [b1]
[zp] [z0 z1 z2] [b2]
re-arranged as
-1
[b0] [x0 x1 x2] [xp]
[b1] = [y0 y1 y2] . [yp]
[b2] [z0 z1 z2] [zp]
the determinant of the 3x3 matrix is:
det = x0(y1*z2 - y2*z1) + x1(y2*z0 - z2*y0) + x2(y0*z1 - y1*z0)
its adjoint is
[y1*z2-y2*z1 x2*z1-x1*z2 x1*y2-x2*y1]
[y2*z0-y0*z2 x0*z2-x2*z0 x2*y0-x0*y2]
[y0*z1-y1*z0 x1*z0-x0*z1 x0*y1-x1*y0]
giving:
[b0] [y1*z2-y2*z1 x2*z1-x1*z2 x1*y2-x2*y1] [xp]
[b1] = ( [y2*z0-y0*z2 x0*z2-x2*z0 x2*y0-x0*y2] . [yp] ) / det
[b2] [y0*z1-y1*z0 x1*z0-x0*z1 x0*y1-x1*y0] [zp]
If you need to test a number of points against the triangle, stop here. Calculate the above 3x3 matrix once for the triangle (dividing it by the determinant as well), and then dot product that result to each point to get the barycentric coords for each point.
If you are only doing it once per triangle, then here is the above multiplied out (courtesy of Maxima):
b0 = ((x1*y2-x2*y1)*zp+xp*(y1*z2-y2*z1)+yp*(x2*z1-x1*z2)) / det
b1 = ((x2*y0-x0*y2)*zp+xp*(y2*z0-y0*z2)+yp*(x0*z2-x2*z0)) / det
b2 = ((x0*y1-x1*y0)*zp+xp*(y0*z1-y1*z0)+yp*(x1*z0-x0*z1)) / det
That's quite a few additions, subtractions and multiplications - three divisions - but no sqrts or trig functions. It obviously does take longer than the pure 2D calcs, but depending on the complexity of your projection heuristics and calcs, this might end up being the fastest route.
As I mentioned - I have no idea what I'm talking about - but maybe this will work, or maybe someone else can come along and correct it.
Update: Disregard, this approach does not work in all cases
I think I have found a valid solution to this problem.
NB: I require a projection to 2D space rather than working with 3D Barycentric co-ordinates as I am challenged to make the most efficient algorithm possible. The additional overhead incurred by finding a suitable projection plane should still be smaller than the overhead incurred when using more complex operations such as sqrt or sin() cos() functions (I guess I could use lookup tables for sin/cos but this would increase the memory footprint and defeats the purpose of this assignment).
My first attempts found the delta between the min/max values on each axis of the polygon, then eliminated the axis with the smallest delta. However, as suggested by #PeterTaylor there are cases where dropping the axis with the smallest delta, can yeild a straight line rather than a triangle when projected into 2D space. THIS IS BAD.
Therefore my revised solution is as follows...
Find each sub delta on each axis for the polygon { abs(v1.x-v0.x), abs(v2.x-v1.x), abs(v0.x-v2.x) }, this results in 3 scalar values per axis.
Next, multiply these scaler values to compute a score. Repeat this, calculating a score for each axis. (This way any 0 deltas force the score to 0, automatically eliminating this axis, avoiding triangle degeneration)
Eliminate the axis with the lowest score to form the projection, e.g. If the lowest score is in the x-axis, project onto the y-z plane.
I have not had time to unit test this approach but after preliminary tests it seems to work rather well. I would be eager to know if this is in-fact the best approach?
After much discussion there is actually a pretty simple way to solve the original problem of knowing which axis to drop when projecting to 2D space. The answer is described in 3D Math Primer for Graphics and Game Development as follows...
"A solution to this dilemma is to
choose the plane of projection so as
to maximize the area of the projected
triangle. This can be done by
examining the plane normal; the
coordinate that has the largest
absolute value is the coordinate that
we will discard. For example, if the
normal is [–1, 0, 0], then we would
discard the x values of the vertices
and p, projecting onto the yz plane."
My original solution which involved computing a score per axis (using sub deltas) is flawed as it is possible to generate a zero score for all three axis, in which case the axis to drop remains undetermined.
Using the normal of the collision plane (which can be precomputed for efficiency) to determine which axis to drop when projecting into 2D is therefore the best approach.
To project a point p onto the plane defined by the vertices v0, v1 & v2 you must calculate a rotation matrix. Let us call the projected point pd
e1 = v1-v0
e2 = v2-v0
r = normalise(e1)
n = normalise(cross(e1,e2))
u = normalise(n X r)
temp = p-v0
pd.x = dot(temp, r)
pd.y = dot(temp, u)
pd.z = dot(temp, n)
Now pd can be projected onto the plane by setting pd.z=0
Also pd.z is the distance between the point and the plane defined by the 3 triangles. i.e. if the projected point lies within the triangle, pd.z is the distance to the triangle.
Another point to note above is that after rotation and projection onto this plane, the vertex v0 lies is at the origin and v1 lies along the x axis.
HTH
I'm not sure that the suggestion is actually the best one. It's not too hard to project to the plane containing the triangle. I assume here that p is actually in that plane.
Let d1 = sqrt((v1-v0).(v1-v0)) - i.e. the distance v0-v1.
Similarly let d2 = sqrt((v2-v0).(v2-v0))
v0 -> (0,0)
v1 -> (d1, 0)
What about v2? Well, you know the distance v0-v2 = d2. All you need is the angle v1-v0-v2. (v1-v0).(v2-v0) = d1 d2 cos(theta). Wlog you can take v2 as having positive y.
Then apply a similar process to p, with one exception: you can't necessarily take it as having positive y. Instead you can check whether it has the same sign of y as v2 by taking the sign of (v1-v0)x(v2-v0) . (v1-v0)x(p-v0).
As an alternative solution, you could use a linear algebra solver on the matrix equation for the tetrahedral case, taking as the fourth vertex of the tetrahedron v0 + (v1-v0)x(v2-v0) and normalising if necessary.
You shouldn't need to determine the optimal area to find a decent projection.
It's not strictly necessary to find the "best" projection at all, just one that's good enough, and that doesn't degenerate to a line when projected into 2D.
EDIT - algorithm deleted due to degenerate case I hadn't thought of

Resources