In particle physics, we have to compute the invariant mass a lot, which is for a two-body decay
When the momenta (p1, p2) are sometimes very large (up to a factor 1000 or more) compared to the masses (m1, m2). In that case, there is large cancellation happening between the last two terms when the calculation is carried out with floating point numbers on a computer.
What kind of numerical tricks can be used to compute this accurately for any inputs?
The question is about suitable numerical tricks to improve the accuracy of the calculation with floating point numbers, so the solution should be language-agnostic. For demonstration purposes, implementations in Python are preferred. Solutions which reformulate the problem and increase the amount of elementary operations are acceptable, but solutions which suggest to use other number types like decimal or multi-precision floating point numbers are not.
Note: The original question presented a simplified 1D dimensional problem in form of a Python expression, but the question is for the general case where the momenta are given in 3D dimensions. The question was reformulated in this way.
With a few tricks listed on Stackoverflow and the transformation described by Jakob Stark in his answer, it is possible to rewrite the equation into a form that does not suffer anymore from catastrophic cancellation.
The original question asked for a solution in 1D, which has a simple solution, but in practice, we need the formula in 3D and then the solution is more complicated. See this notebook for a full derivation.
Example implementation of numerically stable calculation in 3D in Python:
import numpy as np
# numerically stable implementation
#np.vectorize
def msq2(px1, py1, pz1, px2, py2, pz2, m1, m2):
p1_sq = px1 ** 2 + py1 ** 2 + pz1 ** 2
p2_sq = px2 ** 2 + py2 ** 2 + pz2 ** 2
m1_sq = m1 ** 2
m2_sq = m2 ** 2
x1 = m1_sq / p1_sq
x2 = m2_sq / p2_sq
x = x1 + x2 + x1 * x2
a = angle(px1, py1, pz1, px2, py2, pz2)
cos_a = np.cos(a)
if cos_a >= 0:
y1 = (x + np.sin(a) ** 2) / (np.sqrt(x + 1) + cos_a)
else:
y1 = -cos_a + np.sqrt(x + 1)
y2 = 2 * np.sqrt(p1_sq * p2_sq)
return m1_sq + m2_sq + y1 * y2
# numerically stable calculation of angle
def angle(x1, y1, z1, x2, y2, z2):
# cross product
cx = y1 * z2 - y2 * z1
cy = x1 * z2 - x2 * z1
cz = x1 * y2 - x2 * y1
# norm of cross product
c = np.sqrt(cx * cx + cy * cy + cz * cz)
# dot product
d = x1 * x2 + y1 * y2 + z1 * z2
return np.arctan2(c, d)
The numerically stable implementation can never produce a negative result, which is a commonly occurring problem with naive implementations, even in double precision.
Let's compare the numerically stable function with a naive implementation.
# naive implementation
def msq1(px1, py1, pz1, px2, py2, pz2, m1, m2):
p1_sq = px1 ** 2 + py1 ** 2 + pz1 ** 2
p2_sq = px2 ** 2 + py2 ** 2 + pz2 ** 2
m1_sq = m1 ** 2
m2_sq = m2 ** 2
# energies of particles 1 and 2
e1 = np.sqrt(p1_sq + m1_sq)
e2 = np.sqrt(p2_sq + m2_sq)
# dangerous cancelation in third term
return m1_sq + m2_sq + 2 * (e1 * e2 - (px1 * px2 + py1 * py2 + pz1 * pz2))
For the following image, the momenta p1 and p2 are randomly picked from 1 to 1e5, the values m1 and m2 are randomly picked from 1e-5 to 1e5. All implementations get the input values in single precision. The reference in both cases is calculated with mpmath using the naive formula with 100 decimal places.
The naive implementation loses all accuracy for some inputs, while the numerically stable implementation does not.
If you put e.g. m1 = 1e-4, m2 = 1e-4, p1 = 1 and p2 = 1 in the expression, you get about 4e-8 with double precision but 0.0 with single precision calculation. I assume, that your question is about how one can get the 4e-8 as well with single precision calculation.
What you can do is a taylor expansion (around m1 = 0 and m2 = 0) of the expression above.
e ~ e|(m1=0,m2=0) + de/dm1|(m1=0,m2=0) * m1 + de/dm2|(m1=0,m2=0) * m2 + ...
If I calculated correctly, the zeroth and first order terms are 0 and the second order expansion would be
e ~ (p1+p2)/p1 * m1**2 + (p1+p2)/p2 * m2**2
This yields exactly 4e-8 even with single precision calculation. You can of course do more terms in the expansion if you need, until you hit the precision limit of a single float.
Edit
If the mi are not always much smaller than the pi you could further massage the equation to get
The complicated part is now the one in the square brackets. It essentially is sqrt(x+1)-1 for a wide range of x values. If x is very small, we can use the taylor expansion of the square root (e.g. like here). If the x value is larger, the formula works just fine, because the addition and subtraction of 1 are no longer discarding the value of x due to floating point precision. So one threshold for x must be choosen below one switches to the taylor expansion.
Related
Hi so I have a problem from my differential equations class that I am having difficulty solving with the improved Euler's method:
The logistic equation for the population (in thousands) of a certain species is given by dP/dt = 2P-2P^2. With t being time variable in years
Given P(0) = .5, with step size h = .2 (so .2 of t), find population at 1 year.
I used the normal Euler's method and got 634 but am not sure how to implement the modified Euler's method on the given differential equation.
The improved Euler method or explicit midpoint method has two stages
Stage one is an Euler step with half the step size
Stage two is the full step with the slope from the point from step one.
So if dP/dt = Q(P) then
k1 = Q(P0)
k2 = Q(P0 + h/2 * k1)
P1 = P0 + h * k2
or
Phalf = P0+h/2*Q(P0)
P1 = P0 + h*Q(Phalf)
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)
"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'm looking for an universal way of transforming equations in Python 3.2. I've only recently begun playing around with it and stumbled upon some of my old MATLAB homework. I'm able to calculate this in MATLAB but pylab is still a bit of a mystery to me.
So, I have a text file of equations that I'm trying to convert into the the same form of A x = b and then solve some linear algebra problems associated with them in PYLAB.
The text file, "equations.txt",contains collections of linear equations in the following format:
-38 y1 + 35 y2 + 31 y3 = -3047
11 y1 + -13 y2 + -34 y3 = 784
34 y1 + -21 y2 + 19 y3 = 2949
etc.
The file contains the equations for four sets of equations, each set with a different number of variables. Each set of equations is of the exact form shown (3 examples above) with one empty line between each set.
I want to write a program to read all the sets of equations in the files, convert sets of equations into a matrix equation A x = b, and solve the set of equations for the vector x.
My approach has been very "MATLABy", which is a problem because I want to be able to write a program that will solve for all of the variables.
I've tried reading a single equation as a text line, stripped of the carriage return at the end, and splitting line at the = sign because as we know the 2nd element in the split is the right hand side of the equation, that goes into the vector b.
The first element in the split is the part you have to get the coefficients that go in the A matrix. If you split this at white space ' ', you will get a list like
['-38', 'y1', '+', '35', 'y2', '+', '31', 'y3']
Note now that you can pull every 3rd element and get the coefficients that go into the matrix A.
Partial answers would be:
y1 = 90; c2 = 28; x4 = 41; z100 = 59
I'm trying to manipulate them to give me the sum of the entries of the solutions y1, ..., y3 for the first block of equations, the sum of the entries of the solutions c1, ..., c6 for the second block of equations, the sum of the entries of the solutions x1, ..., x13 for the third block of equations, and the sum of the entries of the solutions z1, ..., z100 for the fourth block of equations.
Like, I said - I'm able to do this in MATLAB but not in Python so I'm probably approaching this from the wrong way but this is what I have so far:
import pylab
f = open('equations.txt', 'r')
L=f.readlines()
list_final = []
for line in L:
line_l = line.rstrip()
list_l = line_l.split(";")
list_l = filter(None, list_l)
for expression in list_l:
and ending it with
f.close()
This was just my go at trying to format the equations to all look the same. I realise it's not a lot but I was really hoping someone could get my started because even though I know some python I normally don't use it for math because I have MATLAB for that.
I think this could be useful for many of us who have prior MATLAB experience but not pylab.
How would you go around this? Thank you!
For your example format, it's very easy to process it by numpy.loadtxt():
import numpy as np
data = np.loadtxt("equations.txt", dtype=str)[:, ::3].astype(np.float)
a = data[:, :-1]
b = data[:, -1]
x = np.linalg.solve(a, b)
The steps are:
An alternative approach that is possibly more robust to unstructured input is to use a combination of the Python symbolic math package (sympy), and a few parsing tricks. This scales to the variables in the equations being written in an arbitrary order.
Although sympy has some tools for parsing, (your input is very close in appearance to Mathematica), it appears that the sympy.parsing.mathematica module can't deal with some of the input (particularly leading minus signs).
import sympy
from sympy.parsing.sympy_parser import parse_expr
import re
def text_to_equations(text):
lines = text.split('\n')
lines = [line.split('=') for line in lines]
eqns = []
for lhs, rhs in lines:
# clobber all the spaces
lhs = lhs.replace(' ','')
# *assume* that a number followed by a letter is an
# implicit multiplication
lhs = re.sub(r'(\d)([a-z])', r'\g<1>*\g<2>', lhs)
eqns.append( (parse_expr(lhs), parse_expr(rhs)) )
return eqns
def get_all_symbols(eqns):
symbs = set()
for lhs, rhs in eqns:
for sym in lhs.atoms(sympy.Symbol):
symbs.add(sym)
return symbs
def text_to_eqn_matrix(text):
eqns = text_to_equations(text)
symbs = get_all_symbols(eqns)
n = len(eqns)
m = len(symbs)
A = numpy.zeros((m, n))
b = numpy.zeros((m, 1))
for i, (lhs, rhs) in enumerate(eqns):
d = lhs.as_coefficients_dict()
b[i] = int(rhs)
for j, s in enumerate(symbs):
A[i, j] = d[s]
x = sympy.Matrix([list(symbs)]).T
return sympy.Matrix(A), x, sympy.Matrix(b)
s = '''-38 y1 + 35 y2 + 31 y3 = -3047
11 y1 + -13 y2 + -34 y3 = 784
34 y1 + -21 y2 + 19 y3 = 2949'''
A, x, b = text_to_eqn_matrix(s)
print A
print x
print b
I have a problem and please let me know if my solution is correct.
I have a known point, at location A(x1,y1,z1) and the origin O(0,0,0) and I would like to find the coordinates of a point B(x2,y2,z2) that is located on the line OA, and the distance OB is 1.2 times greater then OA.
So, my idea is to obtain the equation of the line formed by points O and A.
The direction of OA is (-x1, -y1, -z1), so the equation of the line is:
x = -x1*t;
y = -y1*t;
z = -z1*t;
Distance OA is sqrt( (x1-0)^2 + (y1-0)^2 + (z1-0)^2). KNOWN
Distance OB is sqrt( (x2-0)^2 + (y2-0)^2 + (z2-0)^2). UNKNOWN
I can replace the x, y, z points determined for the line equation in the distance OB, and the result should be 1.2 times greater then the distance OA.
So, sqrt( (-x1*t-0)^2 + (-y1*t-0)^2 + (-z1*t-0)^2) = 1.2 * dist(OA).
I find t from here, solving the quadratic equation and I obtain the coordinates of the point by replacing the t in the equation of the line.
Is this correct?
Thank you for your time.
EDIT:
This is my code:
rangeRatio = 1.114;
norm = sqrt((P2(1) - P1(1))^2 + (P2(2) - P1(2))^2 + (P2(3) - P1(3))^2);
P3(1) = P1(1) + ((P2(1,1) - P1(1)) /norm) * rangeRatio;
P3(2) = P1(2) + ((P2(1,2) - P1(2)) /norm) * rangeRatio;
P3(3) = P1(3) + ((P2(1,3) - P1(3)) /norm) * rangeRatio;
I tried also norm = 1, and i get slightly different results but still not always colinear.
Thank you
It is even a lot easier; you can just multiply a, b and c by 1.2. This gives a line that is 1.2 times the size of the original line.