apollo::localization::msf::LocalizationIntegImpl类 参考

interface of msf localization 更多...

#include <localization_integ_impl.h>

apollo::localization::msf::LocalizationIntegImpl 的协作图:

Public 成员函数
	LocalizationIntegImpl ()
	~LocalizationIntegImpl ()
common::Status	Init (const LocalizationIntegParam &params)
void	PcdProcess (const LidarFrame &lidar_frame)
void	RawImuProcessRfu (const ImuData &imu_data)
void	RawObservationProcess (const drivers::gnss::EpochObservation &raw_obs_msg)
void	RawEphemerisProcess (const drivers::gnss::GnssEphemeris &gnss_orbit_msg)
void	GnssBestPoseProcess (const drivers::gnss::GnssBestPose &bestgnsspos_msg)
void	GnssHeadingProcess (const drivers::gnss::Heading &gnssheading_msg)
const LocalizationResult &	GetLastestLidarLocalization () const
const LocalizationResult &	GetLastestIntegLocalization () const
const LocalizationResult &	GetLastestGnssLocalization () const

Protected 成员函数
void	PcdProcessImpl (const LidarFrame &pcd_data)
void	ImuProcessImpl (const ImuData &imu_data)
void	RawObservationProcessImpl (const drivers::gnss::EpochObservation &raw_obs_msg)
void	RawEphemerisProcessImpl (const drivers::gnss::GnssEphemeris &gnss_orbit_msg)
void	GnssBestPoseProcessImpl (const drivers::gnss::GnssBestPose &bestgnsspos_msg)
void	GnssHeadingProcessImpl (const drivers::gnss::Heading &gnssheading_msg)
void	TransferGnssMeasureToLocalization (const MeasureData &measure, LocalizationEstimate *localization)

详细描述

interface of msf localization

在文件 localization_integ_impl.h 第 51 行定义.

构造及析构函数说明

LocalizationIntegImpl()

apollo::localization::msf::LocalizationIntegImpl::LocalizationIntegImpl

(

)

在文件 localization_integ_impl.cc 第 29 行定义.

    : republish_process_(new MeasureRepublishProcess()),
      integ_process_(new LocalizationIntegProcess()),
      gnss_process_(new LocalizationGnssProcess()),
      lidar_process_(new LocalizationLidarProcess()),
      is_use_gnss_bestpose_(true),
      imu_altitude_from_lidar_localization_(0.0),
      imu_altitude_from_lidar_localization_available_(false),
      enable_lidar_localization_(true),
      gnss_antenna_extrinsic_(Eigen::Affine3d::Identity()) {}

~LocalizationIntegImpl()

apollo::localization::msf::LocalizationIntegImpl::~LocalizationIntegImpl

(

)

在文件 localization_integ_impl.cc 第 40 行定义.

                                              {
  delete republish_process_;
  delete lidar_process_;
  delete gnss_process_;
  delete integ_process_;
}

成员函数说明

GetLastestGnssLocalization()

const LocalizationResult & apollo::localization::msf::LocalizationIntegImpl::GetLastestGnssLocalization

(

)

const

在文件 localization_integ_impl.cc 第 375 行定义.

          {
  return lastest_gnss_localization_;
}

GetLastestIntegLocalization()

const LocalizationResult & apollo::localization::msf::LocalizationIntegImpl::GetLastestIntegLocalization

(

)

const

在文件 localization_integ_impl.cc 第 370 行定义.

          {
  return lastest_integ_localization_;
}

GetLastestLidarLocalization()

const LocalizationResult & apollo::localization::msf::LocalizationIntegImpl::GetLastestLidarLocalization

(

)

const

在文件 localization_integ_impl.cc 第 365 行定义.

          {
  return lastest_lidar_localization_;
}

GnssBestPoseProcess()

void apollo::localization::msf::LocalizationIntegImpl::GnssBestPoseProcess

(

const drivers::gnss::GnssBestPose &

bestgnsspos_msg

)

在文件 localization_integ_impl.cc 第 251 行定义.

                                                    {
  if (!is_use_gnss_bestpose_) {
    return;
  }
 
  GnssBestPoseProcessImpl(bestgnsspos_msg);
}

GnssBestPoseProcessImpl()

void apollo::localization::msf::LocalizationIntegImpl::GnssBestPoseProcessImpl ( const drivers::gnss::GnssBestPose &  bestgnsspos_msg )

protected

在文件 localization_integ_impl.cc 第 285 行定义.

                                                    {
  MeasureData measure;
  if (republish_process_->NovatelBestgnssposProcess(bestgnsspos_msg,
                                                    &measure)) {
    integ_process_->MeasureDataProcess(measure);
 
    expert_.AddGnssBestPose(bestgnsspos_msg, measure);
    LocalizationEstimate gnss_localization;
    TransferGnssMeasureToLocalization(measure, &gnss_localization);
    expert_.GetGnssStatus(gnss_localization.mutable_msf_status());
 
    lastest_gnss_localization_ =
        LocalizationResult(LocalizationMeasureState::OK, gnss_localization);
  }
}

GnssHeadingProcess()

void apollo::localization::msf::LocalizationIntegImpl::GnssHeadingProcess

(

const drivers::gnss::Heading &

gnssheading_msg

)

在文件 localization_integ_impl.cc 第 302 行定义.

                                               {
  GnssHeadingProcessImpl(gnssheading_msg);
}

GnssHeadingProcessImpl()

void apollo::localization::msf::LocalizationIntegImpl::GnssHeadingProcessImpl ( const drivers::gnss::Heading &  gnssheading_msg )

protected

在文件 localization_integ_impl.cc 第 307 行定义.

                                               {
  MeasureData measure;
  int heading_status = 0;
  if (republish_process_->GnssHeadingProcess(gnssheading_msg, &measure,
                                             &heading_status)) {
    integ_process_->MeasureDataProcess(measure);
  }
}

ImuProcessImpl()

void apollo::localization::msf::LocalizationIntegImpl::ImuProcessImpl ( const ImuData &  imu_data )

protected

在文件 localization_integ_impl.cc 第 129 行定义.

                                                                  {
  // imu -> lidar
  // imu -> integ -> republish -> lidar -> publish
 
  // start_time = boost::posix_time::microsec_clock::local_time();
  if (enable_lidar_localization_) {
    lidar_process_->RawImuProcess(imu_data);
  }
  integ_process_->RawImuProcess(imu_data);
 
  expert_.AddImu(imu_data);
 
  // integ
  IntegState state;
  LocalizationEstimate integ_localization;
  integ_process_->GetResult(&state, &integ_localization);
  ImuData corrected_imu;
  integ_process_->GetCorrectedImu(&corrected_imu);
  InertialParameter earth_param;
  integ_process_->GetEarthParameter(&earth_param);
  // check msf running status and set msf_status in integ_localization
 
  LocalizationIntegStatus integ_status;
  expert_.AddFusionLocalization(integ_localization);
  expert_.GetFusionStatus(integ_localization.mutable_msf_status(),
                          integ_localization.mutable_sensor_status(),
                          &integ_status);
 
  apollo::localization::Pose* posepb_loc = integ_localization.mutable_pose();
 
  if (imu_altitude_from_lidar_localization_available_) {
    apollo::common::PointENU* position = posepb_loc->mutable_position();
    position->set_z(imu_altitude_from_lidar_localization_);
  }
 
  // set linear acceleration
  Eigen::Vector3d orig_acceleration(corrected_imu.fb[0], corrected_imu.fb[1],
                                    corrected_imu.fb[2]);
  const apollo::common::Quaternion& orientation =
      integ_localization.pose().orientation();
  Eigen::Quaternion<double> quaternion(orientation.qw(), orientation.qx(),
                                       orientation.qy(), orientation.qz());
  Eigen::Vector3d vec_acceleration =
      static_cast<Eigen::Vector3d>(quaternion * orig_acceleration);
 
  // Remove gravity.
  vec_acceleration(2) -= earth_param.g;
 
  apollo::common::Point3D* linear_acceleration =
      posepb_loc->mutable_linear_acceleration();
  linear_acceleration->set_x(vec_acceleration(0));
  linear_acceleration->set_y(vec_acceleration(1));
  linear_acceleration->set_z(vec_acceleration(2));
 
  Eigen::Vector3d vec_acceleration_vrf =
      quaternion.inverse() * vec_acceleration;
 
  apollo::common::Point3D* linear_acceleration_vrf =
      posepb_loc->mutable_linear_acceleration_vrf();
  linear_acceleration_vrf->set_x(vec_acceleration_vrf(0));
  linear_acceleration_vrf->set_y(vec_acceleration_vrf(1));
  linear_acceleration_vrf->set_z(vec_acceleration_vrf(2));
 
  // set angular velocity
  Eigen::Vector3d orig_angular_velocity(
      corrected_imu.wibb[0], corrected_imu.wibb[1], corrected_imu.wibb[2]);
  Eigen::Vector3d vec_angular_velocity = static_cast<Eigen::Vector3d>(
      quaternion.toRotationMatrix() * orig_angular_velocity);
  apollo::common::Point3D* angular_velocity =
      posepb_loc->mutable_angular_velocity();
  angular_velocity->set_x(vec_angular_velocity(0));
  angular_velocity->set_y(vec_angular_velocity(1));
  angular_velocity->set_z(vec_angular_velocity(2));
 
  apollo::common::Point3D* angular_velocity_vrf =
      posepb_loc->mutable_angular_velocity_vrf();
  angular_velocity_vrf->set_x(corrected_imu.wibb[0]);
  angular_velocity_vrf->set_y(corrected_imu.wibb[1]);
  angular_velocity_vrf->set_z(corrected_imu.wibb[2]);
 
  lastest_integ_localization_ =
      LocalizationResult(LocalizationMeasureState(static_cast<int>(state)),
                         integ_localization, integ_status);
 
  InsPva integ_sins_pva;
  double covariance[9][9];
  integ_process_->GetResult(&state, &integ_sins_pva, covariance);
 
  // update republish
  republish_process_->IntegPvaProcess(integ_sins_pva);
 
  if (state != IntegState::NOT_INIT) {
    // pass result of integration localization to lidar process module
    if (enable_lidar_localization_ && state != IntegState::NOT_STABLE) {
      lidar_process_->IntegPvaProcess(integ_sins_pva);
    }
 
    if (!is_use_gnss_bestpose_) {
      // pass localization result to gnss process module
      gnss_process_->IntegSinsPvaProcess(integ_sins_pva, covariance);
    }
  }
}

Init()

Status apollo::localization::msf::LocalizationIntegImpl::Init

(

const LocalizationIntegParam &

params

)

在文件 localization_integ_impl.cc 第 47 行定义.

                                                                       {
  enable_lidar_localization_ = params.enable_lidar_localization;
  if (params.enable_lidar_localization) {
    auto state = lidar_process_->Init(params);
    if (!state.ok()) {
      return state;
    }
  }
 
  auto state = republish_process_->Init(params);
  if (!state.ok()) {
    return state;
  }
 
  state = integ_process_->Init(params);
  if (!state.ok()) {
    return state;
  }
 
  if (params.gnss_mode == static_cast<int>(GnssMode::SELF)) {
    state = gnss_process_->Init(params);
    is_use_gnss_bestpose_ = false;
    if (!state.ok()) {
      return state;
    }
  } else {
    is_use_gnss_bestpose_ = true;
  }
 
  if (params.is_using_raw_gnsspos) {
    gnss_antenna_extrinsic_.translation()(0) =
        params.imu_to_ant_offset.offset_x;
    gnss_antenna_extrinsic_.translation()(1) =
        params.imu_to_ant_offset.offset_y;
    gnss_antenna_extrinsic_.translation()(2) =
        params.imu_to_ant_offset.offset_z;
  } else {
    gnss_antenna_extrinsic_ = Eigen::Affine3d::Identity();
  }
  AINFO << "gnss and imu lever arm: "
        << gnss_antenna_extrinsic_.translation()(0) << " "
        << gnss_antenna_extrinsic_.translation()(1) << " "
        << gnss_antenna_extrinsic_.translation()(2);
 
  expert_.Init(params);
 
  return Status::OK();
}

PcdProcess()

void apollo::localization::msf::LocalizationIntegImpl::PcdProcess

(

const LidarFrame &

lidar_frame

)

在文件 localization_integ_impl.cc 第 96 行定义.

                                                                    {
  PcdProcessImpl(lidar_frame);
}

PcdProcessImpl()

void apollo::localization::msf::LocalizationIntegImpl::PcdProcessImpl ( const LidarFrame &  pcd_data )

protected

在文件 localization_integ_impl.cc 第 100 行定义.

                                                                     {
  // lidar -> republish -> integ
  lidar_process_->PcdProcess(pcd_data);
 
  int state = 0;
  LocalizationEstimate lidar_localization;
 
  state = lidar_process_->GetResult(&lidar_localization);
 
  expert_.AddLidarLocalization(lidar_localization);
 
  MeasureData lidar_measure;
  if (state == 2) {  // only state OK republish lidar msg
    republish_process_->LidarLocalProcess(lidar_localization, &lidar_measure);
    integ_process_->MeasureDataProcess(lidar_measure);
 
    imu_altitude_from_lidar_localization_ =
        lidar_localization.pose().position().z();
    imu_altitude_from_lidar_localization_available_ = true;
  }
 
  lastest_lidar_localization_ =
      LocalizationResult(LocalizationMeasureState(state), lidar_localization);
}

RawEphemerisProcess()

void apollo::localization::msf::LocalizationIntegImpl::RawEphemerisProcess

(

const drivers::gnss::GnssEphemeris &

gnss_orbit_msg

)

在文件 localization_integ_impl.cc 第 242 行定义.

                                                    {
  if (is_use_gnss_bestpose_) {
    return;
  }
 
  RawEphemerisProcessImpl(gnss_orbit_msg);
}

RawEphemerisProcessImpl()

void apollo::localization::msf::LocalizationIntegImpl::RawEphemerisProcessImpl ( const drivers::gnss::GnssEphemeris &  gnss_orbit_msg )

protected

在文件 localization_integ_impl.cc 第 280 行定义.

                                                    {
  gnss_process_->RawEphemerisProcess(gnss_orbit_msg);
}

RawImuProcessRfu()

void apollo::localization::msf::LocalizationIntegImpl::RawImuProcessRfu

(

const ImuData &

imu_data

)

在文件 localization_integ_impl.cc 第 125 行定义.

                                                                    {
  ImuProcessImpl(imu_data);
}

RawObservationProcess()

void apollo::localization::msf::LocalizationIntegImpl::RawObservationProcess

(

const drivers::gnss::EpochObservation &

raw_obs_msg

)

在文件 localization_integ_impl.cc 第 233 行定义.

                                                    {
  if (is_use_gnss_bestpose_) {
    return;
  }
 
  RawObservationProcessImpl(raw_obs_msg);
}

RawObservationProcessImpl()

void apollo::localization::msf::LocalizationIntegImpl::RawObservationProcessImpl ( const drivers::gnss::EpochObservation &  raw_obs_msg )

protected

在文件 localization_integ_impl.cc 第 260 行定义.

                                                    {
  gnss_process_->RawObservationProcess(raw_obs_msg);
 
  MeasureData gnss_measure;
  LocalizationMeasureState state = gnss_process_->GetResult(&gnss_measure);
 
  MeasureData measure;
  if (state == LocalizationMeasureState::OK ||
      state == LocalizationMeasureState::VALID) {
    republish_process_->GnssLocalProcess(gnss_measure, &measure);
    integ_process_->MeasureDataProcess(measure);
  }
 
  LocalizationEstimate gnss_localization;
  TransferGnssMeasureToLocalization(measure, &gnss_localization);
 
  lastest_gnss_localization_ = LocalizationResult(state, gnss_localization);
}

TransferGnssMeasureToLocalization()

void apollo::localization::msf::LocalizationIntegImpl::TransferGnssMeasureToLocalization ( const MeasureData &  measure, LocalizationEstimate *  localization  )

protected

在文件 localization_integ_impl.cc 第 317 行定义.

                                                                    {
  CHECK_NOTNULL(localization);
 
  apollo::common::Header* headerpb = localization->mutable_header();
  apollo::localization::Pose* posepb = localization->mutable_pose();
 
  double timestamp = measure.time;
  localization->set_measurement_time(timestamp);
  headerpb->set_timestamp_sec(timestamp);
 
  UTMCoor utm_xy;
  FrameTransform::LatlonToUtmXY(measure.gnss_pos.longitude,
                                measure.gnss_pos.latitude, &utm_xy);
 
  apollo::common::PointENU* position = posepb->mutable_position();
  position->set_x(utm_xy.x);
  position->set_y(utm_xy.y);
  position->set_z(measure.gnss_pos.height);
 
  apollo::common::Quaternion* quaternion = posepb->mutable_orientation();
  quaternion->set_qx(0.0);
  quaternion->set_qy(0.0);
  quaternion->set_qz(0.0);
  quaternion->set_qw(1.0);
 
  apollo::localization::Uncertainty* uncertainty =
      localization->mutable_uncertainty();
 
  apollo::common::Point3D* position_std_dev =
      uncertainty->mutable_position_std_dev();
  position_std_dev->set_x(-1.0);
  position_std_dev->set_y(-1.0);
  position_std_dev->set_z(-1.0);
 
  if (measure.is_have_variance) {
    position_std_dev->set_x(measure.variance[0][0]);
    position_std_dev->set_y(measure.variance[1][1]);
    position_std_dev->set_z(measure.variance[2][2]);
  }
 
  apollo::common::Point3D* orientation_std_dev =
      uncertainty->mutable_orientation_std_dev();
  orientation_std_dev->set_x(-1.0);
  orientation_std_dev->set_y(-1.0);
  orientation_std_dev->set_z(-1.0);
}

---

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

• modules/localization/msf/local_integ/localization_integ_impl.h
• modules/localization/msf/local_integ/localization_integ_impl.cc