Revision history [back]

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=0.52; % distance between the two back wheels
    wheel_circ=0.964; % circumference of the back wheels
    laser_dist=0.44; % distance from baselink to laser frame
    laser_shift_theta=-0.023; % angle between x-axis and the-direction-from-baselink-to-the-laser
    laser_yaw = -0.0175; % angle between x-axis and the-0-degree-of-the-laser (yaw)

    x0 = [wheel_base;...
    wheel_circ;...
    wheel_circ;...
    laser_dist;...
    laser_shift_theta;...
    laser_yaw];
end

function [x_b, y_b, theta_b] = tf_transform_laser_to_baselink...
    (x_l, y_l, theta_l, laser_dist, laser_shift_theta, laser_yaw)    

    theta_b = theta_l - laser_yaw;
    x_b = x_l - laser_dist * cos(theta_b + laser_shift_theta);
    y_b = y_l - laser_dist * sin(theta_b + laser_shift_theta);
end

function [x, reconst_baselink_encoder, reconst_baselink_laser] =...
    calibrate_full(pose2D_laser, encoder_num, x0)

    global index

    % optimization
    x = fminsearch(@calibrate_it, x0);

%     % use fmincon if you would like to have constraints on the variables        
%     A = [0 0 0 0 1 0];
%     b = 0;
%     x = fmincon(@calibrate_it, x0, A, b);

    % reconstruct the two traces
    reconst_baselink_laser =...
        create_baselink_traj_from_laser(pose2D_laser, x);
    reconst_baselink_encoder =...
        create_baselink_traj_from_encoder(encoder_num, reconst_baselink_laser(1,:), x);     

    function RMS = calibrate_it(x)        

        % reconstruct the trajectory of the baselink based on laser
        reconst_laser = create_baselink_traj_from_laser(pose2D_laser, x);

        % reconstruct path by the new x        
        reconst_encoder =...
            create_baselink_traj_from_encoder(encoder_num, reconst_laser(1,:), x); 

        % interp1 the trace
        reconst_encoder = interp1_odom_to_laser_time(reconst_encoder, pose2D_laser);

        % calculate distance
        dx = reconst_encoder(:,index.x)-reconst_laser(:,index.x);
        dy = reconst_encoder(:,index.y)-reconst_laser(:,index.y);
        RMS = nansum(sqrt(dx.^2 + dy.^2))/numel(dx);        
        fprintf('RMS = %f\n', RMS);
    end
end

function traj = create_baselink_traj_from_laser(laser, x)
    global index

    numSamples = size(laser,1);
    traj = [laser(:,1) nan(numSamples,3)];
    for i=1:numSamples
        [traj(i,index.x), traj(i,index.y), traj(i,index.theta)] =...
            tf_transform_laser_to_baselink...
            (laser(i,index.x), laser(i,index.y), laser(i,index.theta),...
            x(index.laser_dist), x(index.laser_shift_theta), x(index.laser_yaw));
    end
end

function newTheta = make_theta_continuous(theta)

    dth = diff(theta);
    ii = dth > pi;
    dth(ii) = dth(ii) - 2*pi;
    ii = dth < -pi;
    dth(ii) = dth(ii) + 2*pi;

    newTheta = [theta(1); dth];
    newTheta = cumsum(newTheta);
end

function nodom = interp1_odom_to_laser_time(odom, laser)
    global index;
    nodom = [laser(:,index.time)...
        interp1(odom(:,1), odom(:,2:end), laser(:,1))];
end

function pose = create_baselink_traj_from_encoder(encoder, init_pose, x)
    global index;

    encoder_count=30240; % encoder counts per revolution (7560 pulses)

    % initialize pose array
    pose = nan(size(encoder,1), size(encoder,2)+1);    
    pose(:,index.time) = encoder(:,index.time);

    curr_pose = init_pose;
    for i=1:size(encoder,1)
        % compute the movement
        left_wheel = (encoder(i,index.leftwheel)/encoder_count)*x(index.wheel_circ_left);
        right_wheel = (encoder(i,index.rightwheel)/encoder_count)*x(index.wheel_circ_right);
        distance = (left_wheel+right_wheel)/2.0;

        % movement in 3 axis
        dth = (right_wheel-left_wheel)/x(index.wheel_base);        
        dx = distance * cos(curr_pose(index.theta));
        dy = distance * sin(curr_pose(index.theta));

        % update pose
        curr_pose = curr_pose + [0 dx dy dth];        
        pose(i,index.x) = curr_pose(index.x);
        pose(i,index.y) = curr_pose(index.y);
        pose(i,index.theta) = curr_pose(index.theta);                
    end            
end

function plot_traj(traj1, traj2, trajLegend)
    global index;    
    trajColor = {'b', 'r'};

    figure; hold on;
    quiver(traj1(:,index.x), traj1(:,index.y), cos(traj1(:,index.theta)), sin(traj1(:,index.theta)));
    quiver(traj2(:,index.x), traj2(:,index.y), cos(traj2(:,index.theta)), sin(traj2(:,index.theta)));
    hold off;    
    legend(trajLegend)
end

function [pose2D_laser, encoder_num] =...
    load_data(filepath, prefix)

    suffix = {'pose2D_laser', 'encoder_num'};
    for i=1:numel(suffix)
        filename = sprintf('%s/%s_%s.csv', filepath, prefix, suffix{i});
        data = importdata(filename);
        eval(sprintf('%s = data;', suffix{i}));
    end    
end