please excuse my bad english this isn't my native language.
I try to solve a non-linear-least-squares-problem. I have two sets of points.
A set of 3D-points at time t and a set of 2D-imagepoints at time t+1, knowing the correspondences between them.
Now I have the following equation:
x(t+1) = ( x(t) - y(t)*rotz + z(t)*roty + tx )*f/(-x(t)*roty + y(t)*rotx+z(t)+tz)
y(t+1) = ( x(t)*rotz + y(t) - z(t)*rotx + ty )*f/(-x(t)*roty + y(t)*rotx+z(t)+tz)
The variable f is the focallength. I want to determine rotx, roty, rotz, tx, ty, tz so that the residual between the calculated points image points for t+1 and the known image points for t+1 gets minimal.
I take the euclidean distance between the image-points as error measurment.
E = sqrt( (x(t+1) - X)^2 + (y(t+1) - Y)^2 )
X and Y are the known 2d-image-points.
I calculated a matrix A with a row for every point correspendence and with partial derivatives for rotx, roty and so on in the columns.
I also calculated a column vector y with the results of E for a first estimation of rotx, roty and so on.
Now it is possible for me to calculate updates for the variables with:
delta = inv(A' * A) * A' * y
(A' means the transpose of A)
After updating i use the variables for a few more iterations.
But the algorithm is not converging, even if i set the first estimation with the expected solution. So I think I do something wrong.
Do I use the wrong approach? Can anyone give me a link to a good example or can explain his approach for this problem.
Thank you very much! :-)
I solved my problem. The calculated errors for the y-vector have to be negativ, otherwise the algorithm isnt converging.
y(i) = -E(i)
Related
I'm writing a function that takes in an object with a trajectory (including starting position, starting velocity, and acceleration, all represented as Vector3s) in 3D space and if it hits another object, returns the point of collision and time of the collision. I'm using kinematic equations with a timestep to detect possible collisions and I can get the point of collision that way, but once I have that I want to find the exact time that that collision would occur at.I thought of rearranging a kinematic equation to solve for time and plug in what I already had, but I can't figure out how I can use all three axes of motion to do this, since my other values are Vec3's and time is just scalar. I've thought about just doing the calculation on one axis, but I'm not sure if that would lead to an accurate result.
Would it be accurate to calculate just based on one axis, or is there a way to incorporate all three into the calculation? The formula I'm using to solve for time is:
t = (v_init +/- Sqrt((v_init)^2 - (accel * disp * 4 * .5)))/accel;
Where v_init is initial velocity, disp is total displacement, and accel is acceleration. I'm basing this off of the kinematic equation:
d = v*t + .5*a*t^2
Let me write in the general case. The component-wise motion law is
x(t) = x0 + v_x t + 0.5 a_x t^2
y(t) = y0 + v_y t + 0.5 a_y t^2
z(t) = z0 + v_z t + 0.5 a_z t^2
where (x0,y0,z0)^t is the initial position, (v_x, v_y, v_z)^t is the initial velocity vector, and (a_x, a_y, a_z)^t is the vector of acceleration. The 3rd component of the latter may include also the gravity acceleration.
I assume that the collision plane is horizontal, having thus equation z = k. Solve in t the equation
z(t) = k
for finding the time t_c in which the projectile hits the plane. Compute then the collision coordinates x(t_c) and y(t_c) using the above formula by substituting t with t_c.
If the plane has the general equation
a x + b y +c z + d = 0
I suggest to put the frame of reference on the plane, having the xy plane on the collision plane, and then apply the above procedure.
You may also solve the non linear system
x = x0 + v_x t + 0.5 a_x t^2
y = y0 + v_y t + 0.5 a_y t^2
z = z0 + v_z t + 0.5 a_z t^2
a x + b y +c z + d = 0
taking the solution for t>0 (I dropped the dependency on t for x, y and z).
To solve it in C++, you may search a math library, such as Eigen which has a module for non linear systems.
I am currently looking to implement an algorithm that will be able to compute the arc midpoint. From here on out, I will be referring to the diagram below. What is known are the start and end nodes (A and B respectively), the center (point C) and point P which is the intersection point of the line AB and CM (I am able to find this point without knowing point M because line AB is perpendicular to line CM and thus, the slope is -1/m). I also know the arc angle and the radius of the arc. I am looking to find point M.
I have been looking at different sources. Some suggest converting coordinates to polar, computing the mid point from the polar coordinates then reverting back to Cartesian. This involves sin and cos (and arctan) which I am a little reluctant to do since trig functions take computing time.
I have been looking to directly computing point M by treating the arc as a circle and having Line CP as a line that intersects the circle at Point M. I would then get two values and the value closest to point P would be the correct intersection point. However, this method, the algebra becomes long and complex. Then I would need to create special cases for when P = C and for when the line AB is horizontal and vertical. This method is ok but I am wondering if there are any better methods out there that can compute this point that are simpler?
Also, as a side note, I will be creating this algorithm in C++.
A circumference in polar form is expressed by
x = Cx + R cos(alpha)
y = Cy + R sin(alpha)
Where alpha is the angle from center C to point x,y. The goal now is how to get alpha without trigonometry.
The arc-midpoint M, the point S in the middle of the segment AB, and your already-calculated point P, all of them have the same alpha, they are on the same line from C.
Let's get vector vx,vy as C to S. Also calculate its length:
vx = Sx - Cx = (Ax + Bx)/2 - Cx
vy = Sy - Cy = (Ay + By)/2 - Cy
leV = sqrt(vx * vx + vy * vy)
I prefer S to P because we can avoid some issues like infinite CP slope or sign to apply to slope (towards M or its inverse).
By defintions of sin and cos we know that:
sin(alpha) = vy / leV
cos(alpha) = vx / leV
and finally we get
Mx = Cx + R * vx / leV
My = Cy + R * vy / leV
Note: To calculate Ryou need another sqrt function, which is not quick, but it's faster than sin or cos.
For better accuracy use the average of Ra= dist(AC) and Rb= dist(BC)
I would then get two values
This is algebraically unavoidable.
and the value closest to point P would be the correct intersection point.
Only if the arc covers less than 180°.
Then I would need to create special cases for when P = C
This is indeed the most tricky case. If A, B, C lie on a line, you don't know which arc is the arc, and won't be able to answer the question. Unless you have some additional information to start with, e.g. know that the arc goes from A to B in a counter-clockwise direction. In this case, you know the orientation of the triangle ABM and can use that to decide which solition to pick, instead of using the distance.
and for when the line AB is horizontal and vertical
Express a line as ax + by + c = 0 and you can treat all slopes the same. THese are homogeneous coordinates of the line, you can compute them e.g. using the cross product (a, b, c) = (Ax, Ay, 1) × (Bx, By, 1). But more detailed questions on how best to compute these lines or intersect it with the circle should probably go to the Math Stack Exchange.
if there are any better methods out there that can compute this point that are simpler?
Projective geometry and homogeneous coordinates can avoid a lot of nasty corner cases, like circles of infinite radius (also known as lines) or the intersection of parallel lines. But the problem of deciding between two solutions remains, so it probably doesn't make things as simple as you'd like them to be.
In my game i want my towers to shoot where the enemy is going to be by the time the bullet reaches it.
i don't want my bullet to curve, i want it to shoot directly to the estimated location based on the speed the unit is moving and the direction it is moving
My thought is that i determine the direction of my enemy by subtracting its current position from its last position every time it moves. so lets say that its direction is (1,1)
after that i am not 100% sure what logic i would need to do.
i am thinking i need to know the distance from the tower to the enemy to determine the time i need to estimate how far the enemy is going to be when the bullet should reach it.
i really don't know where to start with this one, so if anyone can give me some pointers on how i should handle this solution.
In case of uniformly accelerated linear movement
x(t) = x0 + vx * t
y(t) = y0 + vy * t
where vx, vy are projections of velocity v on coordinate axis. Velocity is vector which scalar value is speed, and direction in the direction of movement. In your case, if your direction vector is normalized, multiply it by speed to get velocity.
You know starting position: (x0, y0). You only need projections vx and vy. If w is angle between x-axis and velocity, then
vx = v * cos(w)
vy = v * sin(w)
As for angle: use atan2f function, or cocos2d function ccpToAngle (which uses atan2f itself) with normalized direction vector.
Suppose , the position vector of your enemy is P, his velocity is E and the speed of your bullet is b. [ I will use upper case letter for vector, and its corresponding lower case for its modulus]
Suppose, after time t the enemy will be hit.
Then, t*B = P + t*E [where B is the velocity of the bullet. Note, you don't know the direction of B, but you the mod value of it, b]
Now try to calculate t ! It is not easy I think, there is another unknown, the directional vector of B. You have to replace it with known variables and t and then solve if for t. Use other vector tools, like dot product to get the angle between P and B, to get to other equations and relation between other vectors.
Update:
I just did some short calculation, I might be wrong. Just let me know if this works.
t is the real positive solution of the following equation:
t^2*(e^2-b^2) + t*(2*p*e*cos(the angle between P and E)) + p^2 = 0
I have a function in my program which rotates a point (x_p, y_p, z_p) around another point (x_m, y_m, z_m) by the angles w_nx and w_ny.
The new coordinates are stored in global variables x_n, y_n, and z_n. Rotation around the y-axis (so changing value of w_nx - so that the y - values are not harmed) is working correctly, but as soon as I do a rotation around the x- or z- axis (changing the value of w_ny) the coordinates aren't accurate any more. I commented on the line I think my fault is in, but I can't figure out what's wrong with that code.
void rotate(float x_m, float y_m, float z_m, float x_p, float y_p, float z_p, float w_nx ,float w_ny)
{
float z_b = z_p - z_m;
float x_b = x_p - x_m;
float y_b = y_p - y_m;
float length_ = sqrt((z_b*z_b)+(x_b*x_b)+(y_b*y_b));
float w_bx = asin(z_b/sqrt((x_b*x_b)+(z_b*z_b))) + w_nx;
float w_by = asin(x_b/sqrt((x_b*x_b)+(y_b*y_b))) + w_ny; //<- there must be that fault
x_n = cos(w_bx)*sin(w_by)*length_+x_m;
z_n = sin(w_bx)*sin(w_by)*length_+z_m;
y_n = cos(w_by)*length_+y_m;
}
What the code almost does:
compute difference vector
convert vector into spherical coordinates
add w_nx and wn_y to the inclination and azimuth angle (see link for terminology)
convert modified spherical coordinates back into Cartesian coordinates
There are two problems:
the conversion is not correct, the computation you do is for two inclination vectors (one along the x axis, the other along the y axis)
even if computation were correct, transformation in spherical coordinates is not the same as rotating around two axis
Therefore in this case using matrix and vector math will help:
b = p - m
b = RotationMatrixAroundX(wn_x) * b
b = RotationMatrixAroundY(wn_y) * b
n = m + b
basic rotation matrices.
Try to use vector math. Decide in which order you rotate, first along x, then along y perhaps.
If you rotate along z-axis, [z' = z]
x' = x*cos a - y*sin a;
y' = x*sin a + y*cos a;
The same repeated for y-axis: [y'' = y']
x'' = x'*cos b - z' * sin b;
z'' = x'*sin b + z' * cos b;
Again rotating along x-axis: [x''' = x'']
y''' = y'' * cos c - z'' * sin c
z''' = y'' * sin c + z'' * cos c
And finally the question of rotating around some specific "point":
First, subtract the point from the coordinates, then apply the rotations and finally add the point back to the result.
The problem, as far as I see, is a close relative to "gimbal lock". The angle w_ny can't be measured relative to the fixed xyz -coordinate system, but to the coordinate system that is rotated by applying the angle w_nx.
As kakTuZ observed, your code converts point to spherical coordinates. There's nothing inherently wrong with that -- with longitude and latitude, one can reach all the places on Earth. And if one doesn't care about tilting the Earth's equatorial plane relative to its trajectory around the Sun, it's ok with me.
The result of not rotating the next reference axis along the first w_ny is that two points that are 1 km a part of each other at the equator, move closer to each other at the poles and at the latitude of 90 degrees, they touch. Even though the apparent purpose is to keep them 1 km apart where ever they are rotated.
if you want to transform coordinate systems rather than only points you need 3 angles. But you are right - for transforming points 2 angles are enough. For details ask Wikipedia ...
But when you work with opengl you really should use opengl functions like glRotatef. These functions will be calculated on the GPU - not on the CPU as your function. The doc is here.
Like many others have said, you should use glRotatef to rotate it for rendering. For collision handling, you can obtain its world-space position by multiplying its position vector by the OpenGL ModelView matrix on top of the stack at the point of its rendering. Obtain that matrix with glGetFloatv, and then multiply it with either your own vector-matrix multiplication function, or use one of the many ones you can obtain easily online.
But, that would be a pain! Instead, look into using the GL feedback buffer. This buffer will simply store the points where the primitive would have been drawn instead of actually drawing the primitive, and then you can access them from there.
This is a good starting point.
I have a picture with a curve which can be defined by the following equation:
y = ax^3 + bx^2 + cx + d
It is obvious how to use the normal Hough transform to detect the curve. However, I want to reduce the parameter space by using the gradient direction (I already got it from edge detection). I am not sure how to use the gradient direction to reduce the parameter space.
An idea I had is to find the derivative dy/dx = 3ax^2 + 2bx + c . Now I have only three parameters hence my task is easier. Is this correct tho? How do I get the d parameter if I use this?
After running Hough for dy/dx = 3x^2 + 2ax + b
you have
c = f(x,y) = y - x^3 + ax^2 + bx
where a and b are known.
Why not another pass, this time looking only for c?
Two dimensional accumulator, and then 1 dim is better then 3 dimensional accumulator, anyway.