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)

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;

% 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)

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

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)

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

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

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