Related
I need to evaluate a function (say)
Fxy = 2*x.^2 +3 *y.^2;
on a ternary grid x-range (0 - 1), y-range (0-1) and 1-x-y (0 - 1).
I am unable to construct the ternary grid on which I need to evaluate the above function. Also, once evaluated I need to plot the function in a ternary contour plot. Ideally, I need the axes to go counter clockwise in the sense (x -> y--> (1-x-y)).
I have tried the function
function tg = triangle_grid ( n, t )
ng = ( ( n + 1 ) * ( n + 2 ) ) / 2;
tg = zeros ( 2, ng );
p = 0;
for i = 0 : n
for j = 0 : n - i
k = n - i - j;
p = p + 1;
tg(1:2,p) = ( i * t(1:2,1) + j * t(1:2,2) + k * t(1:2,3) ) / n;
end
end
return
end
for the number of sub intervals between the triangle edge coordinates
n = 10 (say)
and for the edge coordinates of an equilateral triangle
t = tcoord = [0.0, 0.5, 1.0;
0.0, 1.0*sqrt(3)/2, 0.0];
This generated a triangular grid with the x-axis from 0-1 but the other two are not from 0-1.
I need something like this:
... with the axes range 0-1 (0-100 would also do).
In addition, I need to know the coordinate points for all intersections within the triangular grid. Once I have this I can proceed to evaluate the function in this grid.
My final aim is to get something like this. This is a better representation of what I need to achieve (as compared to the previous plot which I have now removed)
Note that the two ternary plots have iso-value contours which are different in in magnitude. In my case the difference is an order of magnitude, two very different Fxy's.
If I can plot the two ternary plots on top of each other then and evaluate the compositions at the intersection of two iso-value contours on the ternary plane. The compositions should be as read from the ternary plot and not the rectangular grid on which triangle is defined.
Currently there are issues (as highlighted in the comments section, will update this once the problem is closer to solution).
I am the author of ternplot. As you have correctly surmised, ternpcolor does not do what you want, as it is built to grid data automatically. In retrospect, this was not a particularly wise decision, I've made a note to change the design. In the mean time this code should do what you want:
EDIT: I've changed the code to find the intersection of two curves rather than just one.
N = 10;
x = linspace(0, 1, N);
y = x;
% The grid intersections on your diagram are actually rectangularly arranged,
% so meshgrid will build the intersections for us
[xx, yy] = meshgrid(x, y);
zz = 1 - (xx + yy);
% now that we've got the intersections, we can evaluate the function
f1 = #(x, y) 2*x.^2 + 3*y.^2 + 0.1;
Fxy1 = f1(xx, yy);
Fxy1(xx + yy > 1) = nan;
f2 = #(x, y) 3*x.^2 + 2*y.^2;
Fxy2 = f2(xx, yy);
Fxy2(xx + yy > 1) = nan;
f3 = #(x, y) (3*x.^2 + 2*y.^2) * 1000; % different order of magnitude
Fxy3 = f3(xx, yy);
Fxy3(xx + yy > 1) = nan;
subplot(1, 2, 1)
% This constructs the ternary axes
ternaxes(5);
% These are the coordinates of the compositions mapped to plot coordinates
[xg, yg] = terncoords(xx, yy);
% simpletri constructs the correct triangles
tri = simpletri(N);
hold on
% and now we can plot
trisurf(tri, xg, yg, Fxy1);
trisurf(tri, xg, yg, Fxy2);
hold off
view([137.5, 30]);
subplot(1, 2, 2);
ternaxes(5)
% Here we plot the line of intersection of the two functions
contour(xg, yg, Fxy1 - Fxy2, [0 0], 'r')
axis equal
EDIT 2: If you want to find the point of intersection between two contours, you are effectively solving two simultaneous equations. This bit of extra code will solve that for you (notice I've used some anonymous functions in the code above now, as well):
f1level = 1;
f3level = 1000;
intersection = fsolve(#(v) [f1(v(1), v(2)) - f1level; f3(v(1), v(2)) - f3level], [0.5, 0.4]);
% if you don't have the optimization toolbox, this command works almost as well
intersection = fminsearch(#(v) sum([f1(v(1), v(2)) - f1level; f3(v(1), v(2)) - f3level].^2), [0.5, 0.4]);
ternaxes(5)
hold on
contour(xg, yg, Fxy1, [f1level f1level]);
contour(xg, yg, Fxy3, [f3level f3level]);
ternplot(intersection(1), intersection(2), 1 - sum(intersection), 'r.');
hold off
I have played a bit with the file exchange submission https://www.mathworks.com/matlabcentral/fileexchange/2299-alchemyst-ternplot.
if you just do this:
[x,y]=meshgrid(0:0.1:1);
Fxy = 2*x.^2 +3 *y.^2;
ternpcolor(x(:),y(:),Fxy(:))
You get:
The thirds axis is created exactly as you say (1-x-y) inside the ternpcolor function. There are lots of things to "tune" here but I hope it is enough to get you started.
Here is a solution using R and my package ggtern. I have also included the points within proximity underneath, for the purpose of comparison.
library(ggtern)
Fxy = function(x,y){ 2*x^2 + 3*y^2 }
x = y = seq(0,1,length.out = 100)
df = expand.grid(x=x,y=y);
df$z = 1 - df$x - df$y
df = subset(df,z >= 0)
df$value = Fxy(df$x,df$y)
#The Intended Breaks
breaks = pretty(df$value,n=10)
#Create subset of the data, within close proximity to the breaks
df.sub = ldply(breaks,function(b,proximity = 0.02){
s = b - abs(proximity)/2; f = b + abs(proximity)/2
subset(df,value >= s & value <= f)
})
#Plot the ternary diagram
ggtern(df,aes(x,y,z)) +
theme_bw() +
geom_point(data=df.sub,alpha=0.5,color='red',shape=21) +
geom_interpolate_tern(aes(value = value,color=..level..), size = 1, n = 200,
breaks = c(breaks,max(df$value) - 0.01,min(df$value) + 0.01),
base = 'identity',
formula = value ~ poly(x,y,degree=2)) +
labs(title = "Contour Plot on Modelled Surface", x = "Left",y="Top",z="Right")
Which produces the following:
I have a histogram
hist(A, 801)
that currently resembles a normal curve but with max value at y = 1500, and mean at x = 0.5. I want to normalize it, so I tried
h = hist(A, 801)
h = h ./ sum(h)
bar(h)
now I get a normal curve with max at y = .03, but a mean at x = 450.
how do I decrease the frequency so the sum is 1, while retaining the same x range?
A is derived from
A = walk(50000, 800, .05, 2, .25, 0)
where
function [X_new] = walk(N_sim, N, mu, T, sigma, X_init)
delt = T/N;
up = sigma*sqrt(delt);
down = -sigma*sqrt(delt);
p = 1./2.*(1.+mu/sigma*sqrt(delt));
X_new = zeros(N_sim,1);
X_new(1:N_sim,1) = X_init;
ptest = zeros(N_sim,1);
for i = 1:N
ptest(:,1) = rand(N_sim,1);
ptest(:,1) = (ptest(:,1) <= p);
X_new(:,1) = X_new(:,1) + ptest(:,1)*up + (1.-ptest(:,1))*down;
end
The sum is 1 with your code as it stands.
You may want integral equal to 1 (so that you can compare with the theoretical pdf). In that case:
[h, c] = hist(A, 801); %// c contains bin centers. They are equally spaced
h = h / sum(h) / (c(2)-c(1)); %// normalize to area 1
trapz(c,h) %// compute integral. Should be approximately 1
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 would like to plot a plane using a vector that I calculated from 3 points where:
pointA = [0,0,0];
pointB = [-10,-20,10];
pointC = [10,20,10];
plane1 = cross(pointA-pointB, pointA-pointC)
How do I plot 'plane1' in 3D?
Here's an easy way to plot the plane using fill3:
points=[pointA' pointB' pointC']; % using the data given in the question
fill3(points(1,:),points(2,:),points(3,:),'r')
grid on
alpha(0.3)
You have already calculated the normal vector. Now you should decide what are the limits of your plane in x and z and create a rectangular patch.
An explanation : Each plane can be characterized by its normal vector (A,B,C) and another coefficient D. The equation of the plane is AX+BY+CZ+D=0. Cross product between two differences between points, cross(P3-P1,P2-P1) allows finding (A,B,C). In order to find D, simply put any point into the equation mentioned above:
D = -Ax-By-Cz;
Once you have the equation of the plane, you can take 4 points that lie on this plane, and draw the patch between them.
normal = cross(pointA-pointB, pointA-pointC); %# Calculate plane normal
%# Transform points to x,y,z
x = [pointA(1) pointB(1) pointC(1)];
y = [pointA(2) pointB(2) pointC(2)];
z = [pointA(3) pointB(3) pointC(3)];
%Find all coefficients of plane equation
A = normal(1); B = normal(2); C = normal(3);
D = -dot(normal,pointA);
%Decide on a suitable showing range
xLim = [min(x) max(x)];
zLim = [min(z) max(z)];
[X,Z] = meshgrid(xLim,zLim);
Y = (A * X + C * Z + D)/ (-B);
reOrder = [1 2 4 3];
figure();patch(X(reOrder),Y(reOrder),Z(reOrder),'b');
grid on;
alpha(0.3);
Here's what I came up with:
function [x, y, z] = plane_surf(normal, dist, size)
normal = normal / norm(normal);
center = normal * dist;
tangents = null(normal') * size;
res(1,1,:) = center + tangents * [-1;-1];
res(1,2,:) = center + tangents * [-1;1];
res(2,2,:) = center + tangents * [1;1];
res(2,1,:) = center + tangents * [1;-1];
x = squeeze(res(:,:,1));
y = squeeze(res(:,:,2));
z = squeeze(res(:,:,3));
end
Which you would use as:
normal = cross(pointA-pointB, pointA-pointC);
dist = dot(normal, pointA)
[x, y, z] = plane_surf(normal, dist, 30);
surf(x, y, z);
Which plots a square of side length 60 on the plane in question
I want to add to the answer given by Andrey Rubshtein, his code works perfectly well except at B=0. Here is the edited version of his code
Below Code works when A is not 0
normal = cross(pointA-pointB, pointA-pointC);
x = [pointA(1) pointB(1) pointC(1)];
y = [pointA(2) pointB(2) pointC(2)];
z = [pointA(3) pointB(3) pointC(3)];
A = normal(1); B = normal(2); C = normal(3);
D = -dot(normal,pointA);
zLim = [min(z) max(z)];
yLim = [min(y) max(y)];
[Y,Z] = meshgrid(yLim,zLim);
X = (C * Z + B * Y + D)/ (-A);
reOrder = [1 2 4 3];
figure();patch(X(reOrder),Y(reOrder),Z(reOrder),'r');
grid on;
alpha(0.3);
Below Code works when C is not 0
normal = cross(pointA-pointB, pointA-pointC);
x = [pointA(1) pointB(1) pointC(1)];
y = [pointA(2) pointB(2) pointC(2)];
z = [pointA(3) pointB(3) pointC(3)];
A = normal(1); B = normal(2); C = normal(3);
D = -dot(normal,pointA);
xLim = [min(x) max(x)];
yLim = [min(y) max(y)];
[Y,X] = meshgrid(yLim,xLim);
Z = (A * X + B * Y + D)/ (-C);
reOrder = [1 2 4 3];
figure();patch(X(reOrder),Y(reOrder),Z(reOrder),'r');
grid on;
alpha(0.3);
I am trying to reconstruct 3D-Coordinates from the 2D-Pixel-Coordinates in a Camera Picture using a side condition (in MatLab). I do have extrinsic and intrinsic camera parameters.
Using homogenous transformation I can transform 3D-Coordinates from a initial World Coordinate System to my Camera Corrdinate System. So here I have my extrinsic parameters in my Transform Matrix R_world_to_Camera:
R_world_to_Camera = [ r_11, r_12, r_13, t1;
r_21, r_22, r_23, t2;
r_31, r_32, r_33, t3;
0, 0, 0, 1];
For intrinsic parameters I used Caltech's "Camera Calibration Toolbox for MatLab" and got these parameters:
Calibration results (with uncertainties):
Focal Length: fc = [ 1017.21523 1012.54901 ] ± [ NaN NaN ]
Principal point: cc = [ 319.50000 239.50000 ] ± [ NaN NaN ]
Skew: alpha_c = [ 0.00000 ] ± [ NaN ] => angle of pixel axes = 90.00000 ± NaN degrees
Distortion: kc = [ 0.00000 0.00000 0.00000 0.00000 0.00000 ] ± [ NaN NaN NaN NaN NaN ]
Pixel error: err = [ 0.11596 0.14469 ]
Note: The numerical errors are approximately three times the standard deviations (for reference).
So I then get the Camera-Calibration-Matrix K (3x3)
K = [1.017215234570303e+03, 0, 3.195000000000000e+02;
0, 1.012549014668498e+03,2.395000000000000e+02;
0, 0, 1.0000];
and using this I can calculate the 3D -> 2D - Projection-Matrix P (3x4) with:
P = K * [eye(3), zeros(3,1)];
When converting a Point in World-Coordinates [X, Y, Z]_World I transform it first to Camera-Coordinates and then project it to 2D:
% Transformation
P_world = [X; Y; Z; 1]; % homogenous coordinates in World coordinate System
P_camera = R_world_to_Camera * [X; Y; Z; 1];
% Projection
P_pixels = P * camera;
P_pixels = P_pixels / P_pixels(3); % normalize coordinates
So my question now is how to reverse these steps? As side condition I want to set the Z-Coordinate to be known (zero in world-coordinates). I tried the solution proposed here on Stackoverflow, but somehow I get wrong coordinates. Any idea? Every help is appreciated a lot!!
You cannot reverse the step in general: depth and scale information is lost when 3D points are projected onto a 2D image. However if, as you indicate, all your 3D points are on the Z=0 plane, then getting them back from their projections is trivial: compute the inverse Ki = K^-1 of the camera matrix, and apply it to the image points in homogeneous coordinates.
P_camera = Ki * [u, v, 1]'
where [u, v] are the image coordinates, and the apostrophe denotes transposition.
The 3D points you want lie on the rays from the camera centre to the P_camera's. Express both in world coordinates:
P_world = [R|t]_camera_to_world * [P_camera, 1]'
C_world = [R|t]_camera_to_world * [0, 0, 0, 1]'
where [R|t] is the 4x4 coordinate transform.
Now, the set of points on each ray is expressed as
P = C_world + lambda * P_world;
where lambda is a scalar (the coordinate along the ray). You can now impose the condition that P(3) = 0 to find the value of lambda that places your points on the Z = 0 plane.
thanks to some red wine and thorough reading I found the answer in a (german) Master Thesis :)
My "testcode" is as follows:
% Clean-Up First
clear all;
close all;
clc;
% Caamera-Calibration-Matrix
K = [1.017215234570303e+03, 0, 3.195000000000000e+02, 0;
0, 1.012549014668498e+03,2.395000000000000e+02, 0;
0, 0, 1.0000, 0;
0, 0, 0, 0];
% Transforma Matrix from 3D-World-Coordinate System to 3D-Camera-Coordinate System (Origin on CCD-Chip)
%
% The Camera is oriented "looking" into positive X-Direction of the World-Coordinate-System. On the picture,
% positive Y-Direction will be to the left, positive Z-Direction to the top. (right hand coordinate system!)
R_World_to_Cam = [-0.0113242625465167 -0.999822053685344 0.0151163536128891 141.173585444427;
0.00842007509644635 -0.0152123858102325 -0.999848810645587 1611.96528372161;
0.999900032304804 -0.0111955728474261 0.00859117128537919 847.090629282911;
0 0 0 1];
% Projection- and Transforma Matrix P
P = K * R_World_to_Cam;
% arbitrary Points X_World in World-Coordinate-System [mm] (homogenous Coordinates)
% forming a square of size 10m x 4m
X_World_1 = [20000; 2000; 0; 1];
X_World_2 = [20000; -2000; 0; 1];
X_World_3 = [10000; 2000; 0; 1];
X_World_4 = [10000; -2000; 0; 1];
% Transform and Project from 3D-World -> 2D-Picture
X_Pic_1 = P * X_World_1;
X_Pic_2 = P * X_World_2;
X_Pic_3 = P * X_World_3;
X_Pic_4 = P * X_World_4;
% normalize homogenous Coordinates (3rd Element has to be 1!)
X_Pic_1 = X_Pic_1 / X_Pic_1(3);
X_Pic_2 = X_Pic_2 / X_Pic_2(3);
X_Pic_3 = X_Pic_3 / X_Pic_3(3);
X_Pic_4 = X_Pic_4 / X_Pic_4(3);
% Now for reverse procedure take arbitrary points in Camera-Picture...
% (for simplicity, take points from above and "go" 30px to the right and 40px down)
X_Pic_backtransform_1 = X_Pic_1(1:3) + [30; 40; 0];
X_Pic_backtransform_2 = X_Pic_2(1:3) + [30; 40; 0];
X_Pic_backtransform_3 = X_Pic_3(1:3) + [30; 40; 0];
X_Pic_backtransform_4 = X_Pic_4(1:3) + [30; 40; 0];
% ... and transform back following the formula from the Master Thesis (in German):
% Ilker Savas, "Entwicklung eines Systems zur visuellen Positionsbestimmung von Interaktionspartnern"
M_Mat = P(1:3,1:3); % Matrix M is the "top-front" 3x3 part
p_4 = P(1:3,4); % Vector p_4 is the "top-rear" 1x3 part
C_tilde = - inv( M_Mat ) * p_4; % calculate C_tilde
% Invert Projection with Side-Condition ( Z = 0 ) and Transform back to
% World-Coordinate-System
X_Tilde_1 = inv( M_Mat ) * X_Pic_backtransform_1;
X_Tilde_2 = inv( M_Mat ) * X_Pic_backtransform_2;
X_Tilde_3 = inv( M_Mat ) * X_Pic_backtransform_3;
X_Tilde_4 = inv( M_Mat ) * X_Pic_backtransform_4;
mue_N_1 = -C_tilde(3) / X_Tilde_1(3);
mue_N_2 = -C_tilde(3) / X_Tilde_2(3);
mue_N_3 = -C_tilde(3) / X_Tilde_3(3);
mue_N_4 = -C_tilde(3) / X_Tilde_4(3);
% Do the inversion of above steps...
X_World_backtransform_1 = mue_N_1 * inv( M_Mat ) * X_Pic_backtransform_1 + C_tilde;
X_World_backtransform_2 = mue_N_2 * inv( M_Mat ) * X_Pic_backtransform_2 + C_tilde;
X_World_backtransform_3 = mue_N_3 * inv( M_Mat ) * X_Pic_backtransform_3 + C_tilde;
X_World_backtransform_4 = mue_N_4 * inv( M_Mat ) * X_Pic_backtransform_4 + C_tilde;
% Plot everything
figure(1);
% First Bird Perspective of World-Coordinate System...
subplot(1,2,1);
xlabel('Y-World');
ylabel('X-World');
grid on;
axis([-3000 3000 0 22000]);
hold on;
plot( -X_World_1(2), X_World_1(1), 'bo' );
plot( -X_World_2(2), X_World_2(1), 'bo' );
plot( -X_World_3(2), X_World_3(1), 'bo' );
plot( -X_World_4(2), X_World_4(1), 'bo' );
line([-X_World_1(2) -X_World_2(2) -X_World_4(2) -X_World_3(2) -X_World_1(2)], [X_World_1(1) X_World_2(1) X_World_4(1) X_World_3(1) X_World_1(1)], 'Color', 'blue' );
plot( -X_World_backtransform_1(2), X_World_backtransform_1(1), 'ro' );
plot( -X_World_backtransform_2(2), X_World_backtransform_2(1), 'ro' );
plot( -X_World_backtransform_3(2), X_World_backtransform_3(1), 'ro' );
plot( -X_World_backtransform_4(2), X_World_backtransform_4(1), 'ro' );
line([-X_World_backtransform_1(2) -X_World_backtransform_2(2) -X_World_backtransform_4(2) -X_World_backtransform_3(2) -X_World_backtransform_1(2)], [X_World_backtransform_1(1) X_World_backtransform_2(1) X_World_backtransform_4(1) X_World_backtransform_3(1) X_World_backtransform_1(1)], 'Color', 'red' );
hold off;
% ...then the camera picture (perspective!)
subplot(1,2,2);
hold on;
image(ones(480,640).*255);
colormap(gray(256));
axis([0 640 -480 0]);
line([X_Pic_1(1) X_Pic_2(1) X_Pic_4(1) X_Pic_3(1) X_Pic_1(1)], -1*[X_Pic_1(2) X_Pic_2(2) X_Pic_4(2) X_Pic_3(2) X_Pic_1(2)], 'Color', 'blue' );
line([X_Pic_backtransform_1(1) X_Pic_backtransform_2(1) X_Pic_backtransform_4(1) X_Pic_backtransform_3(1) X_Pic_backtransform_1(1)], -1*[X_Pic_backtransform_1(2) X_Pic_backtransform_2(2) X_Pic_backtransform_4(2) X_Pic_backtransform_3(2) X_Pic_backtransform_1(2)], 'Color', 'red' );
hold off;