void K4ACalibrationTransformData::getDepthCameraInfo()

in src/k4a_calibration_transform_data.cpp [256:306]

• 19 lines of code
• 1 McCabe index (conditional complexity)

void K4ACalibrationTransformData::getDepthCameraInfo(sensor_msgs::CameraInfo& camera_info)
{
  camera_info.header.frame_id = tf_prefix_ + depth_camera_frame_;
  camera_info.width = getDepthWidth();
  camera_info.height = getDepthHeight();
  camera_info.distortion_model = sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL;

  k4a_calibration_intrinsic_parameters_t* parameters = &k4a_calibration_.depth_camera_calibration.intrinsics.parameters;

  // The distortion parameters, size depending on the distortion model.
  // For "rational_polynomial", the 8 parameters are: (k1, k2, p1, p2, k3, k4, k5, k6).
  camera_info.D = {parameters->param.k1, parameters->param.k2, parameters->param.p1, parameters->param.p2,
                   parameters->param.k3, parameters->param.k4, parameters->param.k5, parameters->param.k6};

  // clang-format off
  // Intrinsic camera matrix for the raw (distorted) images.
  //     [fx  0 cx]
  // K = [ 0 fy cy]
  //     [ 0  0  1]
  // Projects 3D points in the camera coordinate frame to 2D pixel
  // coordinates using the focal lengths (fx, fy) and principal point
  // (cx, cy).
  camera_info.K = {parameters->param.fx,  0.0f,                   parameters->param.cx,
                   0.0f,                  parameters->param.fy,   parameters->param.cy,
                   0.0f,                  0.0,                    1.0f};

  // Projection/camera matrix
  //     [fx'  0  cx' Tx]
  // P = [ 0  fy' cy' Ty]
  //     [ 0   0   1   0]
  // By convention, this matrix specifies the intrinsic (camera) matrix
  //  of the processed (rectified) image. That is, the left 3x3 portion
  //  is the normal camera intrinsic matrix for the rectified image.
  // It projects 3D points in the camera coordinate frame to 2D pixel
  //  coordinates using the focal lengths (fx', fy') and principal point
  //  (cx', cy') - these may differ from the values in K.
  // For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
  //  also have R = the identity and P[1:3,1:3] = K.
  camera_info.P = {parameters->param.fx,  0.0f,                   parameters->param.cx,   0.0f,
                   0.0f,                  parameters->param.fy,   parameters->param.cy,   0.0f,
                   0.0f,                  0.0,                    1.0f,                   0.0f};

  // Rectification matrix (stereo cameras only)
  // A rotation matrix aligning the camera coordinate system to the ideal
  // stereo image plane so that epipolar lines in both stereo images are
  // parallel.
  camera_info.R = {1.0f, 0.0f, 0.0f,
                   0.0f, 1.0f, 0.0f,
                   0.0f, 0.0f, 1.0f};
  // clang-format on
}