I need some help with programming the calculation of displacement. Given the distance the object has moved in the XY plane, and the yaw (heading) of the object, I'd like to calculate the displacement of the object. For example, the object moved 5m North and 2m East.
The data I have is the distance it travels in the XY plane (X distance and Y distance) and the heading of the device which is determined by the direction of X. I know how to calculate it on paper but I am having trouble to program it.
I've attached two examples that shows how I obtained the calculations. The black dot is the displacement of the object. In the 1st example, the X-distance travelled is 3 meters while the Y-distance is 4 meters and the heading of the X-axis is 70°. After calculating, I managed to get that it has travelled 2.733m South and 4.187m East. In example 2, it travel, the X-distance is -5m and the Y-distance is -12m, the heading is 160°. I calculated that the object travels 8.80m North and 9.566m East.
Example 1
Example 2
Assume you have your XY pane rotated by some angle Zp and have a 2D movement vector within that pane, let's say its direction rotated by Zv – what is total rotation of the vector, relative to the global orientation? Zp + Zv...
So all you need to do is applying the rotation Zp to the movement vector within XY-pane, i. e. apply a rotation matrix to the vector. This matrix for a rotation by z looks like:
cos(z) -sin(z)
sin(z) cos(z)
With our data above:
north = cos(Zp) * x - sin(Zp) * y
east = sin(Zp) * x + cos(Zp) * y
which gives, with your first sample data applied:
north = cos(70°) * 3 - sin(70°) * 4 ~ -2.7327
east = sin(70°) * 3 + cos(70°) * 4 ~ 4.1872
north = cos(160°) * -5 - sin(160°) * -12 ~ 8.8027
east = sin(160°) * -5 + cos(160°) * -12 ~ 9.5662
Corresponding pretty much to the values you calculated (note: negative movement to north is positive movement towards south...).
What you are doing is a conversion from polar to Cartesian coordinates. https://en.wikipedia.org/wiki/Polar_coordinate_system#Converting_between_polar_and_Cartesian_coordinates
If your Cartesian coordinates are not aligned to the polar axis, just compensate by adding the rotation angle to the polar argument.
I iterate through a 100x100 array and pick every time four neighbours (one left of the center node, one above, one right and one below), like in the picture below
the red one is the center node and the blue ones are the neighbours. I struggle to find a convenient way in MATLAB to pick randomly one of the neighbours.
The following assumes that
Each entry is replaced by one of its original neighbours, independently of what happens to other entries.
Each neighbour has the same probability of being picked.
Neighbourhood is defined cyclically. Thus, for example, in the first column the "left" neighbour belongs to the last column.
The code builds a cyclically extended matrix for convenience, and then uses linear indexing to (randomly) select the neighbours.
x = [10 20 30 40; 50 60 70 80; 90 100 110 120]; % example data
x_ext = x([end 1:end 1], [end 1:end 1]); % cyclically extended matrix
ind = bsxfun(#plus, (2:size(x,1)+1).', (1:size(x,2))*(size(x,1)+2)); % linear indices
% of the original matrix in the extended matrix
delta = [-1 1 -size(x_ext,1) size(x_ext,1)]; % possible displacements for neighbours,
% as linear indices
r = delta(randi(4, size(x))); % generate random displacements
result = x_ext(ind + r); % pick neighbours in extended matrix
Example:
>> x
x =
10 20 30 40
50 60 70 80
90 100 110 120
>> result
result =
20 30 70 30
90 100 60 120
50 110 70 40
Here in this code, 50 random coordinates are generated(population size 50).
I have an array,
B = [150 90; -100 -120; -80 130; 140 -70; 60 120; -90 -130].
I want to calculate the distance of each 50 coordinates from each of the coordinates of B array. After calculating the distance, I have to preserve all of the distance values separately in an array(or matrix) to retrieve them afterwards.
Please help me to calculate the distance.
clear all
clc
%Common Parameter Setting
N=2; % Number of variables
M=50; % Populations size 50
F=0.5; % Mutation factor
C=0.9; % Crossover rate
I_max=20; % Max iteration time
Run=1; % The number of test time
X_max=[100,100];
X_min=[-100,-100];
%Func=#Rastrigin;
% 2.The whole test loop
for r=1:Run
iter=0;
% 1.Generate MxN matrix
for m=1:M
for n=1:N
X(m,n)=X_min(n)+rand()*(X_max(n)-X_min(n));
end
fprintf('value of X:');
disp(X);
end
end
I did not completely understand the last part. But if you have 2 lists of coordinates, say
x = randn(10,2); %10 points in 2D
y = randn(3,2); % 3 others points in 2D
and you want the pairwise distance between all points in x and all points in y, you can use pdist2
D = pdist2(x,y);
Now D(1,2) will be the Euclidean distance from x(1) to y(2) and so on.
I want to extract the two points (i.e their values) which are marked with black outline in figure. These minima points are 2 and 5. Then after extraction these marked points coordinates I want to calculate the distance between them.
The code that I am using to plot average values of image, calculate minimas and locations is
I1=imread('open.jpg');
I2=rgb2gray(I1);
figure, title('open');
plot(1:size(I2,1), mean(I2,2));
hold on
horizontalAverages = mean(I2 , 2);
plot(1:size(I2,1) , horizontalAverages)
[Minimas locs] = findpeaks(-horizontalAverages)
plot(locs , -1*Minimas , 'r*')
Minima
-86.5647
-80.3647
-81.3588
-106.9882
-77.0765
-77.8235
-92.2353
-106.2235
-115.3118
-98.3706
locs =
30
34
36
50
93
97
110
121
127
136
It is a bit unclear from your question what you are actually looking for, but the following one liner will get you the local minima:
% Some dummy data
x = 1:11;
y = [3 2 1 0.5 1 2 1 0 1 2 3];
min_idx = ([0 sign(diff(y))] == -1) & ([sign(diff(y)) 0] == 1);
figure
plot(x, y);
hold on;
scatter(x(min_idx), y(min_idx))
hold off;
Use the 'findpeaks' function, if you have the signal processing toolbox.
[y,locs]=findpeaks(-x)
will find the local minima. This function has a ton of options to handle all kinds of special cases, so is very useful.
I have a 3-dimensional array. Think of it as a brick. There are 24 possible rotations of this brick (that keep its edges parallel to coordinate axes). How do I generate all corresponding 3-dimensional arrays?
A die (half a pair of dice) is handy for observing the 24 different orientations, and can suggest operation sequences to generate them. You will see that any of six faces can be uppermost, and the sides below can be rotated into four different cardinal directions. Let us denote two operations: “turn” and “roll”, where turn rotates the die about the z axis from one cardinal to the next, and roll rotates the die 90° away from you, so the away-face becomes the bottom face and the near face the top. These operations can be expressed using rotation matrices as mentioned in the answer of Felipe Lopes, or can be expressed as simple functions that when given (x,y,z) return (-y,x,z) or (x,z,-y), respectively.
Anyhow, if you place the die with 1 on the near face, 2 at right, and 3 on top, you will find that the following sequence of steps generates the twelve different orientations with 1, 2, or 3 spots on top: RTTTRTTTRTTT. Then the sequence RTR exposes 6, 4, 5 where 1, 2, 3 originally were, and a repeat of the sequence RTTTRTTTRTTT generates the twelve orientations with 4, 5, or 6 spots on top. The mentioned sequence is embedded in the following python code.
def roll(v): return (v[0],v[2],-v[1])
def turn(v): return (-v[1],v[0],v[2])
def sequence (v):
for cycle in range(2):
for step in range(3): # Yield RTTT 3 times
v = roll(v)
yield(v) # Yield R
for i in range(3): # Yield TTT
v = turn(v)
yield(v)
v = roll(turn(roll(v))) # Do RTR
p = sequence(( 1, 1, 1))
q = sequence((-1,-1, 1))
for i in sorted(zip(p,q)):
print i
The rationale for printing out a sorted list of transformed pairs of points is twofold: (i) any face orientation can be specified by the locations of two of its corners; (ii) it then is easy to check for uniqueness of each pair, eg by piping output to uniq.
Here is how the sorted output begins:
((-1, -1, -1), (-1, 1, 1))
((-1, -1, -1), (1, -1, 1))
((-1, -1, -1), (1, 1, -1))
((-1, -1, 1), (-1, 1, -1))
((-1, -1, 1), (1, -1, -1))
((-1, -1, 1), (1, 1, 1))
((-1, 1, -1), (-1, -1, 1))
((-1, 1, -1), (1, -1, -1))
((-1, 1, -1), (1, 1, 1))
Let X rotate 90 degrees around the X-axis and Y rotate 90 degrees around the Y-axis then the 24 possible unique combinations are (all possible combinations up to 5 rotations are given except those with four times the same rotation (eg XXXX, XXXXY XYYYY, etc):
1. I
2. X
3. Y
4. XX = YXXY
5. XY
6. YX
7. YY = XYYX
8. XXX = XYXXY = YXXYX = YXYXY = YYXYY
9. XXY = YXXYY = YYYXX
10. XYX = YXY
11. XYY = XXYYX = YYXXX
12. YXX = XXYYY = YYXXY
13. YYX = XXXYY = XYYXX
14. YYY = XXYXX = XYXYX = XYYXY = YXYYX
15. XXXY
16. XXYX = XYXY = YXYY
17. XXYY = YYXX
18. XYXX = YXYX = YYXY
19. XYYY
20. YXXX
21. YYYX
22. XXXYX = XXYXY = XYXYY = YXYYY
23. XYXXX = YXYXX = YYXYX = YYYXY
24. XYYYX = YXXXY
Of course you can use any two 90 degree rotations in place of the X and Y. For example, Y and Z.
Or, if you also use Z, a 90 degree rotation around the Z axis then 4 rotations suffice:
1. I
2. X = YXZ
3. Y = ZYX
4. Z = XZY
5. XX = XYXZ = YXXY = YXYZ = YXZX = YYZZ = YZXZ = ZXXZ = ZZYY
6. XY = YZ = ZX = XZYX = YXZY = ZYXZ
7. XZ = XXZY = YXZZ = YYYX = ZYYY
8. YX = XZZZ = YYXZ = ZYXX = ZZZY
9. YY = XXZZ = XYYX = YZYX = ZXYX = ZYXY = ZYYZ = ZYZX = ZZXX
10. ZY = XXXZ = XZYY = YXXX = ZZYX
11. ZZ = XXYY = XYZY = XZXY = XZYZ = XZZX = YYXX = YZZY = ZXZY
12. XXX
13. XXY = XYZ = XZX = YZZ = ZXZ
14. XXZ = ZYY
15. XYX = YXY = YYZ = YZX = ZXX
16. XYY = YZY = ZXY = ZYZ = ZZX
17. XZZ = YYX
18. YXX = ZZY
19. YYY
20. ZZZ
21. XXXY = XXYZ = XXZX = XYZZ = XZXZ = YZZZ = ZXZZ = ZYYX
22. XXYX = XYXY = XYYZ = XYZX = XZXX = YXYY = YYZY = YZXY = YZYZ = YZZX = ZXXY = ZXYZ = ZXZX = ZYZZ = ZZXZ
23. XYXX = XZZY = YXYX = YYXY = YYYZ = YYZX = YZXX = ZXXX
24. XYYY = YXXZ = YZYY = ZXYY = ZYZY = ZZXY = ZZYZ = ZZZX
These 24 matrices all exist of three column vectors that each exist of two zeroes and a minus one or plus one. On every row there are also exactly two zeroes. As such, they can easily be generated: the first column vector has six possibilities ((1,0,0), (-1,0,0), (0,-1,0), (0,1,0), (0,0,-1) and (0,0,1)), this corresponds to moving the positive X-axis to the positive or negative x, y or z axis. The second column vector only has four possibilities because it must contain a zero where the first column has a non-zero value. Finally the third column vector has only one place left where its plus or minus one can be. This gives 6 * 4 * 2 = 48 matrices, half of them mirror the original as well however (they are combination of a mirror and optionally a rotation). Hence only 24 are pure rotations. The matrices that are mirror operations will have a determinant equal to -1, the determinant of the pure rotations is 1.
James Waldby's answer is inspiring, and I want to add a slightly improved version with only two for-loops.
We know that there are 24 unique orientations. I calculated this by imagining a dice: there are 6 possible choices for the top face, and 4 possible rotations for each face on top.
What if we iterate with that idea? I thought. If we can figure out a way to travel all 6 faces of the dice, then we only need to observe the 4 rotations on each face, and we are done!
So I grabbed the nearest "brick" (in my case, a Vitasoy carton) and started rotating to see what would be the easiest pattern to visit all 6 faces. If we introduce an additional counter-clockwise turn, such that our operations are:
Roll (in a fixed direction, e.g. so that the face facing you is now rotated downwards)
Turn CW (along a fixed axis, e.g. so that the face facing you is turned clockwise, but still facing you)
Turn CCW (along the same axis as the last one)
Then we can visit all faces by doing:
Roll -> Turn CW -> Roll -> Turn CCW -> Roll -> Turn CW -> Roll -> Turn CCW -> Roll -> Turn CW -> Roll -> Turn CCW
With the last roll and turn, we are back to the original orientation. As you can see, it is a repeated sequence of roll + alternating CW turns and CCW turns.
Now, if we expand this to include all rotations of each face we visit, this becomes:
Roll -> 3x Turn CW -> Roll -> 3x Turn CCW -> Roll -> 3x Turn CW -> Roll -> 3x Turn CCW -> Roll -> 3x Turn CW -> Roll -> 3x Turn CCW
...and we are back to where we started! This can be translated into two for-loops (one fewer!):
def sequence(m):
for roll_index in range(6):
m = roll(m)
yield(m)
for turn_index in range(3):
m = turn_cw(m) if roll_index % 2 == 0 else turn_ccw(m)
yield(m)
You can use rotation matrices. Rotating a 3D array around the x-axis means that the element at position (i,j,k) will be mapped to position (i,-k,j). Of course, if your array is 0-indexed, you probably have to replace -k with size-1-k or something like that.
Similarly, rotating around the y-axis maps (i,j,k) to (k,j,-i). These two rotations can be represented as matrices. For the x-axis rotation:
|i'| |1 0 0| |i|
|j'| = |0 0 -1|*|j|
|k'| |0 1 0| |k|
And for the y-axis rotation:
|i'| |0 0 1| |i|
|j'| = |0 1 0|*|j|
|k'| |-1 0 0| |k|
Any general rotation can be described as a sequence of those two rotations. Applying two rotations consecutively is just multiplying the 3x3 matrices. So, if you find all possible products of them, you'd get 24 matrices (including the identity), each one corresponds to a valid rotation of your array. It's a little tricky to find all possible multiplications, because they don't commute.
I think you can just brute-force all products of the form (A^p)*(B^q)*(A^r)*(B^s), where A and B are the two matrices before and p,q,r,s are their powers, and range from 0 to 3 (exponentiating A or B to 4 will take them back to the identity matrix).
Doing it this way, you can generate all 24 valid rotation matrices, and rotate the 3D array using each one of them, taking the care to shift the negative indexes so that you don't access out of bounds.
import numpy as np
def rotations(array):
for x, y, z in permutations([0, 1, 2]):
for sx, sy, sz in itertools.product([-1, 1], repeat=3):
rotation_matrix = np.zeros((3, 3))
rotation_matrix[0, x] = sx
rotation_matrix[1, y] = sy
rotation_matrix[2, z] = sz
if np.linalg.det(rotation_matrix) == 1:
yield np.matmul(rotation_matrix, array)
all_rotations = list(rotations(np.array(array)))
Idea is to generate all coordinates relabelings with possible axis' direction changes, ex. (-z, y, x). The question that remains is whether all coordinates relabelings are obtainable from (x, y, z) axes using only rotations. Half of the 6 * (2^3) = 48 labelings aren't because they are rotations of a mirrored version of the (x, y, z) cooridnates (left-handed coordinates, https://en.wikipedia.org/wiki/Right-hand_rule).
Rows of the corresponding rotation matrix A of relabeling operation will have only one value in each row. The value determines which axis to select on that index, and whether to flip the axis.
A * (x, y, z) = (-z, y, x)
| 0, 0, -1 |
A = | 0, 1, 0 |
| 1, 0, 0 |
We keep only those rotations, whose det(A) == 1 meaning that only rotations were applied by the operation. det(A) == -1 means that it is a rotation with mirroring.