Inconsistent Hector SLAM trajectory correction

Greetings. I am having some issues regarding the Hector SLAM algorithm. I am currently trying to compare the trajectory of a tracked robot: i'm using both the data from the encoders and the SLAM algorithm to track the position of the robot. As you can see from the attached image there are quite some differences from the trajectory calculated with the encoder and the trajectory exported from the Hector SLAM. Moreover the encoder one is more consistent with the actual path of the rover than the SLAM one. As you can see the overall shape of the trajectory is the same, but the SLAM one is scaled in an odd way, and somehow rotated with respect to the encoder one. I was wondering if there was a way to make the SLAM trajectory more consistent with reality or if i am missing something in the matlab enviroment. Can someone plese enlight me about this issue? If needed, of course, i will provide further information. Thanks in advance to everyone interested.

% Load topics
topic_sys = select(bag,'Topic',"/robo/sys");
topic_data_sys = readMessages(topic_sys);

topic_enc = select(bag,'Topic',"/robo/enc");
topic_data_enc = readMessages(topic_enc);

topic_slam = select(bag,"Topic","/slam_out_pose");
topic_data_slam = readMessages(topic_slam);

topic_scan = select(bag,"Topic","/velodyne_points");
topic_data_scan = readMessages(topic_scan);

%% SLAM Trajectory - Finding

for i =1:length(topic_data_slam)
    
    yTraj(i,1) = topic_data_slam{i,1}.Pose.Position.X ;
    xTraj(i,1) = -topic_data_slam{i,1}.Pose.Position.Y ;
    
end

xTraj = xTraj - xTraj(1,1);
yTraj = yTraj - yTraj(1,1);

Answer 1

[Abhaya]

Hi Marco,

As per my understanding, you are comparing the trajectory calculated from your robot's encoders with the trajectory obtained from the Hector SLAM algorithm, and you are seeing a mismatch between them in terms of scaling and rotation.

The difference in the trajectories could be due to difference in the cordinate frames.For example, the encoder might provide position updates relative to a specific reference frame (often robot-centric), while Hector SLAM might use a global or map-centric reference frame. You can use MATLAB procrustes function to align the SLAM trajectory with the encoder data. Here is a sample code on how to implement the function.

%Store the slam trajectory
for i =1:length(topic_data_slam)
    % Assuming Pose.Position.X is forward direction and Pose.Position.Y is lateral
    xTraj_slam(i,1) = topic_data_slam{i,1}.Pose.Position.X ;
    yTraj_slam(i,1) = -topic_data_slam{i,1}.Pose.Position.Y ;
    
end
%Store the encoder trajectory
for i =1:length(topic_data_enc)
    
    xTraj_encoder(i,1) = topic_data_enc{i,1}.Pose.Position.X ;
    yTraj_encoder(i,1) = -topic_data_enc{i,1}.Pose.Position.Y ;
    
end

traj_encoder = [xTraj_encoder, yTraj_encoder];
traj_slam = [xTraj_slam, yTraj_slam];

% Use Procrustes analysis to find the optimal rotation and translation
[d, Z, transform] = procrustes(traj_encoder, traj_slam);

% Apply the transformation to the SLAM trajectory
xTraj_slam_transformed = transform.T(1,1)*xTraj_slam + transform.T(1,2);
yTraj_slam_transformed = transform.T(2,1)*yTraj_slam + transform.T(2,2);

For more information, please refer to MATLAB documentation on procrustes function linked below.

https://www.mathworks.com/help/releases/r2024b/stats/procrustes.html

[Marco Capitanelli]

Thank you for your answer. I just discovered this function and it looks really helpful. Unfortunately the encoders and IMU have a different sample rate, thus the relative arrays have different lengths, which causes an error in the procrustes function. I need to extract a number of samples from the slam pose which is equal to the number of the encoder samples, then i'll be able to apply your solution. I'll let you know. Thanks again.