I have a set of data coordinates in 3D with respect to an Origin, say O1, and another set of data that represents the same movements but in 2D with respect to an Origin, say O2. What can I do to calculate the transformations that are required to implement on the 3D set to be able to compare the data points(2D vs 3D) in a 2D frame?
Setup of data generation
If the measurements are at the same times, then the following might work. I have not actually tried it and make no claim that it is the best solution, but it seems doable. Basically, after adjusting the coordinates so that they have the same origin, you want to minimize the following 2-norm:
|| vec(A*x) - vec(P*B*y) ||
where x is the 2xN array of the 2-D locations, A is a 2x2 rotation matrix that rotates x by the unknown angle theta, P is a 2x3 projection matrix that projects a 3x1 vector onto its first two dimensions (viz., just throws out the third value), B is a 3x3 rotation matrix, and y is the 3xN array of the 3-D locations.
The unknowns in the above are theta in the 2-D rotation matrix, and the three rotation angles needed to define B. The four angles that minimize the above should (at least I think) give you the rotation matrices needed to align the coordinate systems.
Related
I'm having a bit of trouble understanding how axis-angle rotation vectors are used when rotating a vector in 3D space. Why are these used and how do these relate to rotation matrices?
I also found a function called vrrotvec2mat that seems to do what I want but I can't understand the documentation. Specifically, can someone give a more clear explanation (with some example) of the input arguments r and options?
MathWorks explanations are very limited, as follows:
Convert rotation from axis-angle to matrix representation
m = vrrotvec2mat(r) returns a matrix representation of the rotation defined by the axis-angle rotation vector, r.
m = vrrotvec2mat(r,options) returns a matrix representation of rotation defined by the axis-angle rotation vector r, with the default algorithm parameters replaced by values defined in options.
The options structure contains the parameter epsilon that represents the value below which a number will be treated as zero (default value is 1e-12).
The rotation vector, r, is a row vector of four elements, where the first three elements specify the rotation axis, and the last element defines the angle.
To rotate a column vector of three elements, multiply it by the rotation matrix. >To rotate a row vector of three elements, multiply it by the transposed rotation matrix.
If you want to understand the vrrotvec2mat function, you need to know how axis-angle representations of rotations work before we delve into this function. Specifically, you're looking at understanding the Rodrigues Rotation formula, which is also known as the axis-angle rotation formula. I'll explain this to you with some introduction first.
In Linear Algebra, the most standard way to rotate a point, whether it's 2D or 3D is to use a rotation matrix where you pre-multiply (i.e. y = A*x where x is your point represented in a column vector) the 2D or 3D coordinate with this rotation matrix. This rotates the point around the origin of the coordinate system. You can also think of this as rotating a vector v where the tail is at the origin and the head is at the point in 2D or 3D space.
However, another way to do this is to provide what is known as the axis-angle representation which is only valid in 3D space. The axis is described by a unit vector k that describes an axis of rotation about which the vector v rotates by an angle around this axis by the right-hand rule.
Here's a pictorial example I got from Wikipedia:
Source: Rodrigues' Rotation formula
The vector k in our case is pointing straight up and the vector v is pointing on a 45 degree angle northwest. We wish to rotate this vector by an angle of 180 degrees around the axis defined by the vector k, and so if you do this, vrot is the resulting vector. v|| and v_|_ are the parallel and perpendicular projections of v with respect to the vector k. These are shown to derive the Rodrigues formula, which I won't go through here. I'll refer you to the article if you want a full derivation.
The reason why the Rodrigues rotation formula was proposed to rotate things is because very frequently, there are applications where you are rotating about an axis that is not centred at the origin, nor are you rotating with respect to a standard x,y and z axis.
In fact, if you look at the Wikipedia article, you don't need to convert to the matrix form to rotate things. You can use the unit vector and rotation angle directly to rotate your vector, which leads us to his rotation formula:
Source: Rodrigues' Rotation formula
The reason why vrrotvec2mat exists is because you can convert between the axis-angle representation of rotating a vector and a rotation matrix with a rotation with respect to the origin in Linear Algebra. You can then apply the same Linear Algebra theory to rotate a vector/point in 3D space given this rotation matrix. You can convert back and forth between a normal rotation matrix and the Rodrigues formula representation by using vrrotvec2mat and vrrotmat2vec respectively.
The axis-angle representation is essentially a 4 element vector where the first three elements are the x,y and z components of the unit vector k that defines your rotation axis and the last element is the rotation angle theta that rotates your vector with respect to this axis. vrrotvec2mat is no different here and requires a 4 element vector in the order that I just talked about. However, having a quick look at the source, theta is defined in radians.
If you want a concrete example of seeing this work, let's use the above diagram as an example. The unit vector k is pointing upwards on the z axis, and so the first three components are (0,0,1). We wish to rotate by 180 degrees, and so the fourth argument is pi... and so:
>> M = vrrotvec2mat([0 0 1 pi])
M =
-1.0000 -0.0000 0
0.0000 -1.0000 0
0 0 1.0000
This exactly defines a rotation of 180 degrees around the z-axis if you take a look at the standard rotation matrix in Cartesian space around the z axis. If you recall the rotation matrix for this, it's:
If you substitute theta = pi in the above matrix, you will get the same thing as M as seen in the vrrot2vec2mat function. However, ignore the sign of the first row, second column as it's due to numerical precision... which leads us to the second parameter options. Basically, when computing the rotation matrix values using the Rodrigues Rotation formula, there will be times where values in the matrix will be quite small. The options structure has a field called epsilon, where you can specify anything smaller than this threshold is considered zero after the matrix has been calculated. The default of 1e-12 is quite suitable IMHO.
If you'd like to change the default epsilon, simply create a structure that has a single element epsilon that changes this threshold and call the function with this additional second argument... so something like:
>> options.epsilon = 1e-15;
>> M = vrrotvec2mat([0 0 1 pi], options);
In any case, going back to what we were talking about, let's say our given vector v is with respect to the above figure and that it's pointing northwest - specifically at (x,y,z) = (1,0,1). If we use this rotation matrix and rotate this point, we should get it to be parallel to the xz plane and pointing in the opposite direction, and so we should get (x,y,z) = (-1,0,1):
>> M*[1;0;1]
ans =
-1.0000
0.0000
1.0000
You can also get the same result by using the Rodrigues Rotation formula:
>> v = [1;0;1];
>> k = [0;0;1];
>> theta = pi;
>> vrot = v*cos(theta) + cross(k,v)*sin(theta) + k*(k.'*v)*(1-cos(theta))
vrot =
-1.0000
0.0000
1.0000
All in all, it's just another way of rotating a vector around an arbitrary axis, not just limited to the standard x, y or z.
I have extracted Rotation and Translation matrices from the essential matrix. The translation vector has a scale ambiguity. Therefore, I couldn't define its "true" values.
My steps were as follow:
F=estimateF(matches1,matches2,'RANSAC')
E=K2'*F*K1
[U S V]=svd(E)
s=(S(1,1)+S(2,2))/2
S=diag([s s 0])
E_new=U*S*V'
[U S V]=svd(E_new);
R1=U*W*V'
R2=U*W'*V';
t1=U(:,3);
t2=-t1
My problem is how to define the translation of the second camera from the first one in mm.
Unless you know some more information that ties your points to the real world, it's not possible to recover the absolute scale.
For example, if the matches where corners of squares of a calibration chessboard of which you know the size in mm, then you would be able to know how far cameras are from each other in mm.
Given a matrix nx3 that represents n points in 3D space. All points lie on a plane. The plane is given by its normal and a point lying on it. Is there a Matlab function or any Matlabby way to find the area directly from the matrix?
What i was trying to do is write a function that first computes the centroid,c, of the n-gon. Then form triangles : (1,2,c),(2,3,c),...,(n,1,c). Compute their area and sum up. But then i had to organise the polygon points in a cyclic order as they were unordered which i figured was hard. Is there a easy way to do so?
Is there a easier way in Matlab to just call some function on the matrix?
Here is perhaps an easier method.
First suppose that your plane is not parallel to the z-axis.
Then project the polygon down to the xy-plane simply by removing the 3rd coordinate.
Now compute the area A' in the xy-plane by the usual techniques.
If your plane makes an angle θ with the xy-plane, then your 3D
area A = A' / cos θ.
If your plane is parallel to the z-axis, do the same computation
w.r.t. the y-axis instead, projecting to the xz-plane.
To project from 3D to the plane normal to N, take some non-parallel vector A and compute the cross products U = N x A and V = N x U. After normalizing U and V, the dot products P.U and P.V give you 2D coordinates in the plane.
Joseph's solution is even easier (I'd recommend to drop the coordinate with the smallest absolute cosine).
You said the points all lie on a plane and you have the normal. You should then be able to reproject the 3-D points into 2-D coordinates in a new 2-D basis. I am not aware of a canned function in Matlab to do this , but coding it should not be difficult, this answer from Math.SE and this Matlab Central post should help you.
If you already solved the problem of finding the coordinates of the points in the 2-D plane they are in, you could use the Matlab boundary or convex hull function to compute the area of the boundary or convex hull enclosing the points.
[k,v]= boundary(x,y)
or
[k,v] =convhull(x,y)
where k is the vector of indices into points x,y, that define the boundary or convex hull, v is the area enclosed, and x, y are vectors of the x and y coordinates of your points.
What you were describing with trying to find triangles with the points sounds like a first attempt toward Delaunay triangulation. I think more recent versions of Matlab have functions to do Delaunay triangulation as well.
This question entails rotating an image given as a 3D matrix where the first dimension is width, 2nd dimension is height, and 3rd dimension contains x,y,z coordinates.
Currently I am plotting a surface using the following code
Fig.sub1im=surf(ToFparam.ROI.XYZ(:,:,1),ToFparam.ROI.XYZ(:,:,2),ToFparam.ROI.XYZ(:,:,3),zeros(ToFparam.ROI.height,ToFparam.ROI.width,3));
Now, I have a 3-D matrix where it's a 100x50x3. All x data is in the first...page or layer of the 3rd dimension, y is the second layer...z the third. Now I need to apply a 3x3 rotation matrix on the x y and z data. I know how to reshape a matrix to do this I think....just put it into a 3 row by...50000 column matrix then apply the matrix.
Next I need to update my plot in a loop. I was going to then do the following, where I have also included my new matrix calculation.
ToFparam.ROI.XYZ_Vector = ToFparam.ROI.XYZ;
ToFparam.ROI.XYZ_Vector = reshape(ToFparam.ROI.XYZ, [size(ToFparam.ROI.XYZ,1)*size(ToFparam.ROI.XYZ,2),3]);
ToFparam.ROI.XYZ_Vector = ToFparam.ROI.XYZ_Vector';
ToFparam.ROI.XYZ_DICOM = inv(DICOMparam.calib.navi2dicom(1:3,1:3))*inv(Naviparam.data.Endo_RefHomMat(1:3,1:3))*ToFparam.ROI.XYZ_Vector;
%refresh plot standard cuts
set(Fig.sub1im,'CData', Color);
set(Fig.sub1im, 'XData', ToFparam.ROI.XYZ_DICOM(1,:) + DICOMPos(1)/Fig.sub2samp);
set(Fig.sub1im, 'YData', ToFparam.ROI.XYZ_DICOM(2,:) + DICOMPos(2)/Fig.sub2samp);
set(Fig.sub1im, 'ZData', ToFparam.ROI.XYZ_DICOM(3,:) + DICOMPos(3)/Fig.sub2samp);
When I update my plot, I get no errors, but it doesn't look like it's plotting it correctly. It seems like it makes a huge offset on my data and positions it somewhere I don't want it to be positioned. I wouldn't expect that a rotation matrix effects the scaling, just the orientation. Let me know if there are any faster/better ways to accomplish this surf plot, thanks!
For anyone interested, I have found the solution.
If you ever want to rotate a 3-D matrix where the x,y,z data is located in the 3rd dimension, use the following for maximum speed and efficiency.
[m,n,z]=size(inMatrix);
outMatrix=reshape((A*(reshape(double(inMatrix),[m*n 3]))')',[m n 3]);
Where inMatrix is your initial 3d matrix, outMatrix is your output 3d matrix, and A is your rotation matrix. Can be extremely useful when wanting to code rotations in images, since images will have 2 dimensions along their width and height, and the 3rd dimension will be the x, y and z coordinates. This will allow you to easily plot an initial data set, rotate it, then replot it.
I have a 3D matrix of data in matlab, but I want to extract an arbitrarily rotated slice of data from that matrix and store it as a 2D matrix, which I can access. Similar to how the slice() function displays data sliced at any angle, except I would also like to be able to view and modify the data as if it were an array.
I have the coordinates of the pivot-point of the plane as well as the angles of rotation (in x, y and z axis), I have also calculated the equation of the plane in the form:
Ax + By + Cz = D
and can extract a 3D matrix containing only the data that fall on that plane, but I don't know how to then convert that into a simple 2D array.
Another way of doing it would be to somehow rotate the source matrix in the opposite direction of the angle of the plane, so as to line up the plane of data with the XY axis, and simply extract that portion of the matrix, but I do not know if rotating a matrix like that is possible.
I hope this hasn't been answered elsewhere, I've been googling it all day, but none of the problems seem to exactly match mine.
Thanks
You can take a look at the code here. I think the function is similar to what you are trying to solve.
The function extracts an arbitrary plane from a volume given the size of the plane, the center point of the plane, and the plane normal, i.e. [A,B,C]. It also outputs the volumetric index and coordinate of each pixel on the plane.
Aha! May have just solved it myself.
To produce the plane equation I rotate a normal vector of (0,0,1) using rotation matrices and then find D. If I also rotate the following vectors:
(1,0,0) //step in the x direction of our 2D array
and
(0,1,0) //step in the y direction of our 2D array
I'll have the gradients that denote how much my coordinates in x,y,z have to change before I step to the next column in my array, or to the next row.
I'll mock this up ASAP and mark it as the answer if it works
EDIT: Ok slight alteration, when I'm rotating my vectors I should also rotate the point in 3D space that represents the xyz coordinates of x=0,y=0,z=0 (although I'm rotating around the centre of the structure, so it's actually -sizex/2,-sizey/2,-sizez/2, where size is the size of the data, and then I simply add size/2 to each coordinate after the rotations to translate it back to where it should be).
Now that I have the gradient change in 3D as I increase the x coordinate of my 2D array and the gradient change as I increase the y coordinate, I can simply loop through all possible x and y coordinates (the resulting array will be 50x50 for a 50x50x50 array, I'm not sure what it will be for irregular sizes, which I'll need to work out eventually) in my 2D array and calculate the resulting 3D coordinates on my plane in the data. My rotated corner value serves as the starting point. Hooray!
Just got to work out a good test for this encompassing all angles and then I'll approve this as an answer