How to debug odometry error?

edit

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