matlabbe
commented
2 months ago About ARKit jumps, here is a plot of the recorded poses (without any correction, using new Data recording mode at 30 Hz):
If we zoom in on the highest velocity peak, we can see how much ARKit re-localized by the trajectory jump:
Here are the two corresponding point clouds on that jump:
What it should be:
When applying a filtering at maximum velocity of 1 m/s for minimum jump > 2 cm, we can recover a smoother odometry trajectory:
Note that there are still some acceleration peaks, but they are more related to odometry noise than re-localization jumps. Here is a zoom around a noisy area:
script to reproduce these figures (MATLAB/octave) EDIT: using acceleration threshold instead of velocity:
close all
A = importdata('poses.txt', ' ', 1);
% Options:
max_acc=60
max_jump_filtered=0.02
t = A.data(:,1)-A.data(1,1);
x = A.data(:,2);
y = A.data(:,3);
z = A.data(:,4);
##pkg load quaternion
##Q = quaternion(A.data(:,8), A.data(:,5), A.data(:,6), A.data(:,7));
##dangular=zeros(length(Q)-1, 1);
##warning('off', 'q2rot:normalizing')
##for i=1:length(Q)-1
## q1 = Q(i) * quaternion(0,1,0,0) * Q(i)';
## q2 = Q(i+1) * quaternion(0,1,0,0) * Q(i+1)';
## v1 = [q1.x, q1.y, q1.z];
## v2 = [q2.x, q2.y, q2.z];
##
## % Acompute angle between vectors
## rad = dot(v1,v2) / sqrt (norm(v1)^2 * norm(v2)^2);
## if (rad < -1.0)
## rad = -1.0;
## endif
## if (rad > 1.0)
## rad = 1.0;
## endif
## dangular(i) = acos (rad);
##
## if (mod(i, 1000) == 0)
## disp(num2str(i));
## endif
##endfor
##dangular = rad2deg(dangular);
dt=t(2:end) - t(1:end-1);
dx=x(2:end) - x(1:end-1);
dy=y(2:end) - y(1:end-1);
dz=z(2:end) - z(1:end-1);
dlinear=sqrt(dx.*dx+dy.*dy+dz.*dz);
vx=dx./dt;
vy=dy./dt;
vz=dz./dt;
velocity=dlinear./dt;
##angvelocity=dangular./dt;
ax=(vx(2:end) - vx(1:end-1))./dt(1:end-1);
ay=(vy(2:end) - vy(1:end-1))./dt(1:end-1);
az=(vz(2:end) - vz(1:end-1))./dt(1:end-1);
acc=sqrt(ax.^2+ay.^2+az.^2);
offset = [0,0,0]
for i=3:length(x)
x(i) = x(i) + offset(1);
y(i) = y(i) + offset(2);
z(i) = z(i) + offset(3);
max_jump = max([abs(x(i-1)-x(i)), abs(y(i-1)-y(i)), abs(z(i-1)-z(i))]);
if acc(i-2) > max_acc && max_jump > max_jump_filtered
new_val = x(i-1)+vx(i-2)*dt(i-1);
offset(1) = offset(1)+(new_val-x(i));
printf("i=%d t=%f acc=%f vx(i-2)=%f prev_x=%f x=%f dt=%f new_x=%f offset=%f jump=%f\n",
i, t(i), acc(i-2), vx(i-2), x(i-1), x(i), dt(i-1), new_val, offset(1), max_jump)
x(i) = new_val;
vx(i-1) = (x(i) - x(i-1))/dt(i-1);
new_val = y(i-1)+vy(i-2)*dt(i-1);
offset(2) = offset(2)+(new_val-y(i));
y(i) = new_val;
vy(i-1) = (y(i) - y(i-1))/dt(i-1);
new_val = z(i-1)+vz(i-2)*dt(i-1);
offset(3) = offset(3)+(new_val-z(i));
z(i) = new_val;
vz(i-1) = (z(i) - z(i-1))/dt(i-1);
endif
endfor
% recompute velocity data
dx=x(2:end) - x(1:end-1);
dy=y(2:end) - y(1:end-1);
dz=z(2:end) - z(1:end-1);
dlinear=sqrt(dx.*dx+dy.*dy+dz.*dz);
vx=dx./dt;
vy=dy./dt;
vz=dz./dt;
velocity=dlinear./dt;
ax=(vx(2:end) - vx(1:end-1))./dt(1:end-1);
ay=(vy(2:end) - vy(1:end-1))./dt(1:end-1);
az=(vz(2:end) - vz(1:end-1))./dt(1:end-1);
acc=sqrt(ax.^2+ay.^2+az.^2);
##angacc=(angvelocity(2:end) - angvelocity(1:end-1))./dt(1:end-1);
figure
plot(t(1:end-1), dt, '.-')
hold on
plot(t(1:end-1), dlinear, '.-')
legend('dt', 'translation (m)')
title('Abs translation vs dt')
figure
plot(t,x, '.-')
hold on
plot(t,y, '.-')
plot(t,z, '.-')
legend('x', 'y', 'z')
title('Position (m)')
figure
plot(t(1:end-1), velocity, '.-')
hold on
plot(t(1:end-1),vx, '.-')
plot(t(1:end-1),vy, '.-')
plot(t(1:end-1),vz, '.-')
legend('vt','vx', 'vy', 'vz')
title('Linear Velocity (m/s)')
##figure
##plot(t(1:end-1), angvelocity, '.-')
##title('Angular Velocity (deg/s)')
figure
plot(t(1:end-2),acc, '.-')
hold on
plot(t(1:end-2),ax, '.-')
plot(t(1:end-2),ay, '.-')
plot(t(1:end-2),az, '.-')
legend('at','ax', 'ay', 'az')
title('Linear Acceleration m/s^2')
APOS80
libgdal.amissing after building it. Maybe try https://github.com/BeinerChes/gdalios and see what are the differences.Biggest issue right now: not sure if it was because of new iOS updates, but ArKit does now a lot of relocalizations. In 2020, ARkit felt more like a VIO than VSLAM, so the integration with RTAB-Map was pretty smooth and worked well. The new option to filter localization jumps is inspired from ARCore workaround we had to do in the past for similar issue. In both ARKit and ARCore, there is no option to disable re-localization...Update: The PR is ready for QA:
New issues: