Interpolate and plot a surface in a rotated coordinated system

I have a set of 3D measurement datapoints. I have already scatter plotted this data in coord system XYZ, and fit a plane through it.

Now, I want to interpolate and plot a surface through the datapoints, but do so in the new rotated coordinate system X'Y'Z' of the fit plane (which is roughly z = y - constant for my dataset).

My code below shows my attempt at using meshgrid -> griddata -> surf. The problem is most noticeable on the biggest outlier datapoints. The bumps in the interpolated surface aren't normal to the plane since griddata treats z = f(x,y). In other words, a want the residuals of my surface fit to be normal to the plan, not be vertical lines.

I suspect the answer will be to use grid data to fit a hypersurface but I have not had success with this either, not sure how to handle z and v separately. [vq = griddata(x,y,z,v,xq,yq,zq)]

clear; clc; close all;
load('measurement_data.mat');
fig1 = figure(1); hold on; grid on;
scatter3(x,y,z,300,[0.7 0 0],'Marker','.');

% fit a plane through the data and plot
[n,~,p] = affine_fit([x y z]); % Version 1.0.0.0 (894 Bytes) by Fangdi Sun, MATLAB File Exchange
[xw,yw] = meshgrid(linspace(min(x),max(x),2),linspace(min(y),max(y),2));
zw = -n(1)/n(3)*xw - n(2)/n(3)*yw + dot(n,p)/n(3);
surf(xw,yw,zw,'facecolor',[0.7 0 0],'facealpha',0.5);

% interpolate
[xq,yq] = meshgrid(linspace(min(x), max(x), 100), linspace(min(y), max(y), 100));
zq = griddata(x,y,z,xq,yq,'v4');
surf(xq,yq,zq,'FaceColor',[1 0 0],'FaceAlpha',0.3,'EdgeColor','none');
axis equal; axis manual;

Answer 1

[Bruno Luong]

Try this and see if it's OK for you

load('measurement_data.mat');

% fit a plane through the data and plot
xyz = [x y z];
[n,p] = normalfit(xyz); % function defined bellow

% Plane frame
V = eye(3);
V = V(:,[3 1 2]);
V(:,1) = n;
[V,~] = qr(V);
if det(V) < 0
    V(:,1) = -V(:,1);
end
V = V(:,[2 3 1]);

% Coordinares in the plane "frame"
xyzR = (xyz-p)*V;
xR = xyzR(:,1);
yR = xyzR(:,2);
zR = xyzR(:,3);
% create grid points
[minxR,maxxR] = bounds(xR);
[minyR,maxyR] = bounds(yR);
nx = 60;
ny = 60;
[xqgR,yqgR,zqgR] = meshgrid(linspace(minxR,maxxR,nx),linspace(minyR,maxyR,ny),mean(zR,'all'));

% Rotate back to origin coordinates
xyzgR = [xqgR(:) yqgR(:) zqgR(:)];
xyzg = p+xyzgR*V';
xg = reshape(xyzg(:,1),size(xqgR));
yg = reshape(xyzg(:,2),size(yqgR));
zg = reshape(xyzg(:,3),size(zqgR));
% Extract the 4 corners and wrap to close the rectangle
xrec = g2corner(xg);
yrec = g2corner(yg);
zrec = g2corner(zg);

% interpolate in rotates plane frame
zqR = griddata(xR,yR,zR,xqgR,yqgR,'v4');

Warning: Duplicate x-y data points detected: using average values for duplicate points.

xyzR = [xqgR(:) yqgR(:) zqR(:)];
% Rotate back the interpolation to origin coordinates
xyzg = p+xyzR*V';
xg = reshape(xyzg(:,1),size(xqgR));
yg = reshape(xyzg(:,2),size(yqgR));
zq = reshape(xyzg(:,3),size(zqgR));

%%
fig1 = figure(); hold on; grid on;
% data
scatter3(x,y,z,300,[0.7 0 0],'Marker','.');
% rectangle box
plot3(xrec, yrec, zrec, "k", 'LineWidth', 2);
% interpolation surface
surf(xg,yg,zq,'FaceColor',[1 0 0],'FaceAlpha',0.3) %,'EdgeColor','none');
axis equal;
view(50,10)

function xR = g2corner(xqgR)
xR = [xqgR(1,1) xqgR(1,end) xqgR(end,end) xqgR(end,1)  xqgR(1,1)];
end

function [n,p] = normalfit(xyz)
p = mean(xyz,1);
[~,~,V] = svd(xyz-p);
n = V(:,3);
end

[Bruno Luong]

Using ransac to make the robust fit.

load('measurement_data.mat');
% fit a plane through the data and plot
xyz = [x y z];
[model, inlier] = ransac([x y z], @fitFcn, @distFcn, 3, 16);
n = model.n;
p = model.p;
outlier = ~inlier;
fprintf('number of outlier = %d/%d\n', nnz(outlier), numel(outlier))

number of outlier = 25/301

% Plane frame
V = eye(3);
V = V(:,[3 1 2]);
V(:,1) = n;
[V,~] = qr(V);
if det(V) < 0
    V(:,1) = -V(:,1);
end
V = V(:,[2 3 1]);

% Coordinares in the plane "frame"
xyzR = (xyz-p)*V;
xR = xyzR(:,1);
yR = xyzR(:,2);
zR = xyzR(:,3);
% create grid points
[minxR,maxxR] = bounds(xR);
[minyR,maxyR] = bounds(yR);
nx = 60;
ny = 60;
[xqgR,yqgR,zqgR] = meshgrid(linspace(minxR,maxxR,nx),linspace(minyR,maxyR,ny),mean(zR,'all'));

% Rotate back to origin coordinates
xyzgR = [xqgR(:) yqgR(:) zqgR(:)];
xyzg = p+xyzgR*V';
xg = reshape(xyzg(:,1),size(xqgR));
yg = reshape(xyzg(:,2),size(yqgR));
zg = reshape(xyzg(:,3),size(zqgR));
% Extract the 4 corners and wrap to close the rectangle
xrec = g2corner(xg);
yrec = g2corner(yg);
zrec = g2corner(zg);

% interpolate in rotates plane frame
zqR = griddata(xR,yR,zR,xqgR,yqgR,'v4');

Warning: Duplicate x-y data points detected: using average values for duplicate points.

xyzR = [xqgR(:) yqgR(:) zqR(:)];
% Rotate back the interpolation to origin coordinates
xyzg = p+xyzR*V';
xg = reshape(xyzg(:,1),size(xqgR));
yg = reshape(xyzg(:,2),size(yqgR));
zq = reshape(xyzg(:,3),size(zqgR));

%%
fig1 = figure(); hold on; grid on;
% data
scatter3(x,y,z,300,[0.7 0 0],'Marker','.');
% rectangle box
plot3(xrec, yrec, zrec, "k", 'LineWidth', 2);
% interpolation surface
surf(xg,yg,zq,'FaceColor',[1 0 0],'FaceAlpha',0.3) %,'EdgeColor','none');
axis equal;
view(50,10)

function xR = g2corner(xqgR)
xR = [xqgR(1,1) xqgR(1,end) xqgR(end,end) xqgR(end,1)  xqgR(1,1)];
end

function [n,p] = normalfit(xyz)
p = mean(xyz,1);
[~,~,V] = svd(xyz-p);
n = V(:,3);
end

function model = fitFcn(xyz)
[n,p] = normalfit(xyz);
model = struct('n',n,'p',p);
end

function distances = distFcn(model, xyz)
n = model.n;
p = model.p;
distances = abs((xyz-p) * n);
end

[Benjamin]

Accepted this answer becase the surface fit is aligned in the coordinate system, and utilizes v4 method which is C2 continuous

Answer 2

[Matt J]

I'm not familiar with the FEX submission you used, but I can show how I would do it with this one,

https://www.mathworks.com/matlabcentral/fileexchange/87584-object-oriented-tools-to-fit-plot-conics-and-quadrics

load('measurement_data.mat');
fig1 = figure(1); hold on; grid on;
scatter3(x,y,z,300,[0.7 0 0],'Marker','.');

%Fit a plane 
XYZ0=[x,y,z]';
pFit=planarFit(XYZ0);

%Project onto 2D
 R =  pFit.R(:,[2,3,1]);
 b0 = (pFit.normal*pFit.distance).';
 UVW=num2cell(R'*(XYZ0-b0),2); 
[u,v,w]=deal(UVW{:});  %rotated coordinates

  
%Interpolate in 2D  
 F=scatteredInterpolant(u(:),v(:),w(:));
[uq,vq]=ndgrid(  linspace(min(u),max(u))   ,  linspace(min(v),max(v)) );
wq=F( uq , vq );

%Map back to 3D
XYZ=num2cell(R*[uq(:),vq(:),wq(:)]'+b0,2);
[xq,yq,zq]=deal(XYZ{:});


%Plot
f=@(q) reshape(q,size(wq));
surf(f(xq),f(yq),f(zq),'FaceColor',[1 0 0],'FaceAlpha',0.3,'EdgeColor','none');
axis equal; axis manual;

[Benjamin]

Hi Matt, thanks for input. I'm getting a couple errors with this code though.

Error 1

Error using scatteredInterpolant

The input points must be specified in column-vector format.

Error in test2 (line 15)

F=scatteredInterpolant(u,v,w);

corrected with u',v',w'.

Error 2

Error using *

Incorrect dimensions for matrix multiplication. Check that the number of columns in the first matrix matches the number of rows in the second matrix. To operate on each element of the matrix individually, use TIMES (.*) for elementwise multiplication.

Error in test2 (line 20)

XYZ=num2cell(R*[uq;vq;wq]+b0,2);

I'm not sure what the syntax should be

[Matt J]

You want to make sure that uq,vq,wq end up as row vectors.

[Benjamin]

wq is 100x100 double, whereas uq and vq are both 1x100 doubles.

What should I do to make wq a row vector also?

This is where wq is created

wq=F({ uq , vq });

[Matt J]

I have modified my original answer to show the correct code and results.