Related
EDIT: The localization is always about x=1980.000032943933547358028590679168701171875, y=3191.99997642902735606185160577297210693359375
I've written the following code to solve the Time Delay of Arrival problem. That is, given the location of three observers, the velocity of some signal, and the time at which each receiver "saw" the signal, I want to localize the source. Here, we assume that the source and the observers are in a 2-dimensional plane (a planar Euclidean space).
My solution is as follows:
Given the time at which each observer saw the signal, I elect one receiver to be the "baseline", which I take to be t=0. I then subtract that time from the Time of Arrival at the other two observers. Now, I assign to each observer a circle, with the radius given by this difference (the "baseline" observer starts with r=0), and I slowly increment the radius of each circle until all three intersect at some point.
In reality, it's unlikely that they'll ever precisely intersect at a single point, as, for one, I can only increase the radius by a discrete amount with each iteration, and we also assume, but do not known, that the observer's clocks are precisely synchronized.
To solve this, I adapted Paul Bourke's code: http://paulbourke.net/geometry/circlesphere/tvoght.c (reasoning here: http://paulbourke.net/geometry/circlesphere/). My adaptation was more-or-less identical to this one: https://stackoverflow.com/a/19724186/14073182.
My problem is, the code always produces the same localization, to within a few tenths of a unit. I tried generating some psuedodata (i.e. pick some location, compute the expected delay, feed that into the algorithm and see if it reconstructs the correct localization), and the algorithm still produces roughly the same localization... namely, fairly close to the center of the triangle formed by the three receivers.
Any idea what I'm doing wrong here? I've talked with a few people (physicists working in GIS, geodesics, similar fields, and a mathematician), and haven't gotten anywhere.
My code is below (it's fairly brief, all things considered). To localize some source, call localizeSource(a,b,c,&xf,&xy). Where a, b, and c are the delays (radii), and xf, xy are where the coordinates of the localization will be stored.
#define EPSILON 0.0001 // Define some threshold
#define x0 3000.00
#define y0 3600.00
#define x1 2100.00
#define y1 2100.00
#define x2 0960.00
#define y2 3600.00
bool findIntersection(double r0, double r1, double r2, double *xf, double *yf){
double a, dx, dy, d, h, rx, ry;
double point2_x, point2_y;
dx = x1 - x0;
dy = y1 - y0;
d = sqrt((dy*dy) + (dx*dx));
if (d > (r0 + r1))
{
return false;
}
a = ((r0*r0) - (r1*r1) + (d*d)) / (2.0 * d) ;
point2_x = x0 + (dx * a/d);
point2_y = y0 + (dy * a/d);
h = sqrt((r0*r0) - (a*a));
rx = -dy * (h/d);
ry = dx * (h/d);
double intersectionPoint1_x = point2_x + rx;
double intersectionPoint2_x = point2_x - rx;
double intersectionPoint1_y = point2_y + ry;
double intersectionPoint2_y = point2_y - ry;
dx = intersectionPoint1_x - x2;
dy = intersectionPoint1_y - y2;
double d1 = sqrt((dy*dy) + (dx*dx));
dx = intersectionPoint2_x - x2;
dy = intersectionPoint2_y - y2;
double d2 = sqrt((dy*dy) + (dx*dx));
if(fabs(d1 - r2) < EPSILON) {
std::cout << std::setprecision(100) << intersectionPoint1_x << ", " << intersectionPoint1_y << "\n";
*xf = intersectionPoint1_x; *yf = intersectionPoint1_y;
return true;
}
else if(fabs(d2 - r2) < EPSILON) {
std::cout << std::setprecision(100) << intersectionPoint2_x << ", " << intersectionPoint2_y << "\n";
*xf = intersectionPoint2_x; *yf = intersectionPoint2_y;
return true;
}
else {
return false;
}
}
void localizeSource(double r0, double r1, double r2, double *xf, double *yf){
bool foundSource = false;
while(foundSource == false){
foundSource = findIntersection(r0, r1, r2, xf, yf);
r0 += 0.0001; r1 += 0.0001; r2 += 0.0001;
}
}
int main(){
double xf, xy;
localizeSource(1000,3000,0,&xf,&xy);
}
It's not super clear what you are trying to solve... Your question talks about "time delay [sic] of arrival" but then you link to the "intersection of circles" algorithm. TDoA algorithm uses parabolas not circles.
Time Difference of Arrival algorithm:
Someone on SO already wrote sample code using Fang's Method
Issues implementing the Fang Algorithm for TDOA Trilateration
Hyperbolic Position Location Estimation in the Multipath Propagation Environment
Time Difference of Arrival (TDoA) Localization Combining Weighted Least Squares and Firefly Algorithm
If you're just looking for the Triangulation / Trilateration formula:
A = 2*x2 - 2*x1
B = 2*y2 - 2*y1
C = r1*r1 - r2*r2 - x1*x1 + x2*x2 - y1*y1 + y2*y2
D = 2*x3 - 2*x2
E = 2*y3 - 2*y2
F = r2*r2 - r3*r3 - x2*x2 + x3*x3 - y2*y2 + y3*y3
x = (C*E - F*B) / (E*A - B*D)
y = (C*D - A*F) / (B*D - A*E)
I have an ellipse, defined by Center Point, radiusX and radiusY, and I have a Point. I want to find the point on the ellipse that is closest to the given point. In the illustration below, that would be S1.
Now I already have code, but there is a logical error somewhere in it, and I seem to be unable to find it. I broke the problem down to the following code example:
#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <math.h>
using namespace std;
void dostuff();
int main()
{
dostuff();
return 0;
}
typedef std::vector<cv::Point> vectorOfCvPoints;
void dostuff()
{
const double ellipseCenterX = 250;
const double ellipseCenterY = 250;
const double ellipseRadiusX = 150;
const double ellipseRadiusY = 100;
vectorOfCvPoints datapoints;
for (int i = 0; i < 360; i+=5)
{
double angle = i / 180.0 * CV_PI;
double x = ellipseRadiusX * cos(angle);
double y = ellipseRadiusY * sin(angle);
x *= 1.4;
y *= 1.4;
x += ellipseCenterX;
y += ellipseCenterY;
datapoints.push_back(cv::Point(x,y));
}
cv::Mat drawing = cv::Mat::zeros( 500, 500, CV_8UC1 );
for (int i = 0; i < datapoints.size(); i++)
{
const cv::Point & curPoint = datapoints[i];
const double curPointX = curPoint.x;
const double curPointY = curPoint.y * -1; //transform from image coordinates to geometric coordinates
double angleToEllipseCenter = atan2(curPointY - ellipseCenterY * -1, curPointX - ellipseCenterX); //ellipseCenterY * -1 for transformation to geometric coords (from image coords)
double nearestEllipseX = ellipseCenterX + ellipseRadiusX * cos(angleToEllipseCenter);
double nearestEllipseY = ellipseCenterY * -1 + ellipseRadiusY * sin(angleToEllipseCenter); //ellipseCenterY * -1 for transformation to geometric coords (from image coords)
cv::Point center(ellipseCenterX, ellipseCenterY);
cv::Size axes(ellipseRadiusX, ellipseRadiusY);
cv::ellipse(drawing, center, axes, 0, 0, 360, cv::Scalar(255));
cv::line(drawing, curPoint, cv::Point(nearestEllipseX,nearestEllipseY*-1), cv::Scalar(180));
}
cv::namedWindow( "ellipse", CV_WINDOW_AUTOSIZE );
cv::imshow( "ellipse", drawing );
cv::waitKey(0);
}
It produces the following image:
You can see that it actually finds "near" points on the ellipse, but it are not the "nearest" points. What I intentionally want is this: (excuse my poor drawing)
would you extent the lines in the last image, they would cross the center of the ellipse, but this is not the case for the lines in the previous image.
I hope you get the picture. Can anyone tell me what I am doing wrong?
Consider a bounding circle around the given point (c, d), which passes through the nearest point on the ellipse. From the diagram it is clear that the closest point is such that a line drawn from it to the given point must be perpendicular to the shared tangent of the ellipse and circle. Any other points would be outside the circle and so must be further away from the given point.
So the point you are looking for is not the intersection between the line and the ellipse, but the point (x, y) in the diagram.
Gradient of tangent:
Gradient of line:
Condition for perpedicular lines - product of gradients = -1:
When rearranged and substituted into the equation of your ellipse...
...this will give two nasty quartic (4th-degree polynomial) equations in terms of either x or y. AFAIK there are no general analytical (exact algebraic) methods to solve them. You could try an iterative method - look up the Newton-Raphson iterative root-finding algorithm.
Take a look at this very good paper on the subject:
http://www.spaceroots.org/documents/distance/distance-to-ellipse.pdf
Sorry for the incomplete answer - I totally blame the laws of mathematics and nature...
EDIT: oops, i seem to have a and b the wrong way round in the diagram xD
There is a relatively simple numerical method with better convergence than Newtons Method. I have a blog post about why it works http://wet-robots.ghost.io/simple-method-for-distance-to-ellipse/
This implementation works without any trig functions:
def solve(semi_major, semi_minor, p):
px = abs(p[0])
py = abs(p[1])
tx = 0.707
ty = 0.707
a = semi_major
b = semi_minor
for x in range(0, 3):
x = a * tx
y = b * ty
ex = (a*a - b*b) * tx**3 / a
ey = (b*b - a*a) * ty**3 / b
rx = x - ex
ry = y - ey
qx = px - ex
qy = py - ey
r = math.hypot(ry, rx)
q = math.hypot(qy, qx)
tx = min(1, max(0, (qx * r / q + ex) / a))
ty = min(1, max(0, (qy * r / q + ey) / b))
t = math.hypot(ty, tx)
tx /= t
ty /= t
return (math.copysign(a * tx, p[0]), math.copysign(b * ty, p[1]))
Credit to Adrian Stephens for the Trig-Free Optimization.
Here is the code translated to C# implemented from this paper to solve for the ellipse:
http://www.geometrictools.com/Documentation/DistancePointEllipseEllipsoid.pdf
Note that this code is untested - if you find any errors let me know.
//Pseudocode for robustly computing the closest ellipse point and distance to a query point. It
//is required that e0 >= e1 > 0, y0 >= 0, and y1 >= 0.
//e0,e1 = ellipse dimension 0 and 1, where 0 is greater and both are positive.
//y0,y1 = initial point on ellipse axis (center of ellipse is 0,0)
//x0,x1 = intersection point
double GetRoot ( double r0 , double z0 , double z1 , double g )
{
double n0 = r0*z0;
double s0 = z1 - 1;
double s1 = ( g < 0 ? 0 : Math.Sqrt(n0*n0+z1*z1) - 1 ) ;
double s = 0;
for ( int i = 0; i < maxIter; ++i ){
s = ( s0 + s1 ) / 2 ;
if ( s == s0 || s == s1 ) {break; }
double ratio0 = n0 /( s + r0 );
double ratio1 = z1 /( s + 1 );
g = ratio0*ratio0 + ratio1*ratio1 - 1 ;
if (g > 0) {s0 = s;} else if (g < 0) {s1 = s ;} else {break ;}
}
return s;
}
double DistancePointEllipse( double e0 , double e1 , double y0 , double y1 , out double x0 , out double x1)
{
double distance;
if ( y1 > 0){
if ( y0 > 0){
double z0 = y0 / e0;
double z1 = y1 / e1;
double g = z0*z0+z1*z1 - 1;
if ( g != 0){
double r0 = (e0/e1)*(e0/e1);
double sbar = GetRoot(r0 , z0 , z1 , g);
x0 = r0 * y0 /( sbar + r0 );
x1 = y1 /( sbar + 1 );
distance = Math.Sqrt( (x0-y0)*(x0-y0) + (x1-y1)*(x1-y1) );
}else{
x0 = y0;
x1 = y1;
distance = 0;
}
}
else // y0 == 0
x0 = 0 ; x1 = e1 ; distance = Math.Abs( y1 - e1 );
}else{ // y1 == 0
double numer0 = e0*y0 , denom0 = e0*e0 - e1*e1;
if ( numer0 < denom0 ){
double xde0 = numer0/denom0;
x0 = e0*xde0 ; x1 = e1*Math.Sqrt(1 - xde0*xde0 );
distance = Math.Sqrt( (x0-y0)*(x0-y0) + x1*x1 );
}else{
x0 = e0;
x1 = 0;
distance = Math.Abs( y0 - e0 );
}
}
return distance;
}
The following python code implements the equations described at "Distance from a Point to an Ellipse" and uses newton's method to find the roots and from that the closest point on the ellipse to the point.
Unfortunately, as can be seen from the example, it seems to only be accurate outside the ellipse. Within the ellipse weird things happen.
from math import sin, cos, atan2, pi, fabs
def ellipe_tan_dot(rx, ry, px, py, theta):
'''Dot product of the equation of the line formed by the point
with another point on the ellipse's boundary and the tangent of the ellipse
at that point on the boundary.
'''
return ((rx ** 2 - ry ** 2) * cos(theta) * sin(theta) -
px * rx * sin(theta) + py * ry * cos(theta))
def ellipe_tan_dot_derivative(rx, ry, px, py, theta):
'''The derivative of ellipe_tan_dot.
'''
return ((rx ** 2 - ry ** 2) * (cos(theta) ** 2 - sin(theta) ** 2) -
px * rx * cos(theta) - py * ry * sin(theta))
def estimate_distance(x, y, rx, ry, x0=0, y0=0, angle=0, error=1e-5):
'''Given a point (x, y), and an ellipse with major - minor axis (rx, ry),
its center at (x0, y0), and with a counter clockwise rotation of
`angle` degrees, will return the distance between the ellipse and the
closest point on the ellipses boundary.
'''
x -= x0
y -= y0
if angle:
# rotate the points onto an ellipse whose rx, and ry lay on the x, y
# axis
angle = -pi / 180. * angle
x, y = x * cos(angle) - y * sin(angle), x * sin(angle) + y * cos(angle)
theta = atan2(rx * y, ry * x)
while fabs(ellipe_tan_dot(rx, ry, x, y, theta)) > error:
theta -= ellipe_tan_dot(
rx, ry, x, y, theta) / \
ellipe_tan_dot_derivative(rx, ry, x, y, theta)
px, py = rx * cos(theta), ry * sin(theta)
return ((x - px) ** 2 + (y - py) ** 2) ** .5
Here's an example:
rx, ry = 12, 35 # major, minor ellipse axis
x0 = y0 = 50 # center point of the ellipse
angle = 45 # ellipse's rotation counter clockwise
sx, sy = s = 100, 100 # size of the canvas background
dist = np.zeros(s)
for x in range(sx):
for y in range(sy):
dist[x, y] = estimate_distance(x, y, rx, ry, x0, y0, angle)
plt.imshow(dist.T, extent=(0, sx, 0, sy), origin="lower")
plt.colorbar()
ax = plt.gca()
ellipse = Ellipse(xy=(x0, y0), width=2 * rx, height=2 * ry, angle=angle,
edgecolor='r', fc='None', linestyle='dashed')
ax.add_patch(ellipse)
plt.show()
Which generates an ellipse and the distance from the boundary of the ellipse as a heat map. As can be seen, at the boundary the distance is zero (deep blue).
Given an ellipse E in parametric form and a point P
the square of the distance between P and E(t) is
The minimum must satisfy
Using the trigonometric identities
and substituting
yields the following quartic equation:
Here's an example C function that solves the quartic directly and computes sin(t) and cos(t) for the nearest point on the ellipse:
void nearest(double a, double b, double x, double y, double *ecos_ret, double *esin_ret) {
double ax = fabs(a*x);
double by = fabs(b*y);
double r = b*b - a*a;
double c, d;
int switched = 0;
if (ax <= by) {
if (by == 0) {
if (r >= 0) { *ecos_ret = 1; *esin_ret = 0; }
else { *ecos_ret = 0; *esin_ret = 1; }
return;
}
c = (ax - r) / by;
d = (ax + r) / by;
} else {
c = (by + r) / ax;
d = (by - r) / ax;
switched = 1;
}
double cc = c*c;
double D0 = 12*(c*d + 1); // *-4
double D1 = 54*(d*d - cc); // *4
double D = D1*D1 + D0*D0*D0; // *16
double St;
if (D < 0) {
double t = sqrt(-D0); // *2
double phi = acos(D1 / (t*t*t));
St = 2*t*cos((1.0/3)*phi); // *2
} else {
double Q = cbrt(D1 + sqrt(D)); // *2
St = Q - D0 / Q; // *2
}
double p = 3*cc; // *-2
double SS = (1.0/3)*(p + St); // *4
double S = sqrt(SS); // *2
double q = 2*cc*c + 4*d; // *2
double l = sqrt(p - SS + q / S) - S - c; // *2
double ll = l*l; // *4
double ll4 = ll + 4; // *4
double esin = (4*l) / ll4;
double ecos = (4 - ll) / ll4;
if (switched) {
double t = esin;
esin = ecos;
ecos = t;
}
*ecos_ret = copysign(ecos, a*x);
*esin_ret = copysign(esin, b*y);
}
Try it online!
You just need to calculate the intersection of the line [P1,P0] to your elipse which is S1.
If the line equeation is:
and the elipse equesion is:
than the values of S1 will be:
Now you just need to calculate the distance between S1 to P1 , the formula (for A,B points) is:
I've solved the distance issue via focal points.
For every point on the ellipse
r1 + r2 = 2*a0
where
r1 - Euclidean distance from the given point to focal point 1
r2 - Euclidean distance from the given point to focal point 2
a0 - semimajor axis length
I can also calculate the r1 and r2 for any given point which gives me another ellipse that this point lies on that is concentric to the given ellipse. So the distance is
d = Abs((r1 + r2) / 2 - a0)
As propposed by user3235832
you shall solve quartic equation to find the normal to the ellipse (https://www.mathpages.com/home/kmath505/kmath505.htm). With good initial value only few iterations are needed (I use it myself). As an initial value I use S1 from your picture.
The fastest method I guess is
http://wwwf.imperial.ac.uk/~rn/distance2ellipse.pdf
Which has been mentioned also by Matt but as he found out the method doesn't work very well inside of ellipse.
The problem is the theta initialization.
I proposed an stable initialization:
Find the intersection of ellipse and horizontal line passing the point.
Find the other intersection using vertical line.
Choose the one that is closer the point.
Calculate the initial angle based on that point.
I got good results with no issue inside and outside:
As you can see in the following image it just iterated about 3 times to reach 1e-8. Close to axis it is 1 iteration.
The C++ code is here:
double initialAngle(double a, double b, double x, double y) {
auto abs_x = fabs(x);
auto abs_y = fabs(y);
bool isOutside = false;
if (abs_x > a || abs_y > b) isOutside = true;
double xd, yd;
if (!isOutside) {
xd = sqrt((1.0 - y * y / (b * b)) * (a * a));
if (abs_x > xd)
isOutside = true;
else {
yd = sqrt((1.0 - x * x / (a * a)) * (b * b));
if (abs_y > yd)
isOutside = true;
}
}
double t;
if (isOutside)
t = atan2(a * y, b * x); //The point is outside of ellipse
else {
//The point is inside
if (xd < yd) {
if (x < 0) xd = -xd;
t = atan2(y, xd);
}
else {
if (y < 0) yd = -yd;
t = atan2(yd, x);
}
}
return t;
}
double distanceToElipse(double a, double b, double x, double y, int maxIter = 10, double maxError = 1e-5) {
//std::cout <<"p="<< x << "," << y << std::endl;
auto a2mb2 = a * a - b * b;
double t = initialAngle(a, b, x, y);
auto ct = cos(t);
auto st = sin(t);
int i;
double err;
for (i = 0; i < maxIter; i++) {
auto f = a2mb2 * ct * st - x * a * st + y * b * ct;
auto fp = a2mb2 * (ct * ct - st * st) - x * a * ct - y * b * st;
auto t2 = t - f / fp;
err = fabs(t2 - t);
//std::cout << i + 1 << " " << err << std::endl;
t = t2;
ct = cos(t);
st = sin(t);
if (err < maxError) break;
}
auto dx = a * ct - x;
auto dy = b * st - y;
//std::cout << a * ct << "," << b * st << std::endl;
return sqrt(dx * dx + dy * dy);
}
I can quite easily calculate the point of intersection given two lines. If I start with two vertices:
(x1,y1)
(x2,y2)
I can calculate the slope by doing (y1-y2)/(x1-x2), and then calculating the intercept
y1 - slope * x1
Then do that again, so I have to sets of slope and intercept, then just do:
x = (intercept2 - intercept1) / (slope1 - slope2)
y = slope1 * x + intercept1
(disclaimer: this might not even work, but i've gotten something very close to it to work, and it illustrates my general technique)
BUT that only works with data types with decimals, or non integral. Say the vertices are:
(0,1)
(10,2)
To calculate the slope would result in (1-2)/(0-10), which is -1/-10 which is not 1/10, it is 0.
How can I get code that yields a valid result using only integers?
Edit: I can't use floats AT ALL!. No casting, no nothing. Also, values are capped at 65535. And everything is unsigned.
In high school when subtracting fractions, our teachers taught us to find a common denominator
So 1/4 - 1/6 = 3/12 - 2/12 = 1/12
So do the same with your slopes.
int slope1 = n1 / d1; // numerator / denominator
int slope2 = n2 / d2;
// All divisions below should have 0 for remainder
int g = gcd( d1, d2 ); // gcd( 4, 6 ) = 2
int d = d1 * d2 / g; // common denominator (12 above)
int n = (d/d1) * n1 - (d/d2) * n2; // (1 in 1/12 above)
// n1/d1 - n2/d2 == n/d
I hope I got that right.
Hm..
(0,1)
(10,2)
and (y1-y2)/(x1-x2). Well, this is the description of one line, not the intersection of two lines.
As far as I remember lines are described in the form of x * v with x an skalar and v be a vector. Then it's
x * (0,1) = v2 and
x * (10, 2) = v2.
therefore the lines only intersect if exactly one solution to both equitions exist, overlap when there are infinitive numbers of solutions and don't intersect when they are parallel.
http://www.gamedev.net/topic/647810-intersection-point-of-two-vectors/
explains the calcuclation based on the dot - product.
Input: line L passing thru (x1, y1) and (x2, y2), and line M passing thru (X1, Y1) and (X2, Y2)
Output: (x, y) of the intersecting point of two lines L and M
Tell Wolfram Alpha to solve y = (y1-y2)/(x1-x2)*(x-x1)+y1 and y = (Y1-Y2)/(X1-X2)*(x-X1)+Y1 for x, y to get this solution:
http://www.wolframalpha.com/share/clip?f=d41d8cd98f00b204e9800998ecf8427e3at5u9evl8
But I have no idea on how to write a program to implement the above solution for your calculator with only uint16_t ALU.
Thanks to Graham Toal's answer, below is a primitive Rust implementation of the linked C code in their answer, modified to return the point of intersection for the complete line, as opposed to the line segment. It doesn't use much Rust-specific magic so should be reasonably easy to port to other languages.
The function returns a Point where the Lines intersect, if at all, and a flag denoting whether the intersection point lies on both intersected lines (true) or not (false).
/// 2D integer point
struct Point {
/// The x coordinate.
pub x: i32,
/// The y coordinate.
pub y: i32,
}
/// Line primitive
struct Line {
/// Start point
pub start: Point,
/// End point
pub end: Point,
}
/// Check signs of two signed numbers
///
/// Fastest ASM output compared to other methods. See: https://godbolt.org/z/zVx9cD
fn same_signs(a: i32, b: i32) -> bool {
a ^ b >= 0
}
/// Integer-only line segment intersection
///
/// If the point lies on both line segments, the second tuple argument will return `true`.
///
/// Inspired from https://stackoverflow.com/a/61485959/383609, which links to
/// https://webdocs.cs.ualberta.ca/~graphics/books/GraphicsGems/gemsii/xlines.c
fn intersection(l1: &Line, l2: &Line) -> Option<(Point, bool)> {
let Point { x: x1, y: y1 } = l1.start;
let Point { x: x2, y: y2 } = l1.end;
let Point { x: x3, y: y3 } = l2.start;
let Point { x: x4, y: y4 } = l2.end;
// First line coefficients where "a1 x + b1 y + c1 = 0"
let a1 = y2 - y1;
let b1 = x1 - x2;
let c1 = x2 * y1 - x1 * y2;
// Second line coefficients
let a2 = y4 - y3;
let b2 = x3 - x4;
let c2 = x4 * y3 - x3 * y4;
let denom = a1 * b2 - a2 * b1;
// Lines are colinear
if denom == 0 {
return None;
}
// Compute sign values
let r3 = a1 * x3 + b1 * y3 + c1;
let r4 = a1 * x4 + b1 * y4 + c1;
// Sign values for second line
let r1 = a2 * x1 + b2 * y1 + c2;
let r2 = a2 * x2 + b2 * y2 + c2;
// Flag denoting whether intersection point is on passed line segments. If this is false,
// the intersection occurs somewhere along the two mathematical, infinite lines instead.
//
// Check signs of r3 and r4. If both point 3 and point 4 lie on same side of line 1, the
// line segments do not intersect.
//
// Check signs of r1 and r2. If both point 1 and point 2 lie on same side of second line
// segment, the line segments do not intersect.
let is_on_segments = (r3 != 0 && r4 != 0 && same_signs(r3, r4))
|| (r1 != 0 && r2 != 0 && same_signs(r1, r2));
// If we got here, line segments intersect. Compute intersection point using method similar
// to that described here: http://paulbourke.net/geometry/pointlineplane/#i2l
// The denom/2 is to get rounding instead of truncating. It is added or subtracted to the
// numerator, depending upon the sign of the numerator.
let offset = if denom < 0 { -denom / 2 } else { denom / 2 };
let num = b1 * c2 - b2 * c1;
let x = if num < 0 { num - offset } else { num + offset } / denom;
let num = a2 * c1 - a1 * c2;
let y = if num < 0 { num - offset } else { num + offset } / denom;
Some((Point::new(x, y), is_on_segments))
}
I am attempting to calculate the point of intersection between lines for a Optical Flow algorithm using a Hough Transform. However, I am not getting the points that I should be when I use my algorithm for calculating the intersections.
I save the Lines as an instance of a class that I created called ImageLine. Here is the code for my intersection method.
Point ImageLine::intersectionWith(ImageLine other)
{
float A2 = other.Y2() - other.Y1();
float B2 = other.X2() - other.X1();
float C2 = A2*other.X1() + B2*other.Y1();
float A1 = y2 - y1;
float B1 = x2 - x1;
float C1 = A1 * x1 + B1 * y1;
float det = A1*B2 - A2*B1;
if (det == 0)
{
return Point(-1,-1);
}
Point d = Point((B2 * C1 - B1 * C2) / det, -(A1 * C2 - A2 * C1) / det);
return d;
}
Is this method correct, or did I do something wrong? As far as I can tell, it should work, as it does for a single point that I hard-coded through, however, I have not been able to get a good intersection when using real data.
Considering the maths side: if we have two line equations:
y = m1 * x + c1
y = m2 * x + c2
The point of intersection: (X , Y), of two lines described by the following equations:
Y = m1 * X + c1
Y = m2 * X + c2
is the point which satisfies both equation, i.e.:
m1 * X + c1 = m2 * X + c2
(Y - c1) / m1 = (Y - c2) / m2
thus the point of intersection coordinates are:
intersectionX = (c2 - c1) / (m1 - m2)
intersectionY = (m1*c1 - c2*m2) / m1-m2 or intersectionY = m1 * intersectionX + c1
Note: c1, m1 and c2, m2 are calculated by getting any 2 points of a line and putting them in the line equations.
(det == 0) is unlikely to be true when you're using floating-point arithmetic, because it isn't precise.
Something like (fabs(det) < epsilon) is commonly used, for some suitable value of epsilon (say, 1e-6).
If that doesn't fix it, show some actual numbers, along with the expected result and the actual result.
For detailed formula, please go to this page.
But I love code so, here, check this code (I get it from github, so all credit goes to the author of that code):
///Calculate intersection of two lines.
///\return true if found, false if not found or error
bool LineLineIntersect(double x1, double y1, //Line 1 start
double x2, double y2, //Line 1 end
double x3, double y3, //Line 2 start
double x4, double y4, //Line 2 end
double &ixOut, double &iyOut) //Output
{
//http://mathworld.wolfram.com/Line-LineIntersection.html
double detL1 = Det(x1, y1, x2, y2);
double detL2 = Det(x3, y3, x4, y4);
double x1mx2 = x1 - x2;
double x3mx4 = x3 - x4;
double y1my2 = y1 - y2;
double y3my4 = y3 - y4;
double xnom = Det(detL1, x1mx2, detL2, x3mx4);
double ynom = Det(detL1, y1my2, detL2, y3my4);
double denom = Det(x1mx2, y1my2, x3mx4, y3my4);
if(denom == 0.0)//Lines don't seem to cross
{
ixOut = NAN;
iyOut = NAN;
return false;
}
ixOut = xnom / denom;
iyOut = ynom / denom;
if(!isfinite(ixOut) || !isfinite(iyOut)) //Probably a numerical issue
return false;
return true; //All OK
}
Assuming your formulas are correct, try declaring all your intermediate arguments as 'double'. Taking the difference of nearly parallel lines could result in your products being very close to each other, so 'float' may not preserve enough precision.
I've recently implemented an image warping method using OpenCV 2.31(C++). However, this
method is quite time consuming... After some investigations and improvement
I succeed to reduce the processing time from 400ms to about 120ms which is quite nice.
I achieve this result by unrolling the loop (It reduces the time from 400ms to 330ms)
then I enabled optimization flags on my VC++ compiler 2008 express edition (Enabled O2 flag) - this last fix improved the processing to around 120ms.
However, since I have some other processing to implement around this warp, I'd like to reduce down this processing time even more to 20ms - lower than this value will be better of course, but I don't know if it's possible!!!...
One more thing, I'd like to do this using freely available libraries.
All suggestions are more than welcomed.
Bellow, you'll find the method I'm speaking about.
thanks for your help
Ariel B.
cv::Mat Warp::pieceWiseWarp(const cv::Mat &Isource, const cv::Mat &s, TYPE_CONVERSION type)
{
cv::Mat Idest(roi.height,roi.width,Isource.type(),cv::Scalar::all(0));
float xi, xj, xk, yi, yj, yk, x, y;
float X2X1,Y2Y1,X2X,Y2Y,XX1,YY1,X2X1_Y2Y1,a1, a2, a3, a4,b1,b2,c1,c2;
int x1, y1, x2, y2;
char k;
int nc = roi.width;
int nr = roi.height;
int channels = Isource.channels();
int N = nr * nc;
float *alphaPtr = alpha.ptr<float>(0);
float *betaPtr = beta.ptr<float>(0);
char *triMaskPtr = triMask.ptr<char>(0);
uchar *IdestPtr = Idest.data;
for(int i = 0; i < N; i++, IdestPtr += channels - 1)
if((k = triMaskPtr[i]) != -1)// the pixel do belong to delaunay
{
cv::Vec3b t = trianglesMap.row(k);
xi = s.col(1).at<float>(t[0]); yi = s.col(0).at<float>(t[0]);
xj = s.col(1).at<float>(t[1]); yj = s.col(0).at<float>(t[1]);
xk = s.col(1).at<float>(t[2]); yk = s.col(0).at<float>(t[2]);
x = xi + alphaPtr[i]*(xj - xi) + betaPtr[i]*(xk - xi);
y = yi + alphaPtr[i]*(yj - yi) + betaPtr[i]*(yk - yi);
//...some bounds checking here...
x2 = ceil(x); x1 = floor(x);
y2 = ceil(y); y1 = floor(y);
//2. use bilinear interpolation on the pixel location - see wiki for formula...
//...3. copy the resulting intensity (GL) to the destination (i,j)
X2X1 = (x2 - x1);
Y2Y1 = (y2 - y1);
X2X = (x2 - x);
Y2Y = (y2 - y);
XX1 = (x - x1);
YY1 = (y - y1);
X2X1_Y2Y1 = X2X1*Y2Y1;
a1 = (X2X*Y2Y)/(X2X1_Y2Y1);
a2 = (XX1*Y2Y)/(X2X1_Y2Y1);
a3 = (X2X*YY1)/(X2X1_Y2Y1);
a4 = (XX1*YY1)/(X2X1_Y2Y1);
b1 = (X2X/X2X1);
b2 = (XX1/X2X1);
c1 = (Y2Y/Y2Y1);
c2 = (YY1/Y2Y1);
for(int c = 0; c < channels; c++)// Consider implementing this bilinear interpolation elsewhere in another function
{
if(x1 != x2 && y1 != y2)
IdestPtr[i + c] = Isource.at<cv::Vec3b>(y1,x1)[c]*a1
+ Isource.at<cv::Vec3b>(y2,x1)[c]*a2
+ Isource.at<cv::Vec3b>(y1,x2)[c]*a3
+ Isource.at<cv::Vec3b>(y2,x2)[c]*a4;
if(x1 == x2 && y1 == y2)
IdestPtr[i + c] = Isource.at<cv::Vec3b>(y1,x1)[c];
if(x1 != x2 && y1 == y2)
IdestPtr[i + c] = Isource.at<cv::Vec3b>(y1,x1)[c]*b1 + Isource.at<cv::Vec3b>(y1,x2)[c]*b2;
if(x1 == x2 && y1 != y2)
IdestPtr[i + c] = Isource.at<cv::Vec3b>(y1,x1)[c]*c1 + Isource.at<cv::Vec3b>(y2,x1)[c]*c2;
}
}
if(type == CONVERT_TO_CV_32FC3)
Idest.convertTo(Idest,CV_32FC3);
if(type == NORMALIZE_TO_1)
Idest.convertTo(Idest,CV_32FC3,1/255.);
return Idest;
}
I would suggest:
1.Change division by a common factor to multiplication.
i.e. from a = a1/d; b = b1/d to d_1 = 1/d; a = a1*d_1; b = b1*d_1
2.Eliminate the four if test to a single bilinear interpolation.
I'm not sure whether that would help you. You could have a try.