apollo::perception::onboard::TransformWrapper类 参考

#include <transform_wrapper.h>

apollo::perception::onboard::TransformWrapper 的协作图:

Public 成员函数
	TransformWrapper ()
	~TransformWrapper ()=default
void	Init (const std::string &sensor2novatel_tf2_child_frame_id)
void	Init (const std::string &sensor2novatel_tf2_frame_id, const std::string &sensor2novatel_tf2_child_frame_id, const std::string &novatel2world_tf2_frame_id, const std::string &novatel2world_tf2_child_frame_id)
bool	GetSensor2worldTrans (double timestamp, Eigen::Affine3d *sensor2world_trans, Eigen::Affine3d *novatel2world_trans=nullptr)
bool	GetExtrinsics (Eigen::Affine3d *trans)
bool	GetExtrinsics (double timestamp, Eigen::Affine3d *trans)
bool	GetTrans (double timestamp, Eigen::Affine3d *trans, const std::string &frame_id, const std::string &child_frame_id)
bool	GetExtrinsicsBySensorId (const std::string &from_sensor_id, const std::string &to_sensor_id, Eigen::Affine3d *trans)

Protected 成员函数
bool	QueryTrans (double timestamp, StampedTransform *trans, const std::string &frame_id, const std::string &child_frame_id)

详细描述

在文件 transform_wrapper.h 第 65 行定义.

构造及析构函数说明

TransformWrapper()

apollo::perception::onboard::TransformWrapper::TransformWrapper ( )

inline

在文件 transform_wrapper.h 第 67 行定义.

{}

~TransformWrapper()

apollo::perception::onboard::TransformWrapper::~TransformWrapper ( )

default

成员函数说明

GetExtrinsics() [1/2]

bool apollo::perception::onboard::TransformWrapper::GetExtrinsics	(	double	timestamp,
		Eigen::Affine3d *	trans
	)

在文件 transform_wrapper.cc 第 226 行定义.

                                                                           {
  if (sensor2novatel_extrinsics_ == nullptr) {
    StampedTransform trans_sensor2novatel;
    if (!QueryTrans(timestamp, &trans_sensor2novatel,
                    sensor2novatel_tf2_frame_id_,
                    sensor2novatel_tf2_child_frame_id_)) {
      return false;
    }
    sensor2novatel_extrinsics_.reset(new Eigen::Affine3d);
    *sensor2novatel_extrinsics_ =
        trans_sensor2novatel.translation * trans_sensor2novatel.rotation;
    AINFO << "Get sensor2novatel extrinsics successfully.";
  }
  if (!inited_ || trans == nullptr || sensor2novatel_extrinsics_ == nullptr) {
    AERROR << "TransformWrapper get extrinsics failed";
    return false;
  }
  *trans = *sensor2novatel_extrinsics_;
  return true;
}

GetExtrinsics() [2/2]

bool apollo::perception::onboard::TransformWrapper::GetExtrinsics

(

Eigen::Affine3d *

trans

)

在文件 transform_wrapper.cc 第 217 行定义.

                                                         {
  if (!inited_ || trans == nullptr || sensor2novatel_extrinsics_ == nullptr) {
    AERROR << "TransformWrapper get extrinsics failed";
    return false;
  }
  *trans = *sensor2novatel_extrinsics_;
  return true;
}

GetExtrinsicsBySensorId()

bool apollo::perception::onboard::TransformWrapper::GetExtrinsicsBySensorId	(	const std::string &	from_sensor_id,
		const std::string &	to_sensor_id,
		Eigen::Affine3d *	trans
	)

在文件 transform_wrapper.cc 第 326 行定义.

                          {
  if (trans == nullptr) {
    AERROR << "TransformWrapper get extrinsics failed";
    return false;
  }
 
  algorithm::SensorManager* sensor_manager =
      algorithm::SensorManager::Instance();
  std::string frame_id = sensor_manager->GetFrameId(to_sensor_id);
  std::string child_frame_id = sensor_manager->GetFrameId(from_sensor_id);
 
  StampedTransform transform;
  bool status = QueryTrans(0.0, &transform, frame_id, child_frame_id);
  if (status) {
    *trans = transform.translation * transform.rotation;
  }
  return status;
}

GetSensor2worldTrans()

bool apollo::perception::onboard::TransformWrapper::GetSensor2worldTrans	(	double	timestamp,
		Eigen::Affine3d *	sensor2world_trans,
		Eigen::Affine3d *	novatel2world_trans = nullptr
	)

在文件 transform_wrapper.cc 第 165 行定义.

                                        {
  if (!inited_) {
    AERROR << "TransformWrapper not Initialized,"
           << " unable to call GetSensor2worldTrans.";
    return false;
  }
 
  if (sensor2novatel_extrinsics_ == nullptr) {
    StampedTransform trans_sensor2novatel;
    if (!QueryTrans(timestamp, &trans_sensor2novatel,
                    sensor2novatel_tf2_frame_id_,
                    sensor2novatel_tf2_child_frame_id_)) {
      return false;
    }
    sensor2novatel_extrinsics_.reset(new Eigen::Affine3d);
    *sensor2novatel_extrinsics_ =
        trans_sensor2novatel.translation * trans_sensor2novatel.rotation;
    AINFO << "Get sensor2novatel extrinsics successfully.";
  }
 
  StampedTransform trans_novatel2world;
  trans_novatel2world.timestamp = timestamp;
  Eigen::Affine3d novatel2world;
 
  if (!QueryTrans(timestamp, &trans_novatel2world, novatel2world_tf2_frame_id_,
                  novatel2world_tf2_child_frame_id_)) {
    if (FLAGS_obs_enable_local_pose_extrapolation) {
      if (!transform_cache_.QueryTransform(
              timestamp, &trans_novatel2world,
              FLAGS_obs_max_local_pose_extrapolation_latency)) {
        return false;
      }
    } else {
      return false;
    }
  } else if (FLAGS_obs_enable_local_pose_extrapolation) {
    transform_cache_.AddTransform(trans_novatel2world);
  }
 
  novatel2world =
      trans_novatel2world.translation * trans_novatel2world.rotation;
  *sensor2world_trans = novatel2world * (*sensor2novatel_extrinsics_);
  if (novatel2world_trans != nullptr) {
    *novatel2world_trans = novatel2world;
  }
  AINFO << "Get pose timestamp: " << timestamp << ", pose: \n"
        << (*sensor2world_trans).matrix();
  return true;
}

GetTrans()

bool apollo::perception::onboard::TransformWrapper::GetTrans	(	double	timestamp,
		Eigen::Affine3d *	trans,
		const std::string &	frame_id,
		const std::string &	child_frame_id
	)

在文件 transform_wrapper.cc 第 247 行定义.

                                                                 {
  StampedTransform transform;
  if (!QueryTrans(timestamp, &transform, frame_id, child_frame_id)) {
    if (!FLAGS_obs_enable_local_pose_extrapolation ||
        !transform_cache_.QueryTransform(
            timestamp, &transform,
            FLAGS_obs_max_local_pose_extrapolation_latency)) {
      return false;
    }
  }
 
  if (FLAGS_obs_enable_local_pose_extrapolation) {
    transform_cache_.AddTransform(transform);
  }
 
  *trans = transform.translation * transform.rotation;
  return true;
}

Init() [1/2]

void apollo::perception::onboard::TransformWrapper::Init

(

const std::string &

sensor2novatel_tf2_child_frame_id

)

在文件 transform_wrapper.cc 第 141 行定义.

                                                        {
  sensor2novatel_tf2_frame_id_ = FLAGS_obs_sensor2novatel_tf2_frame_id;
  sensor2novatel_tf2_child_frame_id_ = sensor2novatel_tf2_child_frame_id;
  novatel2world_tf2_frame_id_ = FLAGS_obs_novatel2world_tf2_frame_id;
  novatel2world_tf2_child_frame_id_ =
      FLAGS_obs_novatel2world_tf2_child_frame_id;
  transform_cache_.SetCacheDuration(FLAGS_obs_transform_cache_size);
  inited_ = true;
}

Init() [2/2]

void apollo::perception::onboard::TransformWrapper::Init	(	const std::string &	sensor2novatel_tf2_frame_id,
		const std::string &	sensor2novatel_tf2_child_frame_id,
		const std::string &	novatel2world_tf2_frame_id,
		const std::string &	novatel2world_tf2_child_frame_id
	)

在文件 transform_wrapper.cc 第 152 行定义.

                                                       {
  sensor2novatel_tf2_frame_id_ = sensor2novatel_tf2_frame_id;
  sensor2novatel_tf2_child_frame_id_ = sensor2novatel_tf2_child_frame_id;
  novatel2world_tf2_frame_id_ = novatel2world_tf2_frame_id;
  novatel2world_tf2_child_frame_id_ = novatel2world_tf2_child_frame_id;
  transform_cache_.SetCacheDuration(FLAGS_obs_transform_cache_size);
  inited_ = true;
}

QueryTrans()

bool apollo::perception::onboard::TransformWrapper::QueryTrans ( double  timestamp, StampedTransform *  trans, const std::string &  frame_id, const std::string &  child_frame_id  )

protected

在文件 transform_wrapper.cc 第 268 行定义.

                                                                   {
  cyber::Time query_time(timestamp);
  std::string err_string;
  if (!tf2_buffer_->canTransform(frame_id, child_frame_id, query_time,
                                 static_cast<float>(FLAGS_obs_tf2_buff_size),
                                 &err_string)) {
    apollo::transform::TransformStamped latest_transform;
    double latest_buffer_time;
    if (!FLAGS_hardware_trigger) {
      try {
        latest_transform = tf2_buffer_->lookupTransform(
            frame_id, child_frame_id, apollo::cyber::Time(0));
        latest_buffer_time = latest_transform.header().timestamp_sec();
      } catch (tf2::TransformException& ex) {
        AERROR << ex.what();
        return false;
      }
    }
    // In simulation mode, use the latest transform information if query failed.
    if (!cyber::common::GlobalData::Instance()->IsRealityMode()) {
      query_time = cyber::Time(0);
    } else if (!FLAGS_hardware_trigger &&
               (timestamp - latest_buffer_time < 0.015) &&
               (timestamp - latest_buffer_time > 0)) {
      // soft trigger and the latency is within the tolerance range
      query_time = apollo::cyber::Time(0);
    } else {
      AERROR << "Can not find transform. " << FORMAT_TIMESTAMP(timestamp)
             << " frame_id: " << frame_id
             << " child_frame_id: " << child_frame_id
             << " Error info: " << err_string;
      return false;
    }
  }
 
  apollo::transform::TransformStamped stamped_transform;
  try {
    stamped_transform =
        tf2_buffer_->lookupTransform(frame_id, child_frame_id, query_time);
 
    trans->translation =
        Eigen::Translation3d(stamped_transform.transform().translation().x(),
                             stamped_transform.transform().translation().y(),
                             stamped_transform.transform().translation().z());
    trans->rotation =
        Eigen::Quaterniond(stamped_transform.transform().rotation().qw(),
                           stamped_transform.transform().rotation().qx(),
                           stamped_transform.transform().rotation().qy(),
                           stamped_transform.transform().rotation().qz());
  } catch (tf2::TransformException& ex) {
    AERROR << ex.what();
    return false;
  }
  return true;
}

---

该类的文档由以下文件生成:

• modules/perception/common/onboard/transform_wrapper/transform_wrapper.h
• modules/perception/common/onboard/transform_wrapper/transform_wrapper.cc