void K4ACalibrationTransformData::publishImuToDepthTf()

in src/k4a_calibration_transform_data.cpp [182:203]

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

void K4ACalibrationTransformData::publishImuToDepthTf()
{
  k4a_calibration_extrinsics_t* imu_extrinsics =
      &k4a_calibration_.extrinsics[K4A_CALIBRATION_TYPE_DEPTH][K4A_CALIBRATION_TYPE_ACCEL];
  tf2::Vector3 depth_to_imu_translation(imu_extrinsics->translation[0] / 1000.0f,
                                        imu_extrinsics->translation[1] / 1000.0f,
                                        imu_extrinsics->translation[2] / 1000.0f);
  tf2::Matrix3x3 depth_to_imu_rotation(
      imu_extrinsics->rotation[0], imu_extrinsics->rotation[1], imu_extrinsics->rotation[2],
      imu_extrinsics->rotation[3], imu_extrinsics->rotation[4], imu_extrinsics->rotation[5],
      imu_extrinsics->rotation[6], imu_extrinsics->rotation[7], imu_extrinsics->rotation[8]);
  tf2::Transform depth_to_imu_transform(depth_to_imu_rotation, depth_to_imu_translation);

  geometry_msgs::TransformStamped static_transform;
  static_transform.transform = tf2::toMsg(depth_to_imu_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_ + imu_frame_;

  static_broadcaster_.sendTransform(static_transform);
}