Trilateration with limits?

Question

I'm in need of help solving an issue, the problem came up doing one of my small robot experiments, the basic idea, is that each little robot has the ability to approximate the distance, from themselves to an object, however the approximate I'm getting is way too rough, and I'm hoping to calculate something more accurate.

So:
Input: A list of vertex (v_1, v_2, ... v_n), a vertex v_* (robots)
Output: The coordinates for the unknown vertex v_* (object)

Each vertex v_1 to v_n's coordinates are well known (supplied by calling getX() and getY() on the vertex), and its possible to get the approximate range to v_* by calling; getApproximateDistance(v_*), function getApproximateDistance() returns two variables variables, that is; minDistance and maxDistance. - The actual distance lies in between these.

So what I've been trying to do to obtain the coordinates for v_*, is to use trilateration, however I can't seem to find a formula for doing trilateration with limits (lower and upperbound), so that's really what I'm looking for (not really good enough at math, to figure it out myself).

Note: is triangulation the way to go instead?
Note: I would possibly love to know a way to do, performance/accuracy trade-offs.

An example of data:

[Vertex . `getX()` . `getY()` . `minDistance` . `maxDistance`]
[`v_1`  .  2       .  2       .  0.5          .  1  ]
[`v_2`  .  1       .  2       .  0.3          .  1  ]
[`v_3`  .  1.5     .  1       .  0.3          .  0.5]

Picture to show data: http://img52.imageshack.us/img52/6414/unavngivetcb.png

It's obvious that the approximate for v_1 can be better, than [0.5; 1], as the figure that the above data creates is small cut of a annulus (limited by v_3), however how would I calculate that, and possibly find the approximate within that figure (this figure is possibly concave)?

Would this be better suited for MathOverflow?

Answer 1

I would go for a simple discrete approach. The implicit formula for an annulus is trivial and the intersection of multiple annulus if the number of them is high can be computed somewhat efficently with a scanline based approach.

For getting high accuracy with a fast computation an option could be using a multiresolution approach (i.e. first starting in low-res and then recomputing in high-res only samples that are close to a valid point.

A small python toy I wrote can generate a 400x400 pixel image of the intersection area in about 0.5 secs (this is the kind of computation that would get a 100x speedup if done with C).

# x, y, r0, r1
data = [(2.0, 2.0, 0.5, 1.0),
        (1.0, 2.0, 0.3, 1.0),
        (1.5, 1.0, 0.3, 0.5)]

x0 = max(x - r1 for x, y, r0, r1 in data)
y0 = max(y - r1 for x, y, r0, r1 in data)
x1 = min(x + r1 for x, y, r0, r1 in data)
y1 = min(y + r1 for x, y, r0, r1 in data)

def hit(x, y):
    for cx, cy, r0, r1 in data:
        if not (r0**2 <= ((x - cx)**2 + (y - cy)**2) <= r1**2):
            return False
    return True

res = 400
step = 16
white = chr(255)
grey = chr(192)
black = chr(0)
img = [black] * (res * res)

# Low-res pass
cells = {}
for i in xrange(0, res, step):
    y = y0 + i * (y1 - y0) / res
    for j in xrange(0, res, step):
        x = x0 + j * (x1 - x0) / res
        if hit(x, y):
            for h in xrange(-step*2, step*3, step):
                for v in xrange(-step*2, step*3, step):
                    cells[(i+v, j+h)] = True

# High-res pass
for i in xrange(0, res, step):
    for j in xrange(0, res, step):
        if cells.get((i, j), False):
            img[i * res + j] = grey
            img[(i + step - 1) * res + j] = grey
            img[(i + step - 1) * res + (j + step - 1)] = grey
            img[i * res + (j + step - 1)] = grey
            for v in xrange(step):
                y = y0 + (i + v) * (y1 - y0) / res
                for h in xrange(step):
                    x = x0 + (j + h) * (x1 - x0) / res
                    if hit(x, y):
                        img[(i + v)*res + (j + h)] = white

open("result.pgm", "wb").write(("P5\n%i %i 255\n" % (res, res)) +
                               "".join(img))

Another interesting option could be using a GPU if available. Starting from a white picture and drawing in black the exterior of each annulus will leave at the end the intersection area in white.

For example with Python/Qt the code for doing this computation is simply:

img = QImage(res, res, QImage.Format_RGB32)
dc = QPainter(img)
dc.fillRect(0, 0, res, res, QBrush(QColor(255, 255, 255)))
dc.setPen(Qt.NoPen)
dc.setBrush(QBrush(QColor(0, 0, 0)))
for x, y, r0, r1 in data:
    xa1 = (x - r1 - x0) * res / (x1 - x0)
    xb1 = (x + r1 - x0) * res / (x1 - x0)
    ya1 = (y - r1 - y0) * res / (y1 - y0)
    yb1 = (y + r1 - y0) * res / (y1 - y0)
    xa0 = (x - r0 - x0) * res / (x1 - x0)
    xb0 = (x + r0 - x0) * res / (x1 - x0)
    ya0 = (y - r0 - y0) * res / (y1 - y0)
    yb0 = (y + r0 - y0) * res / (y1 - y0)
    p = QPainterPath()
    p.addEllipse(QRectF(xa0, ya0, xb0-xa0, yb0-ya0))
    p.addEllipse(QRectF(xa1, ya1, xb1-xa1, yb1-ya1))
    p.addRect(QRectF(0, 0, res, res))
    dc.drawPath(p)

and the computation part for an 800x800 resolution image takes about 8ms (and I'm not sure it's hardware accelerated).

If only the barycenter of the intersection is to be computed then there is no memory allocation at all. For example a "brute-force" approach is just a few lines of C

typedef struct TReading {
    double x, y, r0, r1;
} Reading;

int hit(double xx, double yy,
        Reading *readings, int num_readings)
{
    while (num_readings--)
    {
        double dx = xx - readings->x;
        double dy = yy - readings->y;
        double d2 = dx*dx + dy*dy;
        if (d2 < readings->r0 * readings->r0) return 0;
        if (d2 > readings->r1 * readings->r1) return 0;
        readings++;
    }
    return 1;
}

int computeLocation(Reading *readings, int num_readings,
                    int resolution,
                    double *result_x, double *result_y)
{
    // Compute bounding box of interesting zone
    double x0 = -1E20, y0 = -1E20, x1 = 1E20, y1 = 1E20;
    for (int i=0; i<num_readings; i++)
    {
        if (readings[i].x - readings[i].r1 > x0)
          x0 = readings[i].x - readings[i].r1;
        if (readings[i].y - readings[i].r1 > y0)
          y0 = readings[i].y - readings[i].r1;
        if (readings[i].x + readings[i].r1 < x1)
          x1 = readings[i].x + readings[i].r1;
        if (readings[i].y + readings[i].r1 < y1)
          y1 = readings[i].y + readings[i].r1;
    }

    // Scan processing
    double ax = 0, ay = 0;
    int total = 0;
    for (int i=0; i<=resolution; i++)
    {
        double yy = y0 + i * (y1 - y0) / resolution;
        for (int j=0; j<=resolution; j++)
        {
            double xx = x0 + j * (x1 - x0) / resolution;
            if (hit(xx, yy, readings, num_readings))
            {
                ax += xx; ay += yy; total += 1;
            }
        }
    }
    if (total)
    {
        *result_x = ax / total;
        *result_y = ay / total;
    }
    return total;
}

And on my PC can compute the barycenter with resolution = 100 in 0.08 ms (x=1.50000, y=1.383250) or with resolution = 400 in 1.3ms (x=1.500000, y=1.383308). Of course a double-step speedup could be implemented even for the barycenter-only version.