multiSensorParameters

Download and Extract Sensor Parameters

Download the YAML file from the Pandaset Devkit [2], and save it to the current directory.

downloadURL = "https://raw.githubusercontent.com/scaleapi/pandaset-devkit/master/docs/static_extrinsic_calibration.yaml";
yamlFileName = "static_extrinsic_calibration.yaml";
websave(yamlFileName,downloadURL); 

Read the YAML file using the helperParsePandasetYAML helper function, which returns a structure containing the sensor parameters. The file contains parameters for eight sensors:

Each sensor stores its extrinsic parameters as a rotation quaternion (w, x, y, z) and a translation vector (x, y, z). Camera sensors additionally include intrinsic parameters: a 3-by-3 camera matrix K and distortion coefficients D. The main_pandar64 lidar defines the reference frame as it is mounted at the origin.

data = helperParsePandasetYAML(yamlFileName)

data = struct with fields:
           back_camera: [1×1 struct]
          front_camera: [1×1 struct]
              front_gt: [1×1 struct]
     front_left_camera: [1×1 struct]
    front_right_camera: [1×1 struct]
           left_camera: [1×1 struct]
         main_pandar64: [1×1 struct]
          right_camera: [1×1 struct]

Add Sensor Mounting Poses and Intrinsic Parameters

Create a multiSensorParameters object to store the sensor mounting poses. In this data set, the extrinsic parameters for each sensor represent a transformation from the reference frame, which is defined by the main_pandar64 lidar sensor, to the frame of that sensor. Inverting the extrinsic parameters provides the transformation from the sensor frame to the reference frame. Use addSensor to add each sensor to the multiSensorParameters object by specifying its transformation to the reference sensor main_pandar64. For the cameras, use the cameraIntrinsicsFromOpenCV function to create cameraIntrinsics objects from their camera matrices and distortion coefficients.

% Get sensor names
sensorNames = string(fieldnames(data));

% Create multiSensorParameters object and add the reference sensor first
refSensor = "main_pandar64";
multiSensorObj = multiSensorParameters(ReferenceFrame=refSensor);
multiSensorObj = addSensor(multiSensorObj,refSensor,"lidar",rigidtform3d());

% Process each remaining sensor
for i = 1:length(sensorNames)
    sensorName = sensorNames(i);
    if sensorName == refSensor
        continue
    end
    sensorData = data.(sensorName);

    % Determine sensor type: non-camera sensors in this file are lidar
    % sensors
    if contains(sensorName, "camera")
        sensorType = "camera";
    else
        sensorType = "lidar";
    end

    % Create extrinsic parameters from quaternions and translations, then invert to get 
    % the transformations from the sensor frame to the reference frame
    quat  = [sensorData.qw,sensorData.qx,sensorData.qy,sensorData.qz];
    trvec = [sensorData.tx,sensorData.ty,sensorData.tz];
    extrinsics = se3(quat,"quat",trvec);
    tformTRef  = inv(extrinsics);

    % Add each sensor to the multiSensorParameters object
    if sensorType == "camera"
        % Extract intrinsic parameters
        intrinsicsMatrix = reshape(sensorData.K,3,3)';
        distortionCoefficients = sensorData.D;

        % All cameras have the same image resolution
        imageSize = [1080 1920];

        % Create cameraIntrinsics object
        camIntrinsics = cameraIntrinsicsFromOpenCV(intrinsicsMatrix,distortionCoefficients,imageSize);

        % Add camera with intrinsics
        multiSensorObj = addSensor(multiSensorObj,sensorName,sensorType,tformTRef,refSensor, ...
            Intrinsics=camIntrinsics);
    else
        % Add sensor without intrinsics
        multiSensorObj = addSensor(multiSensorObj,sensorName,sensorType,tformTRef,refSensor);
    end
end

Visualize the sensor mounting configuration in the reference frame main_pandar64.

plot(multiSensorObj, ShowFrameAxisLabels=false);
hold off

Change Reference Frame to Vehicle Coordinate System

For automated driving applications, the vehicle coordinate system follows the ISO 8855 convention: the origin is on the ground directly below the midpoint of the rear axle, with the x-axis pointing forward, y-axis pointing left, and z-axis pointing up. In the Pandar64 reference frame used by your multiSensorParameters object,

, the x-axis points to the left of the vehicle and the y-axis points backward, which corresponds to a 90-degree rotation about the z-axis relative to the vehicle frame. The Pandar64 lidar sensor is mounted on the roof of the vehicle, approximately 0.36 m forward of and 1.85 m above the rear axle center. Use the changeReferenceFrame object function to transform all sensor mounting poses from the Pandar64 frame to the vehicle coordinate system.

% Pandar64 [X, Y, Z] axes correspond to [Y, -X, Z] in the vehicle frame,
% which is a 90-degree rotation about the Z-axis.
pandarRotation = [0 -1 0; 1 0 0; 0 0 1];

% Pandar64 position, in vehicle coordinates: [forward,left,up] in meters
pandarTranslation = [0.36 0 1.85];

pandarToVehicleTransform = se3(pandarRotation,pandarTranslation);
multiSensorObj = changeReferenceFrame(multiSensorObj,pandarToVehicleTransform,"vehicle");

Visualize the sensor mounting configuration in the vehicle coordinate system.

plot(multiSensorObj, ShowFrameAxisLabels=false);

References

[1] Xiao, Pengchuan, Zhenlei Shao, Steven Hao, et al. “PandaSet: Advanced Sensor Suite Dataset for Autonomous Driving.” 2021 IEEE International Intelligent Transportation Systems Conference (ITSC), September 19, 2021, 3095–101. https://doi.org/10.1109/ITSC48978.2021.9565009.

[2] Scale AI. pandaset-devkit. https://github.com/scaleapi/pandaset-devkit.

Import Robot Model

Import the URDF file as a rigidBodyTree (Robotics System Toolbox) object. Each rigid body in the tree represents a component of the robot, and each joint defines the spatial relationship between connected components.

urdfFileName = "robotisTurtleBot3WafflePi.urdf";
robot = importrobot(urdfFileName);
showdetails(robot)

--------------------
Robot: (10 bodies)

 Idx                       Body Name                      Joint Name                      Joint Type                       Parent Name(Idx)   Children Name(s)
 ---                       ---------                      ----------                      ----------                       ----------------   ----------------
   1                       base_link                      base_joint                           fixed                      base_footprint(0)   camera_link(2)  caster_back_left_link(5)  caster_back_right_link(6)  imu_link(7)  base_scan(8)  wheel_left_link(9)  wheel_right_link(10)  
   2                     camera_link                    camera_joint                           fixed                           base_link(1)   camera_rgb_frame(3)  
   3                camera_rgb_frame                camera_rgb_joint                           fixed                         camera_link(2)   camera_rgb_optical_frame(4)  
   4        camera_rgb_optical_frame        camera_rgb_optical_joint                           fixed                    camera_rgb_frame(3)   
   5           caster_back_left_link          caster_back_left_joint                           fixed                           base_link(1)   
   6          caster_back_right_link         caster_back_right_joint                           fixed                           base_link(1)   
   7                        imu_link                       imu_joint                           fixed                           base_link(1)   
   8                       base_scan                      scan_joint                           fixed                           base_link(1)   
   9                 wheel_left_link                wheel_left_joint                        revolute                           base_link(1)   
  10                wheel_right_link               wheel_right_joint                        revolute                           base_link(1)   
--------------------

Extract Sensor Transforms

The addSensor object function expects transformations from each sensor frame to the reference frame, but the camera on this robot has a chain of frames from camera_link to camera_rgb_frame to camera_rgb_optical_frame. For a camera, the sensor frame is the optical frame, and the optical frame camera_rgb_optical_frame where the z-axis points forward along the optical axis. For details, see . The lidar sensor corresponds to the base_scan body, and the IMU corresponds to imu_link.

sensorNames = ["camera_rgb_optical_frame","imu_link","base_scan"];

Use getTransform (Robotics System Toolbox) to compute the transformation from the working frame of each sensor to the base, accounting for the entire kinematic chain. This is important for the camera, where the optical frame connects to the base through multiple intermediate frames.

% Initialize sensor transformations for all three sensors
tformToRef = repmat(se3,1,3);

% Get transformation from working frame from each sensor to the robot base
config = homeConfiguration(robot);
for i = 1:numel(sensorNames)
    tform = getTransform(robot,config,sensorNames(i),robot.BaseName);
    tformToRef(i) = se3(tform);
end

Extract Sensor Intrinsic Parameters

The URDF file contains sensor-specific parameters in Gazebo extension tags, including camera field of view, image dimensions, and IMU noise characteristics. Use the helperExtractSensorIntrinsicsFromURDF helper function to parse the file and extract the camera and IMU sensor parameters.

params = helperExtractSensorIntrinsicsFromURDF(urdfFileName)

params = struct with fields:
    camera: [1×1 struct]
       imu: [1×1 struct]

Compute the focal length of the camera from the horizontal field of view, assuming a pinhole camera model with square pixels and the principal point at the image center. Then, construct a camera intrinsic parameters object using cameraIntrinsicsFromOpenCV with the intrinsic matrix and distortion coefficients.

imageSize = params.camera.imageSize;
focalLength = (imageSize(2)/2)/tan(params.camera.horizontalFOV/2);
principalPoint = imageSize([2 1])/2;
intrinsicMatrix = [focalLength,0,principalPoint(1);0,focalLength,principalPoint(2);0,0,1];
camIntrinsics = cameraIntrinsicsFromOpenCV(intrinsicMatrix,params.camera.distortion,imageSize);

Construct a factorIMUParameters (Navigation Toolbox) object, from the IMU noise parameters. The URDF file provides noise standard deviations, which must be squared to get the variances required by factorIMUParameters.

imuIntrinsics = factorIMUParameters( ...
    GyroscopeNoise=params.imu.rateNoise.stddev^2, ...
    GyroscopeBiasNoise=params.imu.rateNoise.biasStddev^2, ...
    AccelerometerNoise=params.imu.accelNoise.stddev^2, ...
    AccelerometerBiasNoise=params.imu.accelNoise.biasStddev^2);

Add Sensors to multiSensorParameters

Create a multiSensorParameters object and set the reference frame as the base name of the robot platform. Then add all sensors using the transformations from the sensor frame to the reference frame.

multiSensorObj = multiSensorParameters(ReferenceFrame=robot.BaseName);
multiSensorObj = addSensor(multiSensorObj,sensorNames(1),"camera",tformToRef(1),Intrinsics=camIntrinsics);
multiSensorObj = addSensor(multiSensorObj,sensorNames(2),"IMU",tformToRef(2),Intrinsics=imuIntrinsics);
multiSensorObj = addSensor(multiSensorObj,sensorNames(3),"lidar",tformToRef(3));

Visualize Sensor Configuration

Visualize the sensor mounting poses in the reference frame base_footprint.

plot(multiSensorObj,SensorSize=0.02,FrameSize=0.05);

References

[1] https://emanual.robotis.com/docs/en/platform/turtlebot3/features/. Accessed on April 10th, 2026