iOS various updates

[matlabbe]

• Added Data Recording Mode. Add very low RAM mode for iOS App large-scale scanning #1320
• Added option to filter ARKit localization jumps.
  • TODO: find a default threshold working for most usages (maybe instead thresholding on speed, it could be on acceleration)
  • TODO check if this issue Rtabmap on Android devices #1281 (comment) also happens here
• TODO : add LAS export option. iOS: Add option to export in LAS format #1298
  • Currently blocked by GDAL library libgdal.a missing 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...

[matlabbe]

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