As measuring the transform may be tricky sometimes (e.g., wide wheels, laser at locations hard to measure), the way I debug odometry in the end is to use hector_slam to get the laser pose, get the encoder counts from both wheels, and then minimize the base_link's odometry differences obtained from both sources (laser pose and encoder counts). In this way, I can optimize several parameters at once (e.g., left wheel radius, right wheel radius, base_link width, pose of the laser relative to the base_link), and use my rough measurements as the initial values.

I collect the laser pose and encoder counts by driving the robot in rectangular routes and turning both clockwise and counter-clockwise. Below is my matlab program for such calibration optimization.

```
function calibrate_odometry(filepath, filePrefix)
% calibrate_odometry(filepath, filePrefix)
%
% This program calibrates odometry and the pose of the laser relative to the
% base_link. It expects 2 csv files parsed from your bag file. One csv files
% stores the pose of your laser (retrieved from hector_slam), and the other
% stores the encoder counts of both your wheels (see initialize_index() for
% which columns corresponds to which data). This program will minimize the
% differences in odometry created by both sources. Hence, the variables it
% will optimize are left wheel radius, right wheel radius, base_link width,
% pose of the laser relative to the base_link (see initialize_params() and put
% your default values there).
%
% filepath: path to the csv files.
% filePrefix: prefix of the csv files. One csv file should be named as
% [prefix]_pose2D_laser.csv, and the other is
% [prefix]_encoder_num.csv.
global index
% default value
same_wheel_circ = false;
initialize_index();
x0 = initialize_params();
if same_wheel_circ
index.wheel_circ_right = index.wheel_circ_left;
end
% load data
[pose2D_laser, encoder_num] = load_data(filepath, filePrefix);
% make theta continuous for optimization
pose2D_laser(:,index.theta) =...
make_theta_continuous(pose2D_laser(:,index.theta));
% plot how it looks like first
traj_laser = create_baselink_traj_from_laser(pose2D_laser, x0);
traj_encoder =...
create_baselink_traj_from_encoder(encoder_num, traj_laser(1,:), x0);
plot_traj(traj_laser, traj_encoder, {'baselink\_from\_laser', 'reconstruct\_odom'});
title('before calibration')
% calibrate it
[x, traj_encoder, traj_laser] = calibrate_full(pose2D_laser, encoder_num, x0);
plot_traj(traj_laser, traj_encoder, {'baselink\_from\_laser', 'reconstruct\_odom'});
title('after calibration')
% print optimized number
print_optimized(x)
print_optimized_for_urdf(x)
end
function print_optimized_for_urdf(x)
global index
param = {'wheel_base',...
'wheel_radius_left',...
'wheel_radius_right',...
'rplidar_pos_x',...
'rplidar_pos_y',...
'rplidar_yaw'};
v = [x(index.wheel_base)...
x(index.wheel_circ_left)/(2*pi)...
x(index.wheel_circ_right)/(2*pi)...
x(index.laser_dist)*cos(x(index.laser_shift_theta))...
x(index.laser_dist)*sin(x(index.laser_shift_theta))...
x(index.laser_yaw)];
value = num2cell(v);
[param' value']
end
function print_optimized(x)
global index
param = {'wheel_base',...
'wheel_circ_left',...
'wheel_circ_right',...
'laser_dist',...
'laser_shift_theta',...
'laser_yaw'};
v = [x(index.wheel_base)...
x(index.wheel_circ_left)...
x(index.wheel_circ_right)...
x(index.laser_dist)...
x(index.laser_shift_theta)...
x(index.laser_yaw)];
value = num2cell(v);
[param' value']
end
function initialize_index()
global index
index = struct(...
... % this row is the index for Pose2D
'time', 1, 'x', 2, 'y', 3, 'theta', 4, ...
... % this row is the index for encoder counts
'leftwheel', 2, 'rightwheel', 3,...
... % these two rows are the index for x, the parameters to be optimized
'wheel_base', 1, 'wheel_circ_left', 2, 'wheel_circ_right', 3, ...
'laser_dist', 4, 'laser_shift_theta', 5, 'laser_yaw', 6);
end
function x0 = initialize_params()
wheel_base ...
```

(more)