Autoware API总结（lidar_utils）
作者：互联网
lidar_utils
类：
1.AngleFilter
描述：过滤器类，以检查点的方位角是否在由开始角和结束角定义的范围内。范围定义为沿逆时针方向从开始角到结束角。
公共类型
using VectorT = autoware∷common∷types∷PointXYZIF;公共成员函数
1.1.
AngleFilter(float32_t start_angle,float32_t end_angle)描述：构造函数。
/// @brief 构造函数
/// @param start_angle最小角度，单位为弧度
/// @param end_angle最大角度，单位为弧度
AngleFilter::AngleFilter(float32_t start_angle,float32_t end_angle)
{
using autoware∷common∷geometry∷make_unit_vector2d;
using autoware∷common∷geometry∷cross_2d;
using autoware∷common∷geometry∷get_normal;
using autoware∷common∷geometry∷plus_2d;
using autoware∷common∷geometry∷times_2d;
using autoware∷common∷geometry∷norm_2d;
using autoware∷common∷geometry∷dot_2d;
const auto start_vec = make_unit_vector2d<VectorT>(start_angle);
const auto end_vec = make_unit_vector2d<VectorT>(end_angle);
// 处理两个角度方向相反的情况(small_angle_dir = 0)
if (std∷fabs(dot_2d(start_vec,end_vec) - (-1.0F)) < FEPS) {
m_range_normal = get_normal(start_vec);
} else {
// 范围法线是在可接受范围的中间的单位向量。(规范化(开始+结束)
m_range_normal = plus_2d(start_vec,end_vec);
m_range_normal = times_2d( m_range_normal,(1.0F / norm_2d(m_range_normal)));
// 如果小角度不是在CCW方向，那么我们就需要大角度，这样法线就倒了。
const auto small_angle_dir = cross_2d(start_vec,end_vec);
if ((small_angle_dir + FEPS) < 0.0F) {
m_range_normal = times_2d(m_range_normal,-1.0F);
}
}
// 阈值是可接受角度范围的一半的余弦值。
// 几何解释:
// 一个点的方位角越接近距离法线，它在距离法线上的投影长度就越大。(与夹角的余弦成正比)。
// 因此投影长度是一个点到角度范围的角距离的指示器。角度范围的最小值和最大值定义了这个度量的下界。
const auto thresh = dot_2d(start_vec,m_range_normal);
m_threshold_negative = (thresh + FEPS) < 0.0F;
// Square是预先计算的，因为检查将发生在Square空间中，以避免sqrt()调用。
m_threshold2 = thresh * thresh;
}1.2.
bool8_t operator()(const T & pt) const
描述：检查一个点的方位角是否在逆时针方向的范围[start,end]。该点被视为一个2D矢量，其在范围法线上的投影与一个阈值进行比较。
/// @brief 检查一个点的方位角是否在逆时针方向的范围[start,end]。该点被视为一个2D矢量，其在范围法线上的投影与一个阈值进行比较。
/// @tparam T Point type
/// @param pt point to check
/// @return 如果点包含在范围内则返回true。
template<typename T>
bool8_t operator()(const T & pt) const
{
using common∷geometry∷dot_2d;
bool8_t ret = false;
// 向量大小的平方
const auto pt_len2 = dot_2d(pt,pt);
const auto proj_on_normal = dot_2d(pt,m_range_normal);
const auto proj_on_normal2 = proj_on_normal * proj_on_normal;
const auto is_proj_negative = (proj_on_normal + FEPS) < 0.0F;
// 由于输入向量的投影是按其自身的长度进行缩放的，
// 因此阈值也是按输入向量的长度进行缩放的，以使比较成为可能。
// 为了避免使用平方根计算长度，表达式保持为平方形式，因此进行下列符号检查以确保平方形式的比较的正确性。
if ((!m_threshold_negative) && (!is_proj_negative)) {
ret = (proj_on_normal2) >= (pt_len2 * (m_threshold2 - FEPS));
} else if (m_threshold_negative && (!is_proj_negative)) {
ret = true;
} else if ((!m_threshold_negative) && is_proj_negative) {
ret = false;
} else {
ret = (proj_on_normal2) <= (pt_len2 * (m_threshold2 + FEPS));
}
return ret;
}静态公共属性
static constexpr float32_t PI = 3.14159265359F;
static constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon() * 1e2F;2.DistanceFilter
描述：筛选类，以检查点是否位于由最小和最大半径定义的范围内。
公共成员函数
2.1.
DistanceFilter(float32_t min_radius,float32_t max_radius)描述：构造函数
/// @brief 构造函数
/// @param min_radius 这个点的半径应该大于min_radius
/// @param max_radius这个点的半径应该小于max_radius
DistanceFilter∷DistanceFilter(float32_t min_radius,float32_t max_radius)
: m_min_r2(min_radius * min_radius),m_max_r2(max_radius * max_radius)
{
if (m_max_r2 < m_min_r2) {
throw std∷domain_error("DistanceFilter: max_radius cannot be less than min_radius");
}
}2.2.
bool8_t operator()(const T & pt) const描述：检查点是否在滤镜允许的范围内。检查是用平方的形式，以避免平方根。
/// @brief 检查点是否在过滤器允许的范围内。检查以正方形形式进行，以避免“sqrt”
/// @tparam T点类型
/// @param pt要过滤的指针
/// @return 如果point在过滤器的范围内，则返回True。
template<typename T>
bool8_t operator()(const T & pt) const
{
using common∷geometry∷dot_3d;
auto pt_radius = dot_3d(pt,pt);
return comp∷abs_gte(pt_radius,m_min_r2,FEPS) && comp∷abs_lte(pt_radius,m_max_r2,FEPS);
}静态公共属性
static constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon();3.IntensityIteratorWrapper
公共成员函数
3.1.
explicit IntensityIteratorWrapper(const PointCloud2 & msg);描述：构造函数。
IntensityIteratorWrapper∷IntensityIteratorWrapper( const PointCloud2 & msg)
: m_intensity_it_uint8(msg,"intensity"),m_intensity_it_float32(msg,"intensity")
{
auto && intensity_field_it =
std∷find_if( std∷cbegin(msg.fields),std∷cend(msg.fields),
[](const sensor_msgs∷msg∷PointField & field) {return field.name == "intensity";});
m_intensity_datatype = (*intensity_field_it).datatype;
switch (m_intensity_datatype) {
case sensor_msgs::msg::PointField::UINT8:
case sensor_msgs::msg::PointField::FLOAT32:
break;
default:
throw std::runtime_error("Intensity type not supported: " + m_intensity_datatype);
}
}3.2.
bool8_t IntensityIteratorWrapper∷eof()描述：判断类型是否正确。
bool8_t IntensityIteratorWrapper∷eof()
{
switch (m_intensity_datatype) {
// 由于某种原因，相等操作符(==)不能用于PointCloud2ConstIterator
case sensor_msgs∷msg∷PointField∷UINT8:
return !(m_intensity_it_uint8 != m_intensity_it_uint8.end());
case sensor_msgs∷msg∷PointField∷FLOAT32:
return !(m_intensity_it_float32 != m_intensity_it_float32.end());
default:
throw std∷runtime_error("Intensity type not supported: " + m_intensity_datatype);
}
}3.3.
void IntensityIteratorWrapper∷next()描述：下一个数据
void IntensityIteratorWrapper∷next()
{
switch (m_intensity_datatype) {
case sensor_msgs∷msg∷PointField∷UINT8:
++m_intensity_it_uint8;
break;
case sensor_msgs∷msg∷PointField∷FLOAT32:
++m_intensity_it_float32;
break;
default:
throw std∷runtime_error("Intensity type not supported: " + m_intensity_datatype);
}
}3.4.
void get_current_value(PointFieldValueT & point_field_value)描述：获得当前值。
template<typename PointFieldValueT>
void get_current_value(PointFieldValueT & point_field_value)
{
switch (m_intensity_datatype) {
case sensor_msgs∷msg∷PointField∷UINT8:
point_field_value = *m_intensity_it_uint8;
break;
case sensor_msgs∷msg∷PointField∷FLOAT32:
point_field_value = *m_intensity_it_float32;
break;
default:
throw std∷runtime_error("Intensity type not supported: " + m_intensity_datatype);
}
}4.PointCloudIts
描述：点云迭代器包装器，允许重用迭代器。由于实现了PointCloud2IteratorBase<⋯>∷set_field方法，迭代器的重新初始化代价非常高。
构造函数
PointCloudIts();描述：构造函数
/// @brief 创建新的迭代器包装器，为4个迭代器保留空间，即x，y，z和强度
PointCloudIts∷PointCloudIts() {m_its.reserve(4);}公共成员函数
4.1.
void reset(sensor_msgs∷msg∷PointCloud2 & cloud,uint32_t idx = 0);描述：重置。
/// @brief 将给定云的迭代器重置为给定索引
/// @param[in] cloud将迭代器重置为的点云
/// @param[in] idx迭代器向前移动的索引/偏移量
void PointCloudIts::reset(sensor_msgs::msg::PointCloud2 & cloud,uint32_t idx)
{
// 销毁旧的迭代器
m_its.clear();
// 创建新的迭代器
m_its.emplace_back(cloud,"x");
m_its.emplace_back(cloud,"y");
m_its.emplace_back(cloud,"z");
m_its.emplace_back(cloud,"intensity");
// 到给定索引的高级迭代器
x_it() += idx;
y_it() += idx;
z_it() += idx;
intensity_it() += idx;
}4.2.
sensor_msgs∷PointCloud2Iterator<float32_t> & x_it()描述：返回x字段的迭代器。
/// \brief 返回x字段的迭代器
inline sensor_msgs∷PointCloud2Iterator<float32_t> & x_it()
{
return m_its[0];
}4.3.
sensor_msgs∷PointCloud2Iterator<float32_t> & y_it()描述：返回y字段的迭代器。
/// @brief 返回y字段的迭代器
inline sensor_msgs∷PointCloud2Iterator<float32_t> & y_it()
{
return m_its[1];
}sensor_msgs∷PointCloud2Iterator<float32_t> & z_it()
/// \brief 返回z字段的迭代器
inline sensor_msgs∷PointCloud2Iterator<float32_t> & z_it()
{
return m_its[2];
}
sensor_msgs::PointCloud2Iterator<float32_t> & intensity_it()
描述：
/// \brief 返回“intensity”字段的迭代器
inline sensor_msgs∷PointCloud2Iterator<float32_t> & intensity_it()
{
return m_its[3];
}
StaticTransformer
描述：变换以对给定的点应用常数变换。
构造函数
explicit StaticTransformer(const geometry_msgs::msg::Transform & tf);
描述：构造函数。预计算旋转和平移矩阵从转换msg。
/// \brief 的构造函数。预计算旋转和平移矩阵从转换msg。
/// \param tf转换msg以应用于点。
StaticTransformer∷StaticTransformer(const geometry_msgs∷msg∷Transform & tf)
{
Eigen∷Quaternionf rotation(tf.rotation.w,tf.rotation.x,tf.rotation.y,tf.rotation.z);
if (!comp∷abs_eq(rotation.norm(),1.0f,EPSf)) {
throw std∷domain_error("StaticTransformer: quaternion is not normalized");
}
m_tf.setIdentity();
m_tf.rotate(rotation);
m_tf.translate(Eigen::Vector3f(tf.translation.x,tf.translation.y,tf.translation.z));
}
公共成员函数
void transform(const T & ref,T & out) const
描述：将转换应用到定义了适当点适配器的点。
/// \brief 将转换应用到定义了适当点适配器的点。
/// \tparam T Point type
/// \param ref 输入点
/// \param out 输出点参考
template<typename T>
void transform(const T & ref,T & out) const // NOLINT(假阳性:这不是std∷transform)
{
using common∷geometry∷point_adapter∷x_;
using common∷geometry∷point_adapter∷y_;
using common∷geometry∷point_adapter∷z_;
using common∷geometry∷point_adapter∷xr_;
using common∷geometry∷point_adapter∷yr_;
using common∷geometry∷point_adapter∷zr_;
Eigen∷Matrix<float32_t,3U,1U> ref_mat({x_(ref),y_(ref),z_(ref)});
Eigen∷Vector3f out_mat = m_tf * ref_mat;
xr_(out) = out_mat(0U,0U);
yr_(out) = out_mat(1U,0U);
zr_(out) = out_mat(2U,0U);
}
结构体
_create_custom_pcl_datatype
template<typename T>
struct _create_custom_pcl_datatype;
template<>
struct _create_custom_pcl_datatype<int8_t>
{
static constexpr auto DATATYPE = sensor_msgs∷msg∷PointField∷INT8;
};
template<>
struct _create_custom_pcl_datatype<uint8_t>
{
static constexpr auto DATATYPE = sensor_msgs∷msg∷PointField∷UINT8;
};
template<>
struct _create_custom_pcl_datatype<int16_t>
{
static constexpr auto DATATYPE = sensor_msgs∷msg∷PointField∷INT16;
};
template<>
struct _create_custom_pcl_datatype<uint16_t>
{
static constexpr auto DATATYPE = sensor_msgs∷msg∷PointField∷UINT16;
};
template<>
struct _create_custom_pcl_datatype<int32_t>
{
static constexpr auto DATATYPE = sensor_msgs∷msg∷PointField∷INT32;
};
template<>
struct _create_custom_pcl_datatype<uint32_t>
{
static constexpr auto DATATYPE = sensor_msgs∷msg∷PointField∷UINT32;
};
template<>
struct _create_custom_pcl_datatype<float32_t>
{
static constexpr auto DATATYPE = sensor_msgs∷msg∷PointField∷FLOAT32;
};
template<>
struct _create_custom_pcl_datatype<float64_t>
{
static constexpr auto DATATYPE = sensor_msgs∷msg∷PointField∷FLOAT64;
};
SafeCloudIndices
描述：
struct SafeCloudIndices
{
std∷size_t point_step;
std∷size_t data_length;
};
函数
#include<lidar_utils.hpp>
float32_t fast_atan2(float32_t y,float32_t x)
描述：近似值来自：http://www-labs.iro.umontreal.ca/~mignotte/IFT2425/Documents/EfficientApproximationArctgFunction.pdf
///
/// Approximation was taken from:
/// http://www-labs.iro.umontreal.ca/~mignotte/IFT2425/Documents/EfficientApproximationArctgFunction.pdf
///
/// |Error = fast_atan2(y,x) - atan2f(y,x)| < 0.00468 rad
///
/// Octants:
/// pi/2
/// ` 3 | 2 /
/// ` | /
/// 4 ` | / 1
/// pi -----+----- 0
/// 5 / | ` 8
/// / | `
/// / 6 | 7 `
/// 3pi/2
///
///
float32_t fast_atan2(float32_t y,float32_t x)
{
constexpr float32_t scaling_constant = 0.28086f;
if (x == 0.0f) {
// 特殊情况atan2(0.0,0.0) = 0.0
if (y == 0.0f) {
return 0.0f;
}
// x等于0，所以当(y > 0)时，值是Pi/2；或者当(y< 0)时，值是-Pi/2
return ∷std∷copysign(autoware∷common∷types∷PI_2,y);
}
// 计算y和x的商
float32_t div = y / x;
// 如果|y|小于|x|(|div|<1)，那么我们要么是在1、4、5或8，要么是在2、3、6或7。
if (fabsf(div) < 1.0f) {
// 当在1、4、5或8时
float32_t atan = div / (1.0f + scaling_constant * div * div);
// 如果是4或5，我们需要分别加上pi或-pi
if (x < 0.0f) {
return ∷std∷copysign(autoware∷common∷types∷PI,y) + atan;
}
return atan;
}
// 当在2、3、6或7时
return ∷std∷copysign(autoware∷common∷types∷PI_2,y) - div / (div * div + scaling_constant);
}
LIDAR_UTILS_PUBLIC std∷size_t index_after_last_safe_byte_index(const sensor_msgs∷msg∷PointCloud2 & msg) noexcept
描述：计算点云访问的最小安全长度。
/// \brief 计算点云访问的最小安全长度
/// \param[in] msg要验证的点云消息
/// \ return Byte索引的最后一个点的结束ok访问
std∷size_t index_after_last_safe_byte_index(const sensor_msgs∷msg∷PointCloud2 & msg) noexcept
{
// 从各种真实来源计算预期的数据量
// lint -e9123 NOLINT类型转换到更大的类型绝对没有什么错……
const auto expected_total_data1 = static_cast<std∷size_t>(msg.point_step * (msg.width * msg.height));
// lint -e9123 NOLINT类型转换到更大的类型绝对没有什么错……
const auto expected_total_data2 = static_cast<std::size_t>(msg.row_step * msg.height);
const auto actual_total_data = msg.data.size();
// 取其中最小的
const auto min_data = std∷min(std∷min(expected_total_data1,expected_total_data2),actual_total_data);
// 删除任何不能与point_step正确对齐的数据
const auto last_index = min_data - (min_data % msg.point_step);
return last_index;
}
LIDAR_UTILS_PUBLIC SafeCloudIndices sanitize_point_cloud(const sensor_msgs∷msg∷PointCloud2 & msg);
描述：计算给定点云的安全步幅和最大长度。
/// \brief 计算给定点云的安全步幅和最大长度
/// \return 每个点读取的数据大小ok和最后读取的索引ok，用于
/// 'for (auto idx = 0U;idx<ret.data_length;idx += msg.point_step) {memcpy(&pt,msg.data[idx],ret.point_step);}'
SafeCloudIndices sanitize_point_cloud(const sensor_msgs::msg::PointCloud2 & msg)
{
/// XYZI or XYZ,or throw
auto num_floats = 3U;
if (has_intensity_and_throw_if_no_xyz(msg)) {
num_floats = 4U;
}
return SafeCloudIndices{num_floats * sizeof(float32_t),index_after_last_safe_byte_index(msg)};//返回了SafeCloudIndices结构体
}
LIDAR_UTILS_PUBLIC void init_pcl_msg( sensor_msgs∷msg∷PointCloud2 & msg,const std∷string & frame_id,const std∷size_t size = static_cast<std∷size_t>(MAX_SCAN_POINTS));
描述：初始化点云的头部信息，包括x, y, z和强度。
/// \brief 初始化点云的头部信息，包括x,y,z和强度
/// \param[out] msg一个要初始化的点云消息
/// \param[in] frame_id点云的帧名(假设固定)
/// \param[in] size为底层数据数组预分配的点数
void init_pcl_msg( sensor_msgs∷msg∷PointCloud2 & msg,const std∷string & frame_id,const std::size_t size)
{
init_pcl_msg(
msg,frame_id,size,4U,
"x",1U,sensor_msgs∷msg∷PointField∷FLOAT32,
"y",1U,sensor_msgs∷msg∷PointField∷FLOAT32,
"z",1U,sensor_msgs∷msg∷PointField∷FLOAT32,
"intensity",1U,sensor_msgs∷msg∷PointField∷FLOAT32);
}
LIDAR_UTILS_PUBLIC void init_pcl_msg( sensor_msgs∷msg∷PointCloud2 & msg,const std∷string & frame_id,const std∷size_t size,const uint32_t num_fields,Fields const & ...fields )
描述：初始化点云的报头信息，给定frame id，大小，帧数和字段的参数包。
/// 初始化点云的报头信息，给定帧id，大小，帧数和字段的参数包。
/// \tparam Fields包含字段类型的模板参数包。
/// \param msg点云消息。
/// \param frame_id点云的帧ID。
/// \param size初始化的点云的大小。
/// \param num_fields字段数。
/// \param fields定义字段的参数集合。每个字段必须按照严格的顺序包含如下参数:
/// ' field_name,count,data_type '。应该为每个字段提供这些参数
template<typename ...Fields>
LIDAR_UTILS_PUBLIC void init_pcl_msg( sensor_msgs∷msg∷PointCloud2 & msg,const std∷string & frame_id,const std∷size_t size,const uint32_t num_fields,Fields const & ...fields )
{
msg.height = 1U;
msg.is_bigendian = false;
msg.is_dense = false;
msg.header.frame_id = frame_id;
// 设置字段
sensor_msgs∷PointCloud2Modifier modifier(msg);
modifier.setPointCloud2Fields(num_fields,fields ...);
// 分配内存，以便使用迭代器
modifier.resize(size);
}
LIDAR_UTILS_PUBLIC bool8_t add_point_to_cloud_raw( sensor_msgs∷msg∷PointCloud2 & cloud,const autoware∷common∷types∷PointXYZIF & pt,uint32_t point_cloud_idx);
描述：这个版本优先考虑速度和易于并行化，它假设：—PointXYZIF是一个POD对象，相当于存储在云中的一个点，这意味着相同的字段和相同的字节顺序。
/// \brief 通过memcpy在云中添加一个点而不是使用迭代器
/// 这个版本优先考虑速度和易于并行化
/// 它假设：- PointXYZIF是一个POD对象，相当于存储在云中的一个点，表示相同的字段和相同的字节序。
/// -调用方负责在两个调用之间递增point_cloud_idx。这与add_point_to_cloud不同，add_point_cloud_idx由它自己递增。
bool8_t add_point_to_cloud_raw( sensor_msgs∷msg∷PointCloud2 & cloud,const autoware∷common∷types∷PointXYZIF & pt,uint32_t point_cloud_idx)
{
bool8_t ret = false;
// 由于编译器填充内存对齐边界，实际大小为20。
// 这个检查是为了确保当我们执行16字节的插入时，不会跨越结构的边界。
static_assert(
sizeof(autoware::common∷types∷PointXYZIF) >= ((4U * sizeof(float32_t)) + sizeof(uint16_t)),
"PointXYZF is not expected size: ");
static_assert(
std∷is_trivially_copyable<autoware∷common∷types∷PointXYZIF>∷value,
"PointXYZF is not trivial,add_point_to_cloud instead of add_point_to_cloud_raw");
const uint8_t * casted_point_ptr = reinterpret_cast<const uint8_t *>(&pt);
uint8_t * const cloud_insertion_slot = &cloud.data[cloud.point_step * point_cloud_idx];
std::size_t copy_size = std∷min(static_cast<std::size_t>(cloud.point_step),sizeof(pt));
// 确保我们没有内存溢出
uint8_t * const one_past_last_modified_address = &(cloud_insertion_slot[copy_size]);
uint8_t * const vector_one_past_last_address = &(*cloud.data.end());
if (one_past_last_modified_address <= vector_one_past_last_address) {
// 添加点数据
std::copy(casted_point_ptr,casted_point_ptr + copy_size,cloud_insertion_slot);
ret = true;
}
return ret;
}
bool8_t add_point_to_cloud
LIDAR_UTILS_PUBLIC bool8_t add_point_to_cloud( PointCloudIts & cloud_its,const autoware∷common∷types∷PointXYZIF & pt,uint32_t & point_cloud_idx);
bool8_t add_point_to_cloud( PointCloudIts & cloud_its,const autoware∷common∷types∷PointXYZIF & pt,uint32_t & point_cloud_idx)
{
bool8_t ret = false;
auto & x_it = cloud_its.x_it();
auto & y_it = cloud_its.y_it();
auto & z_it = cloud_its.z_it();
auto & intensity_it = cloud_its.intensity_it();
// 由于编译器填充内存对齐边界，实际大小为20。
// 这个检查是为了确保当我们执行16字节的插入时，不会跨越结构的边界。
static_assert(
sizeof(autoware∷common∷types∷PointXYZIF) >= ((4U * sizeof(float32_t)) + sizeof(uint16_t)),
"PointXYZF is not expected size: ");
if (x_it != x_it.end() && y_it != y_it.end() && z_it != z_it.end() && intensity_it != intensity_it.end())
{
// 添加点数据
*x_it = pt.x;
*y_it = pt.y;
*z_it = pt.z;
*intensity_it = pt.intensity;
// 增加索引以跟踪点云的大小
x_it += 1;
y_it += 1;
z_it += 1;
intensity_it += 1;
++point_cloud_idx;
ret = true;
}
return ret;
}
LIDAR_UTILS_PUBLIC bool8_t add_point_to_cloud( sensor_msgs∷msg∷PointCloud2 & cloud,const autoware∷common∷types∷PointXYZIF & pt,uint32_t & point_cloud_idx);
bool8_t add_point_to_cloud( sensor_msgs∷msg∷PointCloud2 & cloud,const autoware∷common∷types∷PointXYZIF & pt,uint32_t & point_cloud_idx)
{
bool8_t ret = false;
sensor_msgs∷PointCloud2Iterator<float32_t> x_it(cloud,"x");
sensor_msgs∷PointCloud2Iterator<float32_t> y_it(cloud,"y");
sensor_msgs∷PointCloud2Iterator<float32_t> z_it(cloud,"z");
sensor_msgs∷PointCloud2Iterator<float32_t> intensity_it(cloud,"intensity");
x_it += point_cloud_idx;
y_it += point_cloud_idx;
z_it += point_cloud_idx;
intensity_it += point_cloud_idx;
// 由于编译器填充内存对齐边界，实际大小为20。
// 这个检查是为了确保当我们执行16字节的插入时，不会跨越结构的边界。
static_assert(
sizeof(autoware∷common∷types∷PointXYZIF) >= ((4U * sizeof(float32_t)) + sizeof(uint16_t)),
"PointXYZF is not expected size: ");
if (x_it != x_it.end() && y_it != y_it.end() && z_it != z_it.end() && intensity_it != intensity_it.end())
{
// 添加点数据
*x_it = pt.x;
*y_it = pt.y;
*z_it = pt.z;
*intensity_it = pt.intensity;
// 增加索引以跟踪点云的大小
++point_cloud_idx;
ret = true;
}
return ret;
}
LIDAR_UTILS_PUBLIC bool8_t add_point_to_cloud( sensor_msgs∷msg∷PointCloud2 & cloud,const autoware∷common∷types∷PointXYZF & pt,uint32_t & point_cloud_idx);
bool8_t add_point_to_cloud( sensor_msgs∷msg∷PointCloud2 & cloud,const autoware∷common∷types∷PointXYZF & pt,uint32_t & point_cloud_idx)
{
bool8_t ret = false;
sensor_msgs∷PointCloud2Iterator<float32_t> x_it(cloud,"x");
sensor_msgs∷PointCloud2Iterator<float32_t> y_it(cloud,"y");
sensor_msgs∷PointCloud2Iterator<float32_t> z_it(cloud,"z");
x_it += point_cloud_idx;
y_it += point_cloud_idx;
z_it += point_cloud_idx;
static_assert(
sizeof(autoware∷common∷types∷PointXYZIF) >= ((3U * sizeof(float32_t)) + sizeof(uint16_t)),
"PointXYZF is not expected size: ");
if (x_it != x_it.end() && y_it != y_it.end() && z_it != z_it.end())
{
// 添加点数据
*x_it = pt.x;
*y_it = pt.y;
*z_it = pt.z;
// 增加索引以跟踪点云的大小
++point_cloud_idx;
ret = true;
}
return ret;
}
void reset_pcl_msg( sensor_msgs∷msg∷PointCloud2 & msg,const std∷size_t size,uint32_t & point_cloud_idx )
void reset_pcl_msg( sensor_msgs∷msg∷PointCloud2 & msg,const std∷size_t size,uint32_t & point_cloud_idx )
{
sensor_msgs∷PointCloud2Modifier pc_modifier(msg);
pc_modifier.clear();
point_cloud_idx = 0;
pc_modifier.resize(size);
}
void reset_pcl_msg( sensor_msgs∷msg∷PointCloud2 & msg,const std∷size_t new_size )
void reset_pcl_msg( sensor_msgs∷msg∷PointCloud2 & msg,const std∷size_t new_size )
{
sensor_msgs∷PointCloud2Modifier pc_modifier(msg);
pc_modifier.resize(new_size);
}
get_cluster
LIDAR_UTILS_PUBLIC std∷pair<autoware_auto_msgs∷msg∷PointClusters∷_points_type∷iterator,autoware_auto_msgs∷msg∷PointClusters∷_points_type∷iterator get_cluster(autoware_auto_msgs∷msg∷PointClusters& clusters,const std::size_t cls_id);
描述：根据集群id从集群中获取集群。
/// \brief 根据集群id从集群中获取集群
/// \param[in] clusters集群对象
/// \param[in] cls_id目标集群的id
/// \return 指针对，它们是集群点中的目标集群的开始和结束
std∷pair<autoware_auto_msgs∷msg∷PointClusters∷_points_type∷iterator,autoware_auto_msgs∷msg∷PointClusters∷_points_type∷iterator get_cluster(autoware_auto_msgs∷msg∷PointClusters& clusters,const std∷size_t cls_id)
{
if (cls_id >= clusters.cluster_boundary.size()) {
return { clusters.points.end(),clusters.points.end() };
}
uint32_t cls_begin_offset;
uint32_t cls_end_offset; // 这个偏移量是目标集群的结束后元素
if (cls_id == 0U) {
cls_begin_offset = 0U;
cls_end_offset = clusters.cluster_boundary[cls_id];
}
else {
cls_begin_offset = clusters.cluster_boundary[cls_id - 1U];
cls_end_offset = clusters.cluster_boundary[cls_id];
}
return { clusters.points.begin() + cls_begin_offset,clusters.points.begin() + cls_end_offset };
}
LIDAR_UTILS_PUBLIC std∷pair<autoware_auto_msgs∷msg∷PointClusters∷_points_type∷iterator,autoware_auto_msgs∷msg∷PointClusters∷_points_type∷iterator> get_cluster(autoware_auto_msgs∷msg∷PointClusters& clusters,const std∷size_t cls_id);
描述：根据集群id从集群中获取集群。
std∷pair<autoware_auto_msgs∷msg∷PointClusters∷_points_type∷const_iterator,autoware_auto_msgs∷msg∷PointClusters∷_points_type∷const_iterator> get_cluster(const autoware_auto_msgs∷msg∷PointClusters& clusters,const std∷size_t cls_id)
{
if (cls_id >= clusters.cluster_boundary.size()) {
return { clusters.points.end(),clusters.points.end() };
}
uint32_t cls_begin_offset;
uint32_t cls_end_offset; // 这个偏移量是目标集群的结束后元素
if (cls_id == 0U) {
cls_begin_offset = 0U;
cls_end_offset = clusters.cluster_boundary[cls_id];
}
else {
cls_begin_offset = clusters.cluster_boundary[cls_id - 1U];
cls_end_offset = clusters.cluster_boundary[cls_id];
}
return { clusters.points.begin() + cls_begin_offset,clusters.points.begin() + cls_end_offset };
}
has_intensity_and_throw_if_no_xyz
描述：检查pointcloud msg的前三个字段是否为float32_t类型的x, y, z，否则抛出异常；检查pointcloud msg是否有强度字段作为第四个字段，否则返回false
bool8_t has_intensity_and_throw_if_no_xyz(const PointCloud2∷SharedPtr & cloud)
bool8_t has_intensity_and_throw_if_no_xyz(const PointCloud2∷SharedPtr & cloud)
{
return has_intensity_and_throw_if_no_xyz(*cloud);
}
bool8_t has_intensity_and_throw_if_no_xyz(const PointCloud2 & cloud)
bool8_t has_intensity_and_throw_if_no_xyz(const PointCloud2 & cloud)
{
bool8_t ret = true;
// 验证“点”的步骤
if (cloud.fields.size() < 3U) {
throw std∷runtime_error("Invalid PointCloud msg");
}
const auto check_field = [](
const sensor_msgs∷msg∷PointField & field,
const char8_t * const name,
const uint32_t offset,
const decltype(sensor_msgs∷msg∷PointField∷datatype) datatype) -> bool8_t {
bool8_t res = true;
if ((name != field.name) || (offset != field.offset) || (datatype != field.datatype) || (1U != field.count))
{
res = false;
}
return res;
};
if (!check_field(cloud.fields[0U],"x",0U,sensor_msgs∷msg∷PointField∷FLOAT32)) {
throw std∷runtime_error("PointCloud doesn't have correct x field");
} else if (!check_field(cloud.fields[1U],"y",4U,sensor_msgs∷msg∷PointField∷FLOAT32)) {
throw std∷runtime_error("PointCloud doesn't have correct y field");
} else if (!check_field(cloud.fields[2U],"z",8U,sensor_msgs∷msg∷PointField∷FLOAT32)) {
throw std∷runtime_error("PointCloud doesn't have correct z field");
} else {
// do nothing
}
if (cloud.fields.size() >= 4U) {
if (!check_field(cloud.fields[3U],"intensity",12U,sensor_msgs∷msg∷PointField∷FLOAT32)) {
if (!check_field(cloud.fields[3U],"intensity",16U,sensor_msgs∷msg∷PointField∷UINT8)) {
ret = false;
}
}
} else {
ret = false;
}
return ret;
}
LIDAR_UTILS_PUBLIC sensor_msgs∷msg∷PointCloud2∷SharedPtr create_custom_pcl(const std∷vector<std∷string> & field_names,const uint32_t cloud_size)
template<typename T>
LIDAR_UTILS_PUBLIC sensor_msgs∷msg∷PointCloud2∷SharedPtr create_custom_pcl(const std∷vector<std∷string> & field_names,const uint32_t cloud_size)
{
using sensor_msgs∷msg∷PointCloud2;
PointCloud2∷SharedPtr msg = std∷make_shared<PointCloud2>();
const auto field_size = field_names.size();
msg->height = 1U;
msg->width = cloud_size;
msg->fields.resize(field_size);
for (uint32_t i = 0U; i < field_size; i++) {
msg->fields[i].name = field_names[i];
}
msg->point_step = 0U;
for (uint32_t idx = 0U; idx < field_size; ++idx) {
msg->fields[idx].offset = static_cast<uint32_t>(idx * sizeof(T));
msg->fields[idx].datatype = _create_custom_pcl_datatype<T>∷DATATYPE;
msg->fields[idx].count = 1U;
msg->point_step += static_cast<uint32_t>(sizeof(T));
}
const std∷size_t capacity = msg->point_step * cloud_size;
msg->data.clear();
msg->data.reserve(capacity);
for (std∷size_t i = 0; i < capacity; ++i) {
msg->data.emplace_back(0U); // 初始化所有值为0
}
msg->row_step = msg->point_step * msg->width;
msg->is_bigendian = false;
msg->is_dense = false;
msg->header.frame_id = "base_link";
return msg;
}
变量
static const uint32_t MAX_SCAN_POINTS = 57872U;
标签：const,msgs,point,utils,Autoware,API,msg,sensor,cloud 来源： https://blog.csdn.net/Siweimutong/article/details/120677865