Porting special math functions from matlab to numpy - numpy
Here is Matlab code, as published in the MathWorks community section.
[x,y,z] = ellipsoid(0,-0.5,0,6,3.25,3.25);
A=[x(:),y(:),z(:)];
[U, c] = MgnCalibration(A)
function [U,c] = MgnCalibration(X)
% performs magnetometer calibration from a set of data
% using Merayo technique with a non iterative algoritm
% J.Merayo et al. "Scalar calibration of vector magnemoters"
% Meas. Sci. Technol. 11 (2000) 120-132.
%
% X : a Nx3 (or 3xN) data matrix
% each row (columns) contains x, y, z measurements
% N must be such that the data set describes
% as completely as possible the 3D space
% In any case N > 10
%
% The calibration tries to find the best 3D ellipsoid that fits the data set
% and returns the parameters of this ellipsoid
%
% U : shape ellipsoid parameter, (3x3) upper triangular matrix
% c : ellipsoid center, (3x1) vector
%
% Ellipsoid equation : (v-c)'*(U'*U)(v-c) = 1
% with v a rough triaxes magnetometer measurement
%
% calibrated measurement w = U*(v-c)
%
% author : Alain Barraud, Suzanne Lesecq 2008
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[N,m] = size(X);
if m>3&&N==3,X = X';N = m;m = 3;end;%check that X is not transposed
if N<=10,U = [];c = [];return;end;%not enough data no calibration !!
% write the ellipsoid equation as D*p=0
% the best parameter is the solution of min||D*p|| with ||p||=1;
% form D matrix from X measurements
x = X(:,1); y = X(:,2); z = X(:,3);
D = [x.^2, y.^2, z.^2, x.*y, x.*z, y.*z, x, y, z, ones(N,1)];
size(D)
D=triu(qr(D));%avoids to compute the svd of a large matrix
[U,S,V] = svd(D);%because usually N may be very large
p = V(:,end);if p(1)<0,p =-p;end;
% the following matrix A(p) must be positive definite
% The optimization done by svd does not include such a constraint
% With "good" data the constraint is allways satisfied
% With too poor data A may fail to be positive definite
% In this case the calibration fails
%
A = [p(1) p(4)/2 p(5)/2;
p(4)/2 p(2) p(6)/2;
p(5)/2 p(6)/2 p(3)];
[U,ok] = fchol(m,A);
if ~ok,U = [];c = [];return;end%calibration fails too poor data!!
b = [p(7);p(8);p(9)];
v = Utsolve(U,b/2,m);
d = p(10);
s = 1/sqrt(v*v'-d);
c =-Usolve(U,v,m)';%ellipsoid center
U = s*U;%shape ellipsoid parameter
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [A,ok] = fchol(n,A)
% performs Cholesky factoristation
A(1,1:n) = A(1,1:n)/sqrt(A(1,1));
A(2:n,1) = 0;
for j=2:n
A(j,j:n) = A(j,j:n) - A(1:j-1,j)'*A(1:j-1,j:n);
if A(j,j)<=0,ok=0;break;end%A is not positive definite
A(j,j:n) = A(j,j:n)/sqrt(A(j,j));
A(j+1:n,j) = 0;
end
ok=1;
end
function x=Utsolve(U,b,n)
% solves U'*x=b
x(1) = b(1)/U(1,1);
for k=2:n
x(k) = (b(k)-x(1:k-1)*U(1:k-1,k))/U(k,k);
end
end
function x=Usolve(U,b,n)
% solves U*x=b
x(n) = b(n)/U(n,n);
for k=n-1:-1:1
x(k) = (b(k)-U(k,k+1:n)*x(k+1:n)')/U(k,k);
end
end
And here is my port to NumPy, together with the same test data, with most of the porting work done already. However, now I run into the more advanced math functions like qr, svd and triu I have no experience with and no more profound understanding. My current problem is that on the numpy side, when looking with a debugger, the individual elements of D are organized in rows and still wrapped in np.array() calls and Matlab arranges them in columns. When computing qr() or those two entities, the results differ, regardless of whether I transpose D on the numpy side or not.
Why is that?
I was able to verify the utsolve and usolve functions, already.
import numpy as np
from scipy.linalg import qr, svd
def calibrate_magnetometer(calibration_data: np.ndarray):
"""
performs magnetometer calibration from a set of data
using Merayo technique with a non iterative algoritm
J.Merayo et al. "Scalar calibration of vector magnemoters"
Meas. Sci. Technol. 11 (2000) 120-132.
X : a Nx3 (or 3xN) data matrix
each row (columns) contains x, y, z measurements
N must be such that the data set describes
as completely as possible the 3D space
In any case N > 10
The calibration tries to find the best 3D ellipsoid that fits the data set
and returns the parameters of this ellipsoid
U : shape ellipsoid parameter, (3x3) upper triangular matrix
c : ellipsoid center, (3x1) vector
Ellipsoid equation : (v-c)'*(U'*U)(v-c) = 1
with v a rough triaxes magnetometer measurement
calibrated measurement w = U*(v-c)
author : Alain Barraud, Suzanne Lesecq 2008
"""
(n, m) = calibration_data.shape
if m > 3 and n == 3: # check that x is not transposed
calibration_data = calibration_data.T
n = m
m = 3
if n <= 10:
return False # not enough data for calibration
# write the ellipsoid equation as D*p=0
# the best parameter is the solution of min||D*p|| with ||p||=1
# form D matix from X measurements
x = calibration_data[:, 0]
y = calibration_data[:, 1]
z = calibration_data[:, 2]
D = np.array([np.square(x), np.square(y), np.square(z),
np.multiply(x, y), np.multiply(x, z), np.multiply(x, z),
x, y, z,
np.ones(n)])
D = np.triu(qr(D.T)) # avoids to compute the svd of a large matrix
u, s, v = svd(D) # because usually N may be very large
p = v[-1]
if p[0] < 0:
p = -p
# the following matrix a(p) must be positive definite.
# the optimization done by svd does not include such a constrain
# with "good" data the constrain is allways satisfied
# with too poor data A may fail to be positive definite
# in this case the calibration fails
a = np.array([[p[0], p[3] / 2, p[4] / 2],
[p[3] / 2, p[1], p[5] / 2],
[p[4] / 2, p[5] / 2, p[2]]])
uu = np.linalg.cholesky(a)
b = np.array([p[6], p[7], p[8]])
vv = utsolve(uu, b / 2, m)
d = p(10)
s = 1 / np.sqrt(vv * vv.T - d)
calibrated_c = -usolve(uu, vv, m) # ellipsoid center, or offset error
calibrated_u = s * uu # shape ellipsoid parameter
return calibrated_u, calibrated_c
def utsolve(u, b, n):
# solves u.T*x=b
x = np.zeros(n)
x[0] = b[0] / u[0, 0]
for k in range(1, n):
x[k] = (b[k] - np.matmul(x[0:k], u[0:k, k])) / u[k, k]
return x
def usolve(u, b, n):
# solves u*x=b
x = np.zeros((n, 1))
x[n - 1] = b[n - 1] / u[n - 1, n - 1]
for k in reversed(range(0, n - 1)):
x[k] = (b[k] - np.matmul(u[k, k + 1:n], x[k + 1:n].T[0])) / u[k, k]
return x.T[0]
def main():
a = np.array([[0, -0.5000, -3.2500],
[-0.9386, -0.5000, -3.2100],
[-1.8541, -0.5000, -3.0909],
[-2.7239, -0.5000, -2.8958],
[-3.5267, -0.5000, -2.6293],
[-4.2426, -0.5000, -2.2981],
[-4.8541, -0.5000, -1.9103],
[-5.3460, -0.5000, -1.4755],
[-5.7063, -0.5000, -1.0043],
[-5.9261, -0.5000, -0.5084],
[-6.0000, -0.5000, 0],
[-5.9261, -0.5000, 0.5084],
[-5.7063, -0.5000, 1.0043],
[-5.3460, -0.5000, 1.4755],
[-4.8541, -0.5000, 1.9103],
[-4.2426, -0.5000, 2.2981],
[-3.5267, -0.5000, 2.6293],
[-2.7239, -0.5000, 2.8958],
[-1.8541, -0.5000, 3.0909],
[-0.9386, -0.5000, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.8927, -0.6571, -3.2100],
[-1.7634, -0.8103, -3.0909],
[-2.5906, -0.9559, -2.8958],
[-3.3541, -1.0903, -2.6293],
[-4.0350, -1.2102, -2.2981],
[-4.6165, -1.3125, -1.9103],
[-5.0844, -1.3948, -1.4755],
[-5.4271, -1.4552, -1.0043],
[-5.6361, -1.4919, -0.5084],
[-5.7063, -1.5043, 0],
[-5.6361, -1.4919, 0.5084],
[-5.4271, -1.4552, 1.0043],
[-5.0844, -1.3948, 1.4755],
[-4.6165, -1.3125, 1.9103],
[-4.0350, -1.2102, 2.2981],
[-3.3541, -1.0903, 2.6293],
[-2.5906, -0.9559, 2.8958],
[-1.7634, -0.8103, 3.0909],
[-0.8927, -0.6571, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.7593, -0.7988, -3.2100],
[-1.5000, -1.0903, -3.0909],
[-2.2037, -1.3673, -2.8958],
[-2.8532, -1.6228, -2.6293],
[-3.4324, -1.8508, -2.2981],
[-3.9271, -2.0455, -1.9103],
[-4.3250, -2.2021, -1.4755],
[-4.6165, -2.3168, -1.0043],
[-4.7943, -2.3868, -0.5084],
[-4.8541, -2.4103, 0],
[-4.7943, -2.3868, 0.5084],
[-4.6165, -2.3168, 1.0043],
[-4.3250, -2.2021, 1.4755],
[-3.9271, -2.0455, 1.9103],
[-3.4324, -1.8508, 2.2981],
[-2.8532, -1.6228, 2.6293],
[-2.2037, -1.3673, 2.8958],
[-1.5000, -1.0903, 3.0909],
[-0.7593, -0.7988, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.5517, -0.9113, -3.2100],
[-1.0898, -1.3125, -3.0909],
[-1.6011, -1.6937, -2.8958],
[-2.0729, -2.0455, -2.6293],
[-2.4938, -2.3592, -2.2981],
[-2.8532, -2.6272, -1.9103],
[-3.1423, -2.8427, -1.4755],
[-3.3541, -3.0006, -1.0043],
[-3.4833, -3.0969, -0.5084],
[-3.5267, -3.1293, 0],
[-3.4833, -3.0969, 0.5084],
[-3.3541, -3.0006, 1.0043],
[-3.1423, -2.8427, 1.4755],
[-2.8532, -2.6272, 1.9103],
[-2.4938, -2.3592, 2.2981],
[-2.0729, -2.0455, 2.6293],
[-1.6011, -1.6937, 2.8958],
[-1.0898, -1.3125, 3.0909],
[-0.5517, -0.9113, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.2900, -0.9835, -3.2100],
[-0.5729, -1.4552, -3.0909],
[-0.8417, -1.9033, -2.8958],
[-1.0898, -2.3168, -2.6293],
[-1.3110, -2.6856, -2.2981],
[-1.5000, -3.0006, -1.9103],
[-1.6520, -3.2540, -1.4755],
[-1.7634, -3.4397, -1.0043],
[-1.8313, -3.5529, -0.5084],
[-1.8541, -3.5909, 0],
[-1.8313, -3.5529, 0.5084],
[-1.7634, -3.4397, 1.0043],
[-1.6520, -3.2540, 1.4755],
[-1.5000, -3.0006, 1.9103],
[-1.3110, -2.6856, 2.2981],
[-1.0898, -2.3168, 2.6293],
[-0.8417, -1.9033, 2.8958],
[-0.5729, -1.4552, 3.0909],
[-0.2900, -0.9835, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.0000, -1.0084, -3.2100],
[0.0000, -1.5043, -3.0909],
[0.0000, -1.9755, -2.8958],
[0.0000, -2.4103, -2.6293],
[0.0000, -2.7981, -2.2981],
[0.0000, -3.1293, -1.9103],
[0.0000, -3.3958, -1.4755],
[0.0000, -3.5909, -1.0043],
[0.0000, -3.7100, -0.5084],
[0.0000, -3.7500, 0],
[0.0000, -3.7100, 0.5084],
[0.0000, -3.5909, 1.0043],
[0.0000, -3.3958, 1.4755],
[0.0000, -3.1293, 1.9103],
[0.0000, -2.7981, 2.2981],
[0.0000, -2.4103, 2.6293],
[0.0000, -1.9755, 2.8958],
[0.0000, -1.5043, 3.0909],
[0.0000, -1.0084, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.2900, -0.9835, -3.2100],
[0.5729, -1.4552, -3.0909],
[0.8417, -1.9033, -2.8958],
[1.0898, -2.3168, -2.6293],
[1.3110, -2.6856, -2.2981],
[1.5000, -3.0006, -1.9103],
[1.6520, -3.2540, -1.4755],
[1.7634, -3.4397, -1.0043],
[1.8313, -3.5529, -0.5084],
[1.8541, -3.5909, 0],
[1.8313, -3.5529, 0.5084],
[1.7634, -3.4397, 1.0043],
[1.6520, -3.2540, 1.4755],
[1.5000, -3.0006, 1.9103],
[1.3110, -2.6856, 2.2981],
[1.0898, -2.3168, 2.6293],
[0.8417, -1.9033, 2.8958],
[0.5729, -1.4552, 3.0909],
[0.2900, -0.9835, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.5517, -0.9113, -3.2100],
[1.0898, -1.3125, -3.0909],
[1.6011, -1.6937, -2.8958],
[2.0729, -2.0455, -2.6293],
[2.4938, -2.3592, -2.2981],
[2.8532, -2.6272, -1.9103],
[3.1423, -2.8427, -1.4755],
[3.3541, -3.0006, -1.0043],
[3.4833, -3.0969, -0.5084],
[3.5267, -3.1293, 0],
[3.4833, -3.0969, 0.5084],
[3.3541, -3.0006, 1.0043],
[3.1423, -2.8427, 1.4755],
[2.8532, -2.6272, 1.9103],
[2.4938, -2.3592, 2.2981],
[2.0729, -2.0455, 2.6293],
[1.6011, -1.6937, 2.8958],
[1.0898, -1.3125, 3.0909],
[0.5517, -0.9113, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.7593, -0.7988, -3.2100],
[1.5000, -1.0903, -3.0909],
[2.2037, -1.3673, -2.8958],
[2.8532, -1.6228, -2.6293],
[3.4324, -1.8508, -2.2981],
[3.9271, -2.0455, -1.9103],
[4.3250, -2.2021, -1.4755],
[4.6165, -2.3168, -1.0043],
[4.7943, -2.3868, -0.5084],
[4.8541, -2.4103, 0],
[4.7943, -2.3868, 0.5084],
[4.6165, -2.3168, 1.0043],
[4.3250, -2.2021, 1.4755],
[3.9271, -2.0455, 1.9103],
[3.4324, -1.8508, 2.2981],
[2.8532, -1.6228, 2.6293],
[2.2037, -1.3673, 2.8958],
[1.5000, -1.0903, 3.0909],
[0.7593, -0.7988, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.8927, -0.6571, -3.2100],
[1.7634, -0.8103, -3.0909],
[2.5906, -0.9559, -2.8958],
[3.3541, -1.0903, -2.6293],
[4.0350, -1.2102, -2.2981],
[4.6165, -1.3125, -1.9103],
[5.0844, -1.3948, -1.4755],
[5.4271, -1.4552, -1.0043],
[5.6361, -1.4919, -0.5084],
[5.7063, -1.5043, 0],
[5.6361, -1.4919, 0.5084],
[5.4271, -1.4552, 1.0043],
[5.0844, -1.3948, 1.4755],
[4.6165, -1.3125, 1.9103],
[4.0350, -1.2102, 2.2981],
[3.3541, -1.0903, 2.6293],
[2.5906, -0.9559, 2.8958],
[1.7634, -0.8103, 3.0909],
[0.8927, -0.6571, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.9386, -0.5000, -3.2100],
[1.8541, -0.5000, -3.0909],
[2.7239, -0.5000, -2.8958],
[3.5267, -0.5000, -2.6293],
[4.2426, -0.5000, -2.2981],
[4.8541, -0.5000, -1.9103],
[5.3460, -0.5000, -1.4755],
[5.7063, -0.5000, -1.0043],
[5.9261, -0.5000, -0.5084],
[6.0000, -0.5000, 0],
[5.9261, -0.5000, 0.5084],
[5.7063, -0.5000, 1.0043],
[5.3460, -0.5000, 1.4755],
[4.8541, -0.5000, 1.9103],
[4.2426, -0.5000, 2.2981],
[3.5267, -0.5000, 2.6293],
[2.7239, -0.5000, 2.8958],
[1.8541, -0.5000, 3.0909],
[0.9386, -0.5000, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.8927, -0.3429, -3.2100],
[1.7634, -0.1897, -3.0909],
[2.5906, -0.0441, -2.8958],
[3.3541, 0.0903, -2.6293],
[4.0350, 0.2102, -2.2981],
[4.6165, 0.3125, -1.9103],
[5.0844, 0.3948, -1.4755],
[5.4271, 0.4552, -1.0043],
[5.6361, 0.4919, -0.5084],
[5.7063, 0.5043, 0],
[5.6361, 0.4919, 0.5084],
[5.4271, 0.4552, 1.0043],
[5.0844, 0.3948, 1.4755],
[4.6165, 0.3125, 1.9103],
[4.0350, 0.2102, 2.2981],
[3.3541, 0.0903, 2.6293],
[2.5906, -0.0441, 2.8958],
[1.7634, -0.1897, 3.0909],
[0.8927, -0.3429, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.7593, -0.2012, -3.2100],
[1.5000, 0.0903, -3.0909],
[2.2037, 0.3673, -2.8958],
[2.8532, 0.6228, -2.6293],
[3.4324, 0.8508, -2.2981],
[3.9271, 1.0455, -1.9103],
[4.3250, 1.2021, -1.4755],
[4.6165, 1.3168, -1.0043],
[4.7943, 1.3868, -0.5084],
[4.8541, 1.4103, 0],
[4.7943, 1.3868, 0.5084],
[4.6165, 1.3168, 1.0043],
[4.3250, 1.2021, 1.4755],
[3.9271, 1.0455, 1.9103],
[3.4324, 0.8508, 2.2981],
[2.8532, 0.6228, 2.6293],
[2.2037, 0.3673, 2.8958],
[1.5000, 0.0903, 3.0909],
[0.7593, -0.2012, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.5517, -0.0887, -3.2100],
[1.0898, 0.3125, -3.0909],
[1.6011, 0.6937, -2.8958],
[2.0729, 1.0455, -2.6293],
[2.4938, 1.3592, -2.2981],
[2.8532, 1.6272, -1.9103],
[3.1423, 1.8427, -1.4755],
[3.3541, 2.0006, -1.0043],
[3.4833, 2.0969, -0.5084],
[3.5267, 2.1293, 0],
[3.4833, 2.0969, 0.5084],
[3.3541, 2.0006, 1.0043],
[3.1423, 1.8427, 1.4755],
[2.8532, 1.6272, 1.9103],
[2.4938, 1.3592, 2.2981],
[2.0729, 1.0455, 2.6293],
[1.6011, 0.6937, 2.8958],
[1.0898, 0.3125, 3.0909],
[0.5517, -0.0887, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.2900, -0.0165, -3.2100],
[0.5729, 0.4552, -3.0909],
[0.8417, 0.9033, -2.8958],
[1.0898, 1.3168, -2.6293],
[1.3110, 1.6856, -2.2981],
[1.5000, 2.0006, -1.9103],
[1.6520, 2.2540, -1.4755],
[1.7634, 2.4397, -1.0043],
[1.8313, 2.5529, -0.5084],
[1.8541, 2.5909, 0],
[1.8313, 2.5529, 0.5084],
[1.7634, 2.4397, 1.0043],
[1.6520, 2.2540, 1.4755],
[1.5000, 2.0006, 1.9103],
[1.3110, 1.6856, 2.2981],
[1.0898, 1.3168, 2.6293],
[0.8417, 0.9033, 2.8958],
[0.5729, 0.4552, 3.0909],
[0.2900, -0.0165, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.0000, 0.0084, -3.2100],
[0.0000, 0.5043, -3.0909],
[0.0000, 0.9755, -2.8958],
[0.0000, 1.4103, -2.6293],
[0.0000, 1.7981, -2.2981],
[0.0000, 2.1293, -1.9103],
[0.0000, 2.3958, -1.4755],
[0.0000, 2.5909, -1.0043],
[0.0000, 2.7100, -0.5084],
[0.0000, 2.7500, 0],
[0.0000, 2.7100, 0.5084],
[0.0000, 2.5909, 1.0043],
[0.0000, 2.3958, 1.4755],
[0.0000, 2.1293, 1.9103],
[0.0000, 1.7981, 2.2981],
[0.0000, 1.4103, 2.6293],
[0.0000, 0.9755, 2.8958],
[0.0000, 0.5043, 3.0909],
[0.0000, 0.0084, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.2900, -0.0165, -3.2100],
[-0.5729, 0.4552, -3.0909],
[-0.8417, 0.9033, -2.8958],
[-1.0898, 1.3168, -2.6293],
[-1.3110, 1.6856, -2.2981],
[-1.5000, 2.0006, -1.9103],
[-1.6520, 2.2540, -1.4755],
[-1.7634, 2.4397, -1.0043],
[-1.8313, 2.5529, -0.5084],
[-1.8541, 2.5909, 0],
[-1.8313, 2.5529, 0.5084],
[-1.7634, 2.4397, 1.0043],
[-1.6520, 2.2540, 1.4755],
[-1.5000, 2.0006, 1.9103],
[-1.3110, 1.6856, 2.2981],
[-1.0898, 1.3168, 2.6293],
[-0.8417, 0.9033, 2.8958],
[-0.5729, 0.4552, 3.0909],
[-0.2900, -0.0165, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.5517, -0.0887, -3.2100],
[-1.0898, 0.3125, -3.0909],
[-1.6011, 0.6937, -2.8958],
[-2.0729, 1.0455, -2.6293],
[-2.4938, 1.3592, -2.2981],
[-2.8532, 1.6272, -1.9103],
[-3.1423, 1.8427, -1.4755],
[-3.3541, 2.0006, -1.0043],
[-3.4833, 2.0969, -0.5084],
[-3.5267, 2.1293, 0],
[-3.4833, 2.0969, 0.5084],
[-3.3541, 2.0006, 1.0043],
[-3.1423, 1.8427, 1.4755],
[-2.8532, 1.6272, 1.9103],
[-2.4938, 1.3592, 2.2981],
[-2.0729, 1.0455, 2.6293],
[-1.6011, 0.6937, 2.8958],
[-1.0898, 0.3125, 3.0909],
[-0.5517, -0.0887, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.7593, -0.2012, -3.2100],
[-1.5000, 0.0903, -3.0909],
[-2.2037, 0.3673, -2.8958],
[-2.8532, 0.6228, -2.6293],
[-3.4324, 0.8508, -2.2981],
[-3.9271, 1.0455, -1.9103],
[-4.3250, 1.2021, -1.4755],
[-4.6165, 1.3168, -1.0043],
[-4.7943, 1.3868, -0.5084],
[-4.8541, 1.4103, 0],
[-4.7943, 1.3868, 0.5084],
[-4.6165, 1.3168, 1.0043],
[-4.3250, 1.2021, 1.4755],
[-3.9271, 1.0455, 1.9103],
[-3.4324, 0.8508, 2.2981],
[-2.8532, 0.6228, 2.6293],
[-2.2037, 0.3673, 2.8958],
[-1.5000, 0.0903, 3.0909],
[-0.7593, -0.2012, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.8927, -0.3429, -3.2100],
[-1.7634, -0.1897, -3.0909],
[-2.5906, -0.0441, -2.8958],
[-3.3541, 0.0903, -2.6293],
[-4.0350, 0.2102, -2.2981],
[-4.6165, 0.3125, -1.9103],
[-5.0844, 0.3948, -1.4755],
[-5.4271, 0.4552, -1.0043],
[-5.6361, 0.4919, -0.5084],
[-5.7063, 0.5043, 0],
[-5.6361, 0.4919, 0.5084],
[-5.4271, 0.4552, 1.0043],
[-5.0844, 0.3948, 1.4755],
[-4.6165, 0.3125, 1.9103],
[-4.0350, 0.2102, 2.2981],
[-3.3541, 0.0903, 2.6293],
[-2.5906, -0.0441, 2.8958],
[-1.7634, -0.1897, 3.0909],
[-0.8927, -0.3429, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.9386, -0.5000, -3.2100],
[-1.8541, -0.5000, -3.0909],
[-2.7239, -0.5000, -2.8958],
[-3.5267, -0.5000, -2.6293],
[-4.2426, -0.5000, -2.2981],
[-4.8541, -0.5000, -1.9103],
[-5.3460, -0.5000, -1.4755],
[-5.7063, -0.5000, -1.0043],
[-5.9261, -0.5000, -0.5084],
[-6.0000, -0.5000, 0],
[-5.9261, -0.5000, 0.5084],
[-5.7063, -0.5000, 1.0043],
[-5.3460, -0.5000, 1.4755],
[-4.8541, -0.5000, 1.9103],
[-4.2426, -0.5000, 2.2981],
[-3.5267, -0.5000, 2.6293],
[-2.7239, -0.5000, 2.8958],
[-1.8541, -0.5000, 3.0909],
[-0.9386, -0.5000, 3.2100],
[0, -0.5000, 3.2500]])
u = np.arange(1, 17).reshape(4, -1)
b = np.arange(4)
print("utsolve: {}".format(calibrate_magnetometer(a)))
if __name__ == "__main__":
main()
This seems to work like the mathlab code. The remaining issues were mostly to "flatten" the internal np.arrays in D and to select the second returnvalue from qr().
import numpy as np
from scipy.linalg import qr, svd
def calibrate_magnetometer(calibration_data: np.ndarray):
"""
performs magnetometer calibration from a set of data
using Merayo technique with a non iterative algoritm
J.Merayo et al. "Scalar calibration of vector magnemoters"
Meas. Sci. Technol. 11 (2000) 120-132.
X : a Nx3 (or 3xN) data matrix
each row (columns) contains x, y, z measurements
N must be such that the data set describes
as completely as possible the 3D space
In any case N > 10
The calibration tries to find the best 3D ellipsoid that fits the data set
and returns the parameters of this ellipsoid
U : shape ellipsoid parameter, (3x3) upper triangular matrix
c : ellipsoid center, (3x1) vector
Ellipsoid equation : (v-c)'*(U'*U)(v-c) = 1
with v a rough triaxes magnetometer measurement
calibrated measurement w = U*(v-c)
author : Alain Barraud, Suzanne Lesecq 2008
"""
(n, m) = calibration_data.shape
if m > 3 and n == 3: # check that x is not transposed
calibration_data = calibration_data.T
n = m
m = 3
if n <= 10:
return False # not enough data for calibration
# write the ellipsoid equation as D*p=0
# the best parameter is the solution of min||D*p|| with ||p||=1
# form D matix from X measurements
x = calibration_data[:, 0]
y = calibration_data[:, 1]
z = calibration_data[:, 2]
D = [np.square(x), np.square(y), np.square(z),
np.multiply(x,y), np.multiply(x,z), np.multiply(y,z),
x, y, z,
np.ones(n)]
D = np.array(D)
D = np.triu(qr(D.T)[1]) # avoids to compute the svd of a large matrix
u, s, v = svd(D) # because usually N may be very large
p = v[-1]
if p[0] < 0:
p = -p
# the following matrix a(p) must be positive definite.
# the optimization done by svd does not include such a constrain
# with "good" data the constrain is allways satisfied
# with too poor data A may fail to be positive definite
# in this case the calibration fails
a = np.array([[p[0], p[3] / 2, p[4] / 2],
[p[3] / 2, p[1], p[5] / 2],
[p[4] / 2, p[5] / 2, p[2]]])
uu = np.linalg.cholesky(a)
b = np.array([p[6], p[7], p[8]])
vv = utsolve(uu, b / 2, m)
d = p[9]
s = 1 / np.sqrt(vv * vv.T - d)
calibrated_c = -usolve(uu, vv, m) # ellipsoid center, or offset error
calibrated_u = s * uu # shape ellipsoid parameter
return calibrated_u, calibrated_c
def utsolve(u, b, n):
# solves u.T*x=b
x = np.zeros(n)
x[0] = b[0] / u[0, 0]
for k in range(1, n):
x[k] = (b[k] - np.matmul(x[0:k], u[0:k, k])) / u[k, k]
return x
def usolve(u, b, n):
# solves u*x=b
x = np.zeros((n, 1))
x[n - 1] = b[n - 1] / u[n - 1, n - 1]
for k in reversed(range(0, n - 1)):
x[k] = (b[k] - np.matmul(u[k, k + 1:n], x[k + 1:n].T[0])) / u[k, k]
return x.T[0]
Related
Getting the values of only one channel of a numpy array
I would very much be grateful if you don't close this question without even giving a hint of how to solve this problem,please.
I have the following
import numpy as np
layer1 = np.zeros((5,3,4),dtype=np.uint8)
layer1[0,0,0]=20
layer1[1,1,0]=20
layer1[2,2,0]=20
layer1[3,1,0]=20
layer1[4,0,0]=20
layer1[0,0,1] =50
layer1[1,0,1]=50
layer1[2,0,1]=50
print(layer1)
print("---------------")
which gives me
[[[20 50 0 0]
[ 0 0 0 0]
[ 0 0 0 0]]
[[ 0 50 0 0]
[20 0 0 0]
[ 0 0 0 0]]
[[ 0 50 0 0]
[ 0 0 0 0]
[20 0 0 0]]
[[ 0 0 0 0]
[20 0 0 0]
[ 0 0 0 0]]
[[20 0 0 0]
[ 0 0 0 0]
[ 0 0 0 0]]]
How can I reduce a get the values only of one channel ?
For example for channel=0
I want to get
[[20 0 0]
[ 0 20 0]
[ 0 0 20]
[ 0 20 0]
[20 0 0]]
where channel can be 0,1,2 or 3
EDIT: Just in case, the layer1[0,0,0]=20 is just a convenient way to fill up the matrix. My question is how to tranform layer1 once filled to the matrix of (5,3)
EDIT: if the "channel" is 1 then I would get
[[50 0 0]
[ 50 0 0]
[ 50 0 0]
[ 0 0 0]
[0 0 0]]
numpy array indexing is well documented. Don't skip it! In [1]: layer1 = np.zeros((5,3,4),dtype=np.uint8) ...: layer1[0,0,0]=20 ...: layer1[1,1,0]=20 ...: layer1[2,2,0]=20 ...: layer1[3,1,0]=20 ...: layer1[4,0,0]=20 ...: ...: layer1[0,0,1] =50 ...: layer1[1,0,1]=50 ...: layer1[2,0,1]=50 In [2]: layer1.shape Out[2]: (5, 3, 4) In [3]: layer1[:,:,0] Out[3]: array([[20, 0, 0], [ 0, 20, 0], [ 0, 0, 20], [ 0, 20, 0], [20, 0, 0]], dtype=uint8) In [4]: layer1[:,:,2] Out[4]: array([[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]], dtype=uint8)
Numpy get column of two dimensional matrix as array
I have a matrix that looks like that: >> X >> [[5.1 1.4] [4.9 1.4] [4.7 1.3] [4.6 1.5] [5. 1.4]] I want to get its first column as an array of [5.1, 4.9, 4.7, 4.6, 5.] However when I try to get it by X[:,0] i get >> [[5.1] [4.9] [4.7] [4.6] [5. ]] which is something different. How to get it as an array ?
You can use list comprehensions for this kind of thing.. import numpy as np X = np.array([[5.1, 1.4], [4.9, 1.4], [4.7, 1.3], [4.6, 1.5], [5.0, 1.4]]) X_0 = [i for i in X[:,0]] print(X_0) Output.. [5.1, 4.9, 4.7, 4.6, 5.0]
Almost there! Just reshape your result: X[:,0].reshape(1,-1) Outputs: [[5.1 4.9 4.7 4.6 5. ]] Full code: import numpy as np X=np.array([[5.1 ,1.4],[4.9 ,1.4], [4.7 ,1.3], [4.6 ,1.5], [5. , 1.4]]) print(X) print(X[:,0].reshape(1,-1))
With regular numpy array: In [3]: x = np.arange(15).reshape(5,3) In [4]: x Out[4]: array([[ 0, 1, 2], [ 3, 4, 5], [ 6, 7, 8], [ 9, 10, 11], [12, 13, 14]]) In [5]: x[:,0] Out[5]: array([ 0, 3, 6, 9, 12]) With np.matrix (use discouraged if not actually deprecated) In [6]: X = np.matrix(x) In [7]: X Out[7]: matrix([[ 0, 1, 2], [ 3, 4, 5], [ 6, 7, 8], [ 9, 10, 11], [12, 13, 14]]) In [8]: print(X) [[ 0 1 2] [ 3 4 5] [ 6 7 8] [ 9 10 11] [12 13 14]] In [9]: X[:,0] Out[9]: matrix([[ 0], [ 3], [ 6], [ 9], [12]]) In [10]: X[:,0].T Out[10]: matrix([[ 0, 3, 6, 9, 12]]) To get 1d array, convert to array and ravel, or in one step: In [11]: X[:,0].A1 Out[11]: array([ 0, 3, 6, 9, 12])
Partitioned matrix multiplication in tensorflow or pytorch
Assume I have matrices P with the size [4, 4] which partitioned (block) into 4 smaller matrices [2,2]. How can I efficiently multiply this block-matrix into another matrix (not partitioned matrix but smaller)? Let's Assume our original matric is: P = [ 1 1 2 2 1 1 2 2 3 3 4 4 3 3 4 4] Which split into submatrices: P_1 = [1 1 , P_2 = [2 2 , P_3 = [3 3 P_4 = [4 4 1 1] 2 2] 3 3] 4 4] Now our P is: P = [P_1 P_2 P_3 p_4] In the next step, I want to do element-wise multiplication between P and smaller matrices which its size is equal to number of sub-matrices: P * [ 1 0 = [P_1 0 = [1 1 0 0 0 0 ] 0 0] 1 1 0 0 0 0 0 0 0 0 0 0]
You can think of representing your large block matrix in a more efficient way. For instance, a block matrix P = [ 1 1 2 2 1 1 2 2 3 3 4 4 3 3 4 4] Can be represented using a = [ 1 0 b = [ 1 1 0 0 p = [ 1 2 1 0 0 0 1 1 ] 3 4 ] 0 1 0 1 ] As P = a # p # b With (# representing matrix multiplication). Matrices a and b represents/encode the block structure of P and the small p represents the values of each block. Now, if you want to multiply (element-wise) p with a small (2x2) matrix q you simply a # (p * q) # b A simple pytorch example In [1]: a = torch.tensor([[1., 0], [1., 0], [0., 1], [0, 1]]) In [2]: b = torch.tensor([[1., 1., 0, 0], [0, 0, 1., 1]]) In [3]: p=torch.tensor([[1., 2.], [3., 4.]]) In [4]: q = torch.tensor([[1., 0], [0., 0]]) In [5]: a # p # b Out[5]: tensor([[1., 1., 2., 2.], [1., 1., 2., 2.], [3., 3., 4., 4.], [3., 3., 4., 4.]]) In [6]: a # (p*q) # b Out[6]: tensor([[1., 1., 0., 0.], [1., 1., 0., 0.], [0., 0., 0., 0.], [0., 0., 0., 0.]]) I leave it to you as an exercise how to efficiently produce the "structure" matrices a and b given the sizes of the blocks.
Following is a general Tensorflow-based solution that works for input matrices p (large) and m (small) of arbitrary shapes as long as the sizes of p are divisible by the sizes of m on both axes.
def block_mul(p, m):
p_x, p_y = p.shape
m_x, m_y = m.shape
m_4d = tf.reshape(m, (m_x, 1, m_y, 1))
m_broadcasted = tf.broadcast_to(m_4d, (m_x, p_x // m_x, m_y, p_y // m_y))
mp = tf.reshape(m_broadcasted, (p_x, p_y))
return p * mp
Test:
import tensorflow as tf
tf.enable_eager_execution()
p = tf.reshape(tf.constant(range(36)), (6, 6))
m = tf.reshape(tf.constant(range(9)), (3, 3))
print(f"p:\n{p}\n")
print(f"m:\n{m}\n")
print(f"block_mul(p, m):\n{block_mul(p, m)}")
Output (Python 3.7.3, Tensorflow 1.13.1):
p:
[[ 0 1 2 3 4 5]
[ 6 7 8 9 10 11]
[12 13 14 15 16 17]
[18 19 20 21 22 23]
[24 25 26 27 28 29]
[30 31 32 33 34 35]]
m:
[[0 1 2]
[3 4 5]
[6 7 8]]
block_mul(p, m):
[[ 0 0 2 3 8 10]
[ 0 0 8 9 20 22]
[ 36 39 56 60 80 85]
[ 54 57 80 84 110 115]
[144 150 182 189 224 232]
[180 186 224 231 272 280]]
Another solution that uses implicit broadcasting is the following:
def block_mul2(p, m):
p_x, p_y = p.shape
m_x, m_y = m.shape
p_4d = tf.reshape(p, (m_x, p_x // m_x, m_y, p_y // m_y))
m_4d = tf.reshape(m, (m_x, 1, m_y, 1))
return tf.reshape(p_4d * m_4d, (p_x, p_y))
Don't know about the efficient method, but you can try these: Method 1: Using torch.cat() import torch def multiply(a, b): x1 = a[0:2, 0:2]*b[0,0] x2 = a[0:2, 2:]*b[0,1] x3 = a[2:, 0:2]*b[1,0] x4 = a[2:, 2:]*b[1,1] return torch.cat((torch.cat((x1, x2), 1), torch.cat((x3, x4), 1)), 0) a = torch.tensor([[1, 1, 2, 2],[1, 1, 2, 2],[3, 3, 4, 4,],[3, 3, 4, 4]]) b = torch.tensor([[1, 0],[0, 0]]) print(multiply(a, b)) output: tensor([[1, 1, 0, 0], [1, 1, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]]) Method 2: Using torch.nn.functional.pad() import torch.nn.functional as F import torch def multiply(a, b): b = F.pad(input=b, pad=(1, 1, 1, 1), mode='constant', value=0) b[0,0] = 1 b[0,1] = 1 b[1,0] = 1 return a*b a = torch.tensor([[1, 1, 2, 2],[1, 1, 2, 2],[3, 3, 4, 4,],[3, 3, 4, 4]]) b = torch.tensor([[1, 0],[0, 0]]) print(multiply(a, b)) output: tensor([[1, 1, 0, 0], [1, 1, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]])
If the matrices are small, you are probably fine with cat or pad. The solution with factorization is very elegant, as the one with a block_mul implementation. Another solution is turning the 2D block matrix in a 3D volume where each 2D slice is a block (P_1, P_2, P_3, P_4). Then use the power of broadcasting to multiply each 2D slice by a scalar. Finally reshape the output. Reshaping is not immediate but it's doable, port from numpy to pytorch of https://stackoverflow.com/a/16873755/4892874 In Pytorch: import torch h = w = 4 x = torch.ones(h, w) x[:2, 2:] = 2 x[2:, :2] = 3 x[2:, 2:] = 4 # number of blocks along x and y nrows=2 ncols=2 vol3d = x.reshape(h//nrows, nrows, -1, ncols) vol3d = vol3d.permute(0, 2, 1, 3).reshape(-1, nrows, ncols) out = vol3d * torch.Tensor([1, 0, 0, 0])[:, None, None].float() # reshape to original n, nrows, ncols = out.shape out = out.reshape(h//nrows, -1, nrows, ncols) out = out.permute(0, 2, 1, 3) out = out.reshape(h, w) print(out) tensor([[1., 1., 0., 0.], [1., 1., 0., 0.], [0., 0., 0., 0.], [0., 0., 0., 0.]]) I haven't benchmarked this against the others, but this doesn't consume additional memory like padding would do and it doesn't do slow operations like concatenation. It has also ther advantage of being easy to understand and visualize. You can generalize it to any situation by playing with h, w, nrows, ncols.
Although the other answer may be the solution, it is not an efficient way. I come up with another one to tackle the problem (but still is not perfect). The following implementation needs too much memory when our inputs are 3 or 4 dimensions. For example, for input size of 20*75*1024*1024, the following calculation needs around 12gb ram. Here is my implementation: import tensorflow as tf tf.enable_eager_execution() inps = tf.constant([ [1, 1, 1, 1, 2, 2, 2, 2], [1, 1, 1, 1, 2, 2, 2, 2], [1, 1, 1, 1, 2, 2, 2, 2], [1, 1, 1, 1, 2, 2, 2, 2], [3, 3, 3, 3, 4, 4, 4, 4], [3, 3, 3, 3, 4, 4, 4, 4], [3, 3, 3, 3, 4, 4, 4, 4], [3, 3, 3, 3, 4, 4, 4, 4]]) on_cells = tf.constant([[1, 0, 0, 1]]) on_cells = tf.expand_dims(on_cells, axis=-1) # replicate the value to block-size (4*4) on_cells = tf.tile(on_cells, [1, 1, 4 * 4]) # reshape to a format for permutation on_cells = tf.reshape(on_cells, (1, 2, 2, 4, 4)) # permutation on_cells = tf.transpose(on_cells, [0, 1, 3, 2, 4]) # reshape on_cells = tf.reshape(on_cells, [1, 8, 8]) # element-wise operation print(inps * on_cells) Output: tf.Tensor( [[[1 1 1 1 0 0 0 0] [1 1 1 1 0 0 0 0] [1 1 1 1 0 0 0 0] [1 1 1 1 0 0 0 0] [0 0 0 0 4 4 4 4] [0 0 0 0 4 4 4 4] [0 0 0 0 4 4 4 4] [0 0 0 0 4 4 4 4]]], shape=(1, 8, 8), dtype=int32)
Numpy least-squares solution not accurate results
I'm calculating the affine transformation I need from a a few points in a 3D space and using numpy.linalg.lstsq to do so. However, the results I'm getting, while not terribly far off, are not accurate enough, even in trivially simple examples: m = 100 xy = np.array([[0, 0, 0], [m, 0, 0], [m, m, 0], [0, m, 0], [0, 0, m], [m, 0, m], [m, m, m], [0, m, m]]) uv = np.array([[0.5, 0, 0], [m + 0.5, 0, 0], [m+ 0.5, m, 0], [0.5, m, 0], [0.5, 0, m], [m+ 0.5, 0, m], [m+ 0.5, m, m], [0.5, m, m]]) pts_a = np.hstack([uv, np.ones((uv.shape[0], 1))]) pts_b = np.hstack([xy, np.ones((xy.shape[0], 1))]) solution_1 = np.linalg.lstsq(pts_a, pts_b, rcond=None)[0] The result I'm expecting from the above code is: [[1, 0, 0, -0.5], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) The result I'm getting: [[ 1.00000000e+00 3.49047642e-16 3.60109527e-16 -5.00000000e-01] [ 1.77081442e-16 1.00000000e+00 -3.93150475e-16 1.80460546e-15] [ 2.21351803e-16 -3.11848610e-16 1.00000000e+00 -6.28251374e-15] [ 2.76689754e-18 1.06035619e-17 -1.19061095e-17 1.00000000e+00]] Those small differences make a considerable difference in my results. Any ideas how to solve it? NOTE: I can ONLY use numpy and math for my project, so using a different library is sadly not possible! Thanks!
Actually, the difference isn't small but quite big - you have the wrong sign for solution[0,3]. The problem is thath you didn't calculate the desired transformation T but the inverse of this transformation, i.e. T^-1. Let's do some math: T*X=U, with X - original vectors U - transformed vectors transposing it => X^t * T^t = U^t | | | \|/ \|/ \|/ A * x = b In your program A=pts_b and b=pts_a, that means the transformation T is (you have to swap pts_b and pts_b and to transpose the result to get the right matrix): T = np.linalg.lstsq(pts_b, pts_a)[0].T and voila: >>> T array([[ 1.00000000e+00, -8.15320034e-17, -6.59194921e-17, 5.00000000e-01], [ -4.97379910e-16, 1.00000000e+00, 7.77156117e-16, -1.02678283e-14], [ -2.13162819e-16, 4.44089210e-16, 1.00000000e+00, 1.91513472e-15], [ -4.44089205e-18, -8.84708973e-17, 9.88792381e-17, 1.00000000e+00]]) PS: You have solved the equation: X^t = U^t * (T^t)^(-1) | | | \|/ \|/ \|/ b = A * x
how to create 0-1 matrix from probability matrix
(In any language) For a research project, I am stuck on how to convert a matrix P of probability values to a matrix A such that A_ij = 1 with probability P_ij and 0 otherwise? I have looked through various random number generator documentations, but have been unable to figure out how to do this.
If I understand correctly: In [11]: p = np.random.uniform(size=(5,5)) In [12]: p Out[12]: array([[ 0.45481883, 0.21242567, 0.3124863 , 0.00485797, 0.31970718], [ 0.91995847, 0.29907277, 0.59154085, 0.85847147, 0.13227595], [ 0.91914631, 0.5495813 , 0.58648856, 0.08037582, 0.23005148], [ 0.12464628, 0.70657028, 0.75975869, 0.77632964, 0.24587041], [ 0.69259133, 0.183515 , 0.65500547, 0.19526148, 0.26975325]]) In [13]: a = (p.round(1)==0.7).astype(np.int8) In [14]: a Out[14]: array([[0, 0, 0, 0, 0], [0, 0, 0, 0, 0], [0, 0, 0, 0, 0], [0, 1, 0, 0, 0], [1, 0, 1, 0, 0]], dtype=int8)