void K4ACalibrationTransformData::publishRgbToDepthTf()

in src/k4a_calibration_transform_data.cpp [159:180]

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

void K4ACalibrationTransformData::publishRgbToDepthTf()
{
  k4a_calibration_extrinsics_t* rgb_extrinsics =
      &k4a_calibration_.extrinsics[K4A_CALIBRATION_TYPE_DEPTH][K4A_CALIBRATION_TYPE_COLOR];
  tf2::Vector3 depth_to_rgb_translation(rgb_extrinsics->translation[0] / 1000.0f,
                                        rgb_extrinsics->translation[1] / 1000.0f,
                                        rgb_extrinsics->translation[2] / 1000.0f);
  tf2::Matrix3x3 depth_to_rgb_rotation(
      rgb_extrinsics->rotation[0], rgb_extrinsics->rotation[1], rgb_extrinsics->rotation[2],
      rgb_extrinsics->rotation[3], rgb_extrinsics->rotation[4], rgb_extrinsics->rotation[5],
      rgb_extrinsics->rotation[6], rgb_extrinsics->rotation[7], rgb_extrinsics->rotation[8]);
  tf2::Transform depth_to_rgb_transform(depth_to_rgb_rotation, depth_to_rgb_translation);

  geometry_msgs::TransformStamped static_transform;
  static_transform.transform = tf2::toMsg(depth_to_rgb_transform.inverse());

  static_transform.header.stamp = ros::Time::now();
  static_transform.header.frame_id = tf_prefix_ + depth_camera_frame_;
  static_transform.child_frame_id = tf_prefix_ + rgb_camera_frame_;

  static_broadcaster_.sendTransform(static_transform);
}