slamline
Table des matières
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
slamline.txt · Dernière modification : 2021/02/19 20:20 de 127.0.0.1