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...

Update: The PR is ready for QA:

• Google Tango
• ARCore NDK
• ARCore Java
• AREngine?
• ARKit

New issues:

• The map is rotating in yaw on loop closures (saw on iOS and android, doublecheck with tango). It looks like the prior with minimum yaw variance on the rootid is not working.
  • Update: It seems related to gravity links, if they are not aligned with Z. When holding the phone in landscape so that pose up axis is z+ instead of y+, the yaw jumps don't happen anymore. Only affecting GTSAM optimizer, g2o looks fine. That seems also the cause of many rejected loop closures because graph optimization error is too high, then if we rotate the phone to have z+ axis up, loop closures are accepted.
    .
  • Decision: we will keep as "Won't fix". I looked if setting g2o by default would be better, but in multi-session mapping gtsam is better (on maps I tested). Also, the yaw jumps would appear only on loop closures, and it is "annoying" only when getting a lot of consecutive loop closures, which doesn't happen often in real mapping session (we loop once, then move exploring more). The workaround is to turn the phone so that z+ axis (blue one in third person view) is up. I also updated the noise model for gtsam gravity constraints, which seems to help a little.
• On iOS, move "New Data Recording" to an Advanced menu (to avoid misclick and don't confuse people which one to use).

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