I'm working on a program that require me to find the angle between two lines. I have found all of the points coordinates of the lines, referenced into the 0,0 points, but i dont understand how to implement the dot product in Opencv to find the angle.
I use visual C++ using opencv library. I also found out that i can use cv::Mat::dot function to get dot product, but i can't find any good example. I can not understand the explanation in this link.
Can anyone give me a good example? so i can understand how to use that function to find dot product of two lines. Thank you.
If you have two vectors representing your lines, and call them a and b (of type cv::Mat) then the dot product is calculated as
double p = a.dot(b);
Edit
Example code, by comments from author.
float a[2] = {1.0, 2.0};
float b[2] = {3.0, 4.0};
cv::Mat AA(1,2,CV_32FC1,a);
cv::Mat BB(1,2,CV_32FC1,b);
cout << AA << endl;
cout << BB << endl;
cout << AA.dot(BB) << " should be equal to 11" << endl;
Well, the dot product of two vectors A and B is defined as
(length of A) * (length of B) * cos(angle)
where angle represents the angle between the two vectors. So in order to find the angle between the two, first you have to find the dot product, then divide it by both the length of A and the length of B, then take the inverse cosine.
What this looks like in your case is something along these lines, assuming a and b are declared appropriately as cv::Mats:
double dotprod = a.dot(b);
double angle = arccos(dotprod / (a.size().height * b.size().height))
Related
Using a + i b = sqrt(a*a + b*b) * exp(i arctan2(a,b)) I arrive at the following way to compute complex roots. However, I heard that trigonometric functions rather use up performance so I wonder if there is a better way in vanilla c++ (no external libaries).
Example: Let u+iv = sqrt(a+i b)
#include <math.h>
#include <iostream>
int main(){
double a = -1.;
double b = 0;
double r = sqrt(sqrt(a*a+b*b));
double phi = 0.5 * atan2(b, a);
double u = r * cos(phi);
double v = r * sin(phi);
std::cout << u << std::endl;
std::cout << v << "i" << std::endl;
}
This is just meant as a MWE, so it's not written in a class or method.
Yes there is! I'm going to link a good explanation of the process here, but it looks like this can be accomplished by only calculating the magnitude of the original number and subtracting out the real portion of the original number and finally taking the square root of that to find the imaginary part of the square root. The real part can be found by dividing the imaginary part of the original number by 2 * the imaginary part of the root to get your final answer.
https://www.qc.edu.hk/math/Advanced%20Level/Finding%20the%20square%20root%20of%20a%20complex%20number.htm
Let me know if you need more help with the code but this requires no trig functions.
Consider the following minimal working example:
#include <iostream>
#include <math.h>
#include <eigen3/Eigen/Dense>
int main() {
// Set the rotation matrices that give an example of the problem
Eigen::Matrix3d rotation_matrix_1, rotation_matrix_2;
rotation_matrix_1 << 0.15240781108708346, -0.98618841818279246, -0.064840288106743013,
-0.98826031445019891, -0.1527775600229907, 0.00075368177315370682,
-0.0106494132438156, 0.063964216524108775, -0.99789536976680049;
rotation_matrix_2 << -0.12448670851248633, -0.98805453458380521, -0.090836645094957508,
-0.99167686914182451, 0.12086367053038971, 0.044372968742129482,
-0.03286406263376359, 0.095604444636749664, -0.99487674792051639;
// Convert to Euler angles
Eigen::Vector3d euler_angles_1 = rotation_matrix_1.eulerAngles(2, 1, 0)*180.0f/M_PI;
Eigen::Vector3d euler_angles_2 = rotation_matrix_2.eulerAngles(2, 1, 0)*180.0f/M_PI;
// Convert to quaternion
Eigen::Quaternion<double> quaternion_1(rotation_matrix_1);
Eigen::Quaternion<double> quaternion_2(rotation_matrix_2);
// Print out results
std::cout << "Euler angles 1:\nyaw = " << euler_angles_1[0] << "\npitch = " << euler_angles_1[1] << "\nroll = " << euler_angles_1[2] << std::endl;
std::cout << "Quaternion 1:\nw = " << quaternion_1.w() << "\nx = " << quaternion_1.x() << "\ny = " << quaternion_1.y() << "\nz = " << quaternion_1.z() << std::endl;
std::cout << std::endl;
std::cout << "Euler angles 2:\nyaw = " << euler_angles_2[0] << "\npitch = " << euler_angles_2[1] << "\nroll = " << euler_angles_2[2] << std::endl;
std::cout << "Quaternion 2:\nw = " << quaternion_2.w() << "\nx = " << quaternion_2.x() << "\ny = " << quaternion_2.y() << "\nz = " << quaternion_2.z() << std::endl;
}
Whose output is:
Euler angles 1:
yaw = 98.767
pitch = 179.39
roll = -3.66759
Quaternion 1:
w = 0.020826
x = 0.758795
y = -0.650521
z = -0.0248716
Euler angles 2:
yaw = 82.845
pitch = 178.117
roll = -5.48908
Quaternion 2:
w = -0.0193663
x = -0.661348
y = 0.748369
z = 0.0467608
Both rotations are nearly identical (as given by the Euler angles). The expected behavior is that quaternion_2 will have values with same sign as quaternion_1, i.e. for the output to be:
Quaternion 2:
w = 0.0193663
x = 0.661348
y = -0.748369
z = -0.0467608
However, Eigen appears to "flip" the quaternion. I am aware that q and -q represent the same rotation - however, it is visually not appealing, and frankly annoying, that the quaternion would flip sign in each of its values. How can this be rectified for the general case (i.e. that the quaternion always preserves its "handedness", rather than flipping sign for certain rotations)?
When unit quaternions are used to represent 3d rotations, there are two ways to represent each actual rotation - and you can't avoid the 'negative' ones occuring without creating an artificial discontinuity in the space.
Unlike 2d rotations using complex numbers on a unit circle, the farthest point on the unit hypersphere from '0 rotation' has to be '360 degree rotation', not '180 degree'; since there is a 2d-space of possible 180 rotations which needs to be represented, whereas all 360-degree rotations are equivalent regardless of axis.
You can always 'canonicize' by changing the sign of the whole thing when the w component is negative.
There will still be cases where w = 0, these all represent rotations by 180 - e.g. (0,0,1,0) and (0,0,-1,0) represent the same rotation.
And, (0.01, 0.99995,0,0,0) and (-0.01, 0.99995,0,0) represent rotations very close together, but if you change the second one to the equivalent (0.01,-0.99995,0,0) then they are far apart in the 4d vector space.
So, practically speaking, you can still have a concern when you want to find the difference between two rotations to see how close they are. Canonicizing the two individually may not help;
you would generally want to flip signs as needed to make them as close as possible.
Or, to compare rotations q1,q2 : find the quaternion product q1 * q2.conj(); this gives the difference as a rotation quaternion; if it has w < 0, change its signs. For q1 and q2 close together (regardless of initial sign diffs) the result will always be fairly close to (1,0,0,0).
If you only want to check if they are within a certain angle 'th' of each other, you only need the real part of the result. This is equivalent to finding the dot product of q1,q2 (treating them as unit vectors in 4-space), then you check if the abs. value of the result >= cos(th/2).
Another way to find the relative angle: find the vector difference of the two unit vectors, and find the magnitude 'm' of that difference vector, (square root of the sum of squares) which will be in range [0,2]. Then find
th = 4*arcsin(m/2)
... and this will be 0 ... 2*pi.
In cases where m > sqrt(2), th > pi and you are getting the 'wrong side' result (also, the computation will have terrible numeric accuracy as m gets close to 2.0). So, in such cases, change one of the signs (i.e. make m the vector length of the sum of the inputs, rather than the difference); you will then have m <= sqrt(2), th <= pi.
For small m, the arcsin formula has the taylor series
th ~=~ 2*m + (m^3)/12 + ...
So, for small deltas, the relative rotation angle is approximately twice the magnitude of the vector difference (and this is numerically much more reliable than using an inverse-cosine-of-w when w is nearly 1).
The yaw angle is greater than 90 degrees for matrix 1, and less than 90 degrees for matrix 2. This will cause the cosine of the yaw angle to have different signs for the two, which is flipping your Quaternion.
A possible solution would be to check the w value of the Quaternion. If this is negative, you can flip it.
If you have access to the previous and the current quaternion reading, you can flip the sign of the current quaternion if it makes the distance between the quaternions in the 4D vector space smaller.
Flipping the sign will not affect the rotation, but it will ensure that there are no large jumps in 4D vector space when the rotation difference in rotation space (SO(3)) is small.
Quaternion avoidJumps(Quaternion q_Current, Quaternion q_Prev)
{
if ((q_Prev - q_Current).squaredNorm() < (q_Prev + q_Current).squaredNorm())
return -q_Current;
else
return q_Current;
}
Given a set of points P I need to find a line L that best approximates these points. I have tried to use the function gsl_fit_linear from the GNU scientific library. However my data set often contains points that have a line of best fit with undefined slope (x=c), thus gsl_fit_linear returns NaN. It is my understanding that it is best to use total least squares for this sort of thing because it is fast, robust and it gives the equation in terms of r and theta (so x=c can still be represented). I can't seem to find any C/C++ code out there currently for this problem. Does anyone know of a library or something that I can use? I've read a few research papers on this but the topic is still a little fizzy so I don't feel confident implementing my own.
Update:
I made a first attempt at programming my own with armadillo using the given code on this wikipedia page. Alas I have so far been unsuccessful.
This is what I have so far:
void pointsToLine(vector<Point> P)
{
Row<double> x(P.size());
Row<double> y(P.size());
for (int i = 0; i < P.size(); i++)
{
x << P[i].x;
y << P[i].y;
}
int m = P.size();
int n = x.n_cols;
mat Z = join_rows(x, y);
mat U;
vec s;
mat V;
svd(U, s, V, Z);
mat VXY = V(span(0, (n-1)), span(n, (V.n_cols-1)));
mat VYY = V(span(n, (V.n_rows-1)) , span(n, (V.n_cols-1)));
mat B = (-1*VXY) / VYY;
cout << B << endl;
}
the output from B is always 0.5504, Even when my data set changes. As well I thought that the output should be two values, so I'm definitely doing something very wrong.
Thanks!
To find the line that minimises the sum of the squares of the (orthogonal) distances from the line, you can proceed as follows:
The line is the set of points p+r*t where p and t are vectors to be found, and r varies along the line. We restrict t to be unit length. While there is another, simpler, description in two dimensions, this one works with any dimension.
The steps are
1/ compute the mean p of the points
2/ accumulate the covariance matrix C
C = Sum{ i | (q[i]-p)*(q[i]-p)' } / N
(where you have N points and ' denotes transpose)
3/ diagonalise C and take as t the eigenvector corresponding to the largest eigenvalue.
All this can be justified, starting from the (orthogonal) distance squared of a point q from a line represented as above, which is
d2(q) = q'*q - ((q-p)'*t)^2
Im trying to calculate the angle between two edges in a graph, in order to do that I transfer both edges to origin and then used dot product to calculate the angle. my problem is that for some edges like e1 and e2 the output of angle(e1,e2) is -1.#INDOO.
what is this output? is it an error?
Here is my code:
double angle(Edge e1, Edge e2){
Edge t1 = e1, t2 = e2;
Point tail1 = t1.getTail(), head1 = t1.getHead();
Point u(head1.getX() - tail1.getX(), head1.getY() - tail1.getY());
Point tail2 = t2.getTail(), head2 = t2.getHead();
Point v(head2.getX() - tail2.getX(), head2.getY() - tail2.getY());
double dotProduct = u.getX()*v.getX() + u.getY()*v.getY();
double cosAlpha = dotProduct / (e1.getLength()*e2.getLength());
return acos(cosAlpha);
}
Edge is a class that holds two Points, and Point is a class that holds two double numbers as x and y.
Im using angle(e1,e2) to calculate the orthogonal projection length of a vector like b on to a vector like a :
double orthogonalProjectionLength(Edge b, Edge a){
return (b.getLength()*sin(angle(b, a) * (PI / 180)));
}
and this function also sometimes gives me -1.#INDOO. you can see the implementation of Point and Edge here.
My input is a set S of n Points in 2D space. Iv constructed all edges between p and q (p,q are in S) and then tried to calculate the angle like this:
for (int i = 0; i < E.size(); i++)
for (int j = 0; j < E.size(); j++){
if (i == j)
cerr << fixed << angle(E[i], E[j]) << endl; //E : set of all edges
}
If the problem comes from cos() and sin() functions, how can I fix it? is here other libraries that calculate sin and cos in more efficient way?
look at this example.
the inputs in this example are two distinct points(like p and q), and there are two Edges between them (pq and qp). shouldnt the angle(pq , qp) always be 180 ? and angle(pq,pq) and angle(qp,qp) should be 0. my programm shows two different kinds of behavior, sometimes angle(qp,qp) == angle(pq,pq) ==0 and angle(pq , qp) == angle(pq , qp) == 180.0, and sometimes the answer is -1.#INDOO for all four edges.
Here is a code example.
run it for several times and you will see the error.
You want the projection and you go via all this trig? You just need to dot b with the unit vector in the direction of a. So the final answer is
(Xa.Xb + Ya.Yb) / square_root(Xa^2 + Ya^2)
Did you check that cosAlpha doesn't reach 1.000000000000000000000001? That would explain the results, and provide another reason not to go all around the houses like this.
It seems like dividing by zero. Make sure that your vectors always have 0< length.
Answer moved from mine comment
check if your dot product is in <-1,+1> range ...
due to float rounding it can be for example 1.000002045 which will cause acos to fail.
so add two ifs and clamp to this range.
or use faster way: acos(0.99999*dot)
but that lowers the precision for all angles
and also if 0.9999 constant is too big then the error is still present
A recommended way to compute angles is by means of the atan2 function, taking two arguments. It returns the angle on four quadrants.
You can use it in two ways:
compute the angles of u and v separately and subtract: atan2(Vy, Vx) - atan2(Uy, Ux).
compute the cross- and dot-products: atan2(Ux.Vy - Uy.Vx, Ux.Uy + Vx.Vy).
The only case of failure is (0, 0).
I am trying to calculate the centre of mass (x,y,z) coordinates of an object defined in an STL file (stereo lithography, not to be confused with the standard template library). The STL file contains a closed object (or objects) defined by a boundary made of triangles. The triangles themselves are not necessarily in any order, the file is simply the coordinates 3 vertices of each triangle floating in 3D space plus a normal vector to the triangle (the normal should be disregarded as it is not always done properly). There is nothing that links each triangle to one another, it is assumed that the object is closed.
One simple approach would be to divide a volume (in this case, a box) into millions of elements and determine if each element is inside the object defined in the STL file or not, then sum up the moments and calculate the centre of mass. This would work but its far from elegant and extremely slow.
Another method would be to convert the boundary representation into a number of packed tetrahedron solids. Form that I could calculate the centre of mass of each tetrahedron, its volume, and resulting moment and thus calculate the overall centre of mass from the sum of all tetrahedrons. The problem with this is that I don't know how to convert a surface representation of triangles into a volume representation of tetrahedrons (I'm assuming its a fairly non trivial task).
Dose anyone know of any methods or can think up of any methods that I could try? Or maybe even any reference material that talks about this?
For more information about STL files (only the first 2 sections are important, everything else is useless): http://en.wikipedia.org/wiki/STL_%28file_format%29
After a lot of thinking and experimentation I have the answer!
First we add a 4th point to each triangle in to make them into tetrahedrons with a volume centroid. We calculate the volumes and centres of masses and multiply them by each other to get our moments. We sum the moments and divide by total volume to get our overall centroid.
We calculate volumes using the determinate method shown here (equation 32): http://mathworld.wolfram.com/Tetrahedron.html
The centroids of each of the tetrahedrons is simply the average of the 4 points.
The trick here is that due to the way the STL file is created, the triangles have a normal that point outwards from the part surface, following the right hand rule of the 3 verticies used to create the triangle. we can use this to our advantage by allowing us to have a consistent convention in which to determine if a volume of the tetrahedron should be added or subtracted from our net part (this is because the reference point we chose may not necessarily be inside the part and the overall part is not necessarily convex, it is, however a closed object).
Using the determine method to calculate the volume, the first three coordinate points will represent the three points of our triangle. The fourth point would be our common origin. If the normal created by the triangle (following the right hand rule going from point 1, 2, 3) points towards our common reference point, that volume will be calculated as not part of our overall solid, or negative volume (by pointing towards, i mean the vector created by the triangle's normal is pointing loosely towards the same side as a normal plane created by the vector from our reference point to the centroid of the tetrahedron). If the vector is pointing away from the reference point, it is then positive volume or inside the part. If it is normal, then the volume goes to zero as the triangle is in the same plane as the reference point.
We don't need to worry about actually keeping track of any of this as if we are consistent with our inputs (as in the triangles follow the right hand rule with normal facing outwards from the part) the determine will give us the correct sign.
Anyways, heres the code (its even more simple than the explanation).
class data // 3 vertices of each triangle
{
public:
float x1,y1,z1;
float x2,y2,z2;
float x3,y3,z3;
};
int main ()
{
int numTriangles; // pull in the STL file and determine number of triangles
data * triangles = new triangles [numTriangles];
// fill the triangles array with the data in the STL file
double totalVolume = 0, currentVolume;
double xCenter = 0, yCenter = 0, zCenter = 0;
for (int i = 0; i < numTriangles; i++)
{
totalVolume += currentVolume = (triangles[i].x1*triangles[i].y2*triangles[i].z3 - triangles[i].x1*triangles[i].y3*triangles[i].z2 - triangles[i].x2*triangles[i].y1*triangles[i].z3 + triangles[i].x2*triangles[i].y3*triangles[i].z1 + triangles[i].x3*triangles[i].y1*triangles[i].z2 - triangles[i].x3*triangles[i].y2*triangles[i].z1) / 6;
xCenter += ((triangles[i].x1 + triangles[i].x2 + triangles[i].x3) / 4) * currentVolume;
yCenter += ((triangles[i].y1 + triangles[i].y2 + triangles[i].y3) / 4) * currentVolume;
zCenter += ((triangles[i].z1 + triangles[i].z2 + triangles[i].z3) / 4) * currentVolume;
}
cout << endl << "Total Volume = " << totalVolume << endl;
cout << endl << "X center = " << xCenter/totalVolume << endl;
cout << endl << "Y center = " << yCenter/totalVolume << endl;
cout << endl << "Z center = " << zCenter/totalVolume << endl;
}
Extremely fast for calculating centres of mass for STL files.
EDIT: Look up "winding number algorithm" or "crossing number algorithm" - what I try to describe below is a 3-d crossing number algorithm.
I've got a feeling that something like this will work, but I don't have the ability to test it out, right now:
Build the filled-in 3-d structure from the triangles in the STL file iteratively. Start by picking a single point to use as a basis for the 3-d structure. Then, begin your structure by creating a triangular pyramid, with base defined by the first triangle in the STL file, and vertex your chosen point. Each such component of your iteratively built volume would also contain an "intersection parity" - initialize it to 0.
For each subsequent triangle in the STL file, create a similar pyramid, and see if it intersects with the 3-d structure that you've built so far. If it does, calculate the intersection, and segment the existing structure and the new pyramid so that no two components overlap. Keep the "intersection parity" of the outermost part of the new polyhedron 0, but toggle it on all inner portions of the intersection -- if it was 0, make it 1, if it was 1, make it 0.
At the end, you'll have the closed polyhedron defined by all the portions of your structure that have intersection parity 0. Calculate the moments of all of these polyhedrons, and average them together to get your center of mass. I think the complexity would be something like O(n^2).