apollo::localization::msf::velodyne 命名空间参考

类
struct	PointXYZIRT
struct	PointXYZIRTd
struct	PointXYZIT
struct	PointXYZITd
struct	VelodyneFrame

函数
void	LoadPcds (const std::string &file_path, const unsigned int frame_index, const Eigen::Affine3d &pose, VelodyneFrame *velodyne_frame, bool is_global)
void	LoadPcds (const std::string &file_path, const unsigned int frame_index, const Eigen::Affine3d &pose, ::apollo::common::EigenVector3dVec *pt3ds, std::vector< unsigned char > *intensities, bool is_global)
void	LoadPcdPoses (const std::string &file_path, ::apollo::common::EigenAffine3dVec *poses, std::vector< double > *timestamps)
	Load the PCD poses with their timestamps.
void	LoadPcdPoses (const std::string &file_path, ::apollo::common::EigenAffine3dVec *poses, std::vector< double > *timestamps, std::vector< unsigned int > *pcd_indices)
	Load the PCD poses with their timestamps and indices.
void	LoadPosesAndStds (const std::string &file_path, ::apollo::common::EigenAffine3dVec *poses, ::apollo::common::EigenVector3dVec *stds, std::vector< double > *timestamps)
	Load poses and stds their timestamps.
bool	LoadExtrinsic (const std::string &file_path, Eigen::Affine3d *extrinsic)
	Load the velodyne extrinsic from a YAML file.

变量
struct apollo::localization::msf::velodyne::PointXYZIRT	EIGEN_ALIGN16

函数说明

LoadExtrinsic()

bool apollo::localization::msf::velodyne::LoadExtrinsic	(	const std::string &	file_path,
		Eigen::Affine3d *	extrinsic
	)

Load the velodyne extrinsic from a YAML file.

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

                                                                         {
  YAML::Node config = YAML::LoadFile(file_path);
  if (config["transform"]) {
    if (config["transform"]["translation"]) {
      extrinsic->translation()(0) =
          config["transform"]["translation"]["x"].as<double>();
      extrinsic->translation()(1) =
          config["transform"]["translation"]["y"].as<double>();
      extrinsic->translation()(2) =
          config["transform"]["translation"]["z"].as<double>();
      if (config["transform"]["rotation"]) {
        double qx = config["transform"]["rotation"]["x"].as<double>();
        double qy = config["transform"]["rotation"]["y"].as<double>();
        double qz = config["transform"]["rotation"]["z"].as<double>();
        double qw = config["transform"]["rotation"]["w"].as<double>();
        extrinsic->linear() =
            Eigen::Quaterniond(qw, qx, qy, qz).toRotationMatrix();
        return true;
      }
    }
  }
  return false;
}

LoadPcdPoses() [1/2]

void apollo::localization::msf::velodyne::LoadPcdPoses	(	const std::string &	file_path,
		::apollo::common::EigenAffine3dVec *	poses,
		std::vector< double > *	timestamps
	)

Load the PCD poses with their timestamps.

在文件 velodyne_utility.cc 第 94 行定义.

                                                 {
  std::vector<unsigned int> pcd_indices;
  LoadPcdPoses(file_path, poses, timestamps, &pcd_indices);
}

LoadPcdPoses() [2/2]

void apollo::localization::msf::velodyne::LoadPcdPoses	(	const std::string &	file_path,
		::apollo::common::EigenAffine3dVec *	poses,
		std::vector< double > *	timestamps,
		std::vector< unsigned int > *	pcd_indices
	)

Load the PCD poses with their timestamps and indices.

在文件 velodyne_utility.cc 第 101 行定义.

                                                        {
  poses->clear();
  timestamps->clear();
  pcd_indices->clear();
 
  FILE* file = fopen(file_path.c_str(), "r");
  if (file) {
    unsigned int index;
    double timestamp;
    double x, y, z;
    double qx, qy, qz, qr;
    static constexpr int kSize = 9;
    while (fscanf(file, "%u %lf %lf %lf %lf %lf %lf %lf %lf\n", &index,
                  &timestamp, &x, &y, &z, &qx, &qy, &qz, &qr) == kSize) {
      Eigen::Translation3d trans(Eigen::Vector3d(x, y, z));
      Eigen::Quaterniond quat(qr, qx, qy, qz);
      poses->push_back(trans * quat);
      timestamps->push_back(timestamp);
      pcd_indices->push_back(index);
    }
    fclose(file);
  } else {
    AERROR << "Can't open file to read: " << file_path;
  }
}

LoadPcds() [1/2]

void apollo::localization::msf::velodyne::LoadPcds	(	const std::string &	file_path,
		const unsigned int	frame_index,
		const Eigen::Affine3d &	pose,
		::apollo::common::EigenVector3dVec *	pt3ds,
		std::vector< unsigned char > *	intensities,
		bool	is_global
	)

在文件 velodyne_utility.cc 第 39 行定义.

                                                                     {
  Eigen::Affine3d pose_inv = pose.inverse();
  pcl::PointCloud<PointXYZIT>::Ptr cloud(new pcl::PointCloud<PointXYZIT>);
  if (pcl::io::loadPCDFile(file_path, *cloud) >= 0) {
    if (cloud->height == 1 || cloud->width == 1) {
      AINFO << "Un-organized-point-cloud";
      for (unsigned int i = 0; i < cloud->size(); ++i) {
        Eigen::Vector3d pt3d;
        pt3d[0] = (*cloud)[i].x;
        pt3d[1] = (*cloud)[i].y;
        pt3d[2] = (*cloud)[i].z;
 
        if (pt3d[0] == pt3d[0] && pt3d[1] == pt3d[1] && pt3d[2] == pt3d[2]) {
          Eigen::Vector3d pt3d_local;
          if (is_global) {
            pt3d_local = pose_inv * pt3d;
          } else {
            pt3d_local = pt3d;
          }
          unsigned char intensity =
              static_cast<unsigned char>((*cloud)[i].intensity);
          pt3ds->push_back(pt3d_local);
          intensities->push_back(intensity);
        }
      }
    } else {
      for (unsigned int h = 0; h < cloud->height; ++h) {
        for (unsigned int w = 0; w < cloud->width; ++w) {
          double x = cloud->at(w, h).x;
          double y = cloud->at(w, h).y;
          double z = cloud->at(w, h).z;
          Eigen::Vector3d pt3d(x, y, z);
          if (pt3d[0] == pt3d[0] && pt3d[1] == pt3d[1] && pt3d[2] == pt3d[2]) {
            Eigen::Vector3d pt3d_local;
            if (is_global) {
              pt3d_local = pose_inv * pt3d;
            } else {
              pt3d_local = pt3d;
            }
            unsigned char intensity =
                static_cast<unsigned char>(cloud->at(w, h).intensity);
            pt3ds->push_back(pt3d_local);
            intensities->push_back(intensity);
          }
        }
      }
    }
  } else {
    AERROR << "Failed to load PCD file: " << file_path;
  }
}

LoadPcds() [2/2]

void apollo::localization::msf::velodyne::LoadPcds	(	const std::string &	file_path,
		const unsigned int	frame_index,
		const Eigen::Affine3d &	pose,
		VelodyneFrame *	velodyne_frame,
		bool	is_global
	)

在文件 velodyne_utility.cc 第 30 行定义.

                              {
  velodyne_frame->frame_index = frame_index;
  velodyne_frame->pose = pose;
  LoadPcds(file_path, frame_index, pose, &velodyne_frame->pt3ds,
           &velodyne_frame->intensities, is_global);
}

LoadPosesAndStds()

void apollo::localization::msf::velodyne::LoadPosesAndStds	(	const std::string &	file_path,
		::apollo::common::EigenAffine3dVec *	poses,
		::apollo::common::EigenVector3dVec *	stds,
		std::vector< double > *	timestamps
	)

Load poses and stds their timestamps.

在文件 velodyne_utility.cc 第 130 行定义.

                                                     {
  poses->clear();
  stds->clear();
  timestamps->clear();
 
  FILE* file = fopen(file_path.c_str(), "r");
  if (file) {
    unsigned int index;
    double timestamp;
    double x, y, z;
    double qx, qy, qz, qr;
    double std_x, std_y, std_z;
    static constexpr int kSize = 12;
    while (fscanf(file, "%u %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
                  &index, &timestamp, &x, &y, &z, &qx, &qy, &qz, &qr, &std_x,
                  &std_y, &std_z) == kSize) {
      Eigen::Translation3d trans(Eigen::Vector3d(x, y, z));
      Eigen::Quaterniond quat(qr, qx, qy, qz);
      poses->push_back(trans * quat);
      timestamps->push_back(timestamp);
 
      Eigen::Vector3d std;
      std << std_x, std_y, std_z;
      stds->push_back(std);
    }
    fclose(file);
  } else {
    AERROR << "Can't open file to read: " << file_path;
  }
}

变量说明

EIGEN_ALIGN16

struct apollo::localization::msf::velodyne::PointXYZITd apollo::localization::msf::velodyne::EIGEN_ALIGN16