I know that there is set_origin_pose to shift a pose in X/Y/Z.
But I was not able to to rotate a pose along its own X Y or Z axis. I cant simply add an angle to the pose's values, because they refer to the camera's coordinates.
How can a pose be rotated?
Solved by converting the pose to a mat3d, rotating the mat with hom_mat3d_rotate_local and then converting back to a pose:
*shift base pose
set_origin_pose (CalculationPose, X1 ,0, Y1, CalculationPose)
disp_3d_coord_system(3600, CameraParam, CalculationPose, 0.1)
*rotate base pose
pose_to_hom_mat3d(CalculationPose, CalculationMat)
hom_mat3d_rotate_local(CalculationMat, -AngleRad , 'y',CalculationMatRotated)
hom_mat3d_to_pose(CalculationMatRotated, CalculationPose)
Related
I have a stationary camera, which is 640mm above the ground and is tilted slightly forward (about the x-axis?) roughly 30 degrees, so it's looking slightly down towards a flat ground plane.
My goal is to determine the distance from the camera to any small objects that it detects on the ground, for example, if my camera detects an object which is located at pixel [314, 203], I would like to know where on the ground that object would be in world coordinates [x, y, z] with y=0, and the distance to that object.
I've drawn a diagram to better visualize the problem:
camera plane diagram
I have my rotation matrix and translation vector and also my intrinsic matrix, but I'm not sure 1) if the rotation matrix and translation vector are correct given the information/diagram above, and 2) how to proceed with figuring out a mathematical formula for finding distance and real world location. Here is what I have so far:
Rotation matrix R (generated here https://www.andre-gaschler.com/rotationconverter/) from the orientation [-30, 0, 0] (degrees)
R =
[ 1.0000000, 0.0000000, 0.0000000;
0.0000000, 0.8660254, -0.5000000;
0.0000000, 0.5000000, 0.8660254 ]
Camera is 640mm above ground plane
t =
[0, 640, 0]
Intrinsic matrix from calibration information given by camera
fx=349.595, fy=349.505, cx=328.875, cy=178.204
K =
[ 349.595, 0.0000, 328.875;
0.0000, 349.505, 178.204;
0.0000, 0.0000, 1.000 ]
I also have these distortion parameters, I'm unsure what to do with them or if they are relevant to K
k1=-0.170901, k2=0.0255027, k3=-9.30328e-11, p1=0.000117187, p2=6.42836e-05
I get about this far and then I'm lost, any help would be much appreciated.
Also sorry if this is a lot of information or if it's confusing in any way, I'm very much a beginner when it comes to projection matrices
UPDATE:
After some more research and testing on my own I found a formula that seems to give me a somewhat decent approximation. Given pixel [x, y], I find (I think) the direction vector from the camera origin to the pixel coordinate with:
dir_x = (x - cx) / fx
dir_y = (y - cy) / fy
dir_z = 1
which I then multiply by rotation matrix R, which gives me the real-world vector. I then divide my camera height (640mm) by the y-value of that vector, which gives me (I think) the distance to the specified pixel in the real-world. After some testing and measuring by hand, this seems to be an adequate method for finding the distance, but I'm not sure if I'm missing steps for accuracy or if I'm actually doing this completely wrong.
Again, any insight is greatly appreciated.
Let's suppose:
1) XY plane is perpendicular to the surface of the street given by an area between the two green lines and a normal to this plane has the same "direction" as the yellow line,
2) XY has only three degrees of freedom - moving along X, Y, Z (Z is X x Y),
3) a limit for an X- along translation is when P1 is coincident to P2.
How does the θ change while translating by an [x,y,z] vector? How would it change if I added a rotation along any of the given axes and got rid of the point 3) (6 degrees of freedom)? Can you give me any hint where to look for an answer?
Hope everything is clear!
I found a code about image rotation in frequency domain. But I couldn't understand this code. This code works correct. Can anyone describe this code? Actually I have to write a code to rotate an image in frequency domain in polar coordinates. Do you think this code
meet the requirements.
clear;
img=imread('cameraman.tif');
imshow(img); title('original image');
theta=26,5;
N=size(img,1);
M=size(img,2);
fimg=fftshift(fft2(fftshift(img)));
p=ones(N,1)*[-N/2:(N-1)/2]; % horizontal axis
q=-p'; % vertical axis
theta=2*pi*theta/360;
g=1/(N^2).*fimg;
z1=exp(i*pi/N.*((p.^2-q.^2)*cos(theta)-2*p.*q*sin(theta)));
z2=exp(-i*pi/N.*((p.^2-q.^2)*cos(theta)-2*p.*q*sin(theta)));
k=ifft2(fft2(g.*z1).*(fft2(z2)));
figure,
imshow(abs(fftshift(flipud(k))), [0 255]);
title(['Cameraman rotated at ' num2str(theta*360/(2*pi)) ' Degrees']); axis off
As you can see here.
For the rotation, there is no properties. But this code using the shift properties to apply the rotation.
In z1, you have the coordinate (p,q) compute normaly as well, but just by applying the 2D rotation matrix to your coordinate, you can use the shift properties.
And notice, this code change a the sign every where, that why there no minus in z1 instead of z2.
Rotation : [cos(theta) -sin(theta);
sin(theta) cos(theta)];
Unity3D offers the following method:
Rotate(eulerAngles: Vector3, relativeTo: Space = Space.Self);
For example, this will rotate the object around it's local X axis:
transform.Rotate(Vector3(50,0,0) * Time.deltaTime, Space.Local);
If I first rotate 90 degrees around it's local y axis (which is up in unity) and then rotate it around the X axis relative to World, it will basically rotate around the local Z axis, ie:
//setup
transform.Rotate(Vector3(0, 90, 0));
//on update
transform.Rotate(Vector3(50,0,0) * Time.deltaTime, Space.World);
In my own implementation, using quaternions, I have the local rotation implemented, which was easy.
//rotate around local axis
currentRotation *= rotateQuat;
How would I go about implementing the relative to world behaviour using quaternions?
There's probably a way to do it in Unity without explicit calculations, but...
When using quaternions for rotation, it makes more sense to think in terms of a rotation (angle a) around a specific axis (unit vector u) rather than using Euler angles; the quaternion itself (actually a unit quaternion, or "versor") can be represented as a 4-vector (w, x, y, z), where w = cos(0.5*a) and (x, y, z) = (u_x, u_y, u_z) * sin(0.5*a) (meaning a = 2 * arccos(w) and u = (x, y, z) / sin(0.5*a) = (x, y, z) / sin(arccos(w)) ). Following from this, the "identity" quaternion (i.e. no rotation) is (1, 0, 0, 0), as cos(0) = 1 and sin(0) = 0, and very usefully, the inverse/conjugate of a quaternion, (w, -x, -y, -z), represents the opposite rotation (x, y, and z are technically imaginary components; you can also represent a quaternion as w+x*i+y*j+z*k). To apply a rotation to a point p in 3D space using a quaternion, you use p' = (w, x, y, z)*(0, p_x, p_y, p_z)*(w, -x, -y, -z), where the multiplication is performed as shown here: http://en.wikipedia.org/wiki/Quaternion#Ordered_list_form
Thus, performing a world-relative rotation with a quaternion given an already-existing rotation represented by a quaternion is relatively simple if you know what the angle and axis of rotation (in world space) are for the new rotation, as the main addition to the computation is to apply the conjugate of the existing quaternion to the axis of rotation for the new quaternion in order to represent that axis in local/object space; then you just create the quaternion for the new rotation and apply it to the existing one (I'm not sure how Unity orders quaternion composition, but normally if you apply q1 followed by q2 the composed quaternion would be q2*q1 [quaternion multiplication is non-commutative], so it should be something like resultQuat = newQuat * prevQuat;).
I'm using Electro in Lua for some 3D simulations, and I'm running in to something of a mathematical/algorithmic/physics snag.
I'm trying to figure out how I would find the "spin" of a sphere of a sphere that is spinning on some axis. By "spin" I mean a vector along the axis that the sphere is spinning on with a magnitude relative to the speed at which it is spinning. The reason I need this information is to be able to slow down the spin of the sphere by applying reverse torque to the sphere until it stops spinning.
The only information I have access to is the X, Y, and Z unit vectors relative to the sphere. That is, each frame, I can call three different functions, each of which returns a unit vector pointing in the direction of the sphere model's local X, Y and Z axes, respectively. I can keep track of how each of these change by essentially keeping the "previous" value of each vector and comparing it to the "new" value each frame. The question, then, is how would I use this information to determine the sphere's spin? I'm stumped.
Any help would be great. Thanks!
My first answer was wrong. This is my edited answer.
Your unit vectors X,Y,Z can be put together to form a 3x3 matrix:
A = [[x1 y1 z1],
[x2 y2 z2],
[x3 y3 z3]]
Since X,Y,Z change with time, A also changes with time.
A is a rotation matrix!
After all, if you let i=(1,0,0) be the unit vector along the x-axis, then
A i = X so A rotates i into X. Similarly, it rotates the y-axis into Y and the
z-axis into Z.
A is called the direction cosine matrix (DCM).
So using the DCM to Euler axis formula
Compute
theta = arccos((A_11 + A_22 + A_33 - 1)/2)
theta is the Euler angle of rotation.
The magnitude of the angular velocity, |w|, equals
w = d(theta)/dt ~= (theta(t+dt)-theta(t)) / dt
The axis of rotation is given by e = (e1,e2,e3) where
e1 = (A_32 - A_23)/(2 sin(theta))
e2 = (A_13 - A_31)/(2 sin(theta))
e3 = (A_21 - A_12)/(2 sin(theta))
I applaud ~unutbu's, answer, but I think there's a simpler approach that will suffice for this problem.
Take the X unit vector at three successive frames, and compare them to get two deltas:
deltaX1 = X2 - X1
deltaX2 = X3 - X2
(These are vector equations. X1 is a vector, the X vector at time 1, not a number.)
Now take the cross-product of the deltas and you'll get a vector in the direction of the rotation vector.
Now for the magnitude. The angle between the two deltas is the angle swept out in one time interval, so use the dot product:
dx1 = deltaX1/|deltaX1|
dx2 = deltax2/|deltaX2|
costheta = dx1.dx2
theta = acos(costheta)
w = theta/dt
For the sake of precision you should choose the unit vector (X, Y or Z) that changes the most.