Converting area of intersection to generate a coordinate

Question

I was working on a project where I get analog values from a resistive touchscreen and turn them into intersection points.

Here is an example:

Here is my code for the data collection using an Arduino Uno and construction of the points using tool called processing.

#define side1 2
#define side2 3
#define side3 4
#define side4 5
#define contact A0

void setup() {
  pinMode(contact, INPUT); 
  pinMode(side1, OUTPUT);  
  pinMode(side2, OUTPUT);  
  pinMode(side3, OUTPUT);  
  pinMode(side4, OUTPUT);  
  Serial.begin(9600);
}

void loop() {
  int sensorValue1;
  int sensorValue2;
  int sensorValue3;
  int sensorValue4;

  // SENSOR VALUE 1:
  digitalWrite(side1, LOW);
  digitalWrite(side2, HIGH);
  digitalWrite(side3, HIGH);
  digitalWrite(side4, HIGH);
  delay(5);
  for (int i = 0; i < 10; i++){
    sensorValue1 = analogRead(contact);
  }


  // SENSOR VALUE 2:
  digitalWrite(side2, LOW);
  digitalWrite(side3, HIGH);
  digitalWrite(side4, HIGH);
  digitalWrite(side1, HIGH);
  delay(5);
  for (int i = 0; i < 10; i++){
    sensorValue2 = analogRead(contact);
  }


  // SENSOR VALUE 3:
  digitalWrite(side3, LOW);
  digitalWrite(side2, HIGH);
  digitalWrite(side4, HIGH);
  digitalWrite(side1, HIGH);
  delay(5);
  for (int i = 0; i < 10; i++){
    sensorValue3 = analogRead(contact);
  }


  // SENSOR VALUE 2:
  digitalWrite(side4, LOW);
  digitalWrite(side3, HIGH);
  digitalWrite(side2, HIGH);  
  digitalWrite(side1, HIGH);
  delay(5);
  for (int i = 0; i < 10; i++){
    sensorValue4 = analogRead(contact);
  }

  Serial.print(sensorValue1);
  Serial.print(",");
  Serial.print(sensorValue2);
  Serial.print(",");
  Serial.print(sensorValue3);
  Serial.print(",");
  Serial.print(sensorValue4);
  Serial.println();
}

This is the Processing code for the construction of the graph.

import processing.serial.*;


Serial myPort;  // The serial port
int maxNumberOfSensors = 4;   
float[] sensorValues = new float[maxNumberOfSensors];
float sensorValueX;
float sensorValueX1;
float sensorValueY;
float sensorValueY1;
int scaleValue = 2;

void setup () { 
  size(600, 600);  // set up the window to whatever size you want
  //println(Serial.list());  // List all the available serial ports
  String portName = "COM5";
  myPort = new Serial(this, portName, 9600);
  myPort.clear();
  myPort.bufferUntil('\n');  // don't generate a serialEvent() until you get a newline (\n) byte
  background(255);    // set inital background
  smooth();  // turn on antialiasing
}


void draw () {
  //background(255);
  //noFill();
  fill(100,100,100,100);
  ellipse(height,0, scaleValue*sensorValues[0], scaleValue*sensorValues[0]);

  ellipse(0,width, scaleValue*sensorValues[1], scaleValue*sensorValues[1]);
  ellipse(height,width, scaleValue*sensorValues[2], scaleValue*sensorValues[2]);
  ellipse(0,0, scaleValue*sensorValues[3], scaleValue*sensorValues[3]);
  //ellipse(sensorValueY, sensorValueX, 10,10);
  //println(sensorValueY,sensorValueX);
  sensorValueX = ((sensorValues[3]*sensorValues[3])-(sensorValues[2]*sensorValues[2])+600*600)/2000;
  sensorValueX1 = ((sensorValues[0]*sensorValues[0])-(sensorValues[1]*sensorValues[1])+600*600)/2000;
sensorValueY = ((sensorValues[3]*sensorValues[3])-(sensorValues[2]*sensorValues[2])+(600*600))/2000;
  sensorValueY1 = ((sensorValues[1]*sensorValues[1])-(sensorValues[0]*sensorValues[0])+(600*600))/2000;

  line(0, scaleValue*sensorValueX, height,scaleValue* sensorValueX);
  line(scaleValue*sensorValueY, 0, scaleValue*sensorValueY, width);
  ellipse(scaleValue*sensorValueY, scaleValue*sensorValueX, 20,20);
  line(0, scaleValue*sensorValueX1, height,scaleValue* sensorValueX1);
  line(scaleValue*sensorValueY1, 0, scaleValue*sensorValueY1, width);
  ellipse(scaleValue*sensorValueY1, scaleValue*sensorValueX1, 20,20);
  println(scaleValue*sensorValueX,scaleValue*sensorValueY);
}


void serialEvent (Serial myPort) {
  String inString = myPort.readStringUntil('\n');  // get the ASCII string

  if (inString != null) {  // if it's not empty
    inString = trim(inString);  // trim off any whitespace
    int incomingValues[] = int(split(inString, ","));  // convert to an array of ints

    if (incomingValues.length <= maxNumberOfSensors && incomingValues.length > 0) {
      for (int i = 0; i < incomingValues.length; i++) {
        // map the incoming values (0 to  1023) to an appropriate gray-scale range (0-255):

        sensorValues[i] = map(incomingValues[i], 0, 1023, 0, width);
        //println(incomingValues[i]+ " " + sensorValues[i]);
      }
    }
  }
}

I was wondering how I could convert the intersection of those points to a coordinate? Example: in the image, I showed you, I set the parameters for the dimensions to be (600,600). Is it possible to change that intersection are to a coordinate value? Currently, my code is printing out coordinates however they are diagonals such at the x and y values are equal. I want the coordinates of x and y to have different quantities so that I can get coordinates for different sides in the square. Can somebody help?

Answer 1

By reading your code I'm assuming that you know the position of all n sensors and the distance from each n sensor to a target. So what you're essentially trying to do is trilateration (as mentioned by Nico Schertler). In other words determining a relative position based on the distance between n points.

Just a quick definition note in case of confusion:

• Triangulation = Working with angles
• Trilateration = Working with distances

Trilateration requires at least 3 points and distances.

• 1 sensor gives you the distance the target is away from the sensor
• 2 sensors gives you 2 possible locations the target can be
• 3 sensors tells you which of the 2 locations the target is at

The first solution that probably comes to mind is calculating the intersections between 3 sensors treating them as circles. Given that there might be some error in the distances this means that the circles might not always intersect. Which rules out this solution.

The following code has all been done in Processing.

I took the liberty of making a class Sensor.

class Sensor {
    public PVector p; // position
    public float d; // distance from sensor to target (radius of the circle)

    public Sensor(float x, float y) {
        this.p = new PVector(x, y);
        this.d = 0;
    }
}

Now to calculate and approximate the intersection point between the sensors/circles, do the following:

PVector trilateration(Sensor s1, Sensor s2, Sensor s3) { 
    PVector s = PVector.sub(s2.p, s1.p).div(PVector.sub(s2.p, s1.p).mag());
    float a = s.dot(PVector.sub(s3.p, s1.p));

    PVector t = PVector.sub(s3.p, s1.p).sub(PVector.mult(s, a)).div(PVector.sub(s3.p, s1.p).sub(PVector.mult(s, a)).mag());
    float b = t.dot(PVector.sub(s3.p, s1.p));
    float c = PVector.sub(s2.p, s1.p).mag();

    float x = (sq(s1.d) - sq(s2.d) + sq(c)) / (c * 2);
    float y = ((sq(s1.d) - sq(s3.d) + sq(a) + sq(b)) / (b * 2)) - ((a / b) * x);

    s.mult(x);
    t.mult(y);

    return PVector.add(s1.p, s).add(t);
}

Where s1, s2, s3 is any of your 3 sensors, do the following to calculate the the intersection point between the given sensors:

PVector target = trilateration(s1, s2, s3);

While it is possible to calculate the intersection between any amount of sensors. It becomes more and more complex the more sensors you want to include. Especially since you're doing it yourself. If you're able to use external Java libraries, then it would be a lot easier.

If you're able to use external Java libraries, then I highly recommend using com.lemmingapex.trilateration. Then you'd be able to calculate the intersection point between 4 sensors by doing:

Considering s1, s2, s3, s4 as instances of the previously mentioned class Sensor.

double[][] positions = new double[][] { { s1.x, s1.y }, { s2.x, s2.y }, { s3.x, s3.y }, { s4.x, s4.y } };
double[] distances = new double[] { s1.d, s2.d, s3.d, s4.d };

NonLinearLeastSquaresSolver solver = new NonLinearLeastSquaresSolver(
            new TrilaterationFunction(positions, distances),
            new LevenbergMarquardtOptimizer());
Optimum optimum = solver.solve();

double[] target = optimum.getPoint().toArray();
double x = target[0];
double y = target[1];

The following examples, are examples of the trilateration() method I wrote and not an example of the library above.

Example 1 - No Sensor Error

The 3 big circles being any 3 sensors and the single red circle being the approximated point.

Example 2 - With Sensor Error

The 3 big circles being any 3 sensors and the single red circle being the approximated point.

Answer 2

What you need to compute is the point that it nearest to the a set of circles, let denote their centers by (x1,y1), (x2,y2), (x3,y3), (x4,y4) and their radii by r1,r2,r3,r4.

You want to find (x,y) that minimizes

F(x,y) = Sum_i [ square( d2( (x,y), (xi,yi)) - ri) ]

This can be achieved by using Newton's algorithm. Newton's algorithm works from an "initial guess" (let's say at the center of the screen), improved iteratively by solving a series of linear systems (in this case, with 2 variables, easy to solve). M P = -G

where M is the (2x2) matrix of the second order derivatives of F with respect to x and y (called the Hessian), and G the vector of the first order derivatives of F with respect to x and y (the gradient). This gives the "update" vector P, that tells how to move the coordinates:

Then (x,y) is updated by x = x + Px, y = y + Py, and so on and so forth (recompute M and G, solve for P, update x and y, recompute M and G, solve for P, update x and y). In your case it will probably converge in a handful of iterations.

Since you got two variables only, the 2x2 linear solve is trivial, and the expression of F and its derivatives is simple, thus you can implement it without needing an external library.

Note1: the Levenberg-Marquardt algorithm mentioned in the other answer is a variant of Newton's algorithm (specialized for sum of squares, like here, and that neglects some terms, and that regularizes the matrix M by adding small numbers to its diagonal coefficients). More on this here.

Note2: a simple gradient descent will also probably work (a bit simpler to implement, since it only uses first order derivatives), but given that you only got two variables to implement, the 2x2 linear solve is trivial, so Newton is probably worth it (requires a much much smaller number of iterations for convergence, may be critial if your system is interactive).