Related
I have problem to find method to compare two trajectories (curves).
The first original contains points (x,y).
The second one can be offset, smaller or larger scale, and with rotation - also array with points (x,y)
My first method that i did is to find smallest distance between two points and repeat this process in every iteration, sum of it and divide by number of points - then my result tell me value the average error per point:
http://www.mathopenref.com/coorddist.html
And also i find this method:
https://help.scilab.org/docs/6.0.0/en_US/fminsearch.html
But i cant figure out how to use it.
I would like compare both trajectories but my results have to include rotation, or at least offset for beginning.
My current result is calculate error per point (distance)
get coordinate (x,y) second trajectory.
in loop i try to find min_distance between (x,y) from 1. and point from original trajectory.
add smallest_distance what i found in 2 step.
divide sum of smallest distance by number of points from second trajectory.
My result describe average error(distance) per points if we compare with original trajectory.
But i can not figure how to handle if trajectory is rotated, scaled or is shifted.
Please look at my example trajectories:
http://pokazywarka.pl/trajectory/
http://pokazywarka.pl/trajectory2/
So you need to compare shape of 2 curves invariant on rotation,translation and scale.
Solution
Let assume 2 sinwaves for testing. Both rotated and scaled but with the same aspect ratio and one with added noise. I generated them in C++ like this:
struct _pnt2D
{
double x,y;
// inline
_pnt2D() {}
_pnt2D(_pnt2D& a) { *this=a; }
~_pnt2D() {}
_pnt2D* operator = (const _pnt2D *a) { *this=*a; return this; }
//_pnt2D* operator = (const _pnt2D &a) { ...copy... return this; }
};
List<_pnt2D> curve0,curve1; // curves points
_pnt2D p0,u0,v0,p1,u1,v1; // curves OBBs
const double deg=M_PI/180.0;
const double rad=180.0/M_PI;
void rotate2D(double alfa,double x0,double y0,double &x,double &y)
{
double a=x-x0,b=y-y0,c,s;
c=cos(alfa);
s=sin(alfa);
x=x0+a*c-b*s;
y=y0+a*s+b*c;
}
// this code is the init stuff:
int i;
double x,y,a;
_pnt2D p,*pp;
Randomize();
for (x=0;x<2.0*M_PI;x+=0.01)
{
y=sin(x);
p.x= 50.0+(100.0*x);
p.y=180.0-( 50.0*y);
rotate2D(+15.0*deg,200,180,p.x,p.y);
curve0.add(p);
p.x=150.0+( 50.0*x);
p.y=200.0-( 25.0*y)+5.0*Random();
rotate2D(-25.0*deg,250,100,p.x,p.y);
curve1.add(p);
}
OBB oriented bounding box
compute OBB which will find the rotation angle and position of both curves so rotate one of them so they start at the same position and has the same orientation.
If the OBB sizes are too different then the curves are different.
For above example it yealds this result:
Each OBB is defined by start point P and basis vectors U,V where |U|>=|V| and z coordinate of U x V is positive. That will ensure the same winding for all OBBs. It can be done in OBBox_compute by adding this to the end:
// |U|>=|V|
if ((u.x*u.x)+(u.y*u.y)<(v.x*v.x)+(v.y*v.y)) { _pnt2D p; p=u; u=v; v=p; }
// (U x V).z > 0
if ((u.x*v.y)-(u.y*v.x)<0.0)
{
p0.x+=v.x;
p0.y+=v.y;
v.x=-v.x;
v.y=-v.y;
}
So curve0 has p0,u0,v0 and curve1 has p1,u1,v1.
Now we want to rescale,translate and rotate curve1 to match curve0 It can be done like this:
// compute OBB
OBBox_compute(p0,u0,v0,curve0.dat,curve0.num);
OBBox_compute(p1,u1,v1,curve1.dat,curve1.num);
// difference angle = - acos((U0.U1)/(|U0|.|U1|))
a=-acos(((u0.x*u1.x)+(u0.y*u1.y))/(sqrt((u0.x*u0.x)+(u0.y*u0.y))*sqrt((u1.x*u1.x)+(u1.y*u1.y))));
// rotate curve1
for (pp=curve1.dat,i=0;i<curve1.num;i++,pp++)
rotate2D(a,p1.x,p1.y,pp->x,pp->y);
// rotate OBB1
rotate2D(a,0.0,0.0,u1.x,u1.y);
rotate2D(a,0.0,0.0,v1.x,v1.y);
// translation difference = P0-P1
x=p0.x-p1.x;
y=p0.y-p1.y;
// translate curve1
for (pp=curve1.dat,i=0;i<curve1.num;i++,pp++)
{
pp->x+=x;
pp->y+=y;
}
// translate OBB1
p1.x+=x;
p1.y+=y;
// scale difference = |P0|/|P1|
x=sqrt((u0.x*u0.x)+(u0.y*u0.y))/sqrt((u1.x*u1.x)+(u1.y*u1.y));
// scale curve1
for (pp=curve1.dat,i=0;i<curve1.num;i++,pp++)
{
pp->x=((pp->x-p0.x)*x)+p0.x;
pp->y=((pp->y-p0.y)*x)+p0.y;
}
// scale OBB1
u1.x*=x;
u1.y*=x;
v1.x*=x;
v1.y*=x;
You can use Understanding 4x4 homogenous transform matrices to do all this in one step. Here the result:
sampling
in case of non uniform or very different point density between curves or between any parts of it you should re-sample your curves to have common point density. You can use linear or polynomial interpolation for this. You also do not need to store the new sampling in memory but instead you could build function that returns point of each curve parametrized by arc-length from start.
point curve0(double distance);
point curve1(double distance);
comparison
Now you can substract the 2 curves and sum up the abs of the differences. Then divide it by the curve length and threshold the result.
for (double sum=0.0,l=0.0;d<=bigger_curve_length;l+=step)
sum+=fabs(curve0(l)-curve1(l));
sum/=bigger_curve_length;
if (sum>threshold) curves are different
else curves match
You should try this even with +180deg rotation as the orientation difference from OBB has only half of the true range.
Here few related QAs:
compare shapes
How can i produce multi point linear interpolation?
I am solving Closest Point Problem from here
Problem Statement :
We are given an array of n points in the plane, and the problem is to find out the closest pair of points in the array.
INPUT : Input will be two arrays X and Y, X[] stores x coordinates and Y[] stores y coordinates.
OUTPUT : Smallest distance.
My Algorithm :
Note : Approach works only for positive coordinates.
Find Distance between all the coordinates from (0,0) and store it in distance array.
Sort Distance array calculated in previous step.
Find smallest distance by calculating difference between two consecutive values in distance array.
Code :
public class ClosestPoint {
int x[]={2,12,40,5,12,3},y[]={3,30,50,1,10,4}; // x and y coordinates
float distance[] = {0,0,0,0,0,0}; // distance
void calculateDis(){
for(int i=0;i<x.length;i++){
int dis=(x[i]*x[i] + y[i]*y[i]);
distance[i]= (float)Math.sqrt(dis);
}
}
float findClosest() {
float closest = Float.MAX_VALUE;
for(int i=0;i<distance.length-1;i++) {
float pairDis= distance[i+1]-distance[i];
if(closest>pairDis) {
closest =pairDis;
}
}
return closest;
}
public static void main(String arg[]) {
ClosestPoint p =new ClosestPoint();
p.calculateDis(); // calculate distance from 0,0.
Arrays.sort(p.distance);
System.out.println(p.findClosest());
}
}
Correct answer :
1.4
My Answer :
0.099
I am not getting correct answer. Can someone point out flaw in my approach.
Thanks.
The actual problem is in the logic. You are calculating the distances from origin and comparing it. This may lead to the wrong answer.
Consider this example of points (3,4) and (4,3). Both are at same distance from origin - 5. So according to your logic, You sort the distances and take minimum consecutive distance so here your algorithm will return 0 (as after sorting array would be 5.0 , 5.0) but the actual answer is .
I understand that the dot (or inner) product of two quaternions is the angle between the rotations (including the axis-rotation). This makes the dot product equal to the angle between two points on the quaternion hypersphere.
I can not, however, find how to actually compute the dot product.
Any help would be appreciated!
current code:
public static float dot(Quaternion left, Quaternion right){
float angle;
//compute
return angle;
}
Defined are Quaternion.w, Quaternion.x, Quaternion.y, and Quaternion.z.
Note: It can be assumed that the quaternions are normalised.
The dot product for quaternions is simply the standard Euclidean dot product in 4D:
dot = left.x * right.x + left.y * right.y + left.z * right.z + left.w * right.w
Then the angle your are looking for is the arccos of the dot product (note that the dot product is not the angle): acos(dot).
However, if you are looking for the relative rotation between two quaternions, say from q1 to q2, you should compute the relative quaternion q = q1^-1 * q2 and then find the rotation associated withq.
Just NOTE: acos(dot) is very not stable from numerical point of view.
as was said previos, q = q1^-1 * q2 and than angle = 2*atan2(q.vec.length(), q.w)
Should it be 2 x acos(dot) to get the angle between quaternions.
The "right way" to compute the angle between two quaternions
There is really no such thing as the angle between two quaternions, there is only the quaternion that takes one quaternion to another via multiplication. However, you can measure the total angle of rotation of that mapping transformation, by computing the difference between the two quaternions (e.g. qDiff = q1.mul(q2.inverse()), or your library might be able to compute this directly using a call like qDiff = q1.difference(q2)), and then measuring the angle about the axis of the quaternion (your quaternion library probably has a routine for this, e.g. ang = qDiff.angle()).
Note that you will probably need to fix the value, since measuring the angle about an axis doesn't necessarily give the rotation "the short way around", e.g.:
if (ang > Math.PI) {
ang -= 2.0 * Math.PI;
} else if (ang < -Math.PI) {
ang += 2.0 * Math.PI;
}
Measuring the similarity of two quaternions using the dot product
Update: See this answer instead.
I assume that in the original question, the intent of treating the quaternions as 4d vectors is to enable a simple method for measuring the similarity of two quaternions, while still keeping in mind that the quaternions represent rotations. (The actual rotation mapping from one quaternion to another is itself a quaternion, not a scalar.)
Several answers suggest using the acos of the dot product. (First thing to note: the quaternions must be unit quaternions for this to work.) However, the other answers don't take into account the "double cover issue": both q and -q represent the exact same rotation.
Both acos(q1 . q2) and acos(q1 . (-q2)) should return the same value, since q2 and -q2 represent the same rotation. However (with the exception of x == 0), acos(x) and acos(-x) do not return the same value. Therefore, on average (given random quaternions), acos(q1 . q2) will not give you what you expect half of the time, meaning that it will not give you a measure of the angle between q1 and q2, assuming that you care at all that q1 and q2 represent rotations. So even if you only plan to use the dot product or acos of the dot product as a similarity metric, to test how similar q1 and q2 are in terms of the effect they have as a rotation, the answer you get will be wrong half the time.
More specifically, if you are trying to simply treat quaternions as 4d vectors, and you compute ang = acos(q1 . q2), you will sometimes get the value of ang that you expect, and the rest of the time the value you actually wanted (taking into account the double cover issue) will be PI - acos(-q1 . q2). Which of these two values you get will randomly fluctuate between these values depending on exactly how q1 and q2 were computed!.
To solve this problem, you have to normalize the quaternions so that they are in the same "hemisphere" of the double cover space. There are several ways to do this, and to be honest I'm not even sure which of these is the "right" or optimal way. They do all produce different results from other methods in some cases. Any feedback on which of the three normalization forms above is the correct or optimal one would be greatly appreciated.
import java.util.Random;
import org.joml.Quaterniond;
import org.joml.Vector3d;
public class TestQuatNorm {
private static Random random = new Random(1);
private static Quaterniond randomQuaternion() {
return new Quaterniond(
random.nextDouble() * 2 - 1, random.nextDouble() * 2 - 1,
random.nextDouble() * 2 - 1, random.nextDouble() * 2 - 1)
.normalize();
}
public static double normalizedDot0(Quaterniond q1, Quaterniond q2) {
return Math.abs(q1.dot(q2));
}
public static double normalizedDot1(Quaterniond q1, Quaterniond q2) {
return
(q1.w >= 0.0 ? q1 : new Quaterniond(-q1.x, -q1.y, -q1.z, -q1.w))
.dot(
q2.w >= 0.0 ? q2 : new Quaterniond(-q2.x, -q2.y, -q2.z, -q2.w));
}
public static double normalizedDot2(Quaterniond q1, Quaterniond q2) {
Vector3d v1 = new Vector3d(q1.x, q1.y, q1.z);
Vector3d v2 = new Vector3d(q2.x, q2.y, q2.z);
double dot = v1.dot(v2);
Quaterniond q2n = dot >= 0.0 ? q2
: new Quaterniond(-q2.x, -q2.y, -q2.z, -q2.w);
return q1.dot(q2n);
}
public static double acos(double val) {
return Math.toDegrees(Math.acos(Math.max(-1.0, Math.min(1.0, val))));
}
public static void main(String[] args) {
for (int i = 0; i < 1000; i++) {
var q1 = randomQuaternion();
var q2 = randomQuaternion();
double dot = q1.dot(q2);
double dot0 = normalizedDot0(q1, q2);
double dot1 = normalizedDot1(q1, q2);
double dot2 = normalizedDot2(q1, q2);
System.out.println(acos(dot) + "\t" + acos(dot0) + "\t" + acos(dot1)
+ "\t" + acos(dot2));
}
}
}
Also note that:
acos is known to not be very numerically accurate (given some worst-case inputs, up to half of the least significant digits can be wrong);
the implementation of acos is exceptionally slow in the JDK standard libraries;
acos returns NaN if its parameter is even slightly outside [-1,1], which is a common occurrence for dot products of even unit quaternions -- so you need to bound the value of the dot product to that range before calling acos. See this line in the code above:
return Math.toDegrees(Math.acos(Math.max(-1.0, Math.min(1.0, val))));
According to this cheatsheet Eq. (42), there is a more robust and accurate way of computing the angle between two vectors that replaces acos with atan2 (although note that this does not solve the double cover problem either, so you will need to use one of the above normalization forms before applying the following):
ang(q1, q2) = 2 * atan2(|q1 - q2|, |q1 + q2|)
I admit though that I don't understand this formulation, since quaternion subtraction and addition has no geometrical meaning.
I'm given a line segment with two endpoints: (x1,y1) (x2,y2) and a random point: (x3,y3).
If I convert the line segment to polar coordinates, I need to be able to figure out, programmatically, a point on the line segment that is the closest to point (x3,y3).
EDIT:
I made a mistake in my question. The problem isn't trying to find the closest point between the three. The problem is... given a line AB with a start and an end... find ANY point on the line that is closest to point (x3,y3).
I guess this is too late, just in case someone needs it, I just convert back to cartesian coordinates so the formula is easier to look at:
public static double distance(double r1,double t1,double r2,double t2,double r3,double t3)
{
double x1 = (r1*Math.cos(t1));
double x2 = (r1*Math.cos(t2));
double x3 = (r1*Math.cos(t3));
double y1 = (r1*Math.sin(t1));
double y2 = (r1*Math.sin(t2));
double y3 = (r1*Math.sin(t3));
return Math.abs((x2-x1)*(y1-y3)-(x1-x3)*(y2-y1))/Math.sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
}
This question involves elementary school level algebra:
There are only 3 options for the closest point:
one of the points (x1,y1) (x2,y2)
another point between them on the line segment
It's easy to find the distance between (x3,y3) to the two points using Pythagoras:
d1 = d(<x1,y1>,<x3,y3>) = sqrt( (x1-x3)^2 + (y1-y3)^2)
and
d2 = d(<x2,y2>,<x3,y3>) = sqrt( (x2-x3)^2 + (y2-y3)^2)
and now for the "more complicated" part: if there's another point on the segment which is closer to (x3,y3) - if we connect these points - we'll have a new segment that will be diagonal to the original segment - we have to use that together with a first degree equation (for a line) to find that third point and see if it's on the segment or outside (if it's outside of the segment then we'll take the minimum distance between (x3,y3) to the other two points.
The line equation (excuse my English - I learnt it in my native language so bear with me) is:
(I) y = mx + d
where m is easy to calculate:
m = (y1-y2)/(x1-x2)
and now that we know the value of m in order to find d we'll use the values of one of the first two points, for example:
y1 = mx1 +d => d = y1 - mx1
the diagonal line will have m' which is -1/m and if we'll use the values of (x3,y3) we'll find d'. So now we know both the values of (I) as well as the equation for the diagonal line:
(II) y=m'x + d'
and if we'll use these two equations we can find the point on the line through which the original segment goes through which is the closest to (x3,y3) - if this point lies on the segment - we're done, otherwise, like earlier mentioned - we'll take the minimum between d1 and d2
public static Location closest(double x1, double y1, double x2, double y2, double x3, double y3){
Location a = new Location("provider");
Location b = new Location("provider");
Location c = new Location("provider");
//I can't remember which one goes where, you may need to swap all the x/ys around
//Do conversion or something here.
a.setLongitude(x1);
a.setLatitude(y1);
b.setLongitude(x2);
b.setLatitude(y2);
c.setLongitude(x3);
c.setLongitude(y3);
return closest(a, b, c);
}
public static Location closest(Location a, Location b, Location c){
return a.distanceTo(c) < b.distanceTo(c) ? a : b;
}
I'm trying to compare the distance between Point 1 and Point 2 with the distance between Point 1 and Point 3. And I'm trying to find the smaller one. The only problem is that the xy values of all three points are rather large and using the distance formula will likely cause an overflow. Is there another way to find the distances?
Scale the values by a constant, calculate the distance, then "unscale" the values. For example, divide your values by 10^6, or 10^9, or whatever it takes, then calculate the scaled distance and then convert back using your scale constant.
Math.hypot() may be useful in this context, as "the final result is without medium underflow or overflow."
It is the fastest solution:
double dx12=x1-x2;
double dy12=y1-y2;
double dx13=x1-x3;
double dy13=y1-y3;
double r12sq=dx12*dx12+dy12*dy12;
double r13sq=dx13*dx13+dy13*dy13;
int minR= r12sq>r13sq ? Math.sqrt(r13sq) : Math.sqrt(r12sq);
you need to take only one sqrt - that one for the shortest distance.
Normalization by some fixed constant is senseless for double.
If you use integers instead of doubles, the normalization and centering of coordinates by some fixed constant could be useful for some distances and bad for others. For example, if you are dividing by 1000, it is good for coordinates that have differences about some billions, but for differences about some hundreds its effect will be killing. So, you can evaluate the useful coefficient of normalization only after you have the medium dx and dy. Let us you need 4 digits for work
int dx12=x1-x2;
int dy12=y1-y2;
int dx13=x1-x3;
int dy13=y1-y3;
int d=(abs(dx12) +abs(dx13) + abs(dy12) + abs(dy13));
int coeff = d/10000;
if(coeff<1) coeff=1;
int dx12=dx12/coeff;
int dy12=dy12/coeff;
int dx13=dx13/coeff;
int dy13=dy13/coeff;
int r12sq=dx12*dx12+dy12*dy12;
int r13sq=dx13*dx13+dy13*dy13;
int minR= r12sq>r13sq ? Math.sqrt(r13sq) : Math.sqrt(r12sq);
Here you can multiply these int variables without overflow.