I am trying to implement a Kalman filter in order to localize a robot.
I am confused with the prediction step (excluding process noise) x = Fx + u
If x is a state estimation vector: [xLocation, xVelocity] and F is the state transition matrix [[1 1],[0 1]], then the new xLocation would be equal to xLocation + xVelocity + the corresponding component of the motion vector u.
Why is the equation not x = x + u? Shouldn't the predicted location of the robot be the location + motion of the robot?
Maybe there is some confusion with respect to what the matrices actually represent.
The "control vector", u, might be the acceleration externally applied to the system.
In this case, I would expect the equations to look like this:
xlocation = xlocation + xvelocity
xvelocity = xvelocity + uvelocity
These two equations assume that the update is applied every 1 second (otherwise some "delta time" factors would need to be applied and included the transition matrix and the control vector).
For the situation mentioned above, the matrices and vectors are:
The state vector (column vector with 2 entries):
xlocation
xvelocity
The transition matrix (2 x 2 matrix):
1 1
0 1
The control vector (column vector with 2 entries):
0
uvelocity
This link contains nice explanations and visualizations for the Kalman Filter.
Related
Let's say I have this matrix with n=4 and m=5
1 2 3 4 5
6 7 8 9 10
11 12 13 14 15
16 17 18 19 20
Let's say I have a diagonal from the (1,2) point to the (4,5) point. And I have a point P(3,4). How can I check in my algorithm that P is on the diagonal?
TL;DR
Instead of an n-by-m matrix, think about it like a x-y grid. You can get the equation of a line on that grid, and once you have that equation, you put the x coordinate of the point you are interested in checking into your equation. If the y value you calculate from the equation matches the y coordinate of the point you are checking, the point lies on the line.
But How Do I Maths?
First some quick terminology. We have 3 points of interest in this case - the two points that define the line (or "diagonal", as the OP calls it), and the one point that we want to check. I'm going to designate the coordinates of the "diagonal" points with the numbers 1 and 2, and the point we want to check with the letter i. Additionally, for the math we need to do later, I need to treat the horizontal and vertical coordinates of the points separately, and I'll use your n-by-m convention to do so. So when I write n1 in an equation below, that is the n coordinate of the first point used to define the diagonal (so the 1 part of the point (1,2) that you give in your example).
What we are looking for is the equation of a line on our grid. This equation will have the form n = (slope) * m + (intercept).
Okay, now that we have the definitions taken care of, we can write the equations. The first step to solving the problem is finding the slope of your line. This will be the change in the vertical coordinate divided by the change in the horizontal component between the two points that define the line (so (n2 - n1) / (m2 - m1)). Using the values from your example, this will be (4 - 1) / (5 - 2) = 3 / 3 = 1. Note that since you are doing a division here, it is possible that your answer will not be a whole number, so make sure you keep that in mind when declaring your variables in whatever programming language you end up using - unintentional rounding in this step can really mess things up later.
Once we have our slope, the next step is calculating our intercept. We can do this by plugging our slope and the m and n coordinates into the equation for the line we are trying to get. So we start with the equation n1 = (slope) * m1 + (intercept). We can rearrange this equation to (intercept) = n1 - (slope) * m1. Plugging in the values from our example, we get (intercept) = 1 - (1 * 2) = -1.
So now we have the general equation of our line, which for our example is n = (1) * m + (-1).
Now that we have the (slope) and (intercept), we can plug in the coordinates of any point we want to check and see if the numbers match up. Our example point has a m coordinate of 4, so we can plug that into our equation.
n = (1) * (4) + (-1) = 3
Since the n coordinate we calculated using our equation matches the n coordinate of our point in our example, we can say that the sample point DOES fall on the line.
Suppose we wanted to also check to see if the point (2,5) was also on the line. When we plug that point's m coordinate into our equation, we get...
n = (1) * (5) + (-1) = 4
Since the n coordinate we calculated with our equation (4) doesn't match the n coordinate of the point we were checking (2), we know this point DOES NOT fall on the line.
I have two images which one of them is the Original image and the second one is Transformed image.
I have to find out how many degrees Transformed image was rotated using 3x3 transformation matrix. Plus, I need to find how far translated from origin.
Both images are grayscaled and held in matrix variables. Their sizes are same [350 500].
I have found a few lecture notes like this.
Lecture notes say that I should use the following matrix formula for rotation:
For translation matrix the formula is given:
Everything is good. But there are two problems:
I could not imagine how to implement the formulas using MATLAB.
The formulas are shaped to find x',y' values but I already have got x,x',y,y' values. I need to find rotation angle (theta) and tx and ty.
I want to know the equivailence of x, x', y, y' in the the matrix.
I have got the following code:
rotationMatrix = [ cos(theta) sin(theta) 0 ; ...
-sin(theta) cos(theta) 0 ; ...
0 0 1];
translationMatrix = [ 1 0 tx; ...
0 1 ty; ...
0 0 1];
But as you can see, tx, ty, theta variables are not defined before used. How can I calculate theta, tx and ty?
PS: It is forbidden to use Image Processing Toolbox functions.
This is essentially a homography recovery problem. What you are doing is given co-ordinates in one image and the corresponding co-ordinates in the other image, you are trying to recover the combined translation and rotation matrix that was used to warp the points from the one image to the other.
You can essentially combine the rotation and translation into a single matrix by multiplying the two matrices together. Multiplying is simply compositing the two operations together. You would this get:
H = [cos(theta) -sin(theta) tx]
[sin(theta) cos(theta) ty]
[ 0 0 1]
The idea behind this is to find the parameters by minimizing the error through least squares between each pair of points.
Basically, what you want to find is the following relationship:
xi_after = H*xi_before
H is the combined rotation and translation matrix required to map the co-ordinates from the one image to the other. H is also a 3 x 3 matrix, and knowing that the lower right entry (row 3, column 3) is 1, it makes things easier. Also, assuming that your points are in the augmented co-ordinate system, we essentially want to find this relationship for each pair of co-ordinates from the first image (x_i, y_i) to the other (x_i', y_i'):
[p_i*x_i'] [h11 h12 h13] [x_i]
[p_i*y_i'] = [h21 h22 h23] * [y_i]
[ p_i ] [h31 h32 1 ] [ 1 ]
The scale of p_i is to account for homography scaling and vanishing points. Let's perform a matrix-vector multiplication of this equation. We can ignore the 3rd element as it isn't useful to us (for now):
p_i*x_i' = h11*x_i + h12*y_i + h13
p_i*y_i' = h21*x_i + h22*y_i + h23
Now let's take a look at the 3rd element. We know that p_i = h31*x_i + h32*y_i + 1. As such, substituting p_i into each of the equations, and rearranging to solve for x_i' and y_i', we thus get:
x_i' = h11*x_i + h12*y_i + h13 - h31*x_i*x_i' - h32*y_i*x_i'
y_i' = h21*x_i + h22*y_i + h23 - h31*x_i*y_i' - h32*y_i*y_i'
What you have here now are two equations for each unique pair of points. What we can do now is build an over-determined system of equations. Take each pair and build two equations out of them. You will then put it into matrix form, i.e.:
Ah = b
A would be a matrix of coefficients that were built from each set of equations using the co-ordinates from the first image, b would be each pair of points for the second image and h would be the parameters you are solving for. Ultimately, you are finally solving this linear system of equations reformulated in matrix form:
You would solve for the vector h which can be performed through least squares. In MATLAB, you can do this via:
h = A \ b;
A sidenote for you: If the movement between images is truly just a rotation and translation, then h31 and h32 will both be zero after we solve for the parameters. However, I always like to be thorough and so I will solve for h31 and h32 anyway.
NB: This method will only work if you have at least 4 unique pairs of points. Because there are 8 parameters to solve for, and there are 2 equations per point, A must have at least a rank of 8 in order for the system to be consistent (if you want to throw in some linear algebra terminology in the loop). You will not be able to solve this problem if you have less than 4 points.
If you want some MATLAB code, let's assume that your points are stored in sourcePoints and targetPoints. sourcePoints are from the first image and targetPoints are for the second image. Obviously, there should be the same number of points between both images. It is assumed that both sourcePoints and targetPoints are stored as M x 2 matrices. The first columns contain your x co-ordinates while the second columns contain your y co-ordinates.
numPoints = size(sourcePoints, 1);
%// Cast data to double to be sure
sourcePoints = double(sourcePoints);
targetPoints = double(targetPoints);
%//Extract relevant data
xSource = sourcePoints(:,1);
ySource = sourcePoints(:,2);
xTarget = targetPoints(:,1);
yTarget = targetPoints(:,2);
%//Create helper vectors
vec0 = zeros(numPoints, 1);
vec1 = ones(numPoints, 1);
xSourcexTarget = -xSource.*xTarget;
ySourcexTarget = -ySource.*xTarget;
xSourceyTarget = -xSource.*yTarget;
ySourceyTarget = -ySource.*yTarget;
%//Build matrix
A = [xSource ySource vec1 vec0 vec0 vec0 xSourcexTarget ySourcexTarget; ...
vec0 vec0 vec0 xSource ySource vec1 xSourceyTarget ySourceyTarget];
%//Build RHS vector
b = [xTarget; yTarget];
%//Solve homography by least squares
h = A \ b;
%// Reshape to a 3 x 3 matrix (optional)
%// Must transpose as reshape is performed
%// in column major format
h(9) = 1; %// Add in that h33 is 1 before we reshape
hmatrix = reshape(h, 3, 3)';
Once you are finished, you have a combined rotation and translation matrix. If you want the x and y translations, simply pick off column 3, rows 1 and 2 in hmatrix. However, we can also work with the vector of h itself, and so h13 would be element 3, and h23 would be element number 6. If you want the angle of rotation, simply take the appropriate inverse trigonometric function to rows 1, 2 and columns 1, 2. For the h vector, this would be elements 1, 2, 4 and 5. There will be a bit of inconsistency depending on which elements you choose as this was solved by least squares. One way to get a good overall angle would perhaps be to find the angles of all 4 elements then do some sort of average. Either way, this is a good starting point.
References
I learned about homography a while ago through Leow Wee Kheng's Computer Vision course. What I have told you is based on his slides: http://www.comp.nus.edu.sg/~cs4243/lecture/camera.pdf. Take a look at slides 30-32 if you want to know where I pulled this material from. However, the MATLAB code I wrote myself :)
I'm trying to deduct the 2D-transformation parameters from the result.
Given is a large number of samples in an unknown X-Y-coordinate system as well as their respective counterparts in WGS84 (longitude, latitude). Since the area is small, we can assume the target system to be flat, too.
Sadly I don't know which order of scale, rotate, translate was used, and I'm not even sure if there were 1 or 2 translations.
I tried to create a lengthy equation system, but that ended up too complex for me to handle. Basic geometry also failed me, as the order of transformations is unknown and I would have to check every possible combination order.
Is there a systematic approach to this problem?
Figuring out the scaling factor is easy, just choose any two points and find the distance between them in your X-Y space and your WGS84 space and the ratio of them is your scaling factor.
The rotations and translations is a little trickier, but not nearly as difficult when you learn that the result of applying any number of rotations or translations (in 2 dimensions only!) can be reduced to a single rotation about some unknown point by some unknown angle.
Suddenly you have N points to determine 3 unknowns, the axis of rotation (x and y coordinate) and the angle of rotation.
Calculating the rotation looks like this:
Pr = R*(Pxy - Paxis_xy) + Paxis_xy
Pr is your rotated point in X-Y space which then needs to be converted to WGS84 space (if the axes of your coordinate systems are different).
R is the familiar rotation matrix depending on your rotation angle.
Pxy is your unrotated point in X-Y space.
Paxis_xy is the axis of rotation in X-Y space.
To actually find the 3 unknowns, you need to un-scale your WGS84 points (or equivalently scale your X-Y points) by the scaling factor you found and shift your points so that the two coordinate systems have the same origin.
First, finding the angle of rotation: take two corresponding pairs of points P1, P1' and P2, P2' and write out
P1' = R(P1-A) + A
P2' = R(P2-A) + A
where I swapped A = Paxis_xy for brevity. Subtracting the two equations gives:
P2'-P1' = R(P2-P1)
B = R * C
Bx = cos(a) * Cx - sin(a) * Cy
By = cos(a) * Cx + sin(a) * Cy
By + Bx = 2 * cos(a) * Cx
(By + Bx) / (2 * Cx) = cos(a)
...
(By - Bx) / (2 * Cy) = sin(a)
a = atan2(sin(a), cos(a)) <-- to get the right quadrant
And you have your angle, you can also do a quick check that cos(a) * cos(a) + sin(a) * sin(a) == 1 to make sure either you got all the calculations correct or that your system really is an orientation-preserving isometry (consists only of translations and rotations).
Now that we know a we know R and so to find A we do:
P1` = R(P1-A) + A
P1' - R*P1 = (I-R)A
A = (inverse(I-R)) * (P1' - R*P1)
where the inversion of a 2x2 matrix is easy.
EDIT: There is an error in the above, or more specifically one case that needs to be treated separately.
There is one combination of translations and rotations that does not reduce to a single rotation and that is a single translation. You can think of it in terms of fixed points (how many points are unchanged after the operation).
A translation has no fixed points (all points are changed) and a rotation has 1 fixed point (the axis doesn't change). It turns out that two rotations leave 1 fixed point and a translation and a rotation leaves 1 fixed point, which (with a little proof that says the number of fixed points tells you the operation performed) is the reason that arbitrary combinations of these result in a single rotation.
What this means for you is that if your angle comes out as 0 then using the method above will give you A = 0 as well, which is likely incorrect. In this case you have to do A = P1' - P1.
If I understood the question correctly, you have n points (X1,Y1),...,(Xn,Yn), the corresponding points, say, (x1,y1),...,(xn,yn) in another coordinate system, and the former are supposedly obtained from the latter by rotation, scaling and translation.
Note that this data does not determine the fixed point of rotation / scaling, or the order in which the operations "should" be applied. On the other hand, if you know these beforehand or choose them arbitrarily, you will find a rotation, translation and scaling factor that transform the data as supposed to.
For example, you can pick an any point, say, p0 = [X1, Y1]T (column vector) as the fixed point of rotation & scaling and subtract its coordinates from those of two other points to get p2 = [X2-X1, Y2-Y1]T, and p3 = [X3-X1, Y3-Y1]T. Also take the column vectors q2 = [x2-x1, y2-y1]T, q3 = [x3-x1, y3-y1]T. Now [p2 p3] = A*[q2 q3], where A is an unknwon 2x2 matrix representing the roto-scaling. You can solve it (unless you were unlucky and chose degenerate points) as A = [p2 p3] * [q2 q3]-1 where -1 denotes matrix inverse (of the 2x2 matrix [q2 q3]). Now, if the transformation between the coordinate systems really is a roto-scaling-translation, all the points should satisfy Pk = A * (Qk-q0) + p0, where Pk = [Xk, Yk]T, Qk = [xk, yk]T, q0=[x1, y1]T, and k=1,..,n.
If you want, you can quite easily determine the scaling and rotation parameter from the components of A or combine b = -A * q0 + p0 to get Pk = A*Qk + b.
The above method does not react well to noise or choosing degenerate points. If necessary, this can be fixed by applying, e.g., Principal Component Analysis, which is also just a few lines of code if MATLAB or some other linear algebra tools are available.
I'm new to image processing, but I'm using EMGU for C# image analysis. However, I know the homography matrix isn't unique to EMGU, and so perhaps someone with knowledge of another language can explain better.
Please (in as simplified as can be) can someone explain what each element does. I've looked this up online but can't find an answer that I can properly understand (as I said, I'm kinda new to all this!)
I analyse 2 images, both 2 dimensional. Therefore a 3x3 matrix is needed to account for the rotation / translation of the image. If no movement is detected, the homography matrix is:
100,
010,
001
I know from research (eg OpenCV Homography, Transform a point, what is this code doing?) that:
10Tx,
01Ty,
XXX
The 10,01 bit is the rotation of the x and y coordinates. The Tx and Ty bits are the translational movement, but what is the XXX bit? This is what I don't understand? Is it something to do with affine transformations? Please can someone explain:
1. If I'm currently right in what I say above.
2. what the XXX bit means
It's not that difficult to understand if you have a grasp of matrix multiplication. Assume you point x is
/a\
\b/,
and you want to rotate the coordinate system by A:
/3 4\
\5 6/
and and "move it" it by t
/2\
\2/.
The latter matrices are the components of the affine transformation to get the new point y:
y = A*x + t = <a'; b'>T //(T means transposed).
As you know, to get that, one can construct a 3d matrix B and a vector x' looking like
/3 4 2\ /a\
B = |5 6 2| , x' = |b|
\0 0 1/ \1/
such that
/a'\
y' = |b'| = B*x'
\ 1/
from which you can extract y. Let's see how that works. In the original transformation (using addition), the first step would be to carry out the multiplication, ie. the rotating part y_r:
y_r = A*x = <3a+4b; 5a+6b>T
then you add the "absolute" part:
y = y_r + t = <3a+4b+2; 5a+6b+2>T
Now look at how B works. I'll calculate y' row by row:
1) a' = 3*a + 4*b + 2*1
2) b' = 5*a + 6*b + 2*1
3) the rest: 0*a + 0*b + 1*1 = 1
Just what we expected. First, the rotation part gets calculated--addition and multiplication. Then, the x-part of the translational part gets added, multiplied by 1--it stays the same. The same thing for the second row.
In the third row, a and b are dropped (multiplied by 0). The last part is kept the same, and happens to be 1. So, all about that last line is to "drop" the values of the point and keep the 1.
It could be argued, then, that a 2x3 matrix would be enough for that. That's partially true, but has one significant disadvantage: you loose composability. Suppose you are basically satisfied with B, but want to mirror one coordinate. Then you can choose another transformation matrix
/-1 0 0\
C = | 0 1 0|
\ 0 0 1/
and have a result
y'' = C*B*x' = <-3a+4b+2; 5a+6b+2; 1>T
This simple multiplication could not be done that easily with 2x3 matrices, simply because of the properties of matrix multiplication.
In principle, in the above, the last row (the XXX) could also be anything else of the form <0;0;x>. It was there just to drop the point values. It is however necessary exactly like this to make composition by multiplication work.
Finally, wikipedia seems quite informative to me in this case.
First of all affine transformation are those that preserve straight lines and can many of arbitrary dimensionality
Homography describes the mapping across two planes or what happens during pure camera rotation.
The last row represents various shears (that is when x is function of both x, y)
I'm working on a data mining algorithm where i want to pick a random direction from a particular point in the feature space.
If I pick a random number for each of the n dimensions from [-1,1] and then normalize the vector to a length of 1 will I get an even distribution across all possible directions?
I'm speaking only theoretically here since computer generated random numbers are not actually random.
One simple trick is to select each dimension from a gaussian distribution, then normalize:
from random import gauss
def make_rand_vector(dims):
vec = [gauss(0, 1) for i in range(dims)]
mag = sum(x**2 for x in vec) ** .5
return [x/mag for x in vec]
For example, if you want a 7-dimensional random vector, select 7 random values (from a Gaussian distribution with mean 0 and standard deviation 1). Then, compute the magnitude of the resulting vector using the Pythagorean formula (square each value, add the squares, and take the square root of the result). Finally, divide each value by the magnitude to obtain a normalized random vector.
If your number of dimensions is large then this has the strong benefit of always working immediately, while generating random vectors until you find one which happens to have magnitude less than one will cause your computer to simply hang at more than a dozen dimensions or so, because the probability of any of them qualifying becomes vanishingly small.
You will not get a uniformly distributed ensemble of angles with the algorithm you described. The angles will be biased toward the corners of your n-dimensional hypercube.
This can be fixed by eliminating any points with distance greater than 1 from the origin. Then you're dealing with a spherical rather than a cubical (n-dimensional) volume, and your set of angles should then be uniformly distributed over the sample space.
Pseudocode:
Let n be the number of dimensions, K the desired number of vectors:
vec_count=0
while vec_count < K
generate n uniformly distributed values a[0..n-1] over [-1, 1]
r_squared = sum over i=0,n-1 of a[i]^2
if 0 < r_squared <= 1.0
b[i] = a[i]/sqrt(r_squared) ; normalize to length of 1
add vector b[0..n-1] to output list
vec_count = vec_count + 1
else
reject this sample
end while
There is a boost implementation of the algorithm that samples from normal distributions: random::uniform_on_sphere
I had the exact same question when also developing a ML algorithm.
I got to the same conclusion as Jim Lewis after drawing samples for the 2-d case and plotting the resulting distribution of the angle.
Furthermore, if you try to derive the density distribution for the direction in 2d when you draw at random from [-1,1] for the x- and y-axis ,you will see that:
f_X(x) = 1/(4*cos²(x)) if 0 < x < 45⁰
and
f_X(x) = 1/(4*sin²(x)) if x > 45⁰
where x is the angle, and f_X is the probability density distribution.
I have written about this here:
https://aerodatablog.wordpress.com/2018/01/14/random-hyperplanes/
#define SCL1 (M_SQRT2/2)
#define SCL2 (M_SQRT2*2)
// unitrand in [-1,1].
double u = SCL1 * unitrand();
double v = SCL1 * unitrand();
double w = SCL2 * sqrt(1.0 - u*u - v*v);
double x = w * u;
double y = w * v;
double z = 1.0 - 2.0 * (u*u + v*v);