WIKI de Bertrand VANDEPORTAELE

slamline

Jacobian Test

3D Point Transformation

cd g2o-direct-slam-for-lines/g2o/examples/ba_line3d_pixel_sequence

`addpath('../ba_line3d_pixel_sequence/sequence/')`

Fichier: g2o/examples/ba_line3d_pixel_sequence/jacobiansTest.m

```% Transform point extremities

% Previous camera pose
R = [1 0 0;0 -1 0;0 0 -1];
t = [20 20 20]';

% 3D Points
A3d = segs3d(1,1:3); A3d = A3d';
B3d = segs3d(1,4:6); B3d = B3d';

% Segment extremities in previous frame
Ac_ = R'*A3d - R'*t;
Bc_ = R'*B3d - R'*t;

x = [0;0;0;3*[0; 0;0.0075]];
u = x(1:3);
w = x(4:6);

[ti, Ri, TRuw] = expMap(u,w);

% Transform extremities
a = Ri'*Ac_ - Ri'*ti;
b = Ri'*Bc_ - Ri'*ti;

% Analytic Jacobians
Atr = [kron(eye(3), (Ac_-ti)') , -Ri'];
Btr = [kron(eye(3), (Bc_-ti)') , -Ri'];

Auw = Atr*TRuw;
Buw = Btr*TRuw;

Au = Auw(:,1:3)
Aw = Auw(:,4:6)
Bu = Buw(:,1:3)
Bw = Buw(:,4:6)

% Numeric Jacobians
delta = 1e-9;
scalar = 1/(2*delta);
dx = zeros(6,1);
Auw_ = zeros(3,6);
Buw_ = zeros(3,6);

for i = 1:6

% x + dx
dx(i) = delta;
[dt, dR] = expMap(dx(1:3), dx(4:6)); % dx
tp = dt + dR*ti;
Rp = dR * Ri;

% Transform extremities
ap = Rp'*Ac_ - Rp'*tp;
bp = Rp'*Bc_ - Rp'*tp;

errorap = ap - a;
errorbp = bp - b;

% x - dx
dx(i) = -delta;
[dt, dR] = expMap(dx(1:3), dx(4:6)); % dx
tm = dt + dR*ti;
Rm = dR * Ri;

% Transform extremities
am = Rm'*Ac_ - Rm'*tm;
bm = Rm'*Bc_ - Rm'*tm;

erroram = am - a;
errorbm = bm - b;

errora = errorap - erroram;
errorb = errorbp - errorbm;

Auw_(:,i) = scalar*errora;
Buw_(:,i) = scalar*errorb;

dx(i) = 0;
end

Au_ = Auw_(:,1:3)
Aw_ = Auw_(:,4:6)
Bu_ = Buw_(:,1:3)
Bw_ = Buw_(:,4:6)```

Jacobians comparison

```Au =
-0.9997,   -0.0225,         0;
0.0225,   -0.9997,         0;
0,         0,   -1.0000;

Au_ =
-0.9997,   -0.0225,         0;
0.0225,   -0.9997,         0;
0,         0,   -1.0000;

Aw =
0,   -20,    10;
20,     0,     4;
-10,    -4,     0;

Aw_ =
0.4500,  -19.9949,   10.0875;
19.9949,    0.4500,    3.7740;
-10.0000,   -4.0000,         0;

Bu =
-0.9997,   -0.0225,         0;
0.0225,   -0.9997,         0;
0,         0,   -1.0000;

Bu_ =
-0.9997,   -0.0225,         0;
0.0225,   -0.9997,         0;
0,         0,   -1.0000;

Bw =
0,   -20,   -15;
20,     0,     4;
15,    -4,     0;

Bw_ =
0.4500,  -19.9949,  -14.9062;
19.9949,    0.4500,    4.3365;
15.0000,   -4.0000,         0;```

Transformation + Projection

```% Intrinsic parameter matrix
K = [320 0 320; 0 240 240; 0 0 1];

% Projection into the image
a_ = a/a(3);
b_ = b/b(3);

A_a = [
1/a(3), 0, -a(1)/a(3)^2;
0, 1/a(3), -a(2)/a(3)^2;
0, 0, 0];
B_b = [
1/b(3), 0, -b(1)/b(3)^2;
0, 1/b(3), -b(2)/b(3)^2;
0, 0, 0];

ap = K*a_;
bp = K*b_;

APa_ = K(1:2,1:3);
BPb_ = K(1:2,1:3);

% Jacobians of the projection
APw = APa_ * A_a * Aw
BPw = BPb_ * B_b * Bw
APu = APa_ * A_a * Au
BPu = BPb_ * B_b * Bu

% Numeric Jacobians
delta = 1e-9;
scalar = 1/(2*delta);
dx = zeros(6,1);
APuw_ = zeros(2,6);
BPuw_ = zeros(2,6);

for i = 1:6

% x + dx
dx(i) = delta;
[dt, dR] = expMap(dx(1:3), dx(4:6)); % dx
tp = dt + dR*ti;
Rp = dR * Ri;

% Transform extremities
a_p = Rp'*Ac_ - Rp'*tp;
b_p = Rp'*Bc_ - Rp'*tp;

% Projection into the image
ap_ = a_p/a_p(3);
bp_ = b_p/b_p(3);

app = K*ap_;app = app(1:2);
bpp = K*bp_;bpp = bpp(1:2);

errorapp = app - ap(1:2);
errorbpp = bpp - bp(1:2);

% x - dx
dx(i) = -delta;
[dt, dR] = expMap(dx(1:3), dx(4:6)); % dx
tm = dt + dR*ti;
Rm = dR * Ri;

% Transform extremities
a_m = Rm'*Ac_ - Rm'*tm;
b_m = Rm'*Bc_ - Rm'*tm;

% Projection into the image
am_ = a_m/a_m(3);
bm_ = b_m/b_m(3);

apm = K*am_;apm = apm(1:2);
bpm = K*bm_;bpm = bpm(1:2);

errorapm = apm - ap(1:2);
errorbpm = bpm - bp(1:2);

errorap = errorapp - errorapm;
errorbp = errorbpp - errorbpm;

APuw_(:,i) = scalar*errorap;
BPuw_(:,i) = scalar*errorbp;

dx(i) = 0;
end

APu_ = APuw_(:,1:3)
APw_ = APuw_(:,4:6)
BPu_ = BPuw_(:,1:3)
BPw_ = BPuw_(:,4:6)```

Jacobians comparison

```APu =

-15.9960   -0.3600   -3.0192
0.2700  -11.9970    6.0525

APu_ =

-15.9960   -0.3600   -3.0192
0.2700  -11.9969    6.0525

APw =

-30.1921 -332.0768  160.0000
300.5248   24.2099   48.0000

APw_ =

-22.9927 -331.9958  161.3994
300.4640   29.6095   45.2881

BPu =

-15.9960   -0.3600   -3.4692
0.2700  -11.9970   -8.9437

BPu_ =

-15.9959   -0.3600   -3.4692
0.2700  -11.9969   -8.9437

BPw =

52.0375 -333.8767 -240.0000
374.1559  -35.7749   48.0000

BPw_ =

59.2369 -333.7957 -238.4994
374.0952  -30.3754   52.0375```

Transformation + Projection + Line parameters

```cp = (ap + bp)/2;

% Jacobians for the computation of the central point
CPap = [1/2 0;0 1/2];
CPbp = [1/2 0;0 1/2];

CPw = CPap*APw + CPbp*BPw;
CPu = CPap*APu + CPbp*BPu;

th=atan2(bp(2)-ap(2), bp(1)-ap(1));

% Jacobians for the computation of the angle
THap = [ -(ap(2) - bp(2))/((ap(1) - bp(1))^2 + (ap(2) - bp(2))^2), (ap(1) - bp(1))/((ap(1) - bp(1))^2 + (ap(2) - bp(2))^2)];
THbp = [ (ap(2) - bp(2))/((ap(1) - bp(1))^2 + (ap(2) - bp(2))^2), -(ap(1) - bp(1))/((ap(1) - bp(1))^2 + (ap(2) - bp(2))^2)];
THw = THap*APw + THbp*BPw;
THu = THap*APu + THbp*BPu;

% Numeric Jacobians
delta = 1e-9;
scalar = 1/(2*delta);
dx = zeros(6,1);
CPuw_ = zeros(2,6);
THuw_ = zeros(1,6);

for i = 1:6

% x + dx
dx(i) = delta;
[dt, dR] = expMap(dx(1:3), dx(4:6)); % dx
tp = dt + dR*ti;
Rp = dR * Ri;

% Transform extremities
a_p = Rp'*Ac_ - Rp'*tp;
b_p = Rp'*Bc_ - Rp'*tp;

% Projection into the image
ap_ = a_p/a_p(3);
bp_ = b_p/b_p(3);

app = K*ap_;app = app(1:2);
bpp = K*bp_;bpp = bpp(1:2);

% Line parameters
cpp = (app + bpp)/2;
thp = atan2(bpp(2)-app(2), bpp(1)-app(1));

errorcpp = cpp - cp(1:2);
errorthp = thp - th;

% x - dx
dx(i) = -delta;
[dt, dR] = expMap(dx(1:3), dx(4:6)); % dx
tm = dt + dR*ti;
Rm = dR * Ri;

% Transform extremities
a_m = Rm'*Ac_ - Rm'*tm;
b_m = Rm'*Bc_ - Rm'*tm;

% Projection into the image
am_ = a_m/a_m(3);
bm_ = b_m/b_m(3);

apm = K*am_;apm = apm(1:2);
bpm = K*bm_;bpm = bpm(1:2);

% Line parameters
cpm = (apm + bpm)/2;
thm = atan2(bpm(2)-apm(2), bpm(1)-apm(1));

errorcpm = cpm - cp(1:2);
errorthm = thm - th;

errorcp = errorcpp - errorcpm;
errorth = errorthp - errorthm;

CPuw_(:,i) = scalar*errorcp;
THuw_(:,i) = scalar*errorth;

dx(i) = 0;
end

CPu_ = CPuw_(:,1:3)
CPw_ = CPuw_(:,4:6)
THu_ = THuw_(:,1:3)
THw_ = THuw_(:,4:6)```

Jacobians comparison

```CPu =

-15.9960   -0.3600   -3.2442
0.2700  -11.9970   -1.4456

CPu_ =

-15.9960   -0.3600   -3.2442
0.2700  -11.9969   -1.4456

CPw =

10.9227 -332.9767  -40.0000
337.3403   -5.7825   48.0000

CPw_ =

18.1221 -332.8957  -38.5500
337.2796   -0.3829   48.6628

THu =

1.0e-17 *

0         0   -0.3469

THu_ =

1.0e-06 *

0.1110         0         0

THw =

0.2666         0   -1.3325

THw_ =

0.2666   -0.0000   -1.3328```

Transformation + Projection + Line Parameters + Intensity gradients

```sumZ=double(imread('sequence/mireTagLineBlurH3.bmp'));

n=norm(bp-ap,2);
l=floor(n/2);

[err,J] = optim([cp; th], l, sumZ, CPu, CPw, THu, THw)

% Numeric Jacobians
delta = 1e-9;
scalar = 1/(2*delta);
dx = zeros(6,1);
J_ = zeros(1,6);

for i = 1:6

% x + dx
dx(i) = delta;
[dt, dR] = expMap(dx(1:3), dx(4:6)); % dx
tp = dt + dR*ti;
Rp = dR * Ri;

% Transform extremities
a_p = Rp'*Ac_ - Rp'*tp;
b_p = Rp'*Bc_ - Rp'*tp;

% Projection into the image
ap_ = a_p/a_p(3);
bp_ = b_p/b_p(3);

app = K*ap_;app = app(1:2);
bpp = K*bp_;bpp = bpp(1:2);

% Line parameters
cpp = (app + bpp)/2;
thp = atan2(bpp(2)-app(2), bpp(1)-app(1));

errp = optim([cpp; thp], l, sumZ);

errorerrp = errp - err;

% x - dx
dx(i) = -delta;
[dt, dR] = expMap(dx(1:3), dx(4:6)); % dx
tm = dt + dR*ti;
Rm = dR * Ri;

% Transform extremities
a_m = Rm'*Ac_ - Rm'*tm;
b_m = Rm'*Bc_ - Rm'*tm;

% Projection into the image
am_ = a_m/a_m(3);
bm_ = b_m/b_m(3);

apm = K*am_;apm = apm(1:2);
bpm = K*bm_;bpm = bpm(1:2);

% Line parameters
cpm = (apm + bpm)/2;
thm = atan2(bpm(2)-apm(2), bpm(1)-apm(1));

errm = optim([cpm; thm], l, sumZ);

errorerrm = errm - err;

errorerr = errorerrp - errorerrm;

J_(:,i) = scalar*errorerr;

dx(i) = 0;
end```
```function [err,J] = optim(x, l, sumZ, CPu, CPw, THu,THw)
cp = x(1:2);
th = x(3);
A1(1)=cp(1)-l*cos(th);
A1(2)=cp(2)-l*sin(th);
B1(1)=cp(1)+l*cos(th);
B1(2)=cp(2)+l*sin(th);
nb_err=1+2*l;

lineundertest=[-(B1(2)-A1(2));(B1(1)-A1(1))];
lineundertest=lineundertest/norm(lineundertest,2);
a=lineundertest(1);
b=lineundertest(2);

err = 0;
J = zeros(1,6);
for ii=1:nb_err

n=(ii-1)-l;
xa=cp(1)+n*cos(th);
ya=cp(2)+n*sin(th);

%scale of the half vector for gradient computation
s=1e-9;
%parameters for the half steps used for jacobian computation
% dtheta=0.001;
% dx=1;
dtheta=1e-9;
dx=1e-9;
dy=dx;

weight=1;

scalarprod2=(bilinearInterpolation(xa-s*a, ya-s*b, sumZ)-bilinearInterpolation(xa+s*a, ya+s*b, sumZ))/(2*s);

scalarprod2xp=(bilinearInterpolation(xa-s*a+dx, ya-s*b, sumZ)-bilinearInterpolation(xa+s*a+dx, ya+s*b, sumZ))/(2*s);
scalarprod2xm=(bilinearInterpolation(xa-s*a-dx, ya-s*b, sumZ)-bilinearInterpolation(xa+s*a-dx, ya+s*b, sumZ))/(2*s);
scalarprod2yp=(bilinearInterpolation(xa-s*a, ya-s*b+dy, sumZ)-bilinearInterpolation(xa+s*a, ya+s*b+dy, sumZ))/(2*s);
scalarprod2ym=(bilinearInterpolation(xa-s*a, ya-s*b-dy, sumZ)-bilinearInterpolation(xa+s*a, ya+s*b-dy, sumZ))/(2*s);

x0=cp(1);
y0=cp(2);
theta=th;
xtp=x0+n*cos(theta+dtheta);
ytp=y0+n*sin(theta+dtheta);
xtm=x0+n*cos(theta-dtheta);
ytm=y0+n*sin(theta-dtheta);

scalarprod2thetap=(bilinearInterpolation(xtp-s*a, ytp-s*b, sumZ)-bilinearInterpolation(xtp+s*a, ytp+s*b, sumZ))/(2*s);
scalarprod2thetam=(bilinearInterpolation(xtm-s*a, ytm-s*b, sumZ)-bilinearInterpolation(xtm+s*a, ytm+s*b, sumZ))/(2*s);

%jacobian from analytic function
Sl(1)=weight*(scalarprod2xp-scalarprod2xm)/(2*dx);
Sl(2)=weight*(scalarprod2yp-scalarprod2ym)/(2*dx);
Sl(3)=weight*(scalarprod2thetap-scalarprod2thetam)/(2*dtheta);

if nargin > 3
J = Sl*[CPu,CPw;THu,THw];

else
J = Sl;
end

err = err + 255-abs(scalarprod2);
end
end```

Jacobians comparison

```J =

1.0e+06 *

-0.0019    0.0852    0.0103   -2.3988    0.0411   -0.3316

J_ =

1.0e+05 *

-1.7764    0.3553   -0.6750   -0.2132   -1.5632    1.1724```
slamline.txt · Dernière modification: 2018/09/25 10:49 par bvandepo