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.
I've created a 3D-Scene with Blender and computed the Projection Matrix P (Also have information about the Translation T- and Rotation-Matrix R).
Like I mentioned in the title I try to calculate the z-Value or depth to an Vertex (x,y,z) from my given camera C with these Matrices.
Example:
Vertex v = [1.4,1,2.3] and position of camera c = [0,-0.7,10]. The Result should be anything around 10-2.3 = 7.7. Thank you for your help!
Usually rotation matrix is applied before translation. So
transform = R * T
R is the rotation matrix (usually 4 rows and 4 columns)
T is the translation matrix (4 rows and 4 columns)
* is the matrix multiplication wich apply first T and then R
of course I'm assuming you already know how to perform matrix multiplication, I'm not providing anycode because it is not clear if you need python snippet or you are using the exported model somehwere else.
After that you apply the final projection matrix (I'm assuming your projection matrix is already multiplied by view matrix)
final = P * transform
P is the projection matrix ( 4 rows and 4 columns)
transform is your previously obtained (4 rows and 4 columns) matrix
the final matrix is the one that will transform every vector of your 3D model, again here you do a matrix multiplication (but in this case the second operand is a colum vector wich 4th element is 1)
transformedVertex = final * Vec4(originalVertex,1)
transformedVertex is a column vector ( 4x1 matrix)
final is the final matrix (4x4)
a vertex is onl 3 coordinates, so we add 1 to make it (4x1)
* is still matrix multiplication
once transformed the vertex Z value is the one that gets directly mapped into Z buffer and ence into a Depth value.
At this point there is one operation that is done "by convention" and is dividing Z by W to normalize it, then values outside range [0..1] are discarded (nearest than near clip plane or farest than far clip plane).
See also this question:
Why do I divide Z by W?
EDIT:
I may have misinterpreted your question, if you need distance between camera and a point it is simply
function computeDistance( cam, pos)
dx = cam.x-pos.x
dy = cam.y-pos.y
dz = cam.z-pos.z
distance = sqrt( dx*dx + dy*dy + dz*dz)
end function
example
cameraposition = 10,0,0
vertexposition = 2,0,0
the above code
computeDistance ( cameraposition, vertexposition)
outputs
8
Thanks for your help, here is what I was looking for:
Data setup
R rotation matrix 4x4
T translation matrix 4x4
v any vertex in with [x,y,z,1] 4x1
Result
vec4 vector 4x1 (x,y,z,w)
vec4 = R * T * v
The vec4.z value is the result I was looking for. Thanks!
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 :)
From a three dimensional cartesian coordinate, object A's coordinate can be expressed as xyzwpr (green arrow). And from object A's coordinate world, object B can be also expressed as xyzwpr (blue arrow).
Then can anyone write down the C# code for calculating xyzwpr of object B relative to the original coordinate system (red arrow)?
Say A's coordinate is (30,50,70, -15,44,-80) B (60,90,110, 33,150,-90).
And say the order of the rotation is yaw(z)-> pitch(x) -> roll(y)
--- EDIT ---
Can anyone validate below assumptions?
Assumption for xyz of point B.
xyz of point B, the smaller airplane, can be calculated by adding xyz of point A, the first airplane, and the xyz of B and then applying the 3d rotation of A's wpr on the A's xyz.
The order of doing this is;
1) translate the A point to the origin (subtract A which is translate by -Ax,-Ay,-Az)
2) rotate about the origin (can use 3×3 matrix R0 of A)
3) then translate back. (add A which is translate by +Ax,+Ay,+Az)
Assumption for wpr of point B
is simply succession of rotations of two points. AwApArBwBpBr.
--- SOLVED. A few references with detailed explanation and codes ---
Global frame-of-reference VS Local frame-of-reference
3D matrix rotation about an arbitrary point
Euler to matrix conversion
This question has some issues.
First, I think it's not good practice to request directly for code. Instead, show code you tried, ask for errors in your code, or a better approach, or libraries that may help you.
I would suggest rephrasing your question. Now it looks like "Can anyone do my homework, please?".
What problems are you facing? Maybe you don't want to implement matrix multiplication and you would like to know libraries that already do it, or you don't know how to make the call to atan2.
Once you get matrix multiplication, translation matrix build up, rotation matrix build up and atan2 (made by yourself or by a library), you just have to (pseudocode):
Matrix c = a;
Matrix yaw, pitch, roll;
Matrix pos;
buildTranslationMatrix(pos, x, y, z);
buildRotationZMatrix(yaw, w);
buildRotationXMatrix(pitch, p);
buildRotationYMatrix(roll, r);
mult (c, c, pos); //c = c*pos
mult (c, c, yaw); //c = c*yaw
mult (c, c, pitch);
mult (c, c, roll);
decomposePos(c, x, y, z); // obtain final xyz from c
decomposeAngles(c, w, p, r); // obtain final wpr from c
Note the post-multiplication.
Hope I made a constructive criticism. :)
EDIT
Second assumption is correct.
Maybe I misunderstood the first one, but I think it's wrong. As I am more used to transformation matrices than to euler angles (and you pointed that link), I understand it this way:
To obtain xyz (as well as wpr) I would compute the transformation matrix, which contains all the values. The final transformation matrix of the second plane, in the original coordinate system, is computed as:
M = TA * RA * TB * RB
(TA is A translation matrix of plane A and RA is its rotation matrix)
Transformation matrices can be understood this way:
r r r t
r r r t
M = r r r t
s s s w
We only care about rotation and translation. If you multiply TA*RA:
1 0 0 x r r r 0 r r r x
0 1 0 y r r r 0 r r r y
0 0 1 z * r r r 0 = r r r z
0 0 0 1 0 0 0 1 0 0 0 1
which is how we understand the coordinate system of A. Remember that this means first rotating, as if it were at the origin, and then translating to position x, y, z. Post-multiplicating means intern transformation, transformation in the mobile coordinate system. So, if we continue post-multiplicating, we will composite the final transformation matrix.
Also, matrices are associative, so
M = (TA * RA) * (TB * RB)
is the same as
M = ((TA * RA) * TB) * RB
Recapitulation
xyz will be in the last column of M and wpr will have to be decomposed from 3*3 submatrix of M.
I have two cartesian coordinate systems with known unit vectors:
System A(x_A,y_A,z_A)
and
System B(x_B,y_B,z_B)
Both systems share the same origin (0,0,0). I'm trying to calculate a quaternion, so that vectors in system B can be expressed in system A.
I am familiar with the mathematical concept of quaternions. I have already implemented the required math from here: http://content.gpwiki.org/index.php/OpenGL%3aTutorials%3aUsing_Quaternions_to_represent_rotation
One possible solution could be to calculate Euler angles and use them for 3 quaternions. Multiplying them would lead to a final one, so that I could transform my vectors:
v(A) = q*v(B)*q_conj
But this would incorporate Gimbal Lock again, which was the reason NOT to use Euler angles in the beginning.
Any idead how to solve this?
You can calculate the quaternion representing the best possible transformation from one coordinate system to another by the method described in this paper:
Paul J. Besl and Neil D. McKay
"Method for registration of 3-D shapes", Sensor Fusion IV: Control Paradigms and Data Structures, 586 (April 30, 1992); http://dx.doi.org/10.1117/12.57955
The paper is not open access but I can show you the Python implementation:
def get_quaternion(lst1,lst2,matchlist=None):
if not matchlist:
matchlist=range(len(lst1))
M=np.matrix([[0,0,0],[0,0,0],[0,0,0]])
for i,coord1 in enumerate(lst1):
x=np.matrix(np.outer(coord1,lst2[matchlist[i]]))
M=M+x
N11=float(M[0][:,0]+M[1][:,1]+M[2][:,2])
N22=float(M[0][:,0]-M[1][:,1]-M[2][:,2])
N33=float(-M[0][:,0]+M[1][:,1]-M[2][:,2])
N44=float(-M[0][:,0]-M[1][:,1]+M[2][:,2])
N12=float(M[1][:,2]-M[2][:,1])
N13=float(M[2][:,0]-M[0][:,2])
N14=float(M[0][:,1]-M[1][:,0])
N21=float(N12)
N23=float(M[0][:,1]+M[1][:,0])
N24=float(M[2][:,0]+M[0][:,2])
N31=float(N13)
N32=float(N23)
N34=float(M[1][:,2]+M[2][:,1])
N41=float(N14)
N42=float(N24)
N43=float(N34)
N=np.matrix([[N11,N12,N13,N14],\
[N21,N22,N23,N24],\
[N31,N32,N33,N34],\
[N41,N42,N43,N44]])
values,vectors=np.linalg.eig(N)
w=list(values)
mw=max(w)
quat= vectors[:,w.index(mw)]
quat=np.array(quat).reshape(-1,).tolist()
return quat
This function returns the quaternion that you were looking for. The arguments lst1 and lst2 are lists of numpy.arrays where every array represents a 3D vector. If both lists are of length 3 (and contain orthogonal unit vectors), the quaternion should be the exact transformation. If you provide longer lists, you get the quaternion that is minimizing the difference between both point sets.
The optional matchlist argument is used to tell the function which point of lst2 should be transformed to which point in lst1. If no matchlist is provided, the function assumes that the first point in lst1 should match the first point in lst2 and so forth...
A similar function for sets of 3 Points in C++ is the following:
#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace Eigen;
/// Determine rotation quaternion from coordinate system 1 (vectors
/// x1, y1, z1) to coordinate system 2 (vectors x2, y2, z2)
Quaterniond QuaternionRot(Vector3d x1, Vector3d y1, Vector3d z1,
Vector3d x2, Vector3d y2, Vector3d z2) {
Matrix3d M = x1*x2.transpose() + y1*y2.transpose() + z1*z2.transpose();
Matrix4d N;
N << M(0,0)+M(1,1)+M(2,2) ,M(1,2)-M(2,1) , M(2,0)-M(0,2) , M(0,1)-M(1,0),
M(1,2)-M(2,1) ,M(0,0)-M(1,1)-M(2,2) , M(0,1)+M(1,0) , M(2,0)+M(0,2),
M(2,0)-M(0,2) ,M(0,1)+M(1,0) ,-M(0,0)+M(1,1)-M(2,2) , M(1,2)+M(2,1),
M(0,1)-M(1,0) ,M(2,0)+M(0,2) , M(1,2)+M(2,1) ,-M(0,0)-M(1,1)+M(2,2);
EigenSolver<Matrix4d> N_es(N);
Vector4d::Index maxIndex;
N_es.eigenvalues().real().maxCoeff(&maxIndex);
Vector4d ev_max = N_es.eigenvectors().col(maxIndex).real();
Quaterniond quat(ev_max(0), ev_max(1), ev_max(2), ev_max(3));
quat.normalize();
return quat;
}
What language are you using? If c++, feel free to use my open source library:
http://sourceforge.net/p/transengine/code/HEAD/tree/transQuaternion/
The short of it is, you'll need to convert your vectors to quaternions, do your calculations, and then convert your quaternion to a transformation matrix.
Here's a code snippet:
Quaternion from vector:
cQuat nTrans::quatFromVec( Vec vec ) {
float angle = vec.v[3];
float s_angle = sin( angle / 2);
float c_angle = cos( angle / 2);
return (cQuat( c_angle, vec.v[0]*s_angle, vec.v[1]*s_angle,
vec.v[2]*s_angle )).normalized();
}
And for the matrix from quaternion:
Matrix nTrans::matFromQuat( cQuat q ) {
Matrix t;
q = q.normalized();
t.M[0][0] = ( 1 - (2*q.y*q.y + 2*q.z*q.z) );
t.M[0][1] = ( 2*q.x*q.y + 2*q.w*q.z);
t.M[0][2] = ( 2*q.x*q.z - 2*q.w*q.y);
t.M[0][3] = 0;
t.M[1][0] = ( 2*q.x*q.y - 2*q.w*q.z);
t.M[1][1] = ( 1 - (2*q.x*q.x + 2*q.z*q.z) );
t.M[1][2] = ( 2*q.y*q.z + 2*q.w*q.x);
t.M[1][3] = 0;
t.M[2][0] = ( 2*q.x*q.z + 2*q.w*q.y);
t.M[2][1] = ( 2*q.y*q.z - 2*q.w*q.x);
t.M[2][2] = ( 1 - (2*q.x*q.x + 2*q.y*q.y) );
t.M[2][3] = 0;
t.M[3][0] = 0;
t.M[3][1] = 0;
t.M[3][2] = 0;
t.M[3][3] = 1;
return t;
}
I just ran into this same problem. I was on the track to a solution, but I got stuck.
So, you'll need TWO vectors which are known in both coordinate systems. In my case, I have 2 orthonormal vectors in the coordinate system of a device (gravity and magnetic field), and I want to find the quaternion to rotate from device coordinates to global orientation (where North is positive Y, and "up" is positive Z). So, in my case, I've measured the vectors in the device coordinate space, and I'm defining the vectors themselves to form the orthonormal basis for the global system.
With that said, consider the axis-angle interpretation of quaternions, there is some vector V about which the device's coordinates can be rotated by some angle to match the global coordinates. I'll call my (negative) gravity vector G, and magnetic field M (both are normalized).
V, G and M all describe points on the unit sphere.
So do Z_dev and Y_dev (the Z and Y bases for my device's coordinate system).
The goal is to find a rotation which maps G onto Z_dev and M onto Y_dev.
For V to rotate G onto Z_dev the distance between the points defined by G and V must be the same as the distance between the points defined by V and Z_dev. In equations:
|V - G| = |V - Z_dev|
The solution to this equation forms a plane (all points equidistant to G and Z_dev). But, V is constrained to be unit-length, which means the solution is a ring centered on the origin -- still an infinite number of points.
But, the same situation is true of Y_dev, M and V:
|V - M| = |V - Y_dev|
The solution to this is also a ring centered on the origin. These rings have two intersection points, where one is the negative of the other. Either is a valid axis of rotation (the angle of rotation will just be negative in one case).
Using the two equations above, and the fact that each of these vectors is unit length you should be able to solve for V.
Then you just have to find the angle to rotate by, which you should be able to do using the vectors going from V to your corresponding bases (G and Z_dev for me).
Ultimately, I got gummed up towards the end of the algebra in solving for V.. but either way, I think everything you need is here -- maybe you'll have better luck than I did.
Define 3x3 matrices A and B as you gave them, so the columns of A are x_A,x_B, and x_C and the columns of B are similarly defined. Then the transformation T taking coordinate system A to B is the solution TA = B, so T = BA^{-1}. From the rotation matrix T of the transformation you can calculate the quaternion using standard methods.
You need to express the orientation of B, with respect to A as a quaternion Q. Then any vector in B can be transformed to a vector in A e.g. by using a rotation matrix R derived from Q. vectorInA = R*vectorInB.
There is a demo script for doing this (including a nice visualization) in the Matlab/Octave library available on this site: http://simonbox.info/index.php/blog/86-rocket-news/92-quaternions-to-model-rotations
You can compute what you want using only quaternion algebra.
Given two unit vectors v1 and v2 you can directly embed them into quaternion algebra and get the corresponding pure quaternions q1 and q2. The rotation quaternion Q that align the two vectors such that:
Q q1 Q* = q2
is given by:
Q = q1 (q1 + q2)/(||q1 + q2||)
The above product is the quaternion product.