diff --git a/README.md b/README.md
index 6d73bfdfd57ce1fec41b09ea52861365dcf9c53b..7af2b9ef070b629a759c212aee1c1782aee7104e 100644
--- a/README.md
+++ b/README.md
@@ -13,6 +13,7 @@ The RCFS website also includes interactive exercises: [https://rcfs.ch](https://
 |----------|-------------|----|-----|------|-----|
 | MP | Movement primitives with various basis functions | ✅ | ✅ |  |  |
 | spline2D | Concatenated Bernstein basis functions with constraints to encode a signed distance function (2D inputs, 1D output) | ✅ | ✅ |  |  |
+| spline2D_eikonal | Gauss-Newton optimization of an SDF encoded with concatenated cubic polysplines, by considering unit norm derivatives in the cost function | ✅ | ✅ |  |  |
 | IK | Inverse kinematics for a planar manipulator | ✅ | ✅ | ✅ | ✅ |
 | IK_bimanual | Inverse kinematics with a planar bimanual robot |  | ✅ |  |  |
 | IK_nullspace | Inverse kinematics with nullspace projection (position and orientation tracking as primary or secondary tasks) | ✅ | ✅ |  |  |
diff --git a/data/sdf_circle.npy b/data/sdf_circle.npy
new file mode 100644
index 0000000000000000000000000000000000000000..cbea57cdf66dd557dd2ed8dc7b7acf190158f67f
Binary files /dev/null and b/data/sdf_circle.npy differ
diff --git a/data/sdf_circle01.mat b/data/sdf_circle01.mat
new file mode 100644
index 0000000000000000000000000000000000000000..35e6cd750be67e1a5df63f863b62013e190d9e72
Binary files /dev/null and b/data/sdf_circle01.mat differ
diff --git a/doc/rcfs.tex b/doc/rcfs.tex
index 7b1b9a1744a68e625f8809f9f5efdd591d66f102..ab65444668bdd28107b8304639d7cbe43e5a6660 100644
--- a/doc/rcfs.tex
+++ b/doc/rcfs.tex
@@ -3106,7 +3106,7 @@ For proper scaling, the heat source is further normalized over the domain using
 
 The heat source is then diffused over the domain to propagate the information about the unexplored regions to the agent(s). For that purpose, we use the heat equation: a second-order partial differential equation (PDE) that models heat conduction by relating spatial and time derivatives of a scalar field with
 \begin{equation}
-    \frac{d u(\bm{x},t)}{dt}=\alpha\Delta u(\bm{x},t) + s(\bm{x},t), %\beta 
+    \frac{\partial u(\bm{x},t)}{\partial t} = \alpha \Delta u(\bm{x},t) + s(\bm{x},t), %\beta 
      \label{eq:heat}
 \end{equation}
 where $u(\bm{x},t)$ corresponds to the temperature field, $\alpha$ is a thermal diffusivity parameter, and $\Delta u= \frac{\partial^2 u}{\partial x_1^2} + \frac{\partial^2 u}{\partial x_2^2} + \ldots$ is the Laplacian of the function $u$.
diff --git a/matlab/spline2D_eikonal.m b/matlab/spline2D_eikonal.m
new file mode 100644
index 0000000000000000000000000000000000000000..5330fc44d972b8cd5220b4f2af2355a0a1e4fc2f
--- /dev/null
+++ b/matlab/spline2D_eikonal.m
@@ -0,0 +1,378 @@
+function spline2D_eikonal
+%% Gauss-Newton optimization of an SDF encoded with concatenated cubic polysplines, 
+%% by considering unit norm derivatives in the cost function.
+%% In this example, the points used for training are only on the zero-level set (contours of the shape).
+%%
+%% Copyright (c) 2023 Idiap Research Institute <https://www.idiap.ch/>
+%% Written by Sylvain Calinon <https://calinon.ch>
+%% 
+%% This file is part of RCFS <https://robotics-codes-from-scratch.github.io/>
+%% License: MIT
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+param.nbFct = 4; %Number of basis functions for each dimension 
+param.nbSeg = 4; %Number of segments for each dimension 
+param.nbIn = 2; %Dimension of input data (here: 2D surface embedded in 3D)
+param.nbOut = 1; %Dimension of output data (here: height data)
+param.nbDim = 40; %Grid size for each dimension (same as sdf01.mat)
+param.nbIter = 100; %Maximum number of iterations for Gauss-Newton optimization
+param.qt = 1E0; %Tracking weight for Gauss-Newton optimization
+param.qn = 1E-4; %Norm weight for Gauss-Newton optimization
+
+%Input array
+[T1, T2] = meshgrid(linspace(0,1,param.nbDim));
+
+%Points to be used for the eikonal cost
+%id_eikonal = [[1:param.nbDim], param.nbDim^2-[0:param.nbDim-1], [0:param.nbDim-1]*param.nbDim+1, [1:param.nbDim]*param.nbDim]; %Points on the borders
+
+%id_eikonal = ceil(rand(1,400) * (param.nbDim^2-1)); %Points randomly distributed
+
+st = 3;
+idTmp = 1:st:param.nbDim;
+id_eikonal = idTmp' + (idTmp - 1) * param.nbDim; %Points uniformly distributed
+id_eikonal = id_eikonal(:)';
+T_eikonal = [T1(id_eikonal); T2(id_eikonal)];
+
+%Prior surface (SDF generated by demo_SDF01.m)
+load('../data/sdf_circle01.mat');
+x00 = y';
+
+%Reference surface (SDF generated by demo_SDF01.m)
+load('../data/sdf01.mat');
+ox0 = y';
+dx0 = dx;
+
+
+%% Precomputation of Bézier curve in matrix form (as in https://youtu.be/jvPPXbo87ds?t=434 or https://dergipark.org.tr/en/download/article-file/1633884)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%Bézier curve (as in https://youtu.be/jvPPXbo87ds?t=434 or https://dergipark.org.tr/en/download/article-file/1633884)
+param.B0 = zeros(param.nbFct); %Polynomial coefficients matrix
+for n=1:param.nbFct
+	for i=1:param.nbFct
+		param.B0(param.nbFct-i+1,n) = (-1)^(param.nbFct-i-n) * -binomial(param.nbFct-1, i-1) * binomial((param.nbFct-1)-(i-1), (param.nbFct-1)-(n-1)-(i-1));
+	end
+end
+%Matrices for a concatenation of curves
+param.B = kron(eye(param.nbSeg), param.B0);
+
+%Bézier curve constraint: Last control point and first control point of next segment should be the same (w4-w5=0, ...), 
+%and the two control points around should be symmetric (w3-2*w5+w6=0, ...),see https://youtu.be/jvPPXbo87ds?t=1188
+param.C0 = blkdiag(eye(param.nbFct-4), [[1; 0; 0; -1], [0; 1; 1; 2]]); %w4=w5 and w6=-w3+2*w5
+param.C = eye(2);
+for n=1:param.nbSeg-1
+	param.C = blkdiag(param.C, param.C0);
+end
+param.C = blkdiag(param.C, eye(param.nbFct-2));
+
+param.BC = param.B * param.C;
+param.M = kron(param.B * param.C, param.B * param.C); %Resulting transformation matrix
+
+
+%% Encoding with basis functions
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+nbT = param.nbDim / param.nbSeg; %Number of elements for each dimension of the grid
+t = linspace(0, 1-1/nbT, nbT); %Grid range for each dimension
+[param.Psi_all, param.dPsi_all] = computePsiGridFast(t, param);
+
+%Keep rows corresponding to the points used for the eikonal cost
+%param.Psi_eikonal = param.Psi_all(id_eikonal,:,:);
+param.dPsi_eikonal = param.dPsi_all(id_eikonal,:,:);
+
+
+%% Generate training data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+w0 = param.Psi_all \ x00; %Weights for prior SDF
+
+w_all = param.Psi_all \ x0;
+x_all = param.Psi_all * w_all; %SDF reference
+
+dx(1,:) = param.dPsi_all(:,:,1) * w_all;
+dx(2,:) = param.dPsi_all(:,:,2) * w_all;
+
+h = figure('position',[10 10 2600 1000]); 
+msh = contour(T1, T2, reshape(x_all, [param.nbDim, param.nbDim]), [0.0, 0.0]);
+T_contour = msh(:,2:end);
+%T = size(T_contour,2);
+
+%Compute distances and derivatives at the desired points 
+[param.Psi_contour, param.dPsi_contour] = computePsiList(T_contour, param);
+x_contour = param.Psi_contour * w_all;
+dxmat(1,:) = param.dPsi_contour(:,:,1) * w_all;
+dxmat(2,:) = param.dPsi_contour(:,:,2) * w_all;
+
+%%Compute distances and derivatives at the desired points using the raw discrete data
+%t12 = [T1(:)'; T2(:)'];
+%for t=1:T
+%	etmp = t12 - repmat(T_contour(:,t), 1, size(t12,2));
+%	[~,id] = min(etmp(1,:).^2+etmp(2,:).^2); %Find closest point on precomputed grid
+%	x_contour(1,t) = x(id); 
+%	dx_contour(:,t) = dx(:,id);
+%end
+
+
+%% Batch estimation using distances 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+wb_contour = param.Psi_contour \ x_contour; 
+xb = param.Psi_all * wb_contour;
+param.Mu = param.Psi_contour * wb_contour; %SDF reference
+
+%Reconstruction of derivatives
+dxb(1,:) = param.dPsi_all(:,:,1) * wb_contour;
+dxb(2,:) = param.dPsi_all(:,:,2) * wb_contour;
+Jbn = sum(dxb.^2); %Squared norm of gradients
+
+
+%% Gauss-Newton estimation using distances and unit norm derivatives
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+w = w0;
+%w = zeros(size(w_all));
+%w = wb_contour + randn(size(wb_contour)) * 1E-10; %Initialization
+for n=1:param.nbIter
+	[ft, Jt] = f_track(w, param); %Residual and Jacobian for SDF match objective
+	[fn, Jn] = f_norm(w, param); %Residual and Jacobian for unit norm objective
+	dw = (Jt'*Jt * param.qt + Jn'*Jn * param.qn + eye(size(Jn,2))*1E-12) \ (-Jt' * ft * param.qt - Jn' * fn * param.qn); %Gauss-Newton update
+	
+	%Estimate step size with backtracking line search method
+	alpha = 1;
+	cost0 = ft' * ft * param.qt + fn' * fn * param.qn; %Cost
+	while 1
+		w_tmp = w + dw * alpha;
+		ft_tmp = f_track(w_tmp, param); %Residual for SDF match objective
+		fn_tmp = f_norm(w_tmp, param); %Residual for unit norm objective
+		cost = ft_tmp' * ft_tmp * param.qt + fn_tmp' * fn_tmp * param.qn; %Cost
+		if cost < cost0 || alpha < 1E-3
+			break;
+		end
+		alpha = alpha * 0.5;
+	end	
+	w = w + dw * alpha; %Gauss-Newton update step
+	
+	if norm(dw * alpha) < 1E-3
+		break; %Stop optimization when solution is reached
+	end
+end
+disp(['iLQR converged in ' num2str(n) ' iterations. Cost: ' num2str(cost)]);
+
+x = param.Psi_all * w; %Reconstruction of SDF
+
+%Reconstruction of derivatives
+dx(1,:) = param.dPsi_all(:,:,1) * w;
+dx(2,:) = param.dPsi_all(:,:,2) * w;
+Jn = sum(dx.^2); %Squared norm of gradients
+
+
+
+%% Plot evolution
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+colormap(repmat(linspace(1,.4,64),3,1)');
+
+%Solution with cost on matching SDF reference
+subplot(1,2,1); hold on; axis off; 
+
+title('Without eikonal cost','fontsize',30);
+%title(['Cost on matching SDF reference (cost on unit norm: ' num2str(sum((Jbn-1).^2)) ')'],'fontsize',30);
+%Reconstructed surface
+%surface(T1, T2, reshape(xb, [param.nbDim, param.nbDim])-max(xb), 'FaceColor','interp','EdgeColor','interp'); %SDF
+surface(T1, T2, reshape(Jbn, [param.nbDim, param.nbDim])-max(Jbn), 'FaceColor','interp','EdgeColor','interp');
+contour(T1, T2, reshape(xb, [param.nbDim, param.nbDim]), [-1:.02:1], 'linewidth',2); %'color',[0 0 0]
+%Zero contours (boundaries of object)
+msh = contour(T1, T2, reshape(xb, [param.nbDim, param.nbDim]), [0, 0]);
+%plot(msh(1,2:end), msh(2,2:end), '-','linewidth',4,'color',[0 0 .8]);
+id = 1;
+while id<size(msh,2)
+	n = msh(2,id);
+	plot(msh(1,id+1:id+n), msh(2,id+1:id+n), '-','linewidth',4,'color',[0 0 .8]);
+	id = id + n + 1;
+end
+%Observed points
+plot(T_contour(1,:), T_contour(2,:), '.','markersize',22,'color',[.8,0,0]);
+axis tight; axis equal; axis([0 1 0 1]); axis ij; 
+set(gca,'position',[0 0 .5 .9]);
+%sub_pos = get(gca,'position'); % get subplot axis position
+%set(gca,'position',sub_pos.*[1 1 1.2 1.2]) % stretch its width and height
+
+
+%Solution with cost on matching SDF reference and unit norm derivatives
+subplot(1,2,2); hold on; axis off; 
+title('With eikonal cost','fontsize',30);
+%title(['Cost on matching SDF reference and unit norm derivatives (cost on unit norm: ' num2str(sum((Jn-1).^2)) ')'],'fontsize',30);
+%Reconstructed surface
+%surface(T1, T2, reshape(x, [param.nbDim, param.nbDim])-max(x), 'FaceColor','interp','EdgeColor','interp'); %SDF
+surface(T1, T2, reshape(Jn, [param.nbDim, param.nbDim])-max(Jn), 'FaceColor','interp','EdgeColor','interp');
+contour(T1, T2, reshape(x, [param.nbDim, param.nbDim]), [-1:.02:1], 'linewidth',2); %'color',[0 0 0]
+%Zero contours (boundaries of object)
+msh = contour(T1, T2, reshape(x, [param.nbDim, param.nbDim]), [0, 0]);
+%plot(msh(1,2:end), msh(2,2:end), '-','linewidth',4,'color',[0 0 .8]);
+id = 1;
+while id<size(msh,2)
+	n = msh(2,id);
+	plot(msh(1,id+1:id+n), msh(2,id+1:id+n), '-','linewidth',4,'color',[0 0 .8]);
+	id = id + n + 1;
+end
+%plot(T1, T2, '.','markersize',8,'color',[.8,.4,0]);
+%Eikonal points
+plot(T_eikonal(1,:), T_eikonal(2,:), '.','markersize',25,'color',[0,.6,0]);
+
+%Observed points
+plot(T_contour(1,:), T_contour(2,:), '.','markersize',25,'color',[.8,0,0]);
+axis tight; axis equal; axis([0 1 0 1]); axis ij;
+set(gca,'position',[.4 0 .5 .9]);
+
+%size(T_eikonal)
+%size(T_contour)
+
+%print('-dpng','graphs/spline2D_unitGradientNorm01.png');
+waitfor(h);
+end
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%Residual and Jacobian for tracking objective
+function [f, J] = f_track(w, param)
+	f = param.Psi_contour * w - param.Mu; %residual
+	J = param.Psi_contour; %Jacobian
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%Residual and Jacobian for unit norm objective (for points on the whole workspace)
+%function [f, J] = f_norm(w, param)
+%	dx(1,:) = param.dPsi_all(:,:,1) * w;
+%	dx(2,:) = param.dPsi_all(:,:,2) * w;
+%	
+%	%Fast version
+%	dxn = sum(dx.^2)'; %norm
+%	f = dxn - 1; %residual
+%	M = kron(ones(1,size(dx,2)), dx') .* kron(eye(size(dx,2)), ones(1,2));
+%	J = 2 * M * reshape([param.dPsi_all(:,:,1), param.dPsi_all(:,:,2)]', [size(w,1),size(dx,2)*2])'; %Jacobian
+%	
+%%	%Slow version
+%%	f = [];
+%%	J = [];
+%%	for t=1:size(dx,2)
+%%		ftmp = dx(:,t)' * dx(:,t) - 1;
+%%		Jtmp = 2 * dx(:,t)' * squeeze(param.dPsi_all(t,:,:))';
+%%		f = [f; ftmp];
+%%		J = [J; Jtmp];
+%%	end
+%end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%Residual and Jacobian for unit norm objective (for a selection of points used for the eikonal cost)
+function [f, J] = f_norm(w, param)
+	dx(1,:) = param.dPsi_eikonal(:,:,1) * w;
+	dx(2,:) = param.dPsi_eikonal(:,:,2) * w;
+	
+	%Fast version
+	dxn = sum(dx.^2)'; %norm
+	f = dxn - 1; %residual
+	M = kron(ones(1,size(dx,2)), dx') .* kron(eye(size(dx,2)), ones(1,2));
+	J = 2 * M * reshape([param.dPsi_eikonal(:,:,1), param.dPsi_eikonal(:,:,2)]', [size(w,1),size(dx,2)*2])'; %Jacobian
+	
+%	%Slow version
+%	f = [];
+%	J = [];
+%	for t=1:size(dx,2)
+%		ftmp = dx(:,t)' * dx(:,t) - 1;
+%		Jtmp = 2 * dx(:,t)' * squeeze(param.dPsi_eikonal(t,:,:))';
+%		f = [f; ftmp];
+%		J = [J; Jtmp];
+%	end
+end
+
+%%%%%%%%%%%%%%%%
+function b = binomial(n, i)
+	if n>=0 && i>=0
+		b = factorial(n) / (factorial(i) * factorial(n-i));
+	else
+		b = 0;
+	end
+end
+
+%%%%%%%%%%%%%%%%
+function [Psi, dPsi] = computePsiGridFast(t, param)
+	%Time parameters matrix to compute positions
+	T0 = zeros(size(t,2), param.nbFct); 
+	for n=1:param.nbFct
+		T0(:,n) = t.^(n-1);
+	end
+	%Matrices for a concatenation of curves
+	T = kron(eye(param.nbSeg), T0);
+
+	%Derivatives
+	%Time parameters matrix to compute derivatives
+	dT0 = zeros(size(t,2), param.nbFct); 
+	for n=2:param.nbFct
+		dT0(:,n) = (n-1) * t.^(n-2) * param.nbSeg; %Warning: n=1 creates 0/0
+	end
+%	dT0 = zeros(size(t,2), param.nbFct); 
+%	for n=1:param.nbFct-1
+%		dT0(:,n+1) = n * t.^(n-1);
+%	end
+	%Matrices for a concatenation of curves
+	dT = kron(eye(param.nbSeg), dT0);
+	
+	%Second derivatives (to compute Hessians analytically)
+	ddT0 = zeros(size(t,2), param.nbFct); 
+	for n=3:param.nbFct
+		ddT0(:,n) = (n-1) * (n-2) * t.^(n-3) * param.nbSeg^2;
+	end
+%	ddT0 = zeros(size(t,2), param.nbFct); 
+%	for n=1:param.nbFct-2
+%		ddT0(:,n+2) = n * (n-1) * t.^(n-2);
+%	end
+	%Matrices for a concatenation of curves
+	ddT = kron(eye(param.nbSeg), ddT0);
+
+	%Transform to multidimensional basis functions
+	%Psi = kron(s(1).phi, s(2).phi); %Surface primitive
+	Psi = kron(T, T) * param.M; %Surface primitive
+
+	%Transform to multidimensional basis functions
+	%dPsi1 = kron(s(1).dphi, s(2).phi);
+	%dPsi2 = kron(s(1).phi, s(2).dphi); 
+	dPsi(:,:,1) = kron(dT, T) * param.M; %Computation leveraging Kronecker properties
+	dPsi(:,:,2) = kron(T, dT) * param.M; %Computation leveraging Kronecker properties
+end
+
+
+%%%%%%%%%%%%%%%%
+%Fast version
+function [Psi, dPsi] = computePsiList(Tmat, param)
+	Psi = [];
+	dPsi1 = [];
+	dPsi2 = [];
+	%Compute Psi for each point
+	for k=1:size(Tmat,2)
+		T = zeros(size(Tmat,1), param.nbFct);
+		dT = zeros(size(Tmat,1), param.nbFct); 
+		%Compute Psi for each dimension
+		for d=1:size(Tmat,1)
+			tt = mod(Tmat(d,k), 1/param.nbSeg) * param.nbSeg; %Compute residual within the segment in which the point falls
+			id = round(Tmat(d,k) * param.nbSeg - tt); %Determine in which segment the point falls in order to evaluate the basis function accordingly
+			%Handle inputs beyond lower bound
+			if id < 0
+				tt = tt + id;
+				id = 0;
+			end
+			%Handle inputs beyond upper bound
+			if id > (param.nbSeg-1)
+				tt = tt + id - (param.nbSeg-1);
+				id = param.nbSeg-1;
+			end
+			%Evaluate polynomials
+			T(d,:) = tt.^[0:param.nbFct-1];
+			dT(d,2:end) = [1:param.nbFct-1] .* tt.^[0:param.nbFct-2] * param.nbSeg;
+			idl(:,d) = id*param.nbFct + [1:param.nbFct];
+		end
+		%Reconstruct Psi for all dimensions
+		Mtmp = kron(param.BC(idl(:,1),:), param.BC(idl(:,2),:)); %Resulting transformation matrix		
+		Psi = [Psi; kron(T(1,:), T(2,:)) * Mtmp]; %Surface primitive
+		dPsi1 = [dPsi1; kron(dT(1,:), T(2,:)) * Mtmp]; %Computation leveraging Kronecker properties
+		dPsi2 = [dPsi2; kron(T(1,:), dT(2,:)) * Mtmp]; %Computation leveraging Kronecker properties
+	end	
+	dPsi(:,:,1) = dPsi1;
+	dPsi(:,:,2) = dPsi2;
+end
+
diff --git a/python/spline2D_eikonal.py b/python/spline2D_eikonal.py
new file mode 100644
index 0000000000000000000000000000000000000000..bc8acf71355a2f31c26f944a1030a7e96545cd48
--- /dev/null
+++ b/python/spline2D_eikonal.py
@@ -0,0 +1,320 @@
+"""
+Gauss-Newton optimization of an SDF encoded with concatenated cubic polysplines,
+by considering unit norm derivatives in the cost function.
+In this example, the points used for training are only on the zero-level set (contours of the shape).
+
+Copyright (c) 2023 Idiap Research Institute <https://www.idiap.ch/>
+Written by Guillaume Clivaz <guillaume.clivaz@idiap.ch> and Sylvain Calinon <https://calinon.ch>
+
+This file is part of RCFS <https://robotics-codes-from-scratch.github.io/>
+License: MIT
+"""
+
+import numpy as np
+from matplotlib import pyplot as plt
+
+
+def binomial(n, i):
+    if n >= 0 and i >= 0:
+        b = np.math.factorial(n) / (np.math.factorial(i) * np.math.factorial(n - i))
+    else:
+        b = 0
+    return b
+
+
+def block_diag(A, B):
+    out = np.zeros((A.shape[0] + B.shape[0], A.shape[1] + B.shape[1]))
+    out[: A.shape[0], : A.shape[1]] = A
+    out[A.shape[0] :, A.shape[1] :] = B
+    return out
+
+
+def computePsiList(Tmat, param):
+    """From a matrix of values (nbIn, N), compute the concatenated basis functions
+    (Psi = surface primitive, dPsi = Psi derivatives)to encode the SDF.
+    Tmat should have values [t1, t2, ..., tnbIn] in the range [0, 1].
+
+    By example, for 2D:
+    1  Tmat is a vector (2, N), [t1, t2] to compute distances.
+    1. It determines to which segment t1 and t2 correspond, to select the
+    corresponding part of the polynomial matrix.
+    3. t residuals are used to fill T = [1, t, t**2, t**3] and dT=[0, 1, 2t, 3t**2]
+    4. Psi and dPsi are computed with kronecker product (see 6.3 and 6.4 of RFCS)
+    """
+    Psi = []
+    dPsi1 = []
+    dPsi2 = []
+    # Compute Psi for each point
+    for k in range(0, Tmat.shape[1]):
+        T = np.zeros((Tmat.shape[0], param.nbFct))
+        dT = np.zeros((Tmat.shape[0], param.nbFct))
+        idl = np.zeros((4, 2), dtype="int")
+
+        # Compute Psi for each dimension
+        for d in range(0, Tmat.shape[0]):
+            # Compute residual within the segment in which the point falls
+            tt = np.mod(Tmat[d, k], 1 / param.nbSeg) * param.nbSeg
+            # Determine in which segment the point falls in order to evaluate the basis function accordingly
+            id = np.round(Tmat[d, k] * param.nbSeg - tt)
+            # Handle inputs beyond lower bound
+            if id < 0:
+                tt = tt + id
+                id = 0
+            # Handle inputs beyond upper bound
+            if id > (param.nbSeg - 1):
+                tt = tt + id - (param.nbSeg - 1)
+                id = param.nbSeg - 1
+
+            # Evaluate polynomials
+            p1 = np.linspace(0, param.nbFct - 1, param.nbFct)
+            p2 = np.linspace(0, param.nbFct - 2, param.nbFct - 1)
+            T[d, :] = tt**p1
+            dT[d, 1:] = p1[1:] @ tt**p2 * param.nbSeg
+            idl[:, d] = id.astype("int") * param.nbFct + p1
+
+        # Reconstruct Psi for all dimensions
+        Mtmp = np.kron(param.BC[idl[:, 0], :], param.BC[idl[:, 1], :])
+        Psi.append(np.kron(T[0, :], T[1, :]) @ Mtmp)
+        dPsi1.append(np.kron(dT[0, :], T[1, :]) @ Mtmp)
+        dPsi2.append(np.kron(T[0, :], dT[1, :]) @ Mtmp)
+
+    dPsi = np.dstack((np.array(dPsi1), np.array(dPsi2)))
+    Psi = np.array(Psi)
+    return Psi, dPsi
+
+
+def computePsiGridFast(t, param):
+    """Compute the concatenated basis functions (Psi = surface primitive,
+    dPsi = Psi derivatives) to encode the SDF.
+    """
+
+    # Time parameters matrix to compute positions
+    T0 = np.zeros((t.shape[0], param.nbFct))
+    for n in range(param.nbFct):
+        T0[:, n] = t**n
+
+    # Matrices for a concatenation of curves
+    T = np.kron(np.eye(param.nbSeg), T0)
+
+    # Derivatives
+    # Time parameters matrix to compute derivatives
+    dT0 = np.zeros((t.shape[0], param.nbFct))
+    for n in range(1, param.nbFct):
+        dT0[:, n] = n * t ** (n - 1) * param.nbSeg
+
+    # Matrices for a concatenation of curves
+    dT = np.kron(np.eye(param.nbSeg), dT0)
+
+    # Transform to multidimensional basis functions
+    Psi = np.kron(T, T) @ param.M
+
+    # Transform to multidimensional basis functions
+    dPsi = np.kron(dT, T) @ param.M
+    dPsi2 = np.kron(T, dT) @ param.M
+    dPsi = np.dstack((dPsi, dPsi2))
+    return Psi, dPsi
+
+
+# Residual and Jacobian for tracking objective
+def f_track(w, Psi_contour, Mu):
+    """Residual and Jacobian for distance tracking objective"""
+    f = Psi_contour @ w - Mu
+    J = Psi_contour
+    return f, J
+
+
+def f_norm(w, dPsi_eikonal, param, compute_dPsi=True):
+    """Residual and Jacobian for unit norm (eikonal) objective"""
+    dx = np.hstack((dPsi_eikonal[:, :, 0] @ w, dPsi_eikonal[:, :, 1] @ w))
+    f = np.sum(dx**2, axis=1) - 1
+    f = f.reshape(-1, 1)
+    if not compute_dPsi:
+        return f, None
+
+    # Filling dxn (of size N, 2*N) wht diag vector [dx, dy] of size (N, 2)
+    N = param.nbDim**param.nbIn
+    dxn = np.zeros((param.nbDim**param.nbIn, 2 * param.nbDim**param.nbIn))
+    dxn[np.arange(N), np.arange(0, 2 * N, 2)] = dx[:, 0]
+    dxn[np.arange(N), np.arange(1, 2 * N, 2)] = dx[:, 1]
+
+    # # Other approach
+    # I = np.eye(N)
+    # I = np.kron(I, np.ones((1,param.nbIn)))
+    # O = np.ones((1,N))
+    # dxn = np.multiply(np.kron(O, dx), I)
+
+    J = 2 * dxn @ param.dPsi_reshaped
+    return f, J
+
+
+# General parameters
+# ===============================
+
+param = lambda: None  # Lazy way to define an empty class in python
+
+# Concatenated basis functions
+param.nbFct = 4  # Number of Bernstein basis functions for each dimension
+param.nbSeg = 4  # Number of segments for each dimension
+param.nbIn = 2  # Dimension of input data (here: 2D surface embedded in 3D)
+param.nbOut = 1  # Dimension of output data (here: height data)
+param.nbDim = 40  # Grid size for each dimension (same as sdf01.npy)
+
+# Gauss-Newton optimization
+param.nbIter = 100  # Maximum number of iterations
+param.qt = 1e0  # Weighting factor for distance tracking
+param.qn = 1e-4  # Weighting factor for eikonal objective
+
+# Bézier constraint matrix: B is the polynomial matrix, C the continuity constraints
+B0 = np.zeros((param.nbFct, param.nbFct))
+for n in range(1, param.nbFct + 1):
+    for i in range(1, param.nbFct + 1):
+        B0[param.nbFct - i, n - 1] = (
+            (-1) ** (param.nbFct - i - n)
+            * (-binomial(param.nbFct - 1, i - 1))
+            * binomial(param.nbFct - 1 - (i - 1), param.nbFct - 1 - (n - 1) - (i - 1))
+        )
+B = np.kron(np.eye(param.nbSeg), B0)
+C0 = np.array([[1, 0, 0, -1], [0, 1, 1, 2]]).T
+C0 = block_diag(np.eye(param.nbFct - 4), C0)
+C = np.eye(2)
+for n in range(param.nbSeg - 1):
+    C = block_diag(C, C0)
+C = block_diag(C, np.eye(param.nbFct - 2))
+param.BC = B @ C
+param.M = np.kron(param.BC, param.BC)
+
+
+# Load and generate data
+# ===============================
+
+# Load distance measurement Xm. Further, only points on contours (SDF level=0)
+# will be used.
+data = np.load("../data/sdf01.npy", allow_pickle="True").item()
+x0 = data["y"].T
+dx = data["dx"]  # dx = np.zeros((2, x0.shape[0]))
+
+# Compute Psi and dPsi with basis functions encoding
+nb_t = param.nbDim / param.nbSeg  # Number of elements for each dimension of the grid
+t = np.linspace(0, 1 - 1 / nb_t, int(nb_t))  # Grid range for each dimension
+Psi, dPsi = computePsiGridFast(t, param)
+
+# In this example, all the points sampled are used for the Eikonal objective, but
+# a subsample can also be used
+Psi_eikonal, dPsi_eikonal = Psi, dPsi
+
+# Superposition weights estimated from least-squares with SDF reference
+w_all = np.linalg.pinv(Psi_eikonal) @ x0
+x_all = Psi_eikonal @ w_all
+
+dx[0:1, :] = (dPsi_eikonal[:, :, 0] @ w_all).T
+dx[1:2, :] = (dPsi_eikonal[:, :, 1] @ w_all).T
+
+# Sample points on the contour of the shape (using pyplot contour)
+# to compute distances and derivatives at the desired points
+T1, T2 = np.meshgrid(np.linspace(0, 1, param.nbDim), np.linspace(0, 1, param.nbDim))
+T1 *= param.nbDim
+T2 *= param.nbDim
+fig, ax = plt.subplots()
+msh = ax.contour(T1, T2, x_all.reshape([param.nbDim, param.nbDim]).T, levels=[0.0])
+T_contour = msh.collections[0].get_paths()[0].vertices / param.nbDim
+ax.imshow(x0.reshape([param.nbDim, param.nbDim]).T, cmap="gray")
+ax.scatter(T_contour[:, 0] * param.nbDim, T_contour[:, 1] * param.nbDim, marker="x")
+ax.set_title("Points for the tracking objective sampled on this contour")
+plt.show()
+Psi_contour, dPsi_contour = computePsiList(T_contour.T, param)
+Mu = Psi_contour @ w_all
+
+## Or batch estimation using distances
+# x_contour = Psi_contour @ w_all
+# wb_contour = np.linalg.pinv(Psi_contour) @ x_contour
+# Mu = Psi_contour @ wb_contour
+
+
+# Solving Gauss-Newton estimation using distances and unit norm derivatives
+# =========================================================================
+
+# Initialize weights by loading the SDF of a circle to initialize
+data_init = np.load("../data/sdf_circle.npy", allow_pickle="True").item()
+x00 = data_init["y"].T
+w = np.linalg.pinv(Psi_eikonal) @ x00
+
+# Reformat dPsi for Jacobian of eikonal objective by spliting vertically in two,
+# and filling even rows with the first matrix and odd rows with the seconds.
+param.dPsi_reshaped = np.empty([2 * param.nbDim**param.nbIn, w.shape[0]])
+param.dPsi_reshaped[::2, :] = dPsi_eikonal[:, :, 0]
+param.dPsi_reshaped[1::2, :] = dPsi_eikonal[:, :, 1]
+
+for n in range(param.nbIter):
+    # Residual and Jacobian for SDF match objective
+    ft, Jt = f_track(w, Psi_contour, Mu)
+
+    # Residual and Jacobian for unit norm objective
+    fn, Jn = f_norm(w, dPsi_eikonal, param, compute_dPsi=True)
+
+    # Gauss-Newton update (see chapter 6.8 of RCFS.pdf)
+    J_inv = np.linalg.pinv(Jt.T @ Jt * param.qt + Jn.T @ Jn * param.qn)
+    dw = J_inv @ (-Jt.T @ ft * param.qt - Jn.T @ fn * param.qn)
+
+    # Estimate step size with backtracking line search method
+    alpha = 1
+    cost0 = ft.T @ ft * param.qt + fn.T @ fn * param.qn
+    while True:
+        w_tmp = w + dw * alpha
+        ft_tmp, _ = f_track(w_tmp, Psi_contour, Mu)
+        fn_tmp, _ = f_norm(w_tmp, dPsi_eikonal, param, compute_dPsi=False)
+        cost = ft_tmp.T @ ft_tmp * param.qt + fn_tmp.T @ fn_tmp * param.qn
+        if cost < cost0 or alpha < 1e-3:
+            break
+        alpha = alpha * 0.5
+
+    # Gauss-Newton update step
+    w = w + dw * alpha
+    res = np.linalg.norm(dw * alpha)
+    print("Iteration : {} , residual : {}, cost : {}".format(n, res, cost))
+
+    # Stop optimization when solution is reached
+    if res < 1e-3:
+        break
+
+# Estimated SDF and derivatives
+# ===============================
+
+x = Psi @ w
+dx[0:1, :] = (dPsi[:, :, 0] @ w).T
+dx[1:2, :] = (dPsi[:, :, 1] @ w).T
+
+
+# Plotting results
+# ===============================
+
+x0 *= param.nbDim
+x *= param.nbDim
+x_all *= param.nbDim
+
+x0 = x0.reshape([param.nbDim, param.nbDim]).T
+x = x.reshape((param.nbDim, param.nbDim)).T
+dx = dx.reshape((2, param.nbDim, param.nbDim)).T
+x_all = x_all.reshape((param.nbDim, param.nbDim)).T
+
+
+fig = plt.figure(figsize=(20, 10))
+ax0 = fig.add_subplot(131)
+ax1 = fig.add_subplot(132)
+ax2 = fig.add_subplot(133)
+
+ax0.imshow(x0, cmap="gray")
+ax0.contour(T1, T2, x0, levels=np.linspace(x0.min(), x0.max(), 25))
+ax0.contour(T1, T2, x0, levels=[0.0], linewidths=3.0)
+ax0.set_title("Exact SDF")
+
+ax1.imshow(x_all, cmap="gray")
+ax1.contour(T1, T2, x_all, levels=np.linspace(x_all.min(), x_all.max(), 25))
+ax1.contour(T1, T2, x_all, levels=[0.0], linewidths=3.0)
+ax1.set_title("SDF estimated from the reference points (least-square)")
+
+ax2.imshow(x, cmap="gray")
+ax2.contour(T1, T2, x, levels=np.linspace(x.min(), x.max(), 25))
+ax2.contour(T1, T2, x, levels=[0.0], linewidths=3.0)
+ax2.set_title("SDF with Distance + Eikonal objective")
+
+plt.show()