I have a green vehicle which will shortly collide with a blue object (which is 200 away from the cube)
It has a Kinect depth camera D at [-100,0,200] which sees the corner of the cube (grey sphere)
The measured depth is 464 at 6.34° in the X plane and 12.53° in the Y plane.
I want to calculate the position of the corner as it would appear if there was a camera F at [150,0,0], which would see this:
in other words transform the red vector into the yellow vector. I know that this is achieved with a transformation matrix but I can't find out how to compute the matrix from the D-F vector [250,0,-200] or how to use it; my high-school maths dates back 40 years.
math.se has a similar question but it doesn't cover my problem and I can't find anything on robotices.se either.
I realise that I should show some code that I've tried, but I don't know where to start. I would be very grateful if somebody could help me to solve this.
ROS provides the tf library which allows you to transform between frames. You can simply set a static transform between the pose of your camera and the pose of your desired location. Then, you can get the pose of any point detected by your camera in the reference frame of your desired point on your robot. ROS tf will do everything you need and everything I explain below.
The longer answer is that you need to construct a transformation tree. First, compute the static transformation between your two poses. A pose is a 7-dimensional transformation including a translation and orientation. This is best represented as a quaternion and a 3D vector.
Now, for all poses in the reference frame of your kinect, you must transform them to your desired reference frame. Let's call this frame base_link and your camera frame camera_link.
I'm going to go ahead and decide that base_link is the parent of camera_link. Technically these transformations are bidirectional, but because you may need a transformation tree, and because ROS cares about this, you'll want to decide who is the parent.
To convert rotation from camera_link to base_link, you need to compute the rotational difference. This can be done by multiplying the quaternion of base_link's orientation by the conjugate of camera_link's orientation. Here's a super quick Python example:
def rotDiff(self,q1: Quaternion,q2: Quaternion) -> Quaternion:
"""Finds the quaternion that, when applied to q1, will rotate an element to q2"""
conjugate = Quaternion(q2.qx*-1,q2.qy*-1,q2.qz*-1,q2.qw)
return self.rotAdd(q1,conjugate)
def rotAdd(self, q1: Quaternion, q2: Quaternion) -> Quaternion:
"""Finds the quaternion that is the equivalent to the rotation caused by both input quaternions applied sequentially."""
w1 = q1.qw
w2 = q2.qw
x1 = q1.qx
x2 = q2.qx
y1 = q1.qy
y2 = q2.qy
z1 = q1.qz
z2 = q2.qz
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
return Quaternion(x,y,z,w)
Next, you need to add the vectors. The naive approach is to simply add the vectors, but you need to account for rotation when calculating these. What you really need is a coordinate transformation. The position of camera_link relative to base_link is some 3D vector. Based on your drawing, this is [-250, 0, 200]. Next, we need to reproject the vectors to your points of interest into the rotational frame of base_link. I.e., all the points your camera sees at 12.53 degrees that appear at the z = 0 plane to your camera are actually on a 12.53 degree plane relative to base_link and you need to find out what their coordinates are relative to your camera as if your camera was in the same orientation as base_link.
For details on the ensuing math, read this PDF (particularly starting at page 9).
To accomplish this, we need to find your vector's components in base_link's reference frame. I find that it's easiest to read if you convert the quaternion to a rotation matrix, but there is an equivalent direct approach.
To convert a quaternion to a rotation matrix:
def Quat2Mat(self, q: Quaternion) -> rotMat:
m00 = 1 - 2 * q.qy**2 - 2 * q.qz**2
m01 = 2 * q.qx * q.qy - 2 * q.qz * q.qw
m02 = 2 * q.qx * q.qz + 2 * q.qy * q.qw
m10 = 2 * q.qx * q.qy + 2 * q.qz * q.qw
m11 = 1 - 2 * q.qx**2 - 2 * q.qz**2
m12 = 2 * q.qy * q.qz - 2 * q.qx * q.qw
m20 = 2 * q.qx * q.qz - 2 * q.qy * q.qw
m21 = 2 * q.qy * q.qz + 2 * q.qx * q.qw
m22 = 1 - 2 * q.qx**2 - 2 * q.qy**2
result = [[m00,m01,m02],[m10,m11,m12],[m20,m21,m22]]
return result
Now that your rotation is represented as a rotation matrix, it's time to do the final calculation.
Following the MIT lecture notes from my link above, I'll arbitrarily name the vector to your point of interest from the camera A.
Find the rotation matrix that corresponds with the quaternion that represents the rotation between base_link and camera_link and simply perform a matrix multiplication. If you're in Python, you can use numpy to do this, but in the interest of being explicit, here is the long form of the multiplication:
def coordTransform(self, M: RotMat, A: Vector) -> Vector:
"""
M is my rotation matrix that represents the rotation between my frames
A is the vector of interest in the frame I'm rotating from
APrime is A, but in the frame I'm rotating to.
"""
APrime = []
i = 0
for component in A:
APrime.append(component * M[i][0] + component * M[i][1] + component * m[i][2])
i += 1
return APrime
Now, the vectors from camera_link are represented as if camera_link and base_link share an orientation.
Now you may simply add the static translation between camera_link and base_link (or subtract base_link -> camera_link) and the resulting vector will be your point's new translation.
Putting it all together, you can now gather the translation and orientation of every point your camera detects relative to any arbitrary reference frame to gather pose data relevant to your application.
You can put all of this together into a function simply called tf() and stack these transformations up and down a complex transformation tree. Simply add all the transformations up to a common ancestor and subtract all the transformations down to your target node in order to find the transformation of your data between any two arbitrary related frames.
Edit: Hendy pointed out that it's unclear what Quaternion() class I refer to here.
For the purposes of this answer, this is all that's necessary:
class Quaternion():
def __init__(self, qx: float, qy: float, qz: float, qw: float):
self.qx = qx
self.qy = qy
self.xz = qz
self.qw = qw
But if you want to make this class super handy, you can define __mul__(self, other: Quaternion and __rmul__(self, other: Quaternion) to perform quaternion multiplication (order matters, so make sure to do both!). conjugate(self), toEuler(self), toRotMat(self), normalize(self) may also be handy additions.
Note that due to quirks in Python's typing, the above other: Quaternion is only for clarity. You'll need a longer-form if type(other) != Quaternion: raise TypeError('You can only multiply quaternions with other quaternions) error handling block to make that into valid python :)
The following definitions are not necessary for this answer, but they may prove useful to the reader.
import numpy as np
def __mul__(self, other):
if type(other) != Quaternion:
print("Quaternion multiplication only works with other quats")
raise TypeError
r1 = self.qw
r2 = other.qw
v1 = [self.qx,self.qy,self.qz]
v2 = [other.qx,other.qy,other.qz]
rPrime = r1*r2 - np.dot(v1,v2)
vPrimeA = np.multiply(r1,v2)
vPrimeB = np.multiply(r2,v1)
vPrimeC = np.cross(v1,v2)
vPrimeD = np.add(vPrimeA, vPrimeB)
vPrime = np.add(vPrimeD,vPrimeC)
x = vPrime[0]
y = vPrime[1]
z = vPrime[2]
w = rPrime
return Quaternion(x,y,z,w)
def __rmul__(self, other):
if type(other) != Quaternion:
print("Quaternion multiplication only works with other quats")
raise TypeError
r1 = other.qw
r2 = self.qw
v1 = [other.qx,other.qy,other.qz]
v2 = [self.qx,self.qy,self.qz]
rPrime = r1*r2 - np.dot(v1,v2)
vPrimeA = np.multiply(r1,v2)
vPrimeB = np.multiply(r2,v1)
vPrimeC = np.cross(v1,v2)
vPrimeD = np.add(vPrimeA, vPrimeB)
vPrime = np.add(vPrimeD,vPrimeC)
x = vPrime[0]
y = vPrime[1]
z = vPrime[2]
w = rPrime
return Quaternion(x,y,z,w)
def conjugate(self):
return Quaternion(self.qx*-1,self.qy*-1,self.qz*-1,self.qw)
Given a convex 3d polygon (convex hull) How can I determine the correct direction for normal surface/vertex vectors? As the polygon is convex, by correct I mean outward facing (away from the centroid).
def surface_normal(centroid, p1, p2, p3):
a = p2-p1
b = p3-p1
n = np.cross(a,b)
if **test including centroid?** :
return n
else:
return -n # change direction
I actually need the normal vertex vectors as I am exporting as a .obj file, but I am assuming that I would need to calculate the surface vectors before hand and combine them.
This solution should work under the assumption of a convex hull in 3d. You calculate the normal as shown in the question. You can normalize the normal vector with
n /= np.linalg.norm(n) # which should be sqrt(n[0]**2 + n[1]**2 + n[2]**2)
You can then calculate the center point of your input triangle:
pmid = (p1 + p2 + p3) / 3
After that you calculate the distance of the triangle-center to your surface centroid. This is
dist_centroid = np.linalg.norm(pmid - centroid)
The you can calculate the distance of your triangle_center + your normal with the length of the distance to the centroid.
dist_with_normal = np.linalg.norm(pmid + n * dist_centroid - centroid)
If this distance is larger than dist_centroid, then your normal is facing outwards. If it is smaller, it is pointing inwards. If you have a perfect sphere and point towards the centroid, it should almost be zero. This may not be the case for your general surface, but the convexity of the surface should make sure, that this is enough to check for its direction.
if(dist_centroid < dist_with_normal):
n *= -1
Another, nicer option is to use a scalar product.
pmid = (p1 + p2 + p3) / 3
if(np.dot(pmid - centroid, n) < 0):
n *= -1
This checks if your normal and the vector from the mid of your triangle to the centroid have the same direction. If that is not so, change the direction.
I have a straight line in space with an start and end point (x,y,z) and I am attempting to get the angle between this vector and the plane defined by z=0. I am using VB.NET
Here is a picture of the line in my 3d environment (the line I'm intersted in is circled in red) :
It is set to an angle of 70 degrees right now.
You need 2 rays to define an angle.
If you want the angle between a vector and a plane, it is defined for any vector in that plane. However, there is only one minimal value for that, which is the angle between a vector and its projection onto said plane.
Therefore, that minimal value is the one we take when we speak of the angle between a vector and a plane.
This value is also π/2 - the angle between your vector and the the vector that is normal to the plane.You can read more about it all on this site.
With v your vector (thus v.x = end.x - start.x and idem for y and z), n the normal to the plane and a the angle you are looking for, we know from the definition of a scalar product that:
<v,n> = ||v|| * ||n|| * cos(π/2 - a)
We know cos(π/2 - a) = sin(a), and the normal to the z=0 plane is simply the vector n = (0, 0, 1). Thus both the scalar product, v.x * n.x + v.y * n.y + v.z * n.z, and the norm of n, ||n|| = 1, can be simplified a lot. We get the following expression:
sin(a) = v.z / ||v||
Thus finally, the formula by taking the reciprocical of the sine and expliciting the norm of v:
a = Asin(v.z / sqrt( v.x*v.x + v.y*v.y + v.z*v.z ))
According to this documentation the Asin function exists in your System.Math class. It does, however, return the value in radians:
Return Value
Type: System.Double
An angle, θ, measured in radians, such that -π/2 ≤ θ ≤ π/2
-or-
NaN if d < -1 or d > 1 or d equals NaN.
Luckily the same System.Math class contains the value of π so that you can do the conversion:
a *= 180 / Math.PI
"How to find a third point given both (2 points on a line) and (distance from third point to first point)?"
Language: Visual Basic (2012)
The third point is on the same line as the second, and may be either closer to the first point or it may be closer to the second. This is a function that will handle both (from arrays of data).
Strangely I cannot seem to grasp the distance part of this question. Over reading many other questions on finding points from other points, I am unable to find anything clear enough for me to be able to reverse engineer to include a parameter for distance.
I need to be able to use distance to find a point. The function I am writing is basically a much more advanced form of:
Function GetThirdPoint(CenterPoint As Point, SecondPoint As Point, Range As Integer)
Return [Equations here] 'Return third point
End Function
Let's first point coordinates are P1=(x1,y1), second point P2=(x2,y2).
Then length of P1P2 vector is (use Math.Hypot function if available)
Len = Sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1))
Normalized (unit-length) direction vector is
dx = (x2-x1) / Len
dy = (y2-y1) / Len
P3 coordinates for case when P1P3 and P1P2 vectors have the same direction:
x3 = x1 + Distance * dx
y3 = y1 + Distance * dy
for opposite direction:
x3 = x1 - Distance * dx
y3 = y1 - Distance * dy
The general equation of a line is:
A*x + B*y + G = 0 where A, B must not be both equal to 0. (1)
You can find A, B, G easily because you know two points of the line (point one and two). The distance is:
D = sqrt( (x1 - x3)(x1 - x3) + (y1 - y3)(y1 - y3) ) (2)
Third point is on the line so from (1):
A*x3 + B*y3 + G = 0 (3)
From (2) and (3), you can find the solution. Because (2) is second degree you will find two solutions.
I have successfully drawn a Quad2D or Bezier curve in java. I have the equation for the same. But I need to determine whether a particular point(x,y) lies on the curve or not. I tried using Quad2D.contains and a few GeneralPath APIs, I could not get the result accurately.
Can someone help to find out the solution to this?
I think you've meant QuadCurve2D class, which is quadratic Bezier curve.
There seems to be no ready-made method for that, and the problem comes to the distance from the point to the Bezier curve.
Let's P0 will be your point, P1 is a start point, P2 - a control point and P3 is an end point of your curve.
Then point on the curve will be given by
P = B(t)
There is such t, for which distance between P and P0 will be minimal.
F(t) = (B(t)_x - P0_x)^2 + (B(t)_y - P0_y)^2 -> min
If distance is 0 or less than certain error, then P0 is on the curve.
t can be found with Netwon's iterative method by minimizing cost function
t_n = t_n-1 + F'(t) / F''(t)
where F' is first derivative of cost function and F'' is its second derivative.
F'(t) = 2 * (B(t)_x - P0_x) * B'(t)_x + 2 * (B(t)_y - P0_y) * B'(t)_y
F''(t) = 2 * B'(t)_x * B'(t)_x + 2 * (B(t)_x - P0_x) * B''(t)_x +
2 * B'(t)_y * B'(t)_y + 2 * (B(t)_y - P0_x) * B''(t)_y
First derivative of quadratic Bezier curve B'(t) is a line segment with a start point (P2 - P1) and end point (P3 - P2). Second derivative B''(t) is a point P3 - 2 * P2 + P1.
Plugging everything together will give a formula to t for which F(t) is minimal.