apollo::localization::msf::LocalizationLidarProcess类 参考

process lidar msg for localization 更多...

#include <localization_lidar_process.h>

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

Public 类型
typedef Eigen::Affine3d	TransformD
typedef Eigen::Vector3d	Vector3D
typedef Eigen::Matrix3d	Matrix3D

Public 成员函数
	LocalizationLidarProcess ()
	~LocalizationLidarProcess ()
apollo::common::Status	Init (const LocalizationIntegParam &params)
void	PcdProcess (const LidarFrame &lidar_frame)
void	GetResult (int *lidar_status, TransformD *location, Matrix3D *covariance) const
int	GetResult (LocalizationEstimate *lidar_local_msg)
void	IntegPvaProcess (const InsPva &sins_pva_msg)
void	RawImuProcess (const ImuData &imu_msg)

详细描述

process lidar msg for localization

在文件 localization_lidar_process.h 第 70 行定义.

成员类型定义说明

Matrix3D

typedef Eigen::Matrix3d apollo::localization::msf::LocalizationLidarProcess::Matrix3D

在文件 localization_lidar_process.h 第 74 行定义.

TransformD

typedef Eigen::Affine3d apollo::localization::msf::LocalizationLidarProcess::TransformD

在文件 localization_lidar_process.h 第 72 行定义.

Vector3D

typedef Eigen::Vector3d apollo::localization::msf::LocalizationLidarProcess::Vector3D

在文件 localization_lidar_process.h 第 73 行定义.

构造及析构函数说明

LocalizationLidarProcess()

apollo::localization::msf::LocalizationLidarProcess::LocalizationLidarProcess

(

)

在文件 localization_lidar_process.cc 第 33 行定义.

    : locator_(new LocalizationLidar()),
      pose_forecastor_(new PoseForcast()),
      map_path_(""),
      lidar_extrinsic_file_(""),
      lidar_height_file_(""),
      localization_mode_(2),
      yaw_align_mode_(2),
      lidar_filter_size_(17),
      delta_yaw_limit_(0.25 * 3.14159 / 180.0),
      init_delta_yaw_limit_(1.5 * 3.14159 / 180.0),
      compensate_pitch_roll_limit_(0.035),
      utm_zone_id_(50),
      map_coverage_theshold_(0.8),
      lidar_extrinsic_(TransformD::Identity()),
      lidar_height_(),
      is_get_first_lidar_msg_(false),
      cur_predict_location_(TransformD::Identity()),
      pre_predict_location_(TransformD::Identity()),
      velocity_(Vector3D::Zero()),
      pre_location_(TransformD::Identity()),
      location_(TransformD::Identity()),
      pre_location_time_(0.0),
      location_covariance_(Matrix3D::Zero()),
      lidar_status_(LidarState::NOT_VALID),
      reinit_flag_(false),
      imu_lidar_max_delay_time_(0.5),
      is_unstable_reset_(true),
      unstable_count_(0),
      unstable_threshold_(0.3),
      out_map_count_(0),
      forecast_integ_state_(ForecastState::NOT_VALID),
      forecast_timer_(-1) {}

~LocalizationLidarProcess()

apollo::localization::msf::LocalizationLidarProcess::~LocalizationLidarProcess

(

)

在文件 localization_lidar_process.cc 第 67 行定义.

                                                    {
  delete locator_;
  locator_ = nullptr;
 
  delete pose_forecastor_;
  pose_forecastor_ = nullptr;
}

成员函数说明

GetResult() [1/2]

void apollo::localization::msf::LocalizationLidarProcess::GetResult	(	int *	lidar_status,
		TransformD *	location,
		Matrix3D *	covariance
	)		const

在文件 localization_lidar_process.cc 第 208 行定义.

                                                                     {
  CHECK_NOTNULL(lidar_status);
  CHECK_NOTNULL(location);
  CHECK_NOTNULL(covariance);
 
  *lidar_status = static_cast<int>(lidar_status_);
  *location = location_;
  *covariance = location_covariance_;
}

GetResult() [2/2]

int apollo::localization::msf::LocalizationLidarProcess::GetResult

(

LocalizationEstimate *

lidar_local_msg

)

在文件 localization_lidar_process.cc 第 220 行定义.

                                                                             {
  if (lidar_local_msg == nullptr) {
    return static_cast<int>(LidarState::NOT_VALID);
  }
 
  lidar_local_msg->set_measurement_time(pre_location_time_);
 
  apollo::common::Header* headerpb = lidar_local_msg->mutable_header();
  headerpb->set_timestamp_sec(pre_location_time_);
 
  apollo::localization::Pose* posepb = lidar_local_msg->mutable_pose();
  apollo::common::PointENU* position = posepb->mutable_position();
  position->set_x(location_.translation()(0));
  position->set_y(location_.translation()(1));
  position->set_z(location_.translation()(2));
 
  apollo::common::Quaternion* quaternion = posepb->mutable_orientation();
  Eigen::Quaterniond quat(location_.linear());
  quaternion->set_qx(quat.x());
  quaternion->set_qy(quat.y());
  quaternion->set_qz(quat.z());
  quaternion->set_qw(quat.w());
 
  apollo::localization::Uncertainty* uncertainty =
      lidar_local_msg->mutable_uncertainty();
 
  apollo::common::Point3D* position_std_dev =
      uncertainty->mutable_position_std_dev();
  position_std_dev->set_x(location_covariance_(0, 0));
  position_std_dev->set_y(location_covariance_(1, 1));
  position_std_dev->set_z(0.0);
 
  static constexpr double yaw_covariance = 0.15 * 0.15 * DEG_TO_RAD2;
  apollo::common::Point3D* orientation_std_dev =
      uncertainty->mutable_orientation_std_dev();
  orientation_std_dev->set_x(0.0);
  orientation_std_dev->set_y(0.0);
  orientation_std_dev->set_z(yaw_covariance);
 
  MsfStatus* msf_status = lidar_local_msg->mutable_msf_status();
  msf_status->set_local_lidar_status(local_lidar_status_);
  msf_status->set_local_lidar_quality(local_lidar_quality_);
 
  return static_cast<int>(lidar_status_);
}

Init()

Status apollo::localization::msf::LocalizationLidarProcess::Init

(

const LocalizationIntegParam &

params

)

在文件 localization_lidar_process.cc 第 75 行定义.

                                                                          {
  // initial_success_ = false;
  map_path_ = params.map_path;
  lidar_extrinsic_file_ = params.lidar_extrinsic_file;
  lidar_height_file_ = params.lidar_height_file;
  localization_mode_ = params.localization_mode;
  yaw_align_mode_ = params.lidar_yaw_align_mode;
  utm_zone_id_ = params.utm_zone_id;
  map_coverage_theshold_ = params.map_coverage_theshold;
  imu_lidar_max_delay_time_ = params.imu_lidar_max_delay_time;
 
  lidar_filter_size_ = params.lidar_filter_size;
 
  is_unstable_reset_ = params.is_lidar_unstable_reset;
  unstable_threshold_ = params.unstable_reset_threshold;
  if_use_avx_ = params.if_use_avx;
 
  lidar_status_ = LidarState::NOT_VALID;
 
  // reload_map_flag_ = false;
  reinit_flag_ = false;
 
  // buffer
  out_map_count_ = 0;
 
  is_get_first_lidar_msg_ = false;
  cur_predict_location_ = TransformD::Identity();
  pre_predict_location_ = TransformD::Identity();
  pre_location_ = TransformD::Identity();
  velocity_ = Vector3D::Zero();
  location_ = TransformD::Identity();
  location_covariance_ = Matrix3D::Zero();
  local_lidar_status_ = LocalLidarStatus::MSF_LOCAL_LIDAR_UNDEFINED_STATUS;
  local_lidar_quality_ = LocalLidarQuality::MSF_LOCAL_LIDAR_BAD;
 
  bool success = LoadLidarExtrinsic(lidar_extrinsic_file_, &lidar_extrinsic_);
  if (!success) {
    AERROR << "LocalizationLidar: Fail to access the lidar"
              " extrinsic file: "
           << lidar_extrinsic_file_;
    return Status(common::LOCALIZATION_ERROR_LIDAR,
                  "Fail to access the lidar extrinsic file");
  }
 
  success = LoadLidarHeight(lidar_height_file_, &lidar_height_);
  if (!success) {
    AWARN << "LocalizationLidar: Fail to load the lidar"
             " height file: "
          << lidar_height_file_ << " Will use default value!";
    lidar_height_.height = params.lidar_height_default;
  }
 
  if (!locator_->Init(map_path_, lidar_filter_size_, lidar_filter_size_,
                      utm_zone_id_)) {
    local_lidar_status_ = LocalLidarStatus::MSF_LOCAL_LIDAR_MAP_LOADING_FAILED;
    return Status(common::LOCALIZATION_ERROR_LIDAR,
                  "Fail to load localization map!");
  }
 
  locator_->SetVelodyneExtrinsic(lidar_extrinsic_);
  locator_->SetLocalizationMode(localization_mode_);
  locator_->SetImageAlignMode(yaw_align_mode_);
  locator_->SetValidThreshold(static_cast<float>(map_coverage_theshold_));
  locator_->SetVehicleHeight(lidar_height_.height);
  locator_->SetDeltaPitchRollLimit(compensate_pitch_roll_limit_);
 
  const double deg_to_rad = 0.017453292519943;
  const double max_gyro_input = 200 * deg_to_rad;  // 200 degree
  const double max_acc_input = 5.0;                // 5.0 m/s^2
  pose_forecastor_->SetMaxListNum(400);
  pose_forecastor_->SetMaxAccelInput(max_acc_input);
  pose_forecastor_->SetMaxGyroInput(max_gyro_input);
  pose_forecastor_->SetZoneId(utm_zone_id_);
 
  return Status::OK();
}

IntegPvaProcess()

void apollo::localization::msf::LocalizationLidarProcess::IntegPvaProcess

(

const InsPva &

sins_pva_msg

)

在文件 localization_lidar_process.cc 第 266 行定义.

                                                                         {
  pose_forecastor_->PushInspvaData(sins_pva_msg);
}

PcdProcess()

void apollo::localization::msf::LocalizationLidarProcess::PcdProcess

(

const LidarFrame &

lidar_frame

)

在文件 localization_lidar_process.cc 第 164 行定义.

                                                                       {
  if (!CheckState()) {
    AERROR << "PcdProcess: Receive an invalid lidar msg!";
    return;
  }
 
  // pcd process cost time
  Timer timer;
  timer.Start();
 
  static unsigned int pcd_index = 0;
 
  if (!GetPredictPose(lidar_frame.measurement_time, &cur_predict_location_,
                      &forecast_integ_state_)) {
    AINFO << "PcdProcess: Discard a lidar msg because can't get predict pose. "
          << "More info see log in function GetPredictPose.";
    return;
  }
 
  if (forecast_integ_state_ != ForecastState::INCREMENT) {
    forecast_timer_ = -1;
  }
  ++forecast_timer_;
 
  locator_->SetDeltaYawLimit(ComputeDeltaYawLimit(
      forecast_timer_, 10, delta_yaw_limit_, init_delta_yaw_limit_));
 
  if (!is_get_first_lidar_msg_) {
    pre_predict_location_ = cur_predict_location_;
    pre_location_ = cur_predict_location_;
    velocity_ = Vector3D::Zero();
    is_get_first_lidar_msg_ = true;
  }
 
  velocity_ = cur_predict_location_.translation() - pre_location_.translation();
 
  int ret = locator_->Update(pcd_index++, cur_predict_location_, velocity_,
                             lidar_frame, if_use_avx_);
 
  UpdateState(ret, lidar_frame.measurement_time);
 
  timer.End("Lidar process");
}

RawImuProcess()

void apollo::localization::msf::LocalizationLidarProcess::RawImuProcess

(

const ImuData &

imu_msg

)

在文件 localization_lidar_process.cc 第 270 行定义.

                                                                   {
  pose_forecastor_->PushImuData(imu_msg);
}

---

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

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