=====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
segs3d = loadrawdouble('sequence/lines.raw');
% 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));
% Intensity gradients cost function
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));
% Intensity gradients cost function
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
%discrete gradient from sumZ
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