Related
I currently have a stereo camera setup. I have calibrated both cameras and have the intrinsic matrix for both cameras K1 and K2.
K1 = [2297.311, 0, 319.498;
0, 2297.313, 239.499;
0, 0, 1];
K2 = [2297.304, 0, 319.508;
0, 2297.301, 239.514;
0, 0, 1];
I have also determined the Fundamental matrix F between the two cameras using findFundamentalMat() from OpenCV. I have tested the Epipolar constraint using a pair of corresponding points x1 and x2 (in pixel coordinates) and it is very close to 0.
F = [5.672563368940768e-10, 6.265600996978877e-06, -0.00150188302445251;
6.766518121363063e-06, 4.758206104804563e-08, 0.05516598334827842;
-0.001627120880791009, -0.05934224611334332, 1];
x1 = 133,75
x2 = 124.661,67.6607
transpose(x2)*F*x1 = -0.0020
From F I am able to obtain the Essential Matrix E as E = K2'*F*K1. I decompose E using the MATLAB SVD function to get the 4 possibilites of rotation and translation of K2 with respect to K1.
E = transpose(K2)*F*K1;
svd(E);
[U,S,V] = svd(E);
diag_110 = [1 0 0; 0 1 0; 0 0 0];
newE = U*diag_110*transpose(V);
[U,S,V] = svd(newE); //Perform second decompose to get S=diag(1,1,0)
W = [0 -1 0; 1 0 0; 0 0 1];
R1 = U*W*transpose(V);
R2 = U*transpose(W)*transpose(V);
t1 = U(:,3); //norm = 1
t2 = -U(:,3); //norm = 1
Let's say that K1 is used as the coordinate frame for which we make all measurements. Therefore, the center of K1 is at C1 = (0,0,0). With this it should be possible to apply the correct rotation R and translation t such that C2 = R*(0,0,0)+t (i.e. the center of K2 is measured with respect to the center of K1)
Now let's say that using my corresponding pairs x1 and x2. If I know the length of each pixel in both my cameras and since I know the focal length from the intrinsic matrix, I should be able to determine two vectors v1 and v2 for both cameras that intersect at the same point as seen below.
pixel_length = 7.4e-6; //in meters
focal_length = 17e-3; //in meters
dx1 = (133-319.5)*pixel_length; //x-distance from principal point of 640*480 image
dy1 = (75-239.5) *pixel_length; //y-distance from principal point of 640*480 image
v1 = [dx1 dy1 focal_length] - (0,0,0); //vector found using camera center and corresponding image point on the image plane
dx2 = (124.661-319.5)*pixel_length; //same idea
dy2 = (67.6607-239.5)*pixel_length; //same idea
v2 = R * ( [dx2 dy2 focal_length] - (0,0,0) ) + t; //apply R and t to measure v2 with respect to K1 frame
With this vector and knowing the line equation in parametric form, we can then equate the two lines to triangulate and solve the two scalar quantities s and t through the left hand divide function in MATLAB to solve for the system of equations.
C1 + s*v1 = C2 + t*v2
C1-C2 = tranpose([v2 v1])*transpose([s t]) //solve Ax = B form system to find s and t
With s and t determined we can find the triangulated point by plugging back into the line equation. However, my process has not been successful as I cannot find a single R and t solution in which the point is in front of both cameras and where both cameras are pointed forwards.
Is there something wrong with my pipeline or thought process? Is it at all possible to obtain each individual pixel ray?
When you decompose the essential matrix into R and t you get 4 different solutions. Three of them project the points behind one or both cameras, and one of them is correct. You have to test which one is correct by triangulating some sample points.
There is a function in the Computer Vision System Toolbox in MATLAB called cameraPose, which will do that for you.
Should it be not C1-C2 = transpose([v2 -v1] * transpose([t s]). This works.
Checked your code and found that the determinants of both R1 and R2 are -1, which is incorrect because as a rotation matrix R should have a determinant equal to 1. Just take R=-R and try again.
I've studied "Simulating Ocean Water" article by Jerry Tessendorf and tried to program the Statistical Wave Model but I didn't get correct result and I don't understand why.
In my program I tried only to create a wave height field at time t = 0 without any further changes in time. After execution of my program I got not what I was expecting:
Here's my source code:
clear all; close all; clc;
rng(11); % setting seed for random numbers
meshSize = 64; % field size
windDir = [1, 0]; % ||windDir|| = 1
patchSize = 64;
A = 1e+4;
g = 9.81; % gravitational constant
windSpeed = 1e+2;
x1 = linspace(-10, 10, meshSize+1); x = x1(1:meshSize);
y1 = linspace(-10, 10, meshSize+1); y = y1(1:meshSize);
[X,Y] = meshgrid(x, y);
H0 = zeros(size(X)); % height field at time t = 0
for i = 1:meshSize
for j = 1:meshSize
kx = 2.0 * pi / patchSize * (-meshSize / 2.0 + x(i)); % = 2*pi*n / Lx
ky = 2.0 * pi / patchSize * (-meshSize / 2.0 + y(j)); % = 2*pi*m / Ly
P = phillips(kx, ky, windDir, windSpeed, A, g); % phillips spectrum
H0(i,j) = 1/sqrt(2) * (randn(1) + 1i * randn(1)) * sqrt(P);
end
end
H0 = H0 + conj(H0);
surf(X,Y,abs(ifft(H0)));
axis([-10 10 -10 10 -10 10]);
And the phillips function:
function P = phillips(kx, ky, windDir, windSpeed, A, g)
k_sq = kx^2 + ky^2;
L = windSpeed^2 / g;
k = [kx, ky] / sqrt(k_sq);
wk = k(1) * windDir(1) + k(2) * windDir(2);
P = A / k_sq^2 * exp(-1.0 / (k_sq * L^2)) * wk^2;
end
Is there any matlab ocean simulation source code which could help me to understand my mistakes? Fast google search didn't get any results.
Here's a "correct" result I got from "CUDA FFT Ocean Simulation". I didn't achieve this behavior in Matlab yet but I've ploted "surf" in matlab using data from "CUDA FFT Ocean Simulation". Here's what it looks like:
I've made an experiment and got an interesting result:
I've taken generated h0 from "CUDA FFT Ocean Simulation". So I have to do ifft to transform from frequency domain to spatial domain to plot the graph. I've done it for the same h0 using matlab ifft and using cufftExecC2C from CUDA library. Here's the result:
CUDA ifft:
Matlab ifft:
Either I don't understand some aspects of realization of cufftExecC2C or cufftExecC2C and matlab ifft are different algorithms with different results.
By the way parameters for generating such surface are:
meshSize = 32
A = 1e-7
patchSize = 80
windSpeed = 10
Well that was definitely a funny exercise. This is a completely rewritten answer since you found the issues you were asking about by yourself.
Instead of deleting my answer, there is still merit in posting to help you vectorize and/or explain a few bits of code.
I completely rewrote the GUI I gave in my former answer in order to incorporate your changes and add a couple of options. It started to grew arms and legs so I won't put the listing here but you can find the full file there:
ocean_simulator.m.
This is completely self contained and it includes all the calculating functions I vectorized and list separately below.
The GUI will allow you to play with the parameters, animate the waves, export GIF file (and a few other options like the "preset", but they are not too ironed out yet). A few examples of what you can achieve:
Basic
This is what you get with the quick default settings, and a couple of rendering options. This uses a small grid size and a fast time step, so it runs pretty quickly on any machine.
I am quite limited at home (Pentium E2200 32bit), so I could only practice with limited settings. The gui will run even with the settings maxed but it will become to slow to really enjoy.
However, with a quick run of ocean_simulator at work (I7 64 bit, 8 cores, 16GB ram, 2xSSD in Raid), it makes it much more fun! Here are a few examples:
Although done on a much better machine, I didn't use any parallel functionality nor any GPU calculations, so Matlab was only using a portion of these specs, which means it could probably run just as good on any 64bit system with decent RAM
Windy lake
This is a rather flat water surface like a lake. Even high winds do not produce high amplitude waves (but still a lot of mini wavelets). If you're a wind surfer looking at that from your window on top of the hill, your heart is going to skip a beat and your next move is to call Dave "Man! gear up. Meet you in five on the water!"
Swell
This is you looking from the bridge of your boat on the morning, after having battled with the storm all night. The storm has dissipated and the long large waves are the last witness of what was definitely a shaky night (people with sailing experience will know ...).
T-Storm
And this was what you were up to the night before...
second gif done at home, hence the lack of detail ... sorry
To the bottom:
Finally, the gui will let you add a patch around the water domain. In the gui it is transparent so you could add objects underwater or a nice ocean bottom. Unfortunately, the GIF format cannot include an alpha channel so no transparency here (but if you export in a video then you should be ok).
Moreover, the export to GIF degrade the image, the joint between the domain border and the water surface is flawless if you run that in Matlab. In some case it also make Matlab degrade the rendering of the lighting, so this is definitely not the best option for export, but it allows more things to play within matlab.
Now onto the code:
Instead of listing the full GUI, which would be super long (this post is long enough already), I will just list here the re-written version of your code, and explain the changes.
You should notice a massive increase of speed execution (orders of magnitude), thanks to the remaining vectorization, but mostly for two reasons:
(i) A lot of calculations were repeated. Caching values and reusing them is much faster than recalculating full matrices in loops (during the animation part).
(ii) Note how I defined the surface graphic object. It is defined only once (empty even), then all the further calls (in the loop) only update the underlying ZData of the surface object (instead of re-creating a surface object at each iteration.
Here goes:
%% // clear workspace
clear all; close all; clc;
%% // Default parameters
param.meshsize = 128 ; %// main grid size
param.patchsize = 200 ;
param.windSpeed = 100 ; %// what unit ? [m/s] ??
param.winddir = 90 ; %// Azimuth
param.rng = 13 ; %// setting seed for random numbers
param.A = 1e-7 ; %// Scaling factor
param.g = 9.81 ; %// gravitational constant
param.xLim = [-10 10] ; %// domain limits X
param.yLim = [-10 10] ; %// domain limits Y
param.zLim = [-1e-4 1e-4]*2 ;
gridSize = param.meshsize * [1 1] ;
%% // Define the grid X-Y domain
x = linspace( param.xLim(1) , param.xLim(2) , param.meshsize ) ;
y = linspace( param.yLim(1) , param.yLim(2) , param.meshsize ) ;
[X,Y] = meshgrid(x, y);
%% // get the grid parameters which remain constants (not time dependent)
[H0, W, Grid_Sign] = initialize_wave( param ) ;
%% // calculate wave at t0
t0 = 0 ;
Z = calc_wave( H0 , W , t0 , Grid_Sign ) ;
%% // populate the display panel
h.fig = figure('Color','w') ;
h.ax = handle(axes) ; %// create an empty axes that fills the figure
h.surf = handle( surf( NaN(2) ) ) ; %// create an empty "surface" object
%% // Display the initial wave surface
set( h.surf , 'XData',X , 'YData',Y , 'ZData',Z )
set( h.ax , 'XLim',param.xLim , 'YLim',param.yLim , 'ZLim',param.zLim )
%% // Change some rendering options
axis off %// make the axis grid and border invisible
shading interp %// improve shading (remove "faceted" effect)
blue = linspace(0.4, 1.0, 25).' ; cmap = [blue*0, blue*0, blue]; %'// create blue colormap
colormap(cmap)
%// configure lighting
h.light_handle = lightangle(-45,30) ; %// add a light source
set(h.surf,'FaceLighting','phong','AmbientStrength',.3,'DiffuseStrength',.8,'SpecularStrength',.9,'SpecularExponent',25,'BackFaceLighting','unlit')
%% // Animate
view(75,55) %// no need to reset the view inside the loop ;)
timeStep = 1./25 ;
nSteps = 2000 ;
for time = (1:nSteps)*timeStep
%// update wave surface
Z = calc_wave( H0,W,time,Grid_Sign ) ;
h.surf.ZData = Z ;
pause(0.001);
end
%% // This block of code is only if you want to generate a GIF file
%// be carefull on how many frames you put there, the size of the GIF can
%// quickly grow out of proportion ;)
nFrame = 55 ;
gifFileName = 'MyDancingWaves.gif' ;
view(-70,40)
clear im
f = getframe;
[im,map] = rgb2ind(f.cdata,256,'nodither');
im(1,1,1,20) = 0;
iframe = 0 ;
for time = (1:nFrame)*.5
%// update wave surface
Z = calc_wave( H0,W,time,Grid_Sign ) ;
h.surf.ZData = Z ;
pause(0.001);
f = getframe;
iframe= iframe+1 ;
im(:,:,1,iframe) = rgb2ind(f.cdata,map,'nodither');
end
imwrite(im,map,gifFileName,'DelayTime',0,'LoopCount',inf)
disp([num2str(nFrame) ' frames written in file: ' gifFileName])
You'll notice that I changed a few things, but I can assure you the calculations are exactly the same. This code calls a few subfunctions but they are all vectorized so if you want you can just copy/paste them here and run everything inline.
The first function called is initialize_wave.m
Everything calculated here will be constant later (it does not vary with time when you later animate the waves), so it made sense to put that into a block on it's own.
function [H0, W, Grid_Sign] = initialize_wave( param )
% function [H0, W, Grid_Sign] = initialize_wave( param )
%
% This function return the wave height coefficients H0 and W for the
% parameters given in input. These coefficients are constants for a given
% set of input parameters.
% Third output parameter is optional (easy to recalculate anyway)
rng(param.rng); %// setting seed for random numbers
gridSize = param.meshsize * [1 1] ;
meshLim = pi * param.meshsize / param.patchsize ;
N = linspace(-meshLim , meshLim , param.meshsize ) ;
M = linspace(-meshLim , meshLim , param.meshsize ) ;
[Kx,Ky] = meshgrid(N,M) ;
K = sqrt(Kx.^2 + Ky.^2); %// ||K||
W = sqrt(K .* param.g); %// deep water frequencies (empirical parameter)
[windx , windy] = pol2cart( deg2rad(param.winddir) , 1) ;
P = phillips(Kx, Ky, [windx , windy], param.windSpeed, param.A, param.g) ;
H0 = 1/sqrt(2) .* (randn(gridSize) + 1i .* randn(gridSize)) .* sqrt(P); % height field at time t = 0
if nargout == 3
Grid_Sign = signGrid( param.meshsize ) ;
end
Note that the initial winDir parameter is now expressed with a single scalar value representing the "azimuth" (in degrees) of the wind (anything from 0 to 360). It is later translated to its X and Y components thanks to the function pol2cart.
[windx , windy] = pol2cart( deg2rad(param.winddir) , 1) ;
This insure that the norm is always 1.
The function calls your problematic phillips.m separately, but as said before it works even fully vectorized so you can copy it back inline if you like. (don't worry I checked the results against your versions => strictly identical). Note that this function does not output complex numbers so there was no need to compare the imaginary parts.
function P = phillips(Kx, Ky, windDir, windSpeed, A, g)
%// The function now accept scalar, vector or full 2D grid matrix as input
K_sq = Kx.^2 + Ky.^2;
L = windSpeed.^2 ./ g;
k_norm = sqrt(K_sq) ;
WK = Kx./k_norm * windDir(1) + Ky./k_norm * windDir(2);
P = A ./ K_sq.^2 .* exp(-1.0 ./ (K_sq * L^2)) .* WK.^2 ;
P( K_sq==0 | WK<0 ) = 0 ;
end
The next function called by the main program is calc_wave.m. This function finishes the calculations of the wave field for a given time. It is definitely worth having that on its own because this is the mimimun set of calculations which will have to be repeated for each given time when you want to animate the waves.
function Z = calc_wave( H0,W,time,Grid_Sign )
% Z = calc_wave( H0,W,time,Grid_Sign )
%
% This function calculate the wave height based on the wave coefficients H0
% and W, for a given "time". Default time=0 if not supplied.
% Fourth output parameter is optional (easy to recalculate anyway)
% recalculate the grid sign if not supplied in input
if nargin < 4
Grid_Sign = signGrid( param.meshsize ) ;
end
% Assign time=0 if not specified in input
if nargin < 3 ; time = 0 ; end
wt = exp(1i .* W .* time ) ;
Ht = H0 .* wt + conj(rot90(H0,2)) .* conj(wt) ;
Z = real( ifft2(Ht) .* Grid_Sign ) ;
end
The last 3 lines of calculations require a bit of explanation as they received the biggest changes (all for the same result but a much better speed).
Your original line:
Ht = H0 .* exp(1i .* W .* (t * timeStep)) + conj(flip(flip(H0,1),2)) .* exp(-1i .* W .* (t * timeStep));
recalculate the same thing too many times to be efficient:
(t * timeStep) is calculated twice on the line, at each loop, while it is easy to get the proper time value for each line when time is initialised at the beginning of the loop for time = (1:nSteps)*timeStep.
Also note that exp(-1i .* W .* time) is the same than conj(exp(1i .* W .* time)). Instead of doing 2*m*n multiplications to calculate them each, it is faster to calculate one once, then use the conj() operation which is much faster.
So your single line would become:
wt = exp(1i .* W .* time ) ;
Ht = H0 .* wt + conj(flip(flip(H0,1),2)) .* conj(wt) ;
Last minor touch, flip(flip(H0,1),2)) can be replaced by rot90(H0,2) (also marginally faster).
Note that because the function calc_wave is going to be repeated extensively, it is definitely worth reducing the number of calculations (as we did above), but also by sending it the Grid_Sign parameter (instead of letting the function recalculate it every iteration). This is why:
Your mysterious function signCor(ifft2(Ht),meshSize)), simply reverse the sign of every other element of Ht. There is a faster way of achieving that: simply multiply Ht by a matrix the same size (Grid_Sign) which is a matrix of alternated +1 -1 ... and so on.
so signCor(ifft2(Ht),meshSize) becomes ifft2(Ht) .* Grid_Sign.
Since Grid_Sign is only dependent on the matrix size, it does not change for each time in the loop, you only calculate it once (before the loop) then use it as it is for every other iteration. It is calculated as follow (vectorized, so you can also put it inline in your code):
function sgn = signGrid(n)
% return a matrix the size of n with alternate sign for every indice
% ex: sgn = signGrid(3) ;
% sgn =
% -1 1 -1
% 1 -1 1
% -1 1 -1
[x,y] = meshgrid(1:n,1:n) ;
sgn = ones( n ) ;
sgn(mod(x+y,2)==0) = -1 ;
end
Lastly, you will notice a difference in how the grids [Kx,Ky] are defined between your version and this one. They do produce slightly different result, it's just a matter of choice.
To explain with a simple example, let's consider a small meshsize=5. Your way of doing things will split that into 5 values, equally spaced, like so:
Kx(first line)=[-1.5 -0.5 0.5 1.5 2.5] * 2 * pi / patchSize
while my way of producing the grid will produce equally spaced values, but also centered on the domain limits, like so:
Kx(first line)=[-2.50 -1.25 0.0 1.25 2.50] * 2 * pi / patchSize
It seems to respect more your comment % = 2*pi*n / Lx, -N/2 <= n < N/2 on the line where you define it.
I tend to prefer symmetric solutions (plus it is also slightly faster but it is only calculated once so it is not a big deal), so I used my vectorized way, but it is purely a matter of choice, you can definitely keep your way, it only ever so slightly "offset" the whole result matrix, but it doesn't perturbate the calculations per se.
last remains of the first answer
Side programming notes:
I detect you come from the C/C++ world or family. In Matlab you do not need to define decimal number with a coma (like 2.0, you used that for most of your numbers). Unless specifically defined otherwise, Matlab by default cast any number to double, which is a 64 bit floating point type. So writing 2 * pi is enough to get the maximum precision (Matlab won't cast pi as an integer ;-)), you do not need to write 2.0 * pi. Although it will still work if you don't want to change your habits.
Also, (one of the great benefit of Matlab), adding . before an operator usually mean "element-wise" operation. You can add (.+), substract (.-), multiply (.*), divide (./) full matrix element wise this way. This is how I got rid of all the loops in your code. This also work for the power operator: A.^2 will return a matrix the same size as A with every element squared.
Here's the working program.
First of all - source code:
clear all; close all; clc;
rng(13); % setting seed for random numbers
meshSize = 128; % field size
windDir = [0.1,1];
patchSize = 200;
A = 1e-7;
g = 9.81; % gravitational constant
windSpeed = 100;
timeStep = 1/25;
x1 = linspace(-10, 10, meshSize+1); x = x1(1:meshSize);
y1 = linspace(-10, 10, meshSize+1); y = y1(1:meshSize);
[X,Y] = meshgrid(x,y); % wave field
i = 1:meshSize; j = 1:meshSize; % indecies
[I,J] = meshgrid(i,j); % field of indecies
Kx = 2.0 * pi / patchSize * (-meshSize / 2.0 + I); % = 2*pi*n / Lx, -N/2 <= n < N/2
Ky = 2.0 * pi / patchSize * (-meshSize / 2.0 + J); % = 2*pi*m / Ly, -M/2 <= m < M/2
K = sqrt(Kx.^2 + Ky.^2); % ||K||
W = sqrt(K .* g); % deep water frequencies (empirical parameter)
P = zeros(size(X)); % Cant compute P without loops
for i = 1:meshSize
for j = 1:meshSize
P(i,j) = phillips(Kx(i,j), Ky(i,j), windDir, windSpeed, A, g); % phillips spectrum
end
end
H0 = 1/sqrt(2) .* (randn(size(X)) + 1i .* randn(size(X))) .* sqrt(P); % height field at time t = 0
rotate3d on;
for t = 1:10000 % 10000 * timeStep (sec)
Ht = H0 .* exp(1i .* W .* (t * timeStep)) + ...
conj(flip(flip(H0,1),2)) .* exp(-1i .* W .* (t * timeStep));
[az,el] = view;
surf(X,Y,real(signCor(ifft2(Ht),meshSize)));
axis([-10 10 -10 10 -1e-4 1e-4]); view(az,el);
blue = linspace(0.4, 1.0, 25)'; map = [blue*0, blue*0, blue];
%shading interp; % improve shading (remove "faceted" effect)
colormap(map);
pause(1/60);
end
phillips.m: (I've tried to vectorize the computation of Phillips spectrum but I faced with a difficulty which I'll show further)
function P = phillips(kx, ky, windDir, windSpeed, A, g)
k_sq = kx^2 + ky^2;
if k_sq == 0
P = 0;
else
L = windSpeed^2 / g;
k = [kx, ky] / sqrt(k_sq);
wk = k(1) * windDir(1) + k(2) * windDir(2);
P = A / k_sq^2 * exp(-1.0 / (k_sq * L^2)) * wk^2;
if wk < 0
P = 0;
end
end
end
signCor.m: (This function is an absolutely mystery for me... I've copied it from "CUDA FFT Ocean Simulation" realization. Simulation works much worse without it. And again I don't know how to vectorize this function.)
function H = signCor(H1, meshSize)
H = H1;
for i = 1:meshSize
for j = 1:meshSize
if mod(i+j,2) == 0
sign = -1; % works fine if we change signs vice versa
else
sign = 1;
end
H(i,j) = H1(i,j) * sign;
end
end
end
The biggest mistake that I've done is that I used ifft instead of using ifft2, that's why CUDA ifft and Matlab ifft didn't match.
My second mistake was in this lines of code:
kx = 2.0 * pi / patchSize * (-meshSize / 2.0 + x(i)); % = 2*pi*n / Lx
ky = 2.0 * pi / patchSize * (-meshSize / 2.0 + y(j)); % = 2*pi*m / Ly
I should've write:
kx = 2.0 * pi / patchSize * (-meshSize / 2.0 + i); % = 2*pi*n / Lx
ky = 2.0 * pi / patchSize * (-meshSize / 2.0 + j); % = 2*pi*m / Ly
I've played a bit with parameters A, meshSize, patchSize and I came to the conclusion that:
Somehow plausible parameter of wave amplitude is A * (patchSize / meshSize), where A is nothing but a scaling factor.
For 'calm' patchSize / meshSize <= 0.5.
For 'tsunami' patchSize / meshSize >= 3.0.
Difficulty with a vectorization of Phillips spectrum:
I have 2 functions:
% non-vectorized spectrum
function P = phillips1(kx, ky, windDir, windSpeed, A, g)
k_sq = kx^2 + ky^2;
if k_sq == 0
P = 0;
else
L = windSpeed^2 / g;
k = [kx, ky] / sqrt(k_sq);
wk = k(1) * windDir(1) + k(2) * windDir(2);
P = A / k_sq^2 * exp(-1.0 / (k_sq * L^2)) * wk^2;
if wk < 0
P = 0;
end
end
end
% vectorized spectrum
function P = phillips2(Kx, Ky, windDir, windSpeed, A, g)
K_sq = Kx .^ 2 + Ky .^ 2;
L = -g^2 / windSpeed^4;
WK = (Kx ./ K_sq) .* windDir(1) + (Ky ./ K_sq) .* windDir(2);
P = (A ./ (K_sq .^ 2)) .* ( exp(L ./ K_sq) .* (WK .^ 2) );
P(K_sq == 0) = 0;
P(WK < 0) = 0;
P(isinf(P)) = 0;
end
After I compute P1 using phillips1 and P2 using phillips2 I plot their difference:
subplot(2,1,1); surf(X,Y,real(P2-P1)); title('Difference in real part');
subplot(2,1,2); surf(X,Y,imag(P2-P1)); title('Difference in imaginary part');
It perfectly illustrates that there's a huge difference between this 2 spectrums in real part.
I am doing ray tracing and I have A screen described in the world coordinates as Matrices(I had before the X,Y,Z in the screen coordinates and by using transformation and rotation I got it in the world coordinates)
Xw (NXM Matrix)
Yw (NXM Matrix)
Zw (I have got this polynomial (5th order polynomial)by fitting the 3D data Xw and Yw. I have it as f(Xw,Yw))
I have the rays equations too described as usual:
X = Ox + t*Dx
Y = Oy + t*Dy
Z = Oz + t*Dz %(O is the origin point and D is the direction)
So what I did is that I replaced the X and Y in the Polynomial equation f(Xw,Yw) and solved it for t so I can then get the intersection point.
But apparently the method that I used is wrong(The intersection points that I got were somewhere else).
Could any one please help me and tell me what is the mistake. Please support me.
Thanks
This is part of the code:
X_World_coordinate_scr = ScreenXCoordinates.*Rotation_matrix_screen(1,1) + ScreenYCoordinates.*Rotation_matrix_screen(1,2) + ScreenZCoordinates.*Rotation_matrix_screen(1,3) + Zerobase_scr(1);
Y_World_coordinate_scr = ScreenXCoordinates.*Rotation_matrix_screen(2,1) + ScreenYCoordinates.*Rotation_matrix_screen(2,2) + ScreenZCoordinates.*Rotation_matrix_screen(2,3) + Zerobase_scr(2);
Z_World_coordinate_scr = ScreenXCoordinates.*Rotation_matrix_screen(3,1) + ScreenYCoordinates.*Rotation_matrix_screen(3,2) + ScreenZCoordinates.*Rotation_matrix_screen(3,3) + Zerobase_scr(3); % converting the screen coordinates to the world coordinates using the rotation matrix and the translation vector
polymodel = polyfitn([X_World_coordinate_scr(:),Y_World_coordinate_scr(:)],Z_World_coordinate_scr(:),5); % using a function from the MAtlab file exchange and I trust this function. I tried it different data and it gives me the f(Xw,Yw).
ScreenPoly = polyn2sym(polymodel); % Function from Matlab file exchange to give the symbolic shape of the polynomial.
syms X Y Z t Dx Ox Dy Oy oz Dz z;
tsun = matlabFunction(LayerPoly, 'vars',[X,Y,Z]); % just to substitue the symboles from X , Y and Z to (Ox+t*Dx) , (Oy+t*Dy) and (Oz+t*Dz) respectively
Equation = tsun((Ox+t*Dx),(Oy+t*Dy),(Oz+t*Dz));
Answer = solve(Equation,t); % solving it for t but the equation that it is from the 5th order and the answer is RootOf(.... for z)
a = char(Answer); % preparing it to find the roots (Solutions of t)
R = strrep(a,'RootOf(','');
R1 = strrep(R,', z)','');
b = sym(R1);
PolyCoeffs = coeffs(b,z); % get the coefficient of the polynomail
tfun = matlabFunction(PolyCoeffs, 'vars',[Ox,Oy,oz,Dx,Dy,Dz]);
tCounter = zeros(length(Directions),1);
NaNIndices = find(isnan(Surface(:,1))==1); %I have NaN values and I am taking them out
tCounter(NaNIndices) = NaN;
NotNaNIndices = find(isnan(Surface(:,1))==0);
for i = NotNaNIndices' % for loop to calc
OxNew = Surface(i,1);
OyNew = Surface(i,2);
OzNew = Surface(i,3);
DxNew = Directions(i,1);
DyNew = Directions(i,2);
DzNew = Directions(i,3);
P = tfun(OxNew,OyNew,OzNew ,DxNew,DyNew,DzNew);
t = roots(P);
t(imag(t) ~= 0) = []; % getting rid of the complex solutions
tCounter(i) = t;
end
Please support
Thanks in advance
From a couple references (i.e., http://en.wikipedia.org/wiki/Rotation_matrix "Rotation matrix from axis and angle", and exercise 5.15 in "Computer Graphics - Principles and Practice" by Foley et al, 2nd edition in C), I've seen this definition of a rotation matrix (implemented below in Octave) that rotates points by a specified angle about a specified vector. Although I have used it before, I'm now seeing rotation problems that appear to be related to orientation. The problem is recreated in the following Octave code that
takes two unit vectors: src (green in figures) and dst (red in figures),
calculates the angle between them: theta,
calculates the vector normal to both: pivot (blue in figures),
and finally attempts to rotate src into dst by rotating it about vector pivot by angle theta.
% This test fails: rotated unit vector is not at expected location and is no longer normalized.
s = [-0.49647; -0.82397; -0.27311]
d = [ 0.43726; -0.85770; -0.27048]
test_rotation(s, d, 1);
% Determine rotation matrix that rotates the source and normal vectors to the x and z axes, respectively.
normal = cross(s, d);
normal /= norm(normal);
R = zeros(3,3);
R(1,:) = s;
R(2,:) = cross(normal, s);
R(3,:) = normal;
R
% After rotation of the source and destination vectors, this test passes.
s2 = R * s
d2 = R * d
test_rotation(s2, d2, 2);
function test_rotation(src, dst, iFig)
norm_src = norm(src)
norm_dst = norm(dst)
% Determine rotation axis (i.e., normal to two vectors) and rotation angle.
pivot = cross(src, dst);
theta = asin(norm(pivot))
theta_degrees = theta * 180 / pi
pivot /= norm(pivot)
% Initialize matrix to rotate by an angle theta about pivot vector.
ct = cos(theta);
st = sin(theta);
omct = 1 - ct;
M(1,1) = ct - pivot(1)*pivot(1)*omct;
M(1,2) = pivot(1)*pivot(2)*omct - pivot(3)*st;
M(1,3) = pivot(1)*pivot(3)*omct + pivot(2)*st;
M(2,1) = pivot(1)*pivot(2)*omct + pivot(3)*st;
M(2,2) = ct - pivot(2)*pivot(2)*omct;
M(2,3) = pivot(2)*pivot(3)*omct - pivot(1)*st;
M(3,1) = pivot(1)*pivot(3)*omct - pivot(2)*st;
M(3,2) = pivot(2)*pivot(3)*omct + pivot(1)*st;
M(3,3) = ct - pivot(3)*pivot(3)*omct;
% Rotate src about pivot by angle theta ... and check the result.
dst2 = M * src
dot_dst_dst2 = dot(dst, dst2)
if (dot_dst_dst2 >= 0.99999)
"success"
else
"FAIL"
end
% Draw the vectors: green is source, red is destination, blue is normal.
figure(iFig);
x(1) = y(1) = z(1) = 0;
ubounds = [-1.25 1.25 -1.25 1.25 -1.25 1.25];
x(2)=src(1); y(2)=src(2); z(2)=src(3);
plot3(x,y,z,'g-o');
hold on
x(2)=dst(1); y(2)=dst(2); z(2)=dst(3);
plot3(x,y,z,'r-o');
x(2)=pivot(1); y(2)=pivot(2); z(2)=pivot(3);
plot3(x,y,z,'b-o');
x(2)=dst2(1); y(2)=dst2(2); z(2)=dst2(3);
plot3(x,y,z,'k.o');
axis(ubounds, 'square');
view(45,45);
xlabel("xd");
ylabel("yd");
zlabel("zd");
hold off
end
Here are the resulting figures. Figure 1 shows an orientation that doesn't work. Figure 2 shows an orientation that works: the same src and dst vectors but rotated into the first quadrant.
I was expecting the src vector to always rotate onto the dst vector, as shown in Figure 2 by the black circle covering the red circle, for all vector orientations. However Figure 1 shows an orientation where the src vector does not rotate onto the dst vector (i.e., the black circle is not on top of the red circle, and is not even on the unit sphere).
For what it's worth, the references that defined the rotation matrix did not mention orientation limitations, and I derived (in a few hours and a few pages) the rotation matrix equation and didn't spot any orientation limitations there. I'm hoping the problem is an implementation error on my part, but I haven't been able to find it yet in either of my implementations: C and Octave. Have you experienced orientation limitations when implementing this rotation matrix? If so, how did you work around them? I would prefer to avoid the extra translation into the first quadrant if it isn't necessary.
Thanks,
Greg
Seems two minus signs have escaped:
M(1,1) = ct - P(1)*P(1)*omct;
M(1,2) = P(1)*P(2)*omct - P(3)*st;
M(1,3) = P(1)*P(3)*omct + P(2)*st;
M(2,1) = P(1)*P(2)*omct + P(3)*st;
M(2,2) = ct + P(2)*P(2)*omct; %% ERR HERE; THIS IS THE CORRECT SIGN
M(2,3) = P(2)*P(3)*omct - P(1)*st;
M(3,1) = P(1)*P(3)*omct - P(2)*st;
M(3,2) = P(2)*P(3)*omct + P(1)*st;
M(3,3) = ct + P(3)*P(3)*omct; %% ERR HERE; THIS IS THE CORRECT SIGN
Here is a version that is much more compact, faster, and also based on Rodrigues' rotation formula:
function test
% first test: pass
s = [-0.49647; -0.82397; -0.27311];
d = [ 0.43726; -0.85770; -0.27048]
d2 = axis_angle_rotation(s, d)
% Determine rotation matrix that rotates the source and normal vectors to the x and z axes, respectively.
normal = cross(s, d);
normal = normal/norm(normal);
R(1,:) = s;
R(2,:) = cross(normal, s);
R(3,:) = normal;
% Second test: pass
s2 = R * s;
d2 = R * d
d3 = axis_angle_rotation(s2, d2)
end
function vec = axis_angle_rotation(vec, dst)
% These following commands are just here for the function to act
% the same as your original function. Eventually, the function is
% probably best defined as
%
% vec = axis_angle_rotation(vec, axs, angle)
%
% or even
%
% vec = axis_angle_rotation(vec, axs)
%
% where the length of axs defines the angle.
%
axs = cross(vec, dst);
theta = asin(norm(axs));
% some preparations
aa = axs.'*axs;
ra = vec.'*axs;
% location of circle centers
c = ra.*axs./aa;
% first coordinate axis on the circle's plane
u = vec-c;
% second coordinate axis on the circle's plane
v = [axs(2)*vec(3)-axs(3)*vec(2)
axs(3)*vec(1)-axs(1)*vec(3)
axs(1)*vec(2)-axs(2)*vec(1)]./sqrt(aa);
% the output vector
vec = c + u*cos(theta) + v*sin(theta);
end
I have a question connected to this code:
t = -20:0.1:20;
plot3(zeros(size(t)),t,-t.^2);
grid on
hold on
i = 1;
h = plot3([0 0],[0 t(i)],[0 -t(i)^2],'r');
h1 = plot3([-1 0],[0 0],[-400 -200],'g');
for(i=2:length(t))
set(h,'xdata',[-1 0],'ydata',[0 t(i)],'zdata',[-400 -t(i)^2]);
pause(0.01);
end
In this code, I plot two intersecting lines. H1, and H2. H1 is fixed, H2 moves as a function of time. H2 happens to trace a parabola in this example, but its movement could be arbitrary.
How can I calculate and draw the bisector of the angle between these two intersecting lines for every position of the line H2? I would like to see in the plot the bisector and the line H2 moving at the same time.
Solving this problem for one position of H2 is sufficient, since it will be the same procedure for all orientations of H2 relative to H1.
I am not a geometry genius, there is likely an easier way to do this. As of now, no one has responded though, so this will be something.
You have three points in three space:
Let A be the common vertice of the two line segments.
Let B and C be two known points on the two line segments.
Choose an arbitrary distance r where
r <= distance from A to B
r <= distance from A to C
Measure from A along line segment AB a distance of r. This is point RB
Measure from A along line segment AC a distance or r. This is point RC
Find the mid point of line segment connecting RB and RC. This is point M
Line segment AM is the angular bisector of angle CAB.
Each of these steps should be relatively easy to accomplish.
Here is basically MatlabDoug's method with some improvement on the determination of the point he calls M.
t = -20:0.1:20;
plot3(zeros(size(t)),t,-t.^2);
grid on
hold on
v1 = [1 0 200];
v1 = v1/norm(v1);
i = 1;
h = plot3([-1 0],[0 t(i)],[-400 -t(i)^2],'r');
h1 = plot3([-1 0],[0 0],[-400 -200],'g');
l = norm([1 t(i) -t(i)^2+400]);
p = l*v1 + [-1 0 -400];
v2 = (p + [0 t(i) -t(i)^2])/2 - [-1 0 -400];
p2 = [-1 0 -400] + v2/v2(1);
h2 = plot3([-1 p2(1)],[0 p2(2)],[-400 p2(3)],'m');
pause(0.1)
for(i=2:length(t))
l = norm([1 t(i) -t(i)^2+400]);
p = l*v1 + [-1 0 -400];
v2 = (p + [0 t(i) -t(i)^2])/2 - [-1 0 -400];
p2 = [-1 0 -400] + v2/v2(1);
set(h,'xdata',[-1 0],'ydata',[0 t(i)],'zdata',[-400 -t(i)^2]);
set(h2,'xdata',[-1 p2(1)],'ydata',[0 p2(2)],'zdata',[-400 p2(3)]);
pause;
end
I just use the following:
Find the normalized vectors AB, and AC, where A is the common point of the segments.
V = (AB + AC) * 0.5 // produces the direction vector that bisects AB and AC.
Normalize V, then do A + V * length to get the line segment of the desired length that starts at the common point.
(Note that this method does not work on 3 points along a line to produce a perpendicular bisector, it will yield a vector with no length in that case)
I have added a C# implementation (in the XZ plane using Unity 3D Vector3 struct) that handles Perpendicular and Reflex bisectors in case someone that knows MATLAB would translate it.
public Vector3 GetBisector(Vector3 center, Vector3 first, Vector3 second)
{
Vector3 firstDir = (first - center).normalized;
Vector3 secondDir = (second - center).normalized;
Vector3 result = ((firstDir + secondDir) * 0.5f).normalized;
if (IsGreaterThan180(-firstDir, secondDir))
{
// make into a reflex vector
(result.x, result.z) = (-result.x, -result.z);
}
if (result.sqrMagnitude < 0.99f)
{
// we have a colinear set of lines.
// return the perpendicular bisector.
result = Vector3.Cross(Vector3.up, -firstDir).normalized;
}
return result;
}
bool IsGreaterThan180(Vector3 dir, Vector3 dir2)
{
// < 0.0 for clockwise ordering
return (dir2.x * dir.z - dir2.z * dir.x) < 0.0f;
}
Also note that the returned bisector is a vector of unit length. Using "center + bisector * length" could be used to place it into worldspace.