Epsilon Tutorial#

This notebook describes the epsilon mechanism by which numerical singularities are handled in SymForce. The paper addresses the theory in Section VI, and this tutorial demonstrates the idea through examples.

The basic concept is that it is common to have functions in robotics that are smooth but have singularities at given points. Handwritten functions tend to handle them by adding an if statement at the singularity with some kind of approximation or alternate formulation. This is harder to do with symbolic expressions, and also is not kind to branch prediction. SymForce addresses this with a different method - shifting the input to the function away from the singular point with an infinitesimal variable $\epsilon$ (epsilon). This approach is simple and fast for a useful class of removable singularities, with negligible effect to output values for sufficiently small epsilon.

All functions in SymForce that have singularities take epsilon as an argument. In a numerical context, a very small floating point number should be passed in. In the symbolic context, an epsilon symbol should be passed in. Epsilon arguments are currently optional with zero defaults. This is convenient so that playing with expressions in a notebook doesn’t require passing epsilons around. However, this is dangerous and it is extremely important to pass epsilons to get robust behavior or when generating code. Because of this, there are active efforts to make a more intelligent mechanism for the default epsilon to make it less of a footgun to accidentally forget an epsilon and end up with a NaN.

Goal#

We have a function f(x).

In the simplest case, we’re trying to fix a removable singularity at x=0, i.e. f(x).subs(x, 0) == sm.S.NaN

Libraries often do this by checking whether the value of x is close to 0, and using a different method for evaluation there, such as a Taylor expansion. In symbolic expressions, branching like this is messy and expensive.

The idea is to instead make a function f(x, eps) so the value is not NaN, when eps is a small positive number:

f(x, eps).subs(x, 0) != sm.S.NaN

We usually also want that the derivative is not NaN:

f(x, eps).diff(x).subs(x, 0) != NaN

For value continuity we want to match the limit:

f(x, eps).subs(x, 0).limit(eps, 0) == f(x).limit(x, 0)

For derivative continuity we want to match the limit: f(x, eps).diff(x).subs(x, 0).limit(eps, 0) == f(x).diff(x).limit(x, 0)

[1]:

import numpy as np
import plotly.express as px
import sympy as sm

x = sm.Symbol("x")
eps = sm.Symbol("epsilon")

An example: sin(x) / x#

For the whole section below, let’s pretend x is positive so $x = -\epsilon$ is not a fear. We’ll address that later.

[2]:

# The function `sin(x) / x` looks like this:
def f(x):
    return sm.sin(x) / x


f(x)

[2]:

$\displaystyle \frac{\sin{\left(x \right)}}{x}$

[3]:

# And its graph:
x_numerical = np.linspace(-5, 5)
px.line(x=x_numerical, y=np.vectorize(f, otypes=[float])(x_numerical))

[4]:

# It has a removable singularity at 0, of the form 0/0, which gives NaN:
f(x).subs(x, 0)

[4]:

$\displaystyle \text{NaN}$

[5]:

# The derivative has the same issue:
f(x).diff(x).subs(x, 0)

[5]:

$\displaystyle \text{NaN}$

One thought to fix this might be to just push the denominator away from 0. This does resolve the NaN, but it produces the wrong value! (Remember, the value at x=0 should be 1)

[6]:

def f(x, eps):
    return sm.sin(x) / (x + eps)


f(x, eps).subs(x, 0)

[6]:

$\displaystyle 0$

Similarly, the derivative is wrong, and actually diverges (it should be 0):

[7]:

f(x, eps).diff(x).subs(x, 0)

[7]:

$\displaystyle \frac{1}{\epsilon}$

[8]:

f(x, eps).diff(x).subs(x, 0).limit(eps, 0)

[8]:

$\displaystyle \infty$

Instead, what we want to do is perturb the input away from the singularity. Effectively, we’re shifting the graph of the function to the left. For removable singularities in well-behaved functions, the error introduced by this is proportional to epsilon. That looks like this:

[9]:

def f(x, eps):
    x_safe = x + eps
    return sm.sin(x_safe) / x_safe


f(x, eps).subs(x, 0)

[9]:

$\displaystyle \frac{\sin{\left(\epsilon \right)}}{\epsilon}$

[10]:

f(x, eps).subs(x, 0).limit(eps, 0)

[10]:

$\displaystyle 1$

[11]:

f(x, eps).diff(x).subs(x, 0)

[11]:

$\displaystyle \frac{\cos{\left(\epsilon \right)}}{\epsilon} - \frac{\sin{\left(\epsilon \right)}}{\epsilon^{2}}$

[12]:

f(x, eps).diff(x).subs(x, 0).limit(eps, 0)

[12]:

$\displaystyle 0$

Automating Verification#

We can also write a function that automatically checks that you’ve used epsilon correctly, using SymPy’s ability to take derivatives and limits as we’ve been doing above. That looks something like this - similar functions are available in SymForce in symforce.test_util.epsilon_handling.

[13]:

def is_epsilon_correct(func, singularity=0, limit_direction="+"):
    """
    Check epsilon handling for a function that accepts a single value and an
    epsilon.

    For epsilon to be handled correctly, the function must:
        1) evaluate to a non-singularity at x=singularity given epsilon
        2) linear approximation of the original must match that taken with
           epsilon then substituted to zero
    """
    # Create symbols
    x = sm.Symbol("x", real=True)
    epsilon = sm.Symbol("epsilon", positive=True)

    is_correct = True

    # Evaluate expression
    expr_eps = func(x, epsilon)
    expr_raw = expr_eps.subs(epsilon, 0)

    display("Expressions (raw / eps):")
    display(expr_raw)
    display(expr_eps)

    # Sub in zero
    expr_eps_at_x_zero = expr_eps.subs(x, singularity)
    if expr_eps_at_x_zero == sm.S.NaN:
        display("[ERROR] Epsilon handling failed, expression at 0 is NaN.")
        is_correct = False

    # Take constant approximation at singularity and check equivalence
    value_x0_raw = sm.simplify(expr_raw.limit(x, singularity, limit_direction))
    value_x0_eps = expr_eps.subs(x, singularity)
    value_x0_eps_sub2 = sm.simplify(value_x0_eps.limit(epsilon, 0))
    if value_x0_eps_sub2 != value_x0_raw:
        display(
            f"[ERROR] Values at x={singularity} do not match (raw / eps / eps.limit):",
        )
        display(value_x0_raw)
        display(value_x0_eps)
        display(value_x0_eps_sub2)
        is_correct = False

    # Take linear approximation at singularity and check equivalence
    derivative_x0_raw = sm.simplify(
        expr_raw.diff(x).limit(x, singularity, limit_direction),
    )
    derivative_x0_eps = expr_eps.diff(x).subs(x, singularity)
    derivative_x0_eps_sub2 = sm.simplify(derivative_x0_eps.limit(epsilon, 0))
    if derivative_x0_eps_sub2 != derivative_x0_raw:
        display(
            f"[ERROR] Derivatives at x={singularity} do not match (raw / eps / eps.limit):",
        )
        display(derivative_x0_raw)
        display(derivative_x0_eps)
        display(derivative_x0_eps_sub2)
        is_correct = False

    return is_correct

Test sin(x) / x#

[14]:

# Original function fails, singular at x=0
assert is_epsilon_correct(lambda x, eps: sm.sin(x) / x) == False

'Expressions (raw / eps):'

$\displaystyle \frac{\sin{\left(x \right)}}{x}$

'[ERROR] Epsilon handling failed, expression at 0 is NaN.'

'[ERROR] Values at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 1$

$\displaystyle \text{NaN}$

'[ERROR] Derivatives at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 0$

$\displaystyle \text{NaN}$

[15]:

# Original broken attempt
assert is_epsilon_correct(lambda x, eps: sm.sin(x) / (eps + x)) == False

'Expressions (raw / eps):'

$\displaystyle \frac{\sin{\left(x \right)}}{x}$

$\displaystyle \frac{\sin{\left(x \right)}}{\epsilon + x}$

'[ERROR] Values at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 1$

$\displaystyle 0$

'[ERROR] Derivatives at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 0$

$\displaystyle \frac{1}{\epsilon}$

$\displaystyle \infty$

[16]:

# Additive on top/bottom works
assert is_epsilon_correct(lambda x, eps: (eps + sm.sin(x)) / (eps + x)) == True

'Expressions (raw / eps):'

$\displaystyle \frac{\sin{\left(x \right)}}{x}$

$\displaystyle \frac{\epsilon + \sin{\left(x \right)}}{\epsilon + x}$

[17]:

# Replacing all x with (x + eps) works
assert is_epsilon_correct(lambda x, eps: sm.sin(x + eps) / (x + eps)) == True

'Expressions (raw / eps):'

$\displaystyle \frac{\sin{\left(x \right)}}{x}$

$\displaystyle \frac{\sin{\left(\epsilon + x \right)}}{\epsilon + x}$

Test (1 - cos(x)) / x#

[18]:

# Original fails, singular at 0
assert is_epsilon_correct(lambda x, eps: (1 - sm.cos(x)) / x) == False

'Expressions (raw / eps):'

$\displaystyle \frac{1 - \cos{\left(x \right)}}{x}$

'[ERROR] Epsilon handling failed, expression at 0 is NaN.'

'[ERROR] Values at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 0$

$\displaystyle \text{NaN}$

'[ERROR] Derivatives at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle \frac{1}{2}$

$\displaystyle \text{NaN}$

[19]:

# Value passes if we just replace the denominator, because this one is
# ~ x**2 / x unlike the above which is ~ x / x
assert is_epsilon_correct(lambda x, eps: (1 - sm.cos(x)) / (x + eps)) == False

'Expressions (raw / eps):'

$\displaystyle \frac{1 - \cos{\left(x \right)}}{x}$

$\displaystyle \frac{1 - \cos{\left(x \right)}}{\epsilon + x}$

'[ERROR] Derivatives at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle \frac{1}{2}$

$\displaystyle 0$

[20]:

# Derivative also passes if we replace both
assert is_epsilon_correct(lambda x, eps: (1 - sm.cos(x + eps)) / (x + eps)) == True

'Expressions (raw / eps):'

$\displaystyle \frac{1 - \cos{\left(x \right)}}{x}$

$\displaystyle \frac{1 - \cos{\left(\epsilon + x \right)}}{\epsilon + x}$

Test x / sqrt(x**2)#

[21]:

# Original fails, singular at 0
assert is_epsilon_correct(lambda x, eps: x / sm.sqrt(x**2)) == False

'Expressions (raw / eps):'

$\displaystyle \frac{x}{\left|{x}\right|}$

'[ERROR] Epsilon handling failed, expression at 0 is NaN.'

'[ERROR] Values at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 1$

$\displaystyle \text{NaN}$

'[ERROR] Derivatives at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 0$

$\displaystyle \text{NaN}$

[22]:

# Broken fix #1
assert is_epsilon_correct(lambda x, eps: x / sm.sqrt(x**2 + eps**2)) == False

'Expressions (raw / eps):'

$\displaystyle \frac{x}{\left|{x}\right|}$

$\displaystyle \frac{x}{\sqrt{\epsilon^{2} + x^{2}}}$

'[ERROR] Values at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 1$

$\displaystyle 0$

'[ERROR] Derivatives at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 0$

$\displaystyle \frac{1}{\epsilon}$

$\displaystyle \infty$

[23]:

# Broken fix #2
assert (
    is_epsilon_correct(
        lambda x, eps: (x + eps) / sm.sqrt(x**2 + eps**2),
    )
    == False
)

'Expressions (raw / eps):'

$\displaystyle \frac{x}{\left|{x}\right|}$

$\displaystyle \frac{\epsilon + x}{\sqrt{\epsilon^{2} + x^{2}}}$

'[ERROR] Derivatives at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 0$

$\displaystyle \frac{1}{\epsilon}$

$\displaystyle \infty$

[24]:

# Broken fix #3, ugh
assert (
    is_epsilon_correct(
        lambda x, eps: (x + eps) / (eps + sm.sqrt(x**2 + eps**2)),
    )
    == False
)

'Expressions (raw / eps):'

$\displaystyle \frac{x}{\left|{x}\right|}$

$\displaystyle \frac{\epsilon + x}{\epsilon + \sqrt{\epsilon^{2} + x^{2}}}$

'[ERROR] Values at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 1$

$\displaystyle \frac{1}{2}$

'[ERROR] Derivatives at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 0$

$\displaystyle \frac{1}{2 \epsilon}$

$\displaystyle \infty$

[25]:

# Working if you again replace all x with x + eps
assert (
    is_epsilon_correct(
        lambda x, eps: (x + eps) / sm.sqrt((x + eps) ** 2),
    )
    == True
)

'Expressions (raw / eps):'

$\displaystyle \frac{x}{\left|{x}\right|}$

$\displaystyle \frac{\epsilon + x}{\left|{\epsilon + x}\right|}$

Test acos(x) / sqrt(1 - x^2) at 1#

[26]:

# Original fails, singular at 1
assert (
    is_epsilon_correct(
        lambda x, eps: sm.acos(x) / sm.sqrt(1 - x**2),
        singularity=1,
    )
    == False
)

'Expressions (raw / eps):'

$\displaystyle \frac{\operatorname{acos}{\left(x \right)}}{\sqrt{1 - x^{2}}}$

'[ERROR] Epsilon handling failed, expression at 0 is NaN.'

'[ERROR] Values at x=1 do not match (raw / eps / eps.limit):'

$\displaystyle 1$

$\displaystyle \text{NaN}$

'[ERROR] Derivatives at x=1 do not match (raw / eps / eps.limit):'

$\displaystyle - \frac{1}{3}$

$\displaystyle \text{NaN}$

[27]:

# Working if you replace all x with x + eps
assert (
    is_epsilon_correct(
        lambda x, eps: sm.acos(x + eps) / sm.sqrt(1 - (x + eps) ** 2),
        singularity=1,
    )
    == True
)

'Expressions (raw / eps):'

$\displaystyle \frac{\operatorname{acos}{\left(x \right)}}{\sqrt{1 - x^{2}}}$

$\displaystyle \frac{\operatorname{acos}{\left(\epsilon + x \right)}}{\sqrt{1 - \left(\epsilon + x\right)^{2}}}$

Test atan2(0, x)#

[28]:

# Original is singular
assert is_epsilon_correct(lambda x, eps: sm.atan2(0, x)) == False

'Expressions (raw / eps):'

$\displaystyle \operatorname{atan}_{2}{\left(0,x \right)}$

'[ERROR] Epsilon handling failed, expression at 0 is NaN.'

'[ERROR] Values at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 0$

$\displaystyle \text{NaN}$

[29]:

# This works
assert is_epsilon_correct(lambda x, eps: sm.atan2(0, x + eps)) == True

'Expressions (raw / eps):'

$\displaystyle \operatorname{atan}_{2}{\left(0,x \right)}$

$\displaystyle \operatorname{atan}_{2}{\left(0,\epsilon + x \right)}$

Handling negative x#

If we consider an example from above like $sin(x + \epsilon) / (x + \epsilon)$, this can easily be singular for a negative $x$, specifically where $x = -\epsilon$. If $x$ were always negative, we could do $sin(x - \epsilon) / (x - \epsilon)$.

So to handle both cases we can use the sign function as: $sin(x + sign(x) * \epsilon) / (x + sign(x) * \epsilon)$. This gives us the behavior that it always pushes $x$ away from zero because $sign(+) = 1$ and $sign(-) = -1$. However, $sign(0) = 0$ (at least, by SymPy’s definition) which breaks the original zero point. To resolve this we can make a “no zero” version that arbitrarily picks the positive direction when exactly at zero. We’ll call this sign_no_zero(x), or $snz(x)$

We can implement this cleverly with the help of a min function:

[30]:

def sign_no_zero(x):
    # type: (sf.Scalar) -> sf.Scalar
    """
    Returns -1 if x is negative, 1 if x is positive, and 1 if x is zero (given
    a positive epsilon).
    """
    return 2 * sm.Min(sm.sign(x), 0) + 1

[31]:

# Test for sin(x) / x
assert (
    is_epsilon_correct(
        lambda x, eps: (sm.sin(x + eps * sign_no_zero(x))) / (x + eps * sign_no_zero(x))
    )
    == True
)

'Expressions (raw / eps):'

$\displaystyle \frac{\sin{\left(x \right)}}{x}$

$\displaystyle \frac{\sin{\left(\epsilon \left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) + x \right)}}{\epsilon \left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) + x}$

[32]:

# Test for x / sqrt(x**2)
assert (
    is_epsilon_correct(
        lambda x, eps: (x + eps) / sm.sqrt((x + eps) ** 2),
    )
    == True
)

'Expressions (raw / eps):'

$\displaystyle \frac{x}{\left|{x}\right|}$

$\displaystyle \frac{\epsilon + x}{\left|{\epsilon + x}\right|}$

[33]:

# Test for atan2(0, x)
assert (
    is_epsilon_correct(
        lambda x, eps: sm.atan2(0, x + eps * sign_no_zero(x)),
    )
    == True
)

'Expressions (raw / eps):'

$\displaystyle \operatorname{atan}_{2}{\left(0,x \right)}$

$\displaystyle \operatorname{atan}_{2}{\left(0,\epsilon \left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) + x \right)}$

Generalization#

So far it seems like for a function $f(x)$ that is singular at $x=0$, the expression $f(x + \epsilon * snz(x))$ for a small positive $\epsilon$ will be non-singular and retain the same linear approximiation as $f(x)$. So we can easily write a function that does this substitution:

[34]:

def add_epsilon_sign(expr, var, eps):
    return expr.subs(var, var + eps * sign_no_zero(var))

[35]:

# Check known example
assert (
    is_epsilon_correct(
        lambda x, eps: add_epsilon_sign(sm.sin(x) / x, x, eps),
    )
    == True
)

'Expressions (raw / eps):'

$\displaystyle \frac{\sin{\left(x \right)}}{x}$

[36]:

# Try some more complicated thing nobody wants to epsilon by hand
assert (
    is_epsilon_correct(
        lambda x, eps: add_epsilon_sign(
            (x + sm.sin(x) ** 2) / (x * (1 - 1 / x)),
            x,
            eps,
        )
    )
    == True
)

'Expressions (raw / eps):'

$\displaystyle \frac{x + \sin^{2}{\left(x \right)}}{x \left(1 - \frac{1}{x}\right)}$

$\displaystyle \frac{\epsilon \left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) + x + \sin^{2}{\left(\epsilon \left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) + x \right)}}{\left(1 - \frac{1}{\epsilon \left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) + x}\right) \left(\epsilon \left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) + x\right)}$

Another common case is a function $f(x)$ that is singular at $|x| = 1$, where we expect $x$ to be between $-1$ and $1$. In this case we can use a similar idea, using $f(x - \epsilon * snz(x))$. A function to do this would be:

[37]:

def add_epsilon_near_1_sign(expr, var, eps):
    return expr.subs(var, var - eps * sign_no_zero(var))

[38]:

# Check known example
assert (
    is_epsilon_correct(
        lambda x, eps: add_epsilon_near_1_sign(
            sm.cos(x) / sm.sqrt(1 - x**2),
            x,
            eps,
        ),
        singularity=1,
        limit_direction="-",
    )
    == True
)

'Expressions (raw / eps):'

$\displaystyle \frac{\cos{\left(x \right)}}{\sqrt{1 - x^{2}}}$

$\displaystyle \frac{\cos{\left(\epsilon \left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) - x \right)}}{\sqrt{1 - \left(- \epsilon \left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) + x\right)^{2}}}$

Clamping#

We could also imagine clamping away from the singularity, instead of shifting with addition. That would look like this:

[39]:

def add_epsilon_max(expr, var, eps):
    return expr.subs(var, sign_no_zero(var) * sm.Max(eps, sm.Abs(var)))


def add_epsilon_near_1_clamp(expr, var, eps):
    return expr.subs(var, sm.Max(-1 + eps, sm.Min(1 - eps, var)))

However, this will always give a derivative of 0 at the singularity - since the derivative of sign_no_zero(var) * sm.Max(eps, sm.Abs(var)) with respect to var is 0 there. We can see this with an example function, whose derivative at the singularity should be 1. add_epsilon_sign handles this correctly:

[40]:

assert (
    is_epsilon_correct(
        lambda x, eps: add_epsilon_sign((sm.sin(x) + x**2) / x, x, eps),
    )
    == True
)

'Expressions (raw / eps):'

$\displaystyle \frac{x^{2} + \sin{\left(x \right)}}{x}$

$\displaystyle \frac{\left(\epsilon \left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) + x\right)^{2} + \sin{\left(\epsilon \left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) + x \right)}}{\epsilon \left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) + x}$

add_epsilon_max gives the correct value, but not the correct derivative:

[41]:

assert (
    is_epsilon_correct(
        lambda x, eps: add_epsilon_max((sm.sin(x) + x**2) / x, x, eps),
    )
    == False
)

'Expressions (raw / eps):'

$\displaystyle \frac{x^{2} \left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right)^{2} + \sin{\left(\left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) \left|{x}\right| \right)}}{\left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) \left|{x}\right|}$

$\displaystyle \frac{\left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right)^{2} \max\left(\epsilon, \left|{x}\right|\right)^{2} + \sin{\left(\left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) \max\left(\epsilon, \left|{x}\right|\right) \right)}}{\left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) \max\left(\epsilon, \left|{x}\right|\right)}$

'[ERROR] Derivatives at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 1$

$\displaystyle - \frac{2 \left(\epsilon^{2} + \sin{\left(\epsilon \right)}\right) \delta\left(0\right)}{\epsilon} + \frac{4 \epsilon^{2} \delta\left(0\right) + 2 \epsilon \cos{\left(\epsilon \right)} \delta\left(0\right)}{\epsilon}$

$\displaystyle 0$

Numerical Values for Epsilon#

SymForce defaults to using 10 times the floating point epsilon, which we’ve found to be a good value across a wide variety of use cases.

This numerical value is available, for example, as sf.numeric_epsilon and sym.epsilon. Those constants specifically are intended for use with 64-bit floats (e.g. the builtin float type in Python), if using a different floating point type you should typically compute 10 times the epsilon for that type. For example, sym::kDefaultEpsilon<Scalar> in C++ provides the recommended epsilon for each type.

One caveat here is that this works best if anything you need to add $\epsilon$ to has a magnitude near 1 or smaller. If adding $\epsilon$ to something that may be much larger, using an epsilon that is too small will result in not actually changing the value, due to floating point precision. In cases like this, you’ll want to know the approximate size of the value you’re adding to, or an upper bound on that size. If that size is c, you should add something like c * epsilon in your symbolic function instead of just epsilon.

Caveats and Limitations#

So far we’ve assumed the only singularity is at $x=0$, but everything above works just fine for singularities at other locations, for instance with the function $1/(1-x)$ for $x=1$. What isn’t handled well is functions that have multiple singularities, like $1/((x-1)(x-2)(x-3))$. You can compose functions using the singularity handling method above just fine, and everything will be safe. This is almost always all that you need, but if you do encounter a function with multiple singularities that can’t be rewritten as a composition of functions with fewer singularities, that will require something more complex.

It’s also easy to construct singularities where this method works symbolically, but relies on values like $\epsilon^2$ which are too small in actual floating point implementations (see the Pose2_SE2.to_tangent section below). And we could have a composite of multiple functions, so it’s hard to have global visibility into what all your singular points are. Sometimes parameters have a fixed range, sometimes things are normalized, etc.

So a remaining open question is: how well can we generalize this single variable example with a single known singularity to multiple variables and only locally known singularities like at the places we add a division, square root, or atan2 operation?

Case Studies#

Pose2_SE2.from_tangent#

Pose2_SE2.from_tangent before epsilon handling looks like:

def pose2_from_tangent(v):
    theta = v[0]
    R = Rot2.from_tangent([theta])

    a = sm.sin(theta) / theta
    b = (1 - sm.cos(theta)) / theta

    t = geo.Vector2(a * v[1] - b * v[2], b * v[1] + a * v[2])
    return geo.Pose2(R, t)

This has singularities in both a and b that we’d like to fix. The initial version used:

a = sm.sin(theta) / (theta + epsilon)
b = (1 - sm.cos(theta)) / (theta + epsilon)

For a, this falls under the $sin(x) / x$ example above, and b falls under the $(1 - cos(x)) / x$ example above, so we can use the fixes from those examples, modified to handle negative $x$ as in the Generalization section.

Pose2_SE2.to_tangent#

Pose2_SE2.to_tangent before epsilon handling looks like:

def pose2_to_tangent():
    theta = self.R.to_tangent()[0]
    halftheta = 0.5 * theta
    a = (halftheta * sm.sin(theta)) / (1 - sm.cos(theta))

    V_inv = Matrix[[a, halftheta], [-halftheta, a]])
    t_tangent = V_inv * self.t
    return [theta, t_tangent[0], t_tangent[1]]

This has a singularity in a that looks like:

[42]:

assert (
    is_epsilon_correct(
        lambda theta, eps: (0.5 * theta * sm.sin(theta)) / (1 - sm.cos(theta)),
    )
    == False
)

'Expressions (raw / eps):'

$\displaystyle \frac{0.5 x \sin{\left(x \right)}}{1 - \cos{\left(x \right)}}$

'[ERROR] Epsilon handling failed, expression at 0 is NaN.'

'[ERROR] Values at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 1$

$\displaystyle \text{NaN}$

'[ERROR] Derivatives at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 0$

$\displaystyle \text{NaN}$

We might think this can be fixed with with the $x = x + \epsilon$ trick:

[43]:

assert (
    is_epsilon_correct(
        lambda theta, eps: add_epsilon_sign(
            (0.5 * theta * sm.sin(theta)) / (1 - sm.cos(theta)), theta, eps
        )
    )
    == True
)

'Expressions (raw / eps):'

$\displaystyle \frac{0.5 x \sin{\left(x \right)}}{1 - \cos{\left(x \right)}}$

$\displaystyle \frac{0.5 \left(\epsilon \left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) + x\right) \sin{\left(\epsilon \left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) + x \right)}}{1 - \cos{\left(\epsilon \left(2 \min\left(0, \operatorname{sign}{\left(x \right)}\right) + 1\right) + x \right)}}$

But in practice, the denominator’s taylor series is $(1 - (1 - 0.5 \epsilon^2))$, and if $\epsilon$ is near machine precision, then the denominator will end up as exactly 1 - 1 == 0. This might be solved by adding $sqrt(\epsilon)$ instead, but that would introduce a large amount of error. Instead, we do some algebra:

\[\begin{split}\begin{align*} \frac{0.5 \theta \sin(\theta)}{1 - \cos(\theta)} &= \frac{0.5 \theta \sin(\theta)}{1 - \cos(\theta)} \frac{1 + \cos(\theta)}{1 + \cos(\theta)} \\ &= \frac{0.5 \theta \sin(\theta) (1 + \cos(\theta))}{1 - \cos^2(\theta)} \\ &= \frac{0.5 \theta \sin(\theta) (1 + \cos(\theta))}{\sin^2(\theta)} \\ &= \frac{0.5 \theta (1 + \cos(\theta))}{\sin(\theta)} \end{align*}\end{split}\]

Then, the only singularity is at $\sin(\theta) = 0$, which we can fix with:

[44]:

assert (
    is_epsilon_correct(
        lambda theta, eps: (0.5 * (theta + eps) * (1 + sm.cos(theta))) / (sm.sin(theta) + eps)
    )
    == True
)

'Expressions (raw / eps):'

$\displaystyle \frac{0.5 x \left(\cos{\left(x \right)} + 1\right)}{\sin{\left(x \right)}}$

$\displaystyle \frac{\left(0.5 \epsilon + 0.5 x\right) \left(\cos{\left(x \right)} + 1\right)}{\epsilon + \sin{\left(x \right)}}$

Rot3.to_tangent#

Rot3.to_tangent before epsilon handling looks like:

def logmap():
    norm = sm.sqrt(1 - self.q.w**2)
    tangent = 2 * self.q.xyz / norm * sm.acos(self.q.w)
    return tangent

Ignoring the q.xyz variables, the function has a singularity at w == 1, of the form

\[\frac{\arccos(w)}{\sqrt{1 - w^2}}\]

which can be fixed using either the add_epsilon_near_1_sign or add_epsilon_near_1_clamp methods described in the Generalization section.

Multivariate functions#

[45]:

x, y = sm.symbols("x, y", real=True)

[46]:

# Looking at a normalization function
epsilon = sm.Symbol("epsilon")
expr = x / sm.sqrt(x**2 + y**2)
sm.series(expr, x, n=2)
sm.simplify(expr.diff(x).limit(x, 0))

[46]:

$\displaystyle \frac{1}{\left|{y}\right|}$

[47]:

# Simulate a normalization where y = 0
is_epsilon_correct(lambda x, eps: sm.Matrix([x + eps, 0]).normalized()[0])

'Expressions (raw / eps):'

$\displaystyle \frac{x}{\left|{x}\right|}$

$\displaystyle \frac{\epsilon + x}{\left|{\epsilon + x}\right|}$

[47]:

True

[48]:

is_epsilon_correct(
    lambda x, eps: (x + eps) / sm.sqrt((x + eps) ** 2 + (0 + eps) ** 2),
)

'Expressions (raw / eps):'

$\displaystyle \frac{x}{\left|{x}\right|}$

$\displaystyle \frac{\epsilon + x}{\sqrt{\epsilon^{2} + \left(\epsilon + x\right)^{2}}}$

'[ERROR] Values at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 1$

$\displaystyle \frac{\sqrt{2}}{2}$

'[ERROR] Derivatives at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 0$

$\displaystyle \frac{\sqrt{2}}{4 \epsilon}$

$\displaystyle \infty$

[48]:

False

[49]:

is_epsilon_correct(lambda x, eps: (x + eps) / sm.sqrt(eps**2 + (x + eps) ** 2))

'Expressions (raw / eps):'

$\displaystyle \frac{x}{\left|{x}\right|}$

$\displaystyle \frac{\epsilon + x}{\sqrt{\epsilon^{2} + \left(\epsilon + x\right)^{2}}}$

'[ERROR] Values at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 1$

$\displaystyle \frac{\sqrt{2}}{2}$

'[ERROR] Derivatives at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 0$

$\displaystyle \frac{\sqrt{2}}{4 \epsilon}$

$\displaystyle \infty$

[49]:

False

[50]:

is_epsilon_correct(lambda x, eps: (x + eps) / sm.sqrt(eps**2 + (x + eps) ** 2))

'Expressions (raw / eps):'

$\displaystyle \frac{x}{\left|{x}\right|}$

$\displaystyle \frac{\epsilon + x}{\sqrt{\epsilon^{2} + \left(\epsilon + x\right)^{2}}}$

'[ERROR] Values at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 1$

$\displaystyle \frac{\sqrt{2}}{2}$

'[ERROR] Derivatives at x=0 do not match (raw / eps / eps.limit):'

$\displaystyle 0$

$\displaystyle \frac{\sqrt{2}}{4 \epsilon}$

$\displaystyle \infty$

[50]:

False