apollo::localization::msf::LocalizationLidar类 参考

#include <localization_lidar.h>

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

Public 类型
typedef apollo::localization::msf::pyramid_map::PyramidMap	PyramidMap
typedef apollo::localization::msf::pyramid_map::MapNodeIndex	MapNodeIndex
typedef apollo::localization::msf::pyramid_map::PyramidMapNode	PyramidMapNode
typedef apollo::localization::msf::pyramid_map::PyramidMapNodePool	PyramidMapNodePool
typedef apollo::localization::msf::pyramid_map::PyramidMapMatrix	PyramidMapMatrix
typedef apollo::localization::msf::pyramid_map::PyramidMapConfig	PyramidMapConfig
typedef apollo::localization::msf::pyramid_map::FloatMatrix	FloatMatrix
typedef apollo::localization::msf::pyramid_map::UIntMatrix	UIntMatrix

Public 成员函数
	LocalizationLidar ()
	The constructor.
	~LocalizationLidar ()
	The deconstructor.
bool	Init (const std::string &map_path, const unsigned int search_range_x, const unsigned int search_range_y, const int zone_id, const unsigned int resolution_id=0)
void	SetVelodyneExtrinsic (const Eigen::Affine3d &pose)
void	SetVehicleHeight (double height)
void	SetValidThreshold (float valid_threashold)
void	SetImageAlignMode (int mode)
void	SetLocalizationMode (int mode)
void	SetDeltaYawLimit (double limit)
void	SetDeltaPitchRollLimit (double limit)
int	Update (const unsigned int frame_idx, const Eigen::Affine3d &pose, const Eigen::Vector3d velocity, const LidarFrame &lidar_frame, bool use_avx=false)
void	GetResult (Eigen::Affine3d *location, Eigen::Matrix3d *covariance, double *location_score)
void	GetLocalizationDistribution (Eigen::MatrixXd *distribution)

Protected 成员函数
void	ComposeMapNode (const Eigen::Vector3d &trans)
void	RefineAltitudeFromMap (Eigen::Affine3d *pose)

Protected 属性
LidarLocator *	lidar_locator_
int	search_range_x_ = 0
int	search_range_y_ = 0
int	node_size_x_ = 0
int	node_size_y_ = 0
double	resolution_ = 0.125
MapNodeData *	lidar_map_node_
PyramidMapConfig	config_
PyramidMap	map_
PyramidMapNodePool	map_node_pool_
Eigen::Vector2d	map_left_top_corner_
unsigned int	resolution_id_ = 0
int	zone_id_ = 50
bool	is_map_loaded_ = false
double	vehicle_lidar_height_ = 1.7
double	pre_vehicle_ground_height_ = 0.0
bool	is_pre_ground_height_valid_ = false
Eigen::Affine3d	velodyne_extrinsic_

详细描述

在文件 localization_lidar.h 第 71 行定义.

成员类型定义说明

FloatMatrix

typedef apollo::localization::msf::pyramid_map::FloatMatrix apollo::localization::msf::LocalizationLidar::FloatMatrix

在文件 localization_lidar.h 第 82 行定义.

MapNodeIndex

typedef apollo::localization::msf::pyramid_map::MapNodeIndex apollo::localization::msf::LocalizationLidar::MapNodeIndex

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

PyramidMap

typedef apollo::localization::msf::pyramid_map::PyramidMap apollo::localization::msf::LocalizationLidar::PyramidMap

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

PyramidMapConfig

typedef apollo::localization::msf::pyramid_map::PyramidMapConfig apollo::localization::msf::LocalizationLidar::PyramidMapConfig

在文件 localization_lidar.h 第 81 行定义.

PyramidMapMatrix

typedef apollo::localization::msf::pyramid_map::PyramidMapMatrix apollo::localization::msf::LocalizationLidar::PyramidMapMatrix

在文件 localization_lidar.h 第 79 行定义.

PyramidMapNode

typedef apollo::localization::msf::pyramid_map::PyramidMapNode apollo::localization::msf::LocalizationLidar::PyramidMapNode

在文件 localization_lidar.h 第 75 行定义.

PyramidMapNodePool

typedef apollo::localization::msf::pyramid_map::PyramidMapNodePool apollo::localization::msf::LocalizationLidar::PyramidMapNodePool

在文件 localization_lidar.h 第 77 行定义.

UIntMatrix

typedef apollo::localization::msf::pyramid_map::UIntMatrix apollo::localization::msf::LocalizationLidar::UIntMatrix

在文件 localization_lidar.h 第 83 行定义.

构造及析构函数说明

LocalizationLidar()

apollo::localization::msf::LocalizationLidar::LocalizationLidar

(

)

The constructor.

在文件 localization_lidar.cc 第 23 行定义.

    : lidar_locator_(new LidarLocator()),
      search_range_x_(21),
      search_range_y_(21),
      node_size_x_(1024),
      node_size_y_(1024),
      resolution_(0.125),
      lidar_map_node_(nullptr),
      config_("lossy_map"),
      map_(&config_),
      map_node_pool_(25, 8),
      resolution_id_(0),
      is_map_loaded_(false),
      vehicle_lidar_height_(1.7),
      pre_vehicle_ground_height_(0.0),
      is_pre_ground_height_valid_(false),
      velodyne_extrinsic_(Eigen::Affine3d::Identity()) {
  map_left_top_corner_ = Eigen::Vector2d::Zero();
}

~LocalizationLidar()

apollo::localization::msf::LocalizationLidar::~LocalizationLidar

(

)

The deconstructor.

在文件 localization_lidar.cc 第 43 行定义.

                                      {
  if (lidar_map_node_) {
    delete lidar_map_node_;
    lidar_map_node_ = nullptr;
  }
 
  delete lidar_locator_;
  lidar_locator_ = nullptr;
}

成员函数说明

ComposeMapNode()

void apollo::localization::msf::LocalizationLidar::ComposeMapNode ( const Eigen::Vector3d &  trans )

protected

在文件 localization_lidar.cc 第 273 行定义.

                                                                 {
  Eigen::Vector2d center(trans(0), trans(1));
  Eigen::Vector2d left_top_corner(center(0) - node_size_x_ * resolution_ / 2.0,
                                  center(1) - node_size_y_ * resolution_ / 2.0);
 
  // get map node index 2x2
  MapNodeIndex map_node_idx[2][2];
  // top left corner
  map_node_idx[0][0] = MapNodeIndex::GetMapNodeIndex(
      map_.GetMapConfig(), left_top_corner, resolution_id_, zone_id_);
  // top right corner
  map_node_idx[0][1] = map_node_idx[0][0];
  map_node_idx[0][1].n_ += 1;
  // bottom left corner
  map_node_idx[1][0] = map_node_idx[0][0];
  map_node_idx[1][0].m_ += 1;
  // bottom right corner
  map_node_idx[1][1] = map_node_idx[0][0];
  map_node_idx[1][1].n_ += 1;
  map_node_idx[1][1].m_ += 1;
 
  // get map node 2x2
  PyramidMapNode* map_node[2][2] = {nullptr};
  for (unsigned int y = 0; y < 2; ++y) {
    for (unsigned int x = 0; x < 2; ++x) {
      map_node[y][x] =
          static_cast<PyramidMapNode*>(map_.GetMapNodeSafe(map_node_idx[y][x]));
    }
  }
 
  // compose map node
  unsigned int coord_x = 0;
  unsigned int coord_y = 0;
  map_node[0][0]->GetCoordinate(left_top_corner, &coord_x, &coord_y);
  map_left_top_corner_ = map_node[0][0]->GetCoordinate(coord_x, coord_y);
 
  int coord_xi = coord_x;
  int coord_yi = coord_y;
  int range_xs[2][2] = {0};
  int range_ys[2][2] = {0};
  range_xs[0][0] = node_size_x_ - coord_xi;
  range_xs[1][0] = node_size_x_ - coord_xi;
  range_xs[0][1] = coord_xi;
  range_xs[1][1] = coord_xi;
  range_ys[0][0] = node_size_y_ - coord_yi;
  range_ys[0][1] = node_size_y_ - coord_yi;
  range_ys[1][0] = coord_yi;
  range_ys[1][1] = coord_yi;
 
  int src_xs[2][2] = {0};
  int src_ys[2][2] = {0};
  src_xs[0][0] = coord_xi;
  src_xs[1][0] = coord_xi;
  src_xs[0][1] = 0;
  src_xs[1][1] = 0;
  src_ys[0][0] = coord_yi;
  src_ys[0][1] = coord_yi;
  src_ys[1][0] = 0;
  src_ys[1][1] = 0;
 
  int dst_xs[2][2] = {0};
  int dst_ys[2][2] = {0};
  dst_xs[0][0] = 0;
  dst_xs[1][0] = 0;
  dst_xs[0][1] = node_size_x_ - coord_xi;
  dst_xs[1][1] = node_size_x_ - coord_xi;
  dst_ys[0][0] = 0;
  dst_ys[0][1] = 0;
  dst_ys[1][0] = node_size_y_ - coord_yi;
  dst_ys[1][1] = node_size_y_ - coord_yi;
 
  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 2; ++j) {
      int range_x = range_xs[i][j];
      int range_y = range_ys[i][j];
      int src_x = src_xs[i][j];
      int src_y = src_ys[i][j];
      int dst_x = dst_xs[i][j];
      int dst_y = dst_ys[i][j];
      PyramidMapMatrix& map_cells =
          static_cast<PyramidMapMatrix&>(map_node[i][j]->GetMapCellMatrix());
      FloatMatrix* intensity_matrix = map_cells.GetIntensityMatrix(0);
      FloatMatrix* intensity_var_matrix = map_cells.GetIntensityVarMatrix(0);
      FloatMatrix* altitude_matrix = map_cells.GetAltitudeMatrix(0);
      UIntMatrix* count_matrix = map_cells.GetCountMatrix(0);
      for (int y = 0; y < range_y; ++y) {
        int dst_base_x = (dst_y + y) * node_size_x_ + dst_x;
        for (int x = 0; x < range_x; ++x) {
          int dst_idx = dst_base_x + x;
          lidar_map_node_->intensities[dst_idx] =
              (*intensity_matrix)[src_y + y][src_x + x];
          lidar_map_node_->intensities_var[dst_idx] =
              (*intensity_var_matrix)[src_y + y][src_x + x];
          lidar_map_node_->altitudes[dst_idx] =
              (*altitude_matrix)[src_y + y][src_x + x];
          lidar_map_node_->count[dst_idx] =
              (*count_matrix)[src_y + y][src_x + x];
        }
      }
    }
  }
}

GetLocalizationDistribution()

void apollo::localization::msf::LocalizationLidar::GetLocalizationDistribution

(

Eigen::MatrixXd *

distribution

)

在文件 localization_lidar.cc 第 206 行定义.

                                 {
  CHECK_NOTNULL(distribution);
 
  int width = 0;
  int height = 0;
  const double* data = nullptr;
  lidar_locator_->GetSSDDistribution(&data, &width, &height);
 
  if (width > 0 && height > 0 && data != nullptr) {
    *distribution = Eigen::MatrixXd::Zero(height, width);
    for (int row = 0; row < height; ++row) {
      for (int col = 0; col < width; ++col) {
        (*distribution)(row, col) = data[row * width + col];
      }
    }
  }
}

GetResult()

void apollo::localization::msf::LocalizationLidar::GetResult	(	Eigen::Affine3d *	location,
		Eigen::Matrix3d *	covariance,
		double *	location_score
	)

在文件 localization_lidar.cc 第 169 行定义.

                                                          {
  if (!location || !covariance) {
    return;
  }
  double x = 0.0;
  double y = 0.0;
  double z = 0.0;
  double qx = 0.0;
  double qy = 0.0;
  double qz = 0.0;
  double qw = 1.0;
  lidar_locator_->GetLocationPose(&x, &y, &z, &qx, &qy, &qz, &qw);
 
  Eigen::Quaterniond quatd(qw, qx, qy, qz);
  Eigen::Translation3d transd(Eigen::Vector3d(x, y, z));
  *location = transd * quatd;
 
  RefineAltitudeFromMap(location);
 
  int width = 0;
  int height = 0;
  const double* data = nullptr;
  lidar_locator_->GetLocationCovariance(&data, &width, &height);
 
  *location_score = lidar_locator_->GetLocationScore();
 
  if (width >= 3 && height >= 3 && data != nullptr) {
    for (int row = 0; row < 3; ++row) {
      for (int col = 0; col < 3; ++col) {
        (*covariance)(row, col) = data[row * 3 + col];
      }
    }
  }
}

Init()

bool apollo::localization::msf::LocalizationLidar::Init	(	const std::string &	map_path,
		const unsigned int	search_range_x,
		const unsigned int	search_range_y,
		const int	zone_id,
		const unsigned int	resolution_id = 0
	)

在文件 localization_lidar.cc 第 53 行定义.

                                                               {
  // init map
  resolution_id_ = resolution_id;
  zone_id_ = zone_id;
  if (!map_.SetMapFolderPath(map_path)) {
    LOG(FATAL) << "Reflectance map folder is invalid!";
    return false;
  }
  map_node_pool_.Initial(&(map_.GetMapConfig()));
  map_.InitMapNodeCaches(12, 24);
  map_.AttachMapNodePool(&map_node_pool_);
 
  // init locator
  node_size_x_ = map_.GetMapConfig().map_node_size_x_;
  node_size_y_ = map_.GetMapConfig().map_node_size_y_;
  resolution_ = map_.GetMapConfig().map_resolutions_[resolution_id];
 
  lidar_map_node_ = new MapNodeData(node_size_x_, node_size_y_);
 
  search_range_x_ = search_range_x;
  search_range_y_ = search_range_y;
  lidar_locator_->Init(search_range_x_, search_range_y_,
                       static_cast<float>(resolution_), node_size_x_,
                       node_size_y_);
  return true;
}

RefineAltitudeFromMap()

void apollo::localization::msf::LocalizationLidar::RefineAltitudeFromMap ( Eigen::Affine3d *  pose )

protected

在文件 localization_lidar.cc 第 225 行定义.

                                                                 {
  CHECK_NOTNULL(pose);
 
  Eigen::Affine3d lidar_pose = *pose * velodyne_extrinsic_;
  Eigen::Vector3d lidar_trans = lidar_pose.translation();
 
  // Read the altitude from the map
  MapNodeIndex index = MapNodeIndex::GetMapNodeIndex(
      map_.GetMapConfig(), lidar_trans, resolution_id_, zone_id_);
  PyramidMapNode* node =
      static_cast<PyramidMapNode*>(map_.GetMapNodeSafe(index));
  PyramidMapMatrix& matrix =
      static_cast<PyramidMapMatrix&>(node->GetMapCellMatrix());
 
  const double height_diff = vehicle_lidar_height_;
 
  if (!is_pre_ground_height_valid_) {
    is_pre_ground_height_valid_ = true;
    pre_vehicle_ground_height_ = lidar_pose.translation()(2) - height_diff;
  }
 
  float vehicle_ground_alt = 0.0f;
  unsigned int x = 0;
  unsigned int y = 0;
  if (node->GetCoordinate(lidar_trans, &x, &y)) {
    unsigned int count = *matrix.GetCountSafe(y, x);
    if (count > 0) {
      if (matrix.HasGroundAltitude()) {
        const float* ground_alt = matrix.GetGroundAltitudeSafe(y, x);
        if (ground_alt) {
          vehicle_ground_alt = *ground_alt;
        }
      } else {
        const float* altitude = matrix.GetAltitudeSafe(y, x);
        if (altitude) {
          vehicle_ground_alt = *altitude;
        }
      }
    } else {
      vehicle_ground_alt = static_cast<float>(pre_vehicle_ground_height_);
    }
  }
 
  lidar_pose.translation()(2) = vehicle_ground_alt + height_diff;
  Eigen::Affine3d transform_tmp = lidar_pose * velodyne_extrinsic_.inverse();
  pose->translation() = transform_tmp.translation();
}

SetDeltaPitchRollLimit()

void apollo::localization::msf::LocalizationLidar::SetDeltaPitchRollLimit

(

double

limit

)

在文件 localization_lidar.cc 第 114 行定义.

                                                           {
  lidar_locator_->SetDeltaPitchRollLimit(limit);
}

SetDeltaYawLimit()

void apollo::localization::msf::LocalizationLidar::SetDeltaYawLimit

(

double

limit

)

在文件 localization_lidar.cc 第 110 行定义.

                                                     {
  lidar_locator_->SetDeltaYawLimit(limit);
}

SetImageAlignMode()

void apollo::localization::msf::LocalizationLidar::SetImageAlignMode

(

int

mode

)

在文件 localization_lidar.cc 第 102 行定义.

                                                  {
  lidar_locator_->SetImageAlignMode(mode);
}

SetLocalizationMode()

void apollo::localization::msf::LocalizationLidar::SetLocalizationMode

(

int

mode

)

在文件 localization_lidar.cc 第 106 行定义.

                                                    {
  lidar_locator_->SetLocalizationMode(mode);
}

SetValidThreshold()

void apollo::localization::msf::LocalizationLidar::SetValidThreshold

(

float

valid_threashold

)

在文件 localization_lidar.cc 第 98 行定义.

                                                                {
  lidar_locator_->SetValidThreshold(valid_threashold);
}

SetVehicleHeight()

void apollo::localization::msf::LocalizationLidar::SetVehicleHeight

(

double

height

)

在文件 localization_lidar.cc 第 93 行定义.

                                                      {
  vehicle_lidar_height_ = height;
  AINFO << "Set height: " << vehicle_lidar_height_;
}

SetVelodyneExtrinsic()

void apollo::localization::msf::LocalizationLidar::SetVelodyneExtrinsic

(

const Eigen::Affine3d &

pose

)

在文件 localization_lidar.cc 第 84 行定义.

                                                                      {
  velodyne_extrinsic_ = pose;
  Eigen::Vector3d trans = pose.translation();
  Eigen::Quaterniond quat = Eigen::Quaterniond(pose.linear());
 
  lidar_locator_->SetVelodyneExtrinsic(trans.x(), trans.y(), trans.z(),
                                       quat.x(), quat.y(), quat.z(), quat.w());
}

Update()

int apollo::localization::msf::LocalizationLidar::Update	(	const unsigned int	frame_idx,
		const Eigen::Affine3d &	pose,
		const Eigen::Vector3d	velocity,
		const LidarFrame &	lidar_frame,
		bool	use_avx = false
	)

在文件 localization_lidar.cc 第 118 行定义.

                                                                           {
  // check whether loaded map
  if (!is_map_loaded_) {
    map_.LoadMapArea(pose.translation(), resolution_id_, zone_id_,
                     search_range_x_, search_range_y_);
    is_map_loaded_ = true;
    AINFO << "Reflectance locator map first loading is done.";
  }
 
  Eigen::Affine3d imu_pose = pose;
  RefineAltitudeFromMap(&imu_pose);
  // load all needed map
  Eigen::Vector3d pose_trans = imu_pose.translation();
  Eigen::Quaterniond pose_quat(imu_pose.linear());
  pose_quat.normalize();
  map_.LoadMapArea(pose_trans, resolution_id_, zone_id_, 0, 0);
 
  // preload map for next locate
  map_.PreloadMapArea(pose_trans, velocity, resolution_id_, zone_id_);
 
  // generate composed map for compare
  ComposeMapNode(pose_trans);
 
  // pass map node to locator
  int node_width = lidar_map_node_->width;
  int node_height = lidar_map_node_->height;
  int node_level_num = 1;
  lidar_locator_->SetMapNodeData(
      node_width, node_height, node_level_num, &(lidar_map_node_->intensities),
      &(lidar_map_node_->intensities_var), &(lidar_map_node_->altitudes),
      &(lidar_map_node_->count));
  lidar_locator_->SetMapNodeLeftTopCorner(map_left_top_corner_(0),
                                          map_left_top_corner_(1));
 
  // pass lidar points to locator
  int size = static_cast<int>(lidar_frame.pt_xs.size());
  lidar_locator_->SetPointCloudData(
      size, lidar_frame.pt_xs.data(), lidar_frame.pt_ys.data(),
      lidar_frame.pt_zs.data(), lidar_frame.intensities.data());
 
  // compute
  int error = lidar_locator_->Compute(
      pose_trans(0), pose_trans(1), pose_trans(2), pose_quat.x(), pose_quat.y(),
      pose_quat.z(), pose_quat.w(), use_avx);
 
  return error;
}

类成员变量说明

config_

PyramidMapConfig apollo::localization::msf::LocalizationLidar::config_

protected

在文件 localization_lidar.h 第 134 行定义.

is_map_loaded_

bool apollo::localization::msf::LocalizationLidar::is_map_loaded_ = false

protected

在文件 localization_lidar.h 第 140 行定义.

is_pre_ground_height_valid_

bool apollo::localization::msf::LocalizationLidar::is_pre_ground_height_valid_ = false

protected

在文件 localization_lidar.h 第 144 行定义.

lidar_locator_

LidarLocator* apollo::localization::msf::LocalizationLidar::lidar_locator_

protected

在文件 localization_lidar.h 第 126 行定义.

lidar_map_node_

MapNodeData* apollo::localization::msf::LocalizationLidar::lidar_map_node_

protected

在文件 localization_lidar.h 第 132 行定义.

map_

PyramidMap apollo::localization::msf::LocalizationLidar::map_

protected

在文件 localization_lidar.h 第 135 行定义.

map_left_top_corner_

Eigen::Vector2d apollo::localization::msf::LocalizationLidar::map_left_top_corner_

protected

在文件 localization_lidar.h 第 137 行定义.

map_node_pool_

PyramidMapNodePool apollo::localization::msf::LocalizationLidar::map_node_pool_

protected

在文件 localization_lidar.h 第 136 行定义.

node_size_x_

int apollo::localization::msf::LocalizationLidar::node_size_x_ = 0

protected

在文件 localization_lidar.h 第 129 行定义.

node_size_y_

int apollo::localization::msf::LocalizationLidar::node_size_y_ = 0

protected

在文件 localization_lidar.h 第 130 行定义.

pre_vehicle_ground_height_

double apollo::localization::msf::LocalizationLidar::pre_vehicle_ground_height_ = 0.0

protected

在文件 localization_lidar.h 第 143 行定义.

resolution_

double apollo::localization::msf::LocalizationLidar::resolution_ = 0.125

protected

在文件 localization_lidar.h 第 131 行定义.

resolution_id_

unsigned int apollo::localization::msf::LocalizationLidar::resolution_id_ = 0

protected

在文件 localization_lidar.h 第 138 行定义.

search_range_x_

int apollo::localization::msf::LocalizationLidar::search_range_x_ = 0

protected

在文件 localization_lidar.h 第 127 行定义.

search_range_y_

int apollo::localization::msf::LocalizationLidar::search_range_y_ = 0

protected

在文件 localization_lidar.h 第 128 行定义.

vehicle_lidar_height_

double apollo::localization::msf::LocalizationLidar::vehicle_lidar_height_ = 1.7

protected

在文件 localization_lidar.h 第 142 行定义.

velodyne_extrinsic_

Eigen::Affine3d apollo::localization::msf::LocalizationLidar::velodyne_extrinsic_

protected

在文件 localization_lidar.h 第 145 行定义.

zone_id_

int apollo::localization::msf::LocalizationLidar::zone_id_ = 50

protected

在文件 localization_lidar.h 第 139 行定义.

---

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

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