Moving a star marker along a hexagon trajectory? - matlab

I want to move a star marker along hexagon trajectory similar to "Circle trajectory" that I have added at the end of my question. Thanks.
This is the source code to that I have written yet for creating concentric hegzagons but I don't know how to move a star marker which traverses the concentric hexagons, I had written a similar simulation code for circle trajectory but I couldn't do it for hexagon.
%clc; % Clear the command window.
%close all; % Close all figures (except those of imtool.)
%clear; % Erase all existing variables. Or clearvars if you want.
workspace; % Make sure the workspace panel is showing.
format long g;
format compact;
fontSize = 20;
angles = linspace(0, 360, 7);
radii = [20, 35, 50,70];
% First create and draw the hexagons.
numberOfHexagons = 4;
% Define x and y arrays. Each row is one hexagon.
% Columns are the vertices.
x1=radii(1) * cosd(angles)+50;
y1 = radii(1) * sind(angles)+50;
x2=radii(2) * cosd(angles)+50;
y2 = radii(2) * sind(angles)+50;
x3=radii(3) * cosd(angles)+50;
y3 = radii(3) * sind(angles)+50;
x4=radii(4) * cosd(angles)+50;
y4 = radii(4) * sind(angles)+50;
plot(x1 , y1, 'b');
hold on
plot(x2, y2, 'b');
hold on
plot(x3, y3, 'b');
hold on
plot(x4, y4, 'b');
hold on
% Connecting Line:
plot([70 100], [50 50],'color','b')
axis([0 100 0 100])
hold on
Circle trajectory:
% Initialization steps.
format long g;
format compact;
fontSize = 20;
r1 = 50;
r2 = 35;
r3= 20;
xc = 50;
yc = 50;
% Since arclength = radius * (angle in radians),
% (angle in radians) = arclength / radius = 5 / radius.
deltaAngle1 = 5 / r1;
deltaAngle2 = 5 / r2;
deltaAngle3 = 5 / r3;
theta1 = 0 : deltaAngle1 : (2 * pi);
theta2 = 0 : deltaAngle2 : (2 * pi);
theta3 = 0 : deltaAngle3 : (2 * pi);
x1 = r1*cos(theta1) + xc;
y1 = r1*sin(theta1) + yc;
x2 = r2*cos(theta2) + xc;
y2 = r2*sin(theta2) + yc;
x3 = r3*cos(theta3) + xc;
y3 = r3*sin(theta3) + yc;
plot(x1,y1,'color',[1 0.5 0])
hold on
plot(x2,y2,'color',[1 0.5 0])
hold on
plot(x3,y3,'color',[1 0.5 0])
hold on
% Connecting Line:
plot([70 100], [50 50],'color',[1 0.5 0])
% Set up figure properties:
% Enlarge figure to full screen.
set(gcf, 'Units', 'Normalized', 'OuterPosition', [0, 0, 1, 1]);
drawnow;
axis square;
for i = 1 : length(theta1)
plot(x1(i),y1(i),'r*')
pause(0.1)
end
for i = 1 : length(theta2)
plot(x2(i),y2(i),'r*')
pause(0.1)
end
for i = 1 : length(theta3)
plot(x3(i),y3(i),'r*')
pause(0.1)
end

I would generalize your problem with parametric function for the trajectory. In it use rotation kernel which you want here few examples in C++/VCL/GDI (sorry I am not Matlab friendly but the equations should be the same in Matlab too) for circle,square and hexagon rotation kernels:
void getpnt_circle(double &x,double &y,double &pi2,double r,double t) // (x,y) = circle(r,t) t=<0,1>
{
pi2=2.0*M_PI; // circumference(r=1) 6.283185307179586476925286766559
t*=pi2;
x=r*cos(t);
y=r*sin(t);
}
//---------------------------------------------------------------------------
void getpnt_square(double &x,double &y,double &pi2,double r,double t) // (x,y) = square(r,t) t=<0,1>
{
pi2=8.0; // circumference(r=1)
// kernel
const int n=4; // sides
const double x0[n]={+1.0,+1.0,-1.0,-1.0}; // side start point
const double y0[n]={-1.0,+1.0,+1.0,-1.0};
const double dx[n]={ 0.0,-2.0, 0.0,+2.0}; // side tangent
const double dy[n]={+2.0, 0.0,-2.0, 0.0};
int ix;
t-=floor(t); // t = <0, 1.0)
t*=n; // t = <0,n)
ix=floor(t); // side of square
t-=ix; // distance from side start
x=r*(x0[ix]+t*dx[ix]);
y=r*(y0[ix]+t*dy[ix]);
}
//---------------------------------------------------------------------------
void getpnt_hexagon(double &x,double &y,double &pi2,double r,double t) // (x,y) = square(r,t) t=<0,1>
{
pi2=6.0; // circumference(r=1)
// kernel
const int n=6; // sides
const double c60=cos(60.0*M_PI/180.0);
const double s60=sin(60.0*M_PI/180.0);
const double x0[n]={+1.0,+c60,-c60,-1.0,-c60,+c60}; // side start point
const double y0[n]={ 0.0,+s60,+s60, 0.0,-s60,-s60};
const double dx[n]={-c60,-1.0,-c60,+c60,+1.0,+c60}; // side tangent
const double dy[n]={+s60, 0.0,-s60,-s60, 0.0,+s60};
int ix;
t-=floor(t); // t = <0, 1.0)
t*=n; // t = <0,n)
ix=floor(t); // side of square
t-=ix; // distance from side start
x=r*(x0[ix]+t*dx[ix]);
y=r*(y0[ix]+t*dy[ix]);
}
//---------------------------------------------------------------------------
void TMain::draw()
{
if (!_redraw) return;
// clear buffer
bmp->Canvas->Brush->Color=clBlack;
bmp->Canvas->FillRect(TRect(0,0,xs,ys));
int e;
double r,t,x,y,c,dr=15.0,dl=15.0;
int xx,yy,rr=3;
bmp->Canvas->MoveTo(xs2,ys2);
bmp->Canvas->Pen->Color=clAqua;
bmp->Canvas->Brush->Color=clBlue;
for (r=dr,t=0.0;;)
{
// get point from selected kernel
// getpnt_circle (x,y,c,r,t);
// getpnt_square (x,y,c,r,t);
getpnt_hexagon(x,y,c,r,t);
// render it
xx=xs2+x;
yy=ys2+y;
bmp->Canvas->LineTo(xx,yy);
bmp->Canvas->Ellipse(xx-rr,yy-rr,xx+rr,yy+rr);
// update position
r+=dr*dr/(r*c);
t+=dl/(r*c); t-=floor(t);
if (r>=xs2) break;
if (r>=ys2) break;
}
// render backbuffer
Main->Canvas->Draw(0,0,bmp);
_redraw=false;
}
//---------------------------------------------------------------------------
You can ignore the VCL/GDI rendering stuff.
xs,ys is full and xs2,ys2 is half resolution of window to scale the plot properly...
dl is distance between markers [pixels]
dr is distance between spiral screws [pixels]
the spiral is sampled with r,t step depending on the actual circumference (that is what the pi2 or c is for). The getpnt_xxxxx function will return x,y coordinate of your shape from parameter t=<0,1> and actual radius r. It also returns the actual value of circumference/r ratio called pi2
Here preview of the 3 kernels used for spiral ...

Related

Can I compare the current value with previous value in a while loop?

I use a while loop to move the random points each one second as a shot from position to position. Now, how can I compare sigdB as calculated in the current step with sigdB from the previous step?
npts=1;center=[0 0];radius=1000; npts2=1;center2=[0 0];radius2=1000;
velocity = 45/3.6; velocity2 = 45/3.6;
theta = rand(npts, 1) * 2*pi;
g = 0.5 * radius + 0.5 * radius * rand(npts,1);
X_x=center(1)+g.*cos(theta); Y_y=center(2)+g.*sin(theta); XY = [X_x ,Y_y];
theta2 = rand(npts2, 1) * 2*pi; g2 = 0.5 * radius2 + 0.5 * radius2 * rand(npts2,1);
X_x2=center2(1)+g2.*cos(theta2); Y_y2=center2(2)+g.*sin(theta2); XY2 = [X_x2 ,Y_y2];
hfig = figure('Color', 'w'); hax = axes('Parent', hfig);
hdots(1) = plot(XY(1,1),XY(1,2),'Parent', hax,'Marker', '.','Color', 'k','LineStyle', 'none','MarkerSize', 10); hold(hax, 'on'); axis(hax, 'equal');
hdots(2) = plot(XY2(1,1),XY2(1,2),'Parent', hax,'Marker', '.','Color', 'r','LineStyle', 'none','MarkerSize', 10); hold(hax, 'on'); axis(hax, 'equal');
% plot circle to scatter the points in it
t = linspace(0, 2*pi, 100); plot(radius * cos(t) + center(1),radius * sin(t) + center(2))
while all(ishghandle(hdots))
distPoints = pdist2(XY,XY2,'euclidean');
sig=sum(power*(distPoints).^-2);
sigdB=10*log(sig);
direction2 = rand(npts, 1) * 2 *pi; direction = rand(npts, 1) * 2 *pi;
[XY2, direction2] = step(XY2, direction2, velocity2, radius2, center2); [XY, direction] = step(XY, direction, velocity, radius, center);
set(hdots(2), 'XData', XY2(1,1), 'YData', XY2(1,2)); set(hdots(1), 'XData', XY(1,1), 'YData', XY(1,2)); drawnow; pause (1);
end
The moment you call sigdB = (..) you overwrite its previous value. If you want to compare only the previous one, you could use a simple trick to store it separately:
while
sigdB_old = sigdB;
sigdB = 10*log(..);
% More calculations
end
i.e. store it in a temporary variable. If, on the other hand, you want to retain all previous values, you should store it in an array of appropriate size, if possible. This is usually easier in a for loop, given you know how many iterations will take place, but you could simply initialise your array "pretty large" and extend or cut-off rows/columns where needed.

Plot equally spaced markers along a spiral

I want to move a red star marker along the spiral trajectory with an equal distance of 5 units between the red star points on its circumference like in the below image.
vertspacing = 10;
horzspacing = 10;
thetamax = 10*pi;
% Calculation of (x,y) - underlying archimedean spiral.
b = vertspacing/2/pi;
theta = 0:0.01:thetamax;
x = b*theta.*cos(theta)+50;
y = b*theta.*sin(theta)+50;
% Calculation of equidistant (xi,yi) points on spiral.
smax = 0.5*b*thetamax.*thetamax;
s = 0:horzspacing:smax;
thetai = sqrt(2*s/b);
xi = b*thetai.*cos(thetai);
yi = b*thetai.*sin(thetai);
plot(x,y,'b-');
hold on
I want to get a figure that looks like the following:
This is my code for the circle trajectory:
% Initialization steps.
format long g;
format compact;
fontSize = 20;
r1 = 50;
r2 = 35;
r3= 20;
xc = 50;
yc = 50;
% Since arclength = radius * (angle in radians),
% (angle in radians) = arclength / radius = 5 / radius.
deltaAngle1 = 5 / r1;
deltaAngle2 = 5 / r2;
deltaAngle3 = 5 / r3;
theta1 = 0 : deltaAngle1 : (2 * pi);
theta2 = 0 : deltaAngle2 : (2 * pi);
theta3 = 0 : deltaAngle3 : (2 * pi);
x1 = r1*cos(theta1) + xc;
y1 = r1*sin(theta1) + yc;
x2 = r2*cos(theta2) + xc;
y2 = r2*sin(theta2) + yc;
x3 = r3*cos(theta3) + xc;
y3 = r3*sin(theta3) + yc;
plot(x1,y1,'color',[1 0.5 0])
hold on
plot(x2,y2,'color',[1 0.5 0])
hold on
plot(x3,y3,'color',[1 0.5 0])
hold on
% Connecting Line:
plot([70 100], [50 50],'color',[1 0.5 0])
% Set up figure properties:
% Enlarge figure to full screen.
set(gcf, 'Units', 'Normalized', 'OuterPosition', [0, 0, 1, 1]);
drawnow;
axis square;
for i = 1 : length(theta1)
plot(x1(i),y1(i),'r*')
pause(0.1)
end
for i = 1 : length(theta2)
plot(x2(i),y2(i),'r*')
pause(0.1)
end
for i = 1 : length(theta3)
plot(x3(i),y3(i),'r*')
pause(0.1)
end
I can't think of a way to compute distance along a spiral, so I'm approximating it with circles, in hopes that it will still be useful.
My solution relies on the InterX function from FEX, to find the intersection of circles with the spiral. I am providing an animation so it is easier to understand.
The code (tested on R2017a):
function [x,y,xi,yi] = q44916610(doPlot)
%% Input handling:
if nargin < 1 || isempty(doPlot)
doPlot = false;
end
%% Initialization:
origin = [50,50];
vertspacing = 10;
thetamax = 5*(2*pi);
%% Calculation of (x,y) - underlying archimedean spiral.
b = vertspacing/(2*pi);
theta = 0:0.01:thetamax;
x = b*theta.*cos(theta) + origin(1);
y = b*theta.*sin(theta) + origin(2);
%% Calculation of equidistant (xi,yi) points on spiral.
DST = 5; cRes = 360;
numPts = ceil(vertspacing*thetamax); % Preallocation
[xi,yi] = deal(NaN(numPts,1));
if doPlot && isHG2() % Plots are only enabled if the MATLAB version is new enough.
figure(); plot(x,y,'b-'); hold on; axis equal; grid on; grid minor;
hAx = gca; hAx.XLim = [-5 105]; hAx.YLim = [-5 105];
hP = plot(xi,yi,'r*');
else
hP = struct('XData',xi,'YData',yi);
end
hP.XData(1) = origin(1); hP.YData(1) = origin(2);
for ind = 2:numPts
P = InterX([x;y], makeCircle([hP.XData(ind-1),hP.YData(ind-1)],DST/2,cRes));
[~,I] = max(abs(P(1,:)-origin(1)+1i*(P(2,:)-origin(2))));
if doPlot, pause(0.1); end
hP.XData(ind) = P(1,I); hP.YData(ind) = P(2,I);
if doPlot, pause(0.1); delete(hAx.Children(1)); end
end
xi = hP.XData(~isnan(hP.XData)); yi = hP.YData(~isnan(hP.YData));
%% Nested function(s):
function [XY] = makeCircle(cnt, R, nPts)
P = (cnt(1)+1i*cnt(2))+R*exp(linspace(0,1,nPts)*pi*2i);
if doPlot, plot(P,'Color',lines(1)); end
XY = [real(P); imag(P)];
end
end
%% Local function(s):
function tf = isHG2()
try
tf = ~verLessThan('MATLAB', '8.4');
catch
tf = false;
end
end
function P = InterX(L1,varargin)
% DOCUMENTATION REMOVED. For a full version go to:
% https://www.mathworks.com/matlabcentral/fileexchange/22441-curve-intersections
narginchk(1,2);
if nargin == 1
L2 = L1; hF = #lt; %...Avoid the inclusion of common points
else
L2 = varargin{1}; hF = #le;
end
%...Preliminary stuff
x1 = L1(1,:)'; x2 = L2(1,:);
y1 = L1(2,:)'; y2 = L2(2,:);
dx1 = diff(x1); dy1 = diff(y1);
dx2 = diff(x2); dy2 = diff(y2);
%...Determine 'signed distances'
S1 = dx1.*y1(1:end-1) - dy1.*x1(1:end-1);
S2 = dx2.*y2(1:end-1) - dy2.*x2(1:end-1);
C1 = feval(hF,D(bsxfun(#times,dx1,y2)-bsxfun(#times,dy1,x2),S1),0);
C2 = feval(hF,D((bsxfun(#times,y1,dx2)-bsxfun(#times,x1,dy2))',S2'),0)';
%...Obtain the segments where an intersection is expected
[i,j] = find(C1 & C2);
if isempty(i), P = zeros(2,0); return; end
%...Transpose and prepare for output
i=i'; dx2=dx2'; dy2=dy2'; S2 = S2';
L = dy2(j).*dx1(i) - dy1(i).*dx2(j);
i = i(L~=0); j=j(L~=0); L=L(L~=0); %...Avoid divisions by 0
%...Solve system of eqs to get the common points
P = unique([dx2(j).*S1(i) - dx1(i).*S2(j), ...
dy2(j).*S1(i) - dy1(i).*S2(j)]./[L L],'rows')';
function u = D(x,y)
u = bsxfun(#minus,x(:,1:end-1),y).*bsxfun(#minus,x(:,2:end),y);
end
end
Result:
Note that in the animation above, the diameter of the circle (and hence the distance between the red points) is 10 and not 5.

MATLAB's lsqnonlin solver moves away from solution

I have a fitting problem that I am trying to solve using MATLAB's lsqnonlin function and it's doing a pretty poor job. In particular, if I give it the actual solution as the starting point, the solver proceeds to move away from there (despite the fitting function returning a vector of all zeroes for that input) and produces something awful. My code is below, but it's a bit long, so I apologise for that, but I'm not sure exactly what it is about this particular situation that causes this behaviour as in others it's fine.
fitting_demo.m:
% Parameters for image
imagemat = zeros(32, 32);
x_centre = 5; y_centre = -10; radius = 3; psf_sigma = 1; height = 1;
% Generate image to fit
actual_image = image_sphere_thin(x_centre, y_centre, radius, psf_sigma, height, imagemat);
figure
% Plot actual image
subplot(2, 1, 1)
imagesc(actual_image)
axis equal
title('Actual image')
% Set up fitting function
x0 = [x_centre, y_centre, radius, psf_sigma, height]; % The solver starts from the solution!
f = #(x)fit_image_sphere_thin(x, imagemat);
% Fit
options = optimoptions(#lsqnonlin, 'Display', 'iter');
[fit, resnorm, residual, exitflag, output] = lsqnonlin(f, x0, [], [], options);
% Display image generated using fitted parameters
subplot(2, 1, 2)
imagesc(image_sphere_thin(fit(1), fit(2), fit(3), fit(4), fit(5), imagemat))
axis equal
title('Fitted image')
fit_image_sphere_thin.m:
function deltas = fit_image_sphere_thin(x0, actual_image)
x_centre = x0(1);
y_centre = x0(2);
radius = x0(3);
% psf_sigma = x0(4); % Commented out to stop crazy things
psf_sigma = 1;
% height = x0(5); % Commented out to stop crazy things
height = 1;
imagemat = zeros(size(actual_image));
sim_image = image_sphere_thin(x_centre, y_centre, radius, psf_sigma, height, imagemat);
sim_vector = sim_image(:);
actual_vector = actual_image(:);
deltas = actual_vector - sim_vector;
end
image_sphere_thin.m:
function I = image_sphere_thin(x_centre, y_centre, radius, psf_sigma, height, imagemat)
image_width = size(imagemat, 2);
image_height = size(imagemat, 1);
image_centre_x = image_width / 2;
image_centre_y = image_height / 2;
x = (1:image_width) - image_centre_x;
y = -(1:image_height) + image_centre_y;
[x, y] = meshgrid(x, y);
X = [x(:) y(:)];
image_vector = cross_section_sphere_thin(x_centre, y_centre, radius, psf_sigma, height, X);
imagemat = reshape(image_vector, size(imagemat));
I = imagemat;
end
cross_section_sphere_thin.m:
function I = cross_section_sphere_thin(x_centre, y_centre, radius, psf_sigma, height, X)
r = sqrt((X(:, 1) - x_centre).^2 + (X(:, 2) - y_centre).^2);
psf_variance = psf_sigma^2;
I = height * (exp(-(r - radius).^2 / (2 * psf_sigma)) - exp(-(r + radius).^2 / (2 * psf_sigma))) ./ r;
I(r == 0) = (2 * radius * height / psf_variance) * exp(-(radius^2) / (2 * psf_variance));
end

How to plot an equilateral color triangle?

I would like to make a color plot in MATLAB similar to this plot:
I have managed to create all the points [x,y] needed in order to create the vertexes and I have a map with colors of each vertex, so I can get the following.
But I do not get it how to make the axis work.
Code so far:
% Equilateral grid
tcorner = [0.0, 0.5, 1.0;
0.0, 1.0*sqrt(3)/2, 0.0];
tg = triangle_grid( 1/0.05, tcorner );
tgx = tg(1,:);
tgy = tg(2,:);
% Create triangles
tri = delaunay(tgx,tgy);
% Plot
h = trisurf(tri, tgx, tgy, colorvector);
And the grid function:
function triangle_grid(n, tcorner)
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
end
The main problem is that you can't rotate the axes to the right position, because the always flip to the lower side. So, you need to create them.
Here is how:
% Equilateral grid
tcorner = [0.0, 0.5, 1.0; % x
0.0, 1.0*sqrt(3)/2, 0.0]; % y
tg = triangle_grid( 1/0.05, tcorner);
tgx = tg(1,:);
tgy = tg(2,:);
% Create triangles
tri = delaunay(tgx,tgy);
col = rand(size(tgx));
trisurf(tri,tgx,tgy,col)
view(0,90)
colormap('lines')
% setting the axes:
ax = gca;
grid off
ax.YAxis.Visible = 'off';
ticks = (0:20:80).';
% bottom axis:
tickpos = linspace(tcorner(1,1),tcorner(1,3),numel(ticks)+1);
ax.XAxis.FontSize = 14;
ax.XAxis.TickValues = tickpos(1:end-1);
ax.XAxis.TickLabels = ticks;
ax.XAxis.TickLabelRotation = 45;
xlabel('X axis title');
% left & right axis:
ticksxpos = linspace(tcorner(1,1),tcorner(1,3),numel(ticks)*2+1);
ticksypos = linspace(tcorner(2,1),tcorner(2,2),numel(ticks)+1);
text(ticksxpos(numel(ticks)+1:-1:2)-0.03,... % left
ticksypos(end:-1:2)+0.03,...
num2str(ticks),'FontSize',14,...
'VerticalAlignment','bottom',...
'HorizontalAlignment','left',...
'Rotation',-45)
text(ticksxpos(end:-1:numel(ticks)+2)+0.05,... % right
ticksypos(1:end-1)-0.03,...
num2str(ticks),'FontSize',14,...
'VerticalAlignment','bottom',...
'HorizontalAlignment','right')
ax.Parent.Color = 'w';
% titles:
text(tcorner(1,2)/2-0.06,tcorner(2,2)/2+0.06,...
'Left title','FontSize',14,...
'HorizontalAlignment','center',...
'Rotation',45)
text(tcorner(1,2)+tcorner(1,2)/2+0.06,tcorner(2,2)/2+0.06,...
'Right title','FontSize',14,...
'HorizontalAlignment','center',...
'Rotation',-45)
and we get...

Reconstruct 3D-Coordinates in Camera Coordinate System from 2D - Pixels with side condition

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;