小幻Coding - 记录成长

机器人二维地图的各种表示及转换

Fri, 12 Jun 2026 16:26:00 +0800

机器人二维地图表示方式：

占据栅格地图 (nav_msgs::OccupancyGrid)
多层栅格地图 (grid_map)
OpenCV 图像表示

占据栅格地图 (nav_msgs::OccupancyGrid)

栅格地图即将一块区域分割成 m*n 个正方形格子，栅格地图的分辨率就表示每个格子的边长表示物理上的距离，比如分辨率为 1 米的栅格地图表示 100 米 * 100 米区域需要 100 * 100 个格子，如下所示：

nav_msgs::OccupancyGrid 是 ROS 中表示二维占据栅格的数据结构，它包括以下内容：

# 元数据
std_msgs/Header header

# 地图信息
MapMetaData info

# 地图数据
int8[] data

元数据为标准的 ROS 消息头

地图信息包括以下内容：

字段	类型	说明
map_load_time	time	地图加载时间
resolution	float32	每个栅格的边长（米/格），如 0.05 表示每格 5cm
width	uint32	地图宽度（栅格数）
height	uint32	地图高度（栅格数）
origin	geometry_msgs/Pose	地图坐标系原点在世界坐标系中的位姿（左下角栅格的位姿）

地图数据是一维数组，长度为 width × height，按行优先（row-major）排列。每个元素取值：

值	含义
0	空闲（Free）
100	占据（Occupied）
-1（即 255 的补码）	未知（Unknown）

data 的索引 0 对应栅格地图左下角，x 沿宽度方向递增（向右）， y 沿高度方向递增（向上）

 y ↑
 |
 | • P(3, 4)
 | /
 | /
------+------→ x
 |

坐标转数据： value(x, y) = data[y * width + x]

世界坐标和栅格坐标的转换：

1
2

grid_x = (world_x - origin.position.x) / resolution
grid_y = (world_y - origin.position.y) / resolution

多层栅格地图 (grid_map)

grid_map 用于表示栅格地图和操作栅格地图，支持同一位置存储多层数据。

grid_map 底层使用 Eigen::MatrixXf 来存储每一层的数据

grid_map 的坐标表示：

(0, 0) 位于地图左下角：
行号 → y 方向（从地图底部到顶部）
列号 → x 方向（从地图左侧到右侧

栅格访问方式

矩阵索引访问栅格坐标 (x, y) -> (row, col)

grid_map::Index index(5, 10); //(row = y, col = x)
float& val = mat.at("layer", index);

// 或者直接拿到 Eigne 的 matrix
Eigen::MatrixXf& mat = map["elevation"];
float& val = mat(5, 10);

已知世界坐标 (2.5m, 1.0m) ，使用世界坐标访问

grid_map::Position worldPos(2.5, 1.0);

// 自动将世界坐标按照分辨率转为索引
grid_map::Index index;
map.getIndex(worldPos, index);
float val = map.at("elevation", index);

// 或者一步到位
float val = map.atPosition("elevation", worldPos);

获取地图信息

获取栅格的宽高

1
2
3

grid_map::Size size = map.getSize();
int cols = size.x(); // (矩阵列数, x 方向栅格数)
int rows = size.y(); // (矩阵行数, y 方向栅格数)

获取物理世界的宽高

1
2
3

grid_map::Length length = map.getLength(); // 单位为 m
double lenX = length.x(); // (物理宽)
double lenY = length.y(); // (物理高)

地图原点，grid_map 的地图原点不一定是在左下角，即坐标可能为负数
1

grid_map::Position pos = map.getPosition();

OpenCV 图像表示

可以使用 OpenCV 表示栅格地图，一般一个栅格对应一个像素

坐标变换

OpenCV 中的坐标 Y 轴向下，以左上角为坐标原点

------+------→ x
 y |
 |
 | • P(3, height - 1 - 4)
 |
 |
------+------→ x'

及 grid_map 中 (x, y) 对应于 OpenCV 中就是 (x, height - 1 - y)

像素访问

(x, y) 对应 (row, col)

cv::Mat image;

int x = 10;
int y = 5;

image.at<uchar>(5, 10);

ros2-control介绍及使用

Mon, 08 Jun 2026 09:46:00 +0800

介绍

ros2_control 是一个使用 ROS2 控制机器人的框架，它将控制逻辑和硬件接口解耦，可以非常方便的替换控制逻辑或者硬件接口。

比如对于两轮差速小车，控制器可以使用两轮差速控制器，小车轮子的硬件接口可以随时切换为仿真小车或真实小车。

下面是官方的架构图：

Controller: 实现的各个控制器
Controller Manager: 负责控制器生命周期管理，包括向资源管理器（Resource Manager）申请硬件接口
Resource Manager: 硬件接口管理
State Interface: 只能够读数据的状态类接口
Command Interface: 可读可写的命令类接口
Sensor: 传感器类型的接口实现，只会导出状态类接口
System: 多自由度硬件，如工业机器人
Actuator: 单自由度硬件，如阀门等

自定义硬件及控制器

实现一个 Echo 硬件，给它设置一个数值，他会将数值输出

硬件实现

自定义硬件需要继承对应的接口类并实现对应方法

#pragma once

#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/hardware_component_interface.hpp>

namespace robotics_codehub {

class EchoSystemHardware : public hardware_interface::SystemInterface {
public:

 // 生命周期回调
 CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override;

 // 读取数据
 hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;

 // 写数据
 hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;

private:
 double echo_value_;
};
}

在 on_init 方法里解析 urdf 中声明的 hardware 参数

// 解析 urdf 中的 hardware 参数
EchoSystemHardware::CallbackReturn EchoSystemHardware::on_init(
 const HardwareComponentInterfaceParams& params) {
 if (HardwareComponentInterface::on_init(params) != CallbackReturn::SUCCESS) {
 return CallbackReturn::FAILURE;
 }

 // 从 hardware 标签中的 <param> 读取配置
 auto it = params.hardware_info.hardware_parameters.find("echo_value");
 if (it != params.hardware_info.hardware_parameters.end()) {
 echo_value_ = std::stod(it->second);
 } else {
 echo_value_ = 999;
 }

 RCLCPP_INFO(get_logger(), "Init EchoSystemHardware, echo value: %f", echo_value_);

 return CallbackReturn::SUCCESS;
}

read 方法读取硬件数据，这个函数会被周期性调用，这里直接读 echo_value 的值，实际硬件可能是读取串口数据

hardware_interface::return_type EchoSystemHardware::read(const rclcpp::Time& time, const rclcpp::Duration& period) {
 (void)time;
 (void)period;

 for (const auto& [name, desc] : joint_state_interfaces_) {
 RCLCPP_INFO(get_logger(), "Read EchoSystemHardware, read value: %f", echo_value_);
 set_state(name, echo_value_);
 }

 return hardware_interface::return_type::OK;
}

write 方法将数据写入硬件，同样会被周期性调用，这里将值保存在 echo_value_ ，实际可能是写串口

hardware_interface::return_type EchoSystemHardware::write(const rclcpp::Time& time, const rclcpp::Duration& period)
{
 for (const auto& [name, desc] : joint_command_interfaces_) {
 echo_value_ = get_command(name);
 RCLCPP_INFO(get_logger(), "Write EchoSystemHardware, write value: %f", echo_value_);
 }
 return hardware_interface::return_type::OK;
}

由于硬件实现是以插件的方式动态加载，还需要在最后注册以下

#include <pluginlib/class_list_macros.hpp>

// 子类， 基类
PLUGINLIB_EXPORT_CLASS(robotics_codehub::EchoSystemHardware, hardware_interface::SystemInterface)

在 cmake 中将插件编译成动态库

find_package(ament_cmake REQUIRED)
find_package(robotics_cmake REQUIRED)
find_package(pluginlib REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
add_library(echo_system_hardware SHARED
hardware/echo_system_hardware.cpp
)
ament_target_dependencies(echo_system_hardware
hardware_interface
pluginlib
rclcpp
rclcpp_lifecycle
)

写一个 echo_system_hardware_plugin.xml 描述文件，内容如下：

<?xml version="1.0"?>
<!-- path: so 文件名称 -->
<library path="echo_system_hardware">
<!-- name: 插件名称 -->
<!-- type: 插件类的类型，robotics_codehub 是命名空间 -->
<!-- base_class_type: 插件基类类型 -->
<class
name="ros2_control_echo/EchoSystemHardware"
type="robotics_codehub::EchoSystemHardware"
base_class_type="hardware_interface::SystemInterface">
<description>My Echo System Hardware Interface</description>
</class>
</library>

在项目根目录下的 CMakeLists.txt 中安装 xml 文件：

pluginlib_export_plugin_description_file(
hardware_interface
hardware/echo_system_hardware_plugin.xml
)

实现控制器

定义控制器支持的参数的 yaml 文件，

echo_system_controller_parameters:
echo_system_hardware_interface: {
type: string,
default_value: "",
description: "Name of echo system hardware interface",
read_only: true,
}

这里只定义一个 string 类型参数，为 EchoSystemHardware 提供的接口名称

在 CMakeLists 中生成 C++ 代码

find_package(generate_parameter_library REQUIRED)
generate_parameter_library(
echo_system_controller_params # 生成的库名
controller/echo_system_controller_params.yaml # 你的参数定义文件
)

定义控制器类

#pragma once

#include <memory>

#include <std_msgs/msg/float64.hpp>

#include <controller_interface/controller_interface.hpp>
#include <realtime_tools/realtime_thread_safe_box.hpp>
#include <realtime_tools/realtime_publisher.hpp>

#include "ros2_control_echo/echo_system_controller_params.hpp"

namespace robotics_codehub {

class EchoSystemController : public controller_interface::ControllerInterface {
public:
 EchoSystemController();

 // 命令类接口配置
 controller_interface::InterfaceConfiguration command_interface_configuration() const override;

 // 状态类接口配置
 controller_interface::InterfaceConfiguration state_interface_configuration() const override;

 // 节点生命周期
 CallbackReturn on_init() override;

 CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;

 CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;

 CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;

 // 控制管理器周期性调用 update
 controller_interface::return_type update(const rclcpp::Time& time,
 const rclcpp::Duration& period) override;

private:
 std::shared_ptr<echo_system_controller_parameters::ParamListener> param_listener_;
 echo_system_controller_parameters::Params params_;

 rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr sub_;
 rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_;

 realtime_tools::RealtimePublisher<std_msgs::msg::Float64>::SharedPtr realtime_pub_;

 realtime_tools::RealtimeThreadSafeBox<double> recv_;
};

} // namespace robotics_codehub

控制器实现之导出命令及接口

controller_interface::InterfaceConfiguration EchoSystemController::command_interface_configuration()
 const {
 RCLCPP_INFO(get_node()->get_logger(), "command interface configuration");

 controller_interface::InterfaceConfiguration conf;
 conf.type = controller_interface::interface_configuration_type::INDIVIDUAL;
 conf.names.push_back(params_.echo_system_hardware_interface);

 return conf;
}

controller_interface::InterfaceConfiguration EchoSystemController::state_interface_configuration()
 const {
 RCLCPP_INFO(get_node()->get_logger(), "state interface configuration");

 controller_interface::InterfaceConfiguration conf;
 conf.type = controller_interface::interface_configuration_type::INDIVIDUAL;
 conf.names.push_back(params_.echo_system_hardware_interface);
 return conf;
}

即控制器要控制的命令接口，控制器启动时会由 Controller Manager 绑定对应接口

实现生命周期函数，在 on_init 时获取参数

controller_interface::CallbackReturn EchoSystemController::on_init() {
 param_listener_ =
 std::make_shared<echo_system_controller_parameters::ParamListener>(get_node());
 params_ = param_listener_->get_params();

 RCLCPP_INFO(get_node()->get_logger(), "on init: %s", params_.echo_system_hardware_interface.data());

 return CallbackReturn::SUCCESS;
}

在 on_configure 时创建 ROS 话题

controller_interface::CallbackReturn EchoSystemController::on_configure(
 const rclcpp_lifecycle::State& previous_state) {
 (void)previous_state;
 RCLCPP_INFO(get_node()->get_logger(), "on configure echo sensor controller");

 sub_ = get_node()->create_subscription<std_msgs::msg::Float64>(
 "~/set_echo_value", rclcpp::SystemDefaultsQoS(),
 [this](const std::shared_ptr<std_msgs::msg::Float64> msg) -> void {
 recv_.set(msg->data);
 });

 pub_ = get_node()->create_publisher<std_msgs::msg::Float64>("~/echo_value",
 rclcpp::SystemDefaultsQoS());
 realtime_pub_ =
 std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Float64>>(pub_);

 return CallbackReturn::SUCCESS;
}

set_echo_value 会设置 echo 值，设置后保存在 recv_ 变量里，recv 是 realtime_tools::RealtimeThreadSafeBox<double> 类型，可以在 ros 回调线程和实时线程直接安全的转递值

实现 update 函数， update 函数会周期性在实时线程调用

controller_interface::return_type EchoSystemController::update(const rclcpp::Time& time,
 const rclcpp::Duration& period) {
 (void)time;
 (void)period;

 auto ref = recv_.try_get();
 if (ref.has_value()) {
 double val = ref.value();
 for (auto& cmd : command_interfaces_) {
 RCLCPP_INFO(get_node()->get_logger(), "update value: %f", val);
 (void)cmd.set_value(val);
 }
 }

 for (auto& state : state_interfaces_) {
 auto maybe = state.get_optional(0);
 if (maybe.has_value()) {
 std_msgs::msg::Float64 msg;
 msg.data = maybe.value();
 RCLCPP_INFO(get_node()->get_logger(), "update state name: %s, value: %f", state.get_name().data(), msg.data);
 realtime_pub_->try_publish(msg);
 }
 }

 return controller_interface::return_type::OK;
}

update 分为两步：

从 recv_ 中获取值，然后设置值
从 state_interfaces_ 中获取值并通过 realtime_pub 安全的发布

最后导出插件

1
2

PLUGINLIB_EXPORT_CLASS(robotics_codehub::EchoSystemController,
 controller_interface::ControllerInterface)

编译成动态库

find_package(controller_interface REQUIRED)
add_library(echo_system_controller SHARED
controller/echo_system_controller.cpp
)
ament_target_dependencies(echo_system_controller
controller_interface
pluginlib
rclcpp
rclcpp_lifecycle
echo_system_controller_params # 自动生成的参数库
)
install(
TARGETS echo_system_controller
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin
)

声明插件

<?xml version="1.0"?>
<library path="echo_system_controller">
<class
name="ros2_control_echo/EchoSystemController"
type="robotics_codehub::EchoSystemController"
base_class_type="controller_interface::ControllerInterface">
<description>My Echo System Hardware Controller</description>
</class>
</library>

CMakeLists.txt 中安装

pluginlib_export_plugin_description_file(
controller_interface
controller/echo_system_controller_plugin.xml
)

ros2_control 框架的使用

在 urdf 里面声明硬件接口

<?xml version="1.0"?>
<robot name="my_sensor_robot">
<!-- ros2_control 硬件接口声明 -->
<ros2_control name="EchoSystemHardware" type="system">
<hardware>
<plugin>ros2_control_echo/EchoSystemHardware</plugin>
<param name="echo_value">11.111</param>
</hardware>
<joint name="echo_system_hardware">
<command_interface name="echo_value"/>
<state_interface name="echo_value"/>
</joint>
</ros2_control>
<link name="base_link"/>
<link name="system_link"/>
<joint name="echo_system_hardware" type="continuous">
<parent link="base_link"/>
<child link="system_link"/>
</joint>
</robot>

发布 urdf

`1`	`ros2 run robot_state_publisher robot_state_publisher --ros-args -p robot_description:="$(cat install/ros2_control_echo/share/ros2_control_echo/urdf/echo_system_hardware.urdf)"`

写 controller 的配置文件，包括控制器配置

controller_manager:
ros__parameters:
update_rate: 1
echo_system_controller:
ros__parameters:
type: "ros2_control_echo/EchoSystemController"
echo_system_hardware_interface: "echo_system_hardware/echo_value"

启动 Controller Manager

`1`	`ros2 run controller_manager ros2_control_node --ros-args --params-file install/ros2_control_echo/share/ros2_control_echo/config/echo_sensor_ros_control.yaml`

启动成功后会加载 Echo Hardware ，并且周期性调用 read 和 write ，下面是启动后的日志

[INFO] [1780908053.701387441] [controller_manager]: Using Steady (Monotonic) clock for triggering controller manager cycles.
[INFO] [1780908053.704657475] [controller_manager]: Subscribing to '/robot_description' topic for robot description.
[INFO] [1780908053.707790704] [controller_manager]: update rate is 1 Hz
[INFO] [1780908053.707835626] [controller_manager]: Overruns handling is : enabled
[INFO] [1780908053.707840170] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50
[WARN] [1780908053.707904519] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling.
[INFO] [1780908053.891317702] [controller_manager]: Received robot description from topic.
[INFO] [1780908053.891438034] [controller_manager]: Enforcing command limits is disabled. Command limits from URDF will be ignored.
[INFO] [1780908053.893929516] [controller_manager]: Loading hardware 'EchoSystemHardware'
[INFO] [1780908053.894856764] [controller_manager]: Loaded hardware 'EchoSystemHardware' from plugin 'ros2_control_echo/EchoSystemHardware'
[INFO] [1780908053.894948711] [controller_manager]: Initialize hardware 'EchoSystemHardware'
[INFO] [1780908053.916472060] [controller_manager.hardware_component.system.EchoSystemHardware]: Init EchoSystemHardware, echo value: 11.111000
[INFO] [1780908053.916593742] [controller_manager]: Successful initialization of hardware 'EchoSystemHardware'
[INFO] [1780908053.916794337] [controller_manager]: Activating component 'EchoSystemHardware'.
[INFO] [1780908053.916835732] [resource_manager]: 'configure' hardware 'EchoSystemHardware'
[INFO] [1780908053.916854440] [resource_manager]: Successful 'configure' of hardware 'EchoSystemHardware'
[INFO] [1780908053.916871576] [resource_manager]: 'activate' hardware 'EchoSystemHardware'
[INFO] [1780908053.916896015] [resource_manager]: Successful 'activate' of hardware 'EchoSystemHardware'
[INFO] [1780908053.917060798] [controller_manager]: Registering statistics for : EchoSystemHardware
[INFO] [1780908053.917296139] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services...
[INFO] [1780908055.708362763] [controller_manager.hardware_component.system.EchoSystemHardware]: Read EchoSystemHardware, read value: 11.111000
[INFO] [1780908055.708689766] [controller_manager.hardware_component.system.EchoSystemHardware]: Write EchoSystemHardware, write value: nan
[INFO] [1780908056.708382930] [controller_manager.hardware_component.system.EchoSystemHardware]: Read EchoSystemHardware, read value: nan
[INFO] [1780908056.708499832] [controller_manager.hardware_component.system.EchoSystemHardware]: Write EchoSystemHardware, write value: nan
[INFO] [1780908057.708349017] [controller_manager.hardware_component.system.EchoSystemHardware]: Read EchoSystemHardware, read value: nan
[INFO] [1780908057.708548208] [controller_manager.hardware_component.system.EchoSystemHardware]: Write EchoSystemHardware, write value: nan
[INFO] [1780908058.708411996] [controller_manager.hardware_component.system.EchoSystemHardware]: Read EchoSystemHardware, read value: nan
[INFO] [1780908058.708649124] [controller_manager.hardware_component.system.EchoSystemHardware]: Write EchoSystemHardware, write value: na

启动控制器

`1`	`ros2 run controller_manager spawner echo_system_controller --param-file install/ros2_control_echo/share/ros2_control_echo/config/echo_sensor_ros_control.yaml`

提示加载成功后，控制器使用的话题就能看到了

通过接口设置 echo 的值

`1`	`ros2 topic pub /echo_system_controller/set_echo_value std_msgs/msg/Float64 "{data: 1343242.34}"`

通过接口查看 echo 的值

`1`	`ros2 topic echo /echo_system_controller/echo_value`

完整代码路径： https://github.com/xhcoding/robotics-codehub/ros2/ros2_control_echo

PCL 点云手记

Tue, 21 Apr 2026 17:29:00 +0800

点云即三维空间中点的集合，PCL 库专门处理点云数据，包括点云进行滤波，分割，匹配，查找等操作

数据结构

PCL 使用 pcl::PointCloud<T> 来存储点云，其中 T 是点的类型，下面是一些 PCL 定义的常用点类型：

pcl::PointXYZ : 仅包括三维空间坐标信息
pcl::PointXYZI : 包括三维坐标和强度值，常用于激光雷达
pcl::PointXYZL : 包括三维坐标和标签，常用于点云分割或分类
pcl::PointXY : 二维点结构，用于处理投影或二维平面数据
pcl::PointXYZRGB : 包含三维坐标和 RGB 颜色信息
pcl::PointXYZRGBA : 包含三维坐标和 RGBA 颜色信息

还有更多的点类型，用于不同的任务会，后续会逐渐接触

pcl::PointCloud<T> 中包括以下主要的数据成员：

width : 点云数据的宽度，对于无组织的点，宽度就是点的个数，对于有组织的点，宽度就是一行点个数
height : 点云数据的高度，对于无组织的数据，高度为 1，对于有组织的点，高度就是行数，总的点数量为 width * height
points : 点的数据
is_dense : 点是否包含 Inf/NaN 值

pcl::PointCloud<T> 的增删遍历点的成员函数和标准库中的集合类似，也支持迭代器

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

template <typename T>
pcl::PointCloud<T>::Ptr genPointCloud(int n)
{
 srand(time(NULL));
 typename pcl::PointCloud<T>::Ptr cloud(new pcl::PointCloud<T>());
 cloud->resize(n);

 for (int i = 0; i < n; i++) {
 auto& p = (*cloud)[i];
 p.x = 1024 * rand () / (RAND_MAX + 1.0f);
 p.y = 1024 * rand () / (RAND_MAX + 1.0f);
 p.z = 1024 * rand () / (RAND_MAX + 1.0f);
 }
 return cloud;
}

#include <iostream>
#include <format>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

int main(int argc, char* argv[]) {
 auto cloud = genPointCloud<pcl::PointXYZ>(5);
 std::cout << std::format("width: {}, height: {}, is_dense: {}, is organized: {}\n", cloud->width, cloud->height, cloud->is_dense, cloud->isOrganized());

 for (const auto& point : *cloud) {
 std::cout << std::format("({}, {}, {})\n", point.x, point.y, point.z);
 }

 return 0;
}

width: 5, height: 1, is_dense: true, is organized: false
(-0.042350292, 0.7831731, 0.9874444)
(-0.11391306, 0.35958624, 0.083025455)
(-0.57813406, -0.5630765, -0.2032013)
(-0.30399895, -0.48886824, -0.71495104)
(0.69121265, -0.97114754, -0.49422264)

IO

文件

PCL 保存文件的格式为 .pcd ，下面是读写 PCD 文件的例子


#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

int main([[maybe_unused]] int argc, [[maybe_unused]] char* argv[]) {
 pcl::PointCloud<pcl::PointXYZ> cloud = *genPointCloud<pcl::PointXYZ>(5);

 pcl::io::savePCDFileASCII("/tmp/test_pcd.pcd", cloud);

 std::cout << "Saved " << cloud.size() << " data points to test_pcd.pcd." << std::endl;

 for (const auto& point : cloud) {
 std::cout << point.x << " " << point.y << " " << point.z << std::endl;
 }

 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
 if (pcl::io::loadPCDFile("/tmp/test_pcd.pcd", *cloud_ptr) == -1) {
 PCL_ERROR("Couldn't read file\n");
 return -1;
 }

 std::cout << "Loaded " << cloud_ptr->width * cloud_ptr->height << " data points" << std::endl;
 for (const auto& point : *cloud_ptr) {
 std::cout << point.x << " " << point.y << " " << point.z << std::endl;
 }
 return 0;
}

Saved 5 data points to test_pcd.pcd.
0.902143 0.32537 -0.246792
-0.427928 -0.0364971 -0.7693
0.0920715 -0.533491 0.701829
-0.292022 0.932315 0.600912
0.245783 0.191243 -0.526157
Loaded 5 data points
0.902143 0.32537 -0.246792
-0.427928 -0.0364971 -0.7693
0.0920715 -0.533491 0.701829
-0.292022 0.932315 0.600912
0.245783 0.191243 -0.526157

pcd 文件内容如下：

VERSION 0.7 # 版本 FIELDS x y z # data 中点的字段格式 SIZE 4 4 4 # 每个字段的大小 TYPE F F F # 每个字段的类型 COUNT 1 1 1 # 每个字段元素数量 WIDTH 5 # 宽度 HEIGHT 1 # 高度 VIEWPOINT 0 0 0 1 0 0 0 # 平移变换和旋转变换 POINTS 5 # 点的数量 DATA ascii # 数据格式，ascii 表示文本格式，每行一个点， binary 为二进制格式，binary_compressed 为压缩的二进制格式 0.902143 0.32537 -0.246792 -0.427928 -0.0364971 -0.7693 0.0920715 -0.533491 0.701829 -0.292022 0.932315 0.600912 0.245783 0.191243 -0.526157

可视化

可以使用 pcl_viewer 工具查看 pcd 文件

或者在代码中调用 pcl_viewer 可视化点云

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

struct Color { uint8_t r, g, b; };

// 传入 n，返回 n 个视觉上区分度大的颜色
std::vector<Color> generateColors(int n)
{
 std::vector<Color> colors;
 colors.reserve(n);

 for (int i = 0; i < n; ++i)
 {
 // 黄金角度 137.508°，每迭代一次旋转一个角度
 // 人眼对色相变化最敏感，所以用 HSV 空间的 H 均匀分布
 double hue = fmod(i * 137.508, 360.0); // 0~360
 double s = 0.85; // 饱和度
 double v = 0.95; // 明度

 // HSV → RGB
 double c = v * s;
 double x = c * (1.0 - fabs(fmod(hue / 60.0, 2.0) - 1.0));
 double m = v - c;

 double r = 0, g = 0, b = 0;
 if (hue < 60) { r = c; g = x; }
 else if (hue < 120) { r = x; g = c; }
 else if (hue < 180) { g = c; b = x; }
 else if (hue < 240) { g = x; b = c; }
 else if (hue < 300) { r = x; b = c; }
 else { r = c; b = x; }

 colors.push_back({
 static_cast<uint8_t>((r + m) * 255),
 static_cast<uint8_t>((g + m) * 255),
 static_cast<uint8_t>((b + m) * 255)
 });
 }
 return colors;
}


void pclViewer(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
 pcl::visualization::PCLVisualizer viewer;
 viewer.setBackgroundColor(0.05, 0.05, 0.05);

 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color(cloud, 0, 255, 0);
 viewer.addPointCloud<pcl::PointXYZ>(cloud, color, "cloud");

 while (!viewer.wasStopped()) {
 viewer.spinOnce(100);
 }

}

void pclViewer(pcl::PointCloud<pcl::PointXYZI>::ConstPtr cloud)
{
 pcl::visualization::PCLVisualizer viewer;
 viewer.setBackgroundColor(0.05, 0.05, 0.05);

 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> color(cloud, 0, 255, 0);
 viewer.addPointCloud<pcl::PointXYZI>(cloud, color, "cloud");

 while (!viewer.wasStopped()) {
 viewer.spinOnce(100);
 }

}

void pclViewer(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
{
 pcl::visualization::PCLVisualizer viewer;
 viewer.setBackgroundColor(0.05, 0.05, 0.05);

 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
 viewer.addPointCloud<pcl::PointXYZRGB>(cloud, rgb, "cloud");

 while (!viewer.wasStopped()) {
 viewer.spinOnce(100);
 }

}


template <typename PointT>
void pclViewCluster(const std::shared_ptr<pcl::PointCloud<PointT>>& cloud, const std::vector<pcl::PointIndices>& clusters)
{
 auto cloud_colored = std::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
 cloud_colored->reserve(cloud->size());
 auto colors = generateColors(clusters.size());
 for (size_t i = 0; i < clusters.size(); i++) {
 auto& cluster = clusters[i];
 auto color = colors[i];
 for (const auto& idx : cluster.indices) {
 auto& p = (*cloud)[idx];
 cloud_colored->emplace_back(p.x, p.y, p.z, color.r, color.g, color.b);
 }
 }
 pclViewer(cloud_colored);
}

查找

点云处理中查找主要包括两方面：

指定一个点，查找它最近的 k 个邻居
指定一个点，查找在指定半径范围内的邻居

KdTree

KdTree 是一个数据结构，能够高效的查找 K 近邻或半径邻居

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <iostream>
#include <format>
#include <vector>
#include <ctime>

int main() {
 auto cloud = genPointCloud(1000);

 pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;

 kdtree.setInputCloud(cloud);

 pcl::PointXYZ searchPoint;
 searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f);
 searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
 searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f);

 // k 近邻搜索
 int K = 5;
 std::vector<int> pointIdxKNNSearch(K);
 std::vector<float> pointKNNSquaredDistance(K);

 std::cout << std::format("K nearest neighbor search at ({}, {}, {}) with K: {}\n",
 searchPoint.x, searchPoint.y, searchPoint.z, K);

 if (kdtree.nearestKSearch(searchPoint, K, pointIdxKNNSearch, pointKNNSquaredDistance) > 0) {
 for (size_t i = 0; i < pointIdxKNNSearch.size(); i++) {
 auto& p = (*cloud)[pointIdxKNNSearch[i]];
 std::cout << std::format("find {}: point: ({}, {}, {}), squared distance: {}", i, p.x,
 p.y, p.z, pointKNNSquaredDistance[i])
 << std::endl;
 }
 }

 // 半径搜索
 std::vector<int> pointIdxRadiusSearch;
 std::vector<float> pointRadiusSquaredDistance;

 float radius = 256.0f * rand() / (RAND_MAX + 1.0f);

 std::cout << std::format("Neighbors within radius search at ({}, {}, {}) with radius: {}",
 searchPoint.x, searchPoint.y, searchPoint.z, radius)
 << std::endl;

 if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) >
 0) {
 for (size_t i = 0; i < pointIdxRadiusSearch.size(); i++) {
 auto& p = (*cloud)[pointIdxRadiusSearch[i]];
 std::cout << std::format("find {}: point: ({}, {}, {}), squared distance: {}", i, p.x,
 p.y, p.z, pointRadiusSquaredDistance[i])
 << std::endl;
 }
 }

 return 0;
}

K nearest neighbor search at (843.17914, 724.95197, 942.59766) with K: 5
find 0: point: (838.75775, 707.2718, 923.97406), squared distance: 678.9756
find 1: point: (860.5885, 722.6938, 968.37213), squared distance: 972.5088
find 2: point: (867.26166, 656.1342, 939.431), squared distance: 5325.878
find 3: point: (891.3544, 763.66846, 989.54846), squared distance: 6024.198
find 4: point: (896.51337, 697.6261, 893.1407), squared distance: 6037.2354
Neighbors within radius search at (843.17914, 724.95197, 942.59766) with radius: 98.56139
find 0: point: (838.75775, 707.2718, 923.97406), squared distance: 678.9756
find 1: point: (860.5885, 722.6938, 968.37213), squared distance: 972.5088
find 2: point: (867.26166, 656.1342, 939.431), squared distance: 5325.878
find 3: point: (891.3544, 763.66846, 989.54846), squared distance: 6024.198
find 4: point: (896.51337, 697.6261, 893.1407), squared distance: 6037.2354
find 5: point: (908.95496, 783.8818, 909.6123), squared distance: 8887.214

特征

特征是通过周围一圈领域点算出来，描述局部表面形状，集合结构等

积分图法向量估计

积分图即预处理后的累加和图，任意矩形区域的和都可以用 4 次查表算出

#include <iostream>
#include <format>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <filesystem>

using PointT = pcl::PointXYZ;

int main([[maybe_unused]] int argc, [[maybe_unused]] char* argv[]) {
 std::filesystem::path home(std::getenv("HOME"));
 auto pcd_file =
 std::filesystem::path(home / "Documents/Dataset/table_scene_mug_stereo_textured.pcd");

 pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
 pcl::PCDReader reader;
 reader.read(pcd_file.c_str(), *cloud);
 std::cout << std::format("PointCloud has {} data points.\n", cloud->size());

 // 估计法向量
 pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);

 pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
 ne.setNormalEstimationMethod(ne.AVERAGE_3D_GRADIENT);
 ne.setMaxDepthChangeFactor(0.02f);
 ne.setNormalSmoothingSize(10.0f);
 ne.setInputCloud(cloud);
 ne.compute(*normals);

 pcl::visualization::PCLVisualizer viewer(" PCL Viewer");
 viewer.setBackgroundColor(0.0, 0.0, 0.5);
 viewer.addPointCloudNormals<PointT, pcl::Normal>(cloud, normals);

 while (!viewer.wasStopped()) {
 viewer.spinOnce();
 }

 return 0;
}

`1`	`PointCloud has 307200 data points.`

滤波

PCL 中的滤波指保留点云中满足指定条件的点

直通滤波(PassThrough filter)

直通滤波的条件是指定维度值的范围，比如保留 z 轴在 [0, 1] 区间内的点

#include <format>
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>

int main() {
 auto cloud = genPointCloud(5);
 auto filtered_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

 cloud->emplace_back(0.0, 0.0, 0.0);
 cloud->emplace_back(0.0, 0.0, 1.0);

 std::cout << "Cloud before filtering: " << std::endl;
 for (const auto& point : *cloud) {
 std::cout << std::format("({}, {}, {})\n", point.x, point.y, point.z);
 }

 pcl::PassThrough<pcl::PointXYZ> pass; // 创建直通滤波
 pass.setInputCloud(cloud); // 设置输入点云
 pass.setFilterFieldName("z"); // 设置需要过滤的维度
 pass.setFilterLimits(0.0, 1.0); // 设置范围
 // pass.setNegative(true); // 反向过滤
 pass.filter(*filtered_cloud); // 开始过滤并设置到输出点云

 std::cout << "Cloud after filtering: " << std::endl;
 for (const auto& point : *filtered_cloud) {
 std::cout << std::format("({}, {}, {})\n", point.x, point.y, point.z);
 }
 return 0;
}

Cloud before filtering:
(0.35222197, -0.15188313, -0.106395245)
(-0.3974061, -0.4731059, 0.29260206)
(-0.7318983, 0.6671047, 0.44130373)
(-0.7347655, 0.8545809, -0.036173344)
(-0.46070004, -0.2774682, -0.9167619)
(0, 0, 0)
(0, 0, 1)
Cloud after filtering:
(-0.3974061, -0.4731059, 0.29260206)
(-0.7318983, 0.6671047, 0.44130373)
(0, 0, 0)
(0, 0, 1)

体素滤波(VoxelGrid filter)

体素就是三维空间中的一个立方体，体素滤波就是用一个个立方体去装填整个点云，然后每个立方体里只保留一个点，从而精简点云数量。

对于每个包含点的立方体，PCL 会计算其中所有点的重心，并用这个重心来代表这个立方体里原本所有的点，这种下采样算法能够保留点云几何形状和结构。

#include <iostream>
#include <filesystem>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

int main() {
 auto cloud = std::make_shared<pcl::PCLPointCloud2>();
 auto filtered_cloud = std::make_shared<pcl::PCLPointCloud2>();

 std::filesystem::path home(std::getenv("HOME"));
 // Fill in the cloud data
 pcl::PCDReader reader;
 reader.read(std::filesystem::path( home / "Documents/Dataset/table_scene_lms400.pcd").c_str(),
 *cloud);

 std::cout << "PointCloud before filtering: " << cloud->width * cloud->height << " data points ("
 << pcl::getFieldsList(*cloud) << ")." << std::endl;

 // Create the filtering object
 pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
 sor.setInputCloud(cloud); // 设置输入点云
 sor.setLeafSize(0.01f, 0.01f, 0.01f); // 设置立方体大小 0.1 * 0.1 * 0.1
 sor.filter(*filtered_cloud);

 std::cout << "PointCloud after filtering: " << filtered_cloud->width * filtered_cloud->height
 << " data points (" << pcl::getFieldsList(*filtered_cloud) << ")." << std::endl;

 pcl::PCDWriter writer;
 writer.write(std::filesystem::path(home / "Documents/Dataset/table_scene_lms400_downsampled.pcd"),
 *filtered_cloud, Eigen::Vector4f::Zero(), Eigen::Quaternionf::Identity(), false);

 return (0);
}

1
2

PointCloud before filtering: 460400 data points (x y z intensity distance sid).
PointCloud after filtering: 41049 data points (x y z intensity distance sid).

统计离群点移除滤波

激光扫描生成的点云数据集通常具有变化的点密度。此外，测量误差会引入稀疏的离群点，进一步破坏数据质量。

统计离群点滤波算法：对于每个点，计算它到所有领域点的平均距离，如果平均距离落在全局距离均值和标准差所定义的区间外，则视为离群点

#include <iostream>
#include <filesystem>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/statistical_outlier_removal.h>

int main() {
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

 std::filesystem::path home(std::getenv("HOME"));
 std::filesystem::path dataset = home / "Documents/Dataset";
 // Fill in the cloud data
 pcl::PCDReader reader;
 reader.read(std::filesystem::path(dataset / "table_scene_lms400.pcd").c_str(),*cloud);

 std::cout << "PointCloud before filtering: " << cloud->width * cloud->height << " data points ("
 << pcl::getFieldsList(*cloud) << ")." << std::endl;

 pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
 sor.setInputCloud(cloud);
 sor.setMeanK(50); // 领域中点的数量
 sor.setStddevMulThresh(1.0); // 标准差的阈值，大于 1.0 的点被认为是离群点
 sor.filter(*cloud_filtered);

 pcl::PCDWriter writer;
 writer.write<pcl::PointXYZ>(std::filesystem::path(dataset / "table_scene_lms400_inliers.pcd").c_str(), *cloud_filtered, false);

 sor.setNegative(true);
 sor.filter(*cloud_filtered);
 writer.write<pcl::PointXYZ>(std::filesystem::path(dataset / "table_scene_lms400_outliers.pcd").c_str(), *cloud_filtered,false);

 return 0;
}

`1`	`PointCloud before filtering: 460400 data points (x y z).`

条件滤波

条件滤波即按照给定条件过滤点云中的点，只是给定的这个条件是用 PCL 中的结构

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/conditional_removal.h>

int main() {
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

 // Fill in the cloud data
 cloud->width = 5;
 cloud->height = 1;
 cloud->resize(cloud->width * cloud->height);

 for (auto& point : *cloud) {
 point.x = 1024 * rand() / (RAND_MAX + 1.0f);
 point.y = 1024 * rand() / (RAND_MAX + 1.0f);
 point.z = 1024 * rand() / (RAND_MAX + 1.0f);
 }

 // 构建 and 条件
 pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZ>());

 // 添加一个比较条件，z > 0
 range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(
 new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GT, 0.0)));
 // 添加一个比较条件，z < 0.8
 range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(
 new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, 0.8)));

 // 构建滤波
 pcl::ConditionalRemoval<pcl::PointXYZ> cond_rem;
 cond_rem.setCondition(range_cond);
 cond_rem.setInputCloud(cloud);
 cond_rem.setKeepOrganized(true); // 保持点云的数据格式，即将不符合条件的点设置为 NAN ，点云总数不变
 cond_rem.filter(*cloud_filtered);

 std::cout << "Cloud before filtering: " << std::endl;
 for (const auto& point : *cloud)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;
 // display pointcloud after filtering
 std::cout << "Cloud after filtering: " << std::endl;
 for (const auto& point : *cloud_filtered)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;

 return 0;
}

Cloud before filtering:
 0.352222 -0.151883 -0.106395
 -0.397406 -0.473106 0.292602
 -0.731898 0.667105 0.441304
 -0.734766 0.854581 -0.0361733
 -0.4607 -0.277468 -0.916762
Cloud after filtering:
 nan nan nan
 -0.397406 -0.473106 0.292602
 -0.731898 0.667105 0.441304
 nan nan nan
 nan nan nan

半径离群点移除滤波

指定半径和邻居数量，对于一个点，如果指定半径内其它点数量小于指定的邻居数量，则视为离群点

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/radius_outlier_removal.h>

int main() {
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

 // Fill in the cloud data
 cloud->width = 5;
 cloud->height = 1;
 cloud->resize(cloud->width * cloud->height);

 for (auto& point : *cloud) {
 point.x = 1024 * rand() / (RAND_MAX + 1.0f);
 point.y = 1024 * rand() / (RAND_MAX + 1.0f);
 point.z = 1024 * rand() / (RAND_MAX + 1.0f);
 }

 pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
 outrem.setInputCloud(cloud);
 outrem.setRadiusSearch(1.0); // 设置半径
 outrem.setMinNeighborsInRadius(1); // 设置邻居数量
 outrem.setKeepOrganized(true);
 outrem.filter(*cloud_filtered);

 std::cout << "Cloud before filtering: " << std::endl;
 for (const auto& point : *cloud)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;
 // display pointcloud after filtering
 std::cout << "Cloud after filtering: " << std::endl;
 for (const auto& point : *cloud_filtered)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;

 return 0;
}

Cloud before filtering:
 0.352222 -0.151883 -0.106395
 -0.397406 -0.473106 0.292602
 -0.731898 0.667105 0.441304
 -0.734766 0.854581 -0.0361733
 -0.4607 -0.277468 -0.916762
Cloud after filtering:
 0.352222 -0.151883 -0.106395
 -0.397406 -0.473106 0.292602
 -0.731898 0.667105 0.441304
 -0.734766 0.854581 -0.0361733
 nan nan nan

投影滤波

将点投影到指定的参数模型上，比如将点投影到平面上，对应的参数模型为： aX + bY + cZ +d = 0

支持的投影模型及系数：

宏定义	模型	系数个数	系数含义简述
SACMODEL_PLANE	平面	4	[a,b,c,d] 平面方程 ax+by+cz+d=0
SACMODEL_LINE	3D 直线	6	基准点 + 方向向量
SACMODEL_CIRCLE2D	2D 平面圆	3	圆心(cx,cy) + 半径 r
SACMODEL_CIRCLE3D	3D 空间圆	7	圆心+平面法向+半径
SACMODEL_SPHERE	球体	4	球心(cx,cy,cz) + 半径 r
SACMODEL_CYLINDER	圆柱体	7	轴线基准点+轴线方向+底面半径
SACMODEL_CONE	圆锥体	7	顶点+轴线方向+锥角

#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>

int main() {
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);

 // Fill in the cloud data
 cloud->width = 5;
 cloud->height = 1;
 cloud->points.resize(cloud->width * cloud->height);

 for (auto& point : *cloud) {
 point.x = 1024 * rand() / (RAND_MAX + 1.0f);
 point.y = 1024 * rand() / (RAND_MAX + 1.0f);
 point.z = 1024 * rand() / (RAND_MAX + 1.0f);
 }

 std::cout << "Cloud before projection: " << std::endl;
 for (const auto& point : *cloud)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;

 // 设置投影系数
 pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
 coefficients->values.resize(4);
 coefficients->values[0] = coefficients->values[1] = 0; // x = y = 0;
 coefficients->values[2] = 1.0; // z = 1.0;
 coefficients->values[3] = 0;

 pcl::ProjectInliers<pcl::PointXYZ> proj;
 proj.setModelType(pcl::SACMODEL_PLANE);
 proj.setInputCloud(cloud);
 proj.setModelCoefficients(coefficients);
 proj.filter(*cloud_projected);

 std::cout << "Cloud after projection: " << std::endl;
 for (const auto& point : *cloud_projected)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;
 return 0;
}

Cloud before projection:
 0.352222 -0.151883 -0.106395
 -0.397406 -0.473106 0.292602
 -0.731898 0.667105 0.441304
 -0.734766 0.854581 -0.0361733
 -0.4607 -0.277468 -0.916762
Cloud after projection:
 0.352222 -0.151883 0
 -0.397406 -0.473106 0
 -0.731898 0.667105 0
 -0.734766 0.854581 0
 -0.4607 -0.277468 0

索引提取滤波

即指定点的索引，将点从点云中提取出来

#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>

int main() {
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

 // Fill in the cloud data
 cloud->width = 5;
 cloud->height = 1;
 cloud->points.resize(cloud->width * cloud->height);

 for (auto& point : *cloud) {
 point.x = 1024 * rand() / (RAND_MAX + 1.0f);
 point.y = 1024 * rand() / (RAND_MAX + 1.0f);
 point.z = 1024 * rand() / (RAND_MAX + 1.0f);
 }

 std::cout << "Cloud before projection: " << std::endl;
 for (const auto& point : *cloud)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;

 // 指定点的索引，这里指定 1， 3， 实际上一般通过模板匹配或其它方式获得
 pcl::PointIndices::Ptr indices(new pcl::PointIndices());
 indices->indices.push_back(1);
 indices->indices.push_back(3);

 pcl::ExtractIndices<pcl::PointXYZ> extract;
 extract.setInputCloud(cloud);
 extract.setIndices(indices);
 extract.setNegative(false); // false 表示保留索引点，true 表示去掉索引点
 extract.setKeepOrganized(true);
 extract.filter(*cloud_filtered);

 std::cout << "Cloud after projection: " << std::endl;
 for (const auto& point : *cloud_filtered)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;
 return 0;
}

Cloud before projection:
 0.352222 -0.151883 -0.106395
 -0.397406 -0.473106 0.292602
 -0.731898 0.667105 0.441304
 -0.734766 0.854581 -0.0361733
 -0.4607 -0.277468 -0.916762
Cloud after projection:
 nan nan nan
 -0.397406 -0.473106 0.292602
 nan nan nan
 -0.734766 0.854581 -0.0361733
 nan nan nan

分割

点云分割即找出点云中的指定模型

平面模型分割

平面模型分割即在点云中找出一个平面

常用的是 SAC_RANSAC 算法：

随机猜 3 个点算平面
数有多少点贴合这个平面
重复 N 次，留下贴合点最多的那个平面

实际工程中可以用来剔除地面，剔除墙面，分离背景和障碍物等

#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

int main() {
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

 // Fill in the cloud data
 cloud->width = 15;
 cloud->height = 1;
 cloud->points.resize(cloud->width * cloud->height);

 for (auto& point : *cloud) {
 point.x = 1024 * rand() / (RAND_MAX + 1.0f);
 point.y = 1024 * rand() / (RAND_MAX + 1.0f);
 point.z = 1024 * rand() / (RAND_MAX + 1.0f);
 }

 // 设置离群点
 (*cloud)[0].z = 2.0;
 (*cloud)[3].z = -2.0;
 (*cloud)[6].z = 4.0;

 std::cout << "Cloud before projection: " << std::endl;
 for (const auto& point : *cloud)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;

 pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
 pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

 pcl::SACSegmentation<pcl::PointXYZ> seg;
 seg.setOptimizeCoefficients(true);
 seg.setModelType(pcl::SACMODEL_PLANE);
 seg.setMethodType(pcl::SAC_RANSAC);
 seg.setDistanceThreshold(0.01); // 距离系数，点到平面距离小于 0.01 为内点

 seg.setInputCloud(cloud);
 seg.segment(*inliers, *coefficients);

 if (inliers->indices.size() == 0) {
 std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
 return -1;
 }

 std::cout << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1]
 << " " << coefficients->values[2] << " " << coefficients->values[3] << std::endl;

 std::cout << "Model inliers: " << inliers->indices.size() << std::endl;
 for (const auto& idx : inliers->indices)
 std::cout << idx << " " << cloud->points[idx].x << " " << cloud->points[idx].y << " "
 << cloud->points[idx].z << std::endl;
 return 0;
}

Cloud before projection:
 0.352222 -0.151883 2
 -0.397406 -0.473106 0.292602
 -0.731898 0.667105 0.441304
 -0.734766 0.854581 -2
 -0.4607 -0.277468 -0.916762
 0.183749 0.968809 0.512055
 -0.998983 -0.463871 4
 0.716053 0.525135 -0.523004
 0.439387 0.56706 0.905417
 -0.579787 0.898706 -0.504929
 -0.757228 -0.749072 -0.656812
 -0.863624 0.853522 0.870082
 -0.571022 0.121624 -0.462813
 -0.129718 -0.613142 0.391768
 -0.165891 0.926158 0.1143
Model coefficients: 0.910654 -0.197082 -0.363135 0.375378
Model inliers: 4
0 0.352222 -0.151883 2
1 -0.397406 -0.473106 0.292602
12 -0.571022 0.121624 -0.462813
14 -0.165891 0.926158 0.1143

圆柱模型分割

即在点云中找出一个圆柱体

#include <iostream>
#include <format>
#include <filesystem>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>

using PointT = pcl::PointXYZ;

int main() {
 std::filesystem::path home(std::getenv("HOME"));
 auto pcd_file =
 std::filesystem::path(home / "Documents/Dataset/table_scene_mug_stereo_textured.pcd");

 pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
 pcl::PCDReader reader;
 reader.read(pcd_file.c_str(), *cloud);
 std::cout << std::format("PointCloud has {} data points.\n", cloud->size());

 pclViewer(cloud);

 // 移除 NaN 和场景外的点
 pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
 pcl::PassThrough<PointT> pass;
 pass.setInputCloud(cloud);
 pass.setFilterFieldName("z");
 pass.setFilterLimits(0, 1.5);
 pass.filter(*cloud_filtered);
 std::cout << std::format("PointCloud has {} data points after passthrough filter.\n", cloud->size());

 pclViewer(cloud_filtered);

 // 估计点的法向量
 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
 pcl::NormalEstimation<PointT, pcl::Normal> ne;
 pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
 ne.setSearchMethod(tree);
 ne.setInputCloud(cloud_filtered);
 ne.setKSearch(50);
 ne.compute(*cloud_normals);

 pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
 pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);
 pcl::ExtractIndices<PointT> extract;
 pcl::ExtractIndices<pcl::Normal> extract_normals;

 // 先进行平面分割
 pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
 seg.setOptimizeCoefficients(true);
 seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
 seg.setNormalDistanceWeight(0.1);
 seg.setMethodType(pcl::SAC_RANSAC);
 seg.setMaxIterations(100);
 seg.setDistanceThreshold(0.03);
 seg.setInputCloud(cloud_filtered);
 seg.setInputNormals(cloud_normals);
 seg.segment(*inliers_plane, *coefficients_plane);

 std::cout << "Plane coefficients: " << *coefficients_plane << std::endl;

 pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>);
 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
 extract.setInputCloud(cloud_filtered);
 extract.setIndices(inliers_plane);
 extract.setNegative(true); // 剔除平面
 extract.filter(*cloud_filtered2);
 extract_normals.setNegative(true);
 extract_normals.setInputCloud(cloud_normals);
 extract_normals.setIndices(inliers_plane);
 extract_normals.filter(*cloud_normals2);

 pclViewer(cloud_filtered2);

 // 圆柱分割
 seg.setOptimizeCoefficients(true);
 seg.setModelType(pcl::SACMODEL_CYLINDER);
 seg.setMethodType(pcl::SAC_RANSAC);
 seg.setNormalDistanceWeight(0.1);
 seg.setMaxIterations(10000);
 seg.setDistanceThreshold(0.05);
 seg.setRadiusLimits(0, 0.1);
 seg.setInputCloud(cloud_filtered2);
 seg.setInputNormals(cloud_normals2);
 seg.segment(*inliers_cylinder, *coefficients_cylinder);
 std::cout << "cylinder coefficients: " << *coefficients_cylinder << std::endl;

 // 提取圆柱
 pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
 extract.setInputCloud(cloud_filtered2);
 extract.setIndices(inliers_cylinder);
 extract.setNegative(false);
 extract.filter(*cloud_cylinder);

 pclViewer(cloud_cylinder);
 return 0;
}

PointCloud has 307200 data points.
PointCloud has 307200 data points after passthrough filter.
Plane coefficients: header:
seq: 0 stamp: 0 frame_id:
values[]
 values[0]: 0.016223
 values[1]: -0.83761
 values[2]: -0.546028
 values[3]: 0.528923

cylinder coefficients: header:
seq: 0 stamp: 0 frame_id:
values[]
 values[0]: 0.0441161
 values[1]: 0.461857
 values[2]: 1.02395
 values[3]: 0.0227775
 values[4]: -0.836755
 values[5]: -0.547104
 values[6]: 0.0387651

欧几里得聚类分割

算法步骤：

对点云 P 创建 kdtree
初始化聚类队列为 C ，初始化空队列 Q
对于 P 中的每一个点 $p_i$，执行下面的步骤：
1. 将 $P_i$ 加到队列 Q 中，对 Q 中的每个点 $P_i$ 执行下面步骤：
  1. 查找 $P_i$ 的邻居点，如果邻居点没有经过处理，将邻居点插入队列 Q 中
2. 当 Q 中所有点都处理后，将 Q 中的点作为一类添加到 C 中
当所有点处理完成后，算法结束

#include <format>
#include <iostream>
#include <filesystem>

#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/search/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>

int main()
{
 auto home = std::filesystem::path(getenv("HOME"));
 auto dataset = home / "Documents/Dataset/table_scene_lms400.pcd";
 auto cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
 auto cloud_f = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
 pcl::PCDReader reader;
 reader.read(dataset.c_str(), *cloud);
 std::cout << "PointCloud before filter has: " << cloud->size() << " data points" << std::endl;

 pclViewer(cloud);
 // 下采样
 pcl::VoxelGrid<pcl::PointXYZ> vg;
 auto cloud_filtered = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
 vg.setInputCloud(cloud);
 vg.setLeafSize(0.01f, 0.01f, 0.01f);
 vg.filter(*cloud_filtered);
 std::cout << "PointCloud after filter has: " << cloud_filtered->size() << " data points" << std::endl;

 pclViewer(cloud_filtered);

 // 平面分割
 pcl::SACSegmentation<pcl::PointXYZ> seg;
 auto inliers = std::make_shared<pcl::PointIndices>();
 auto coefficients = std::make_shared<pcl::ModelCoefficients>();
 auto cloud_plane = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
 seg.setOptimizeCoefficients(true);
 seg.setModelType(pcl::SACMODEL_PLANE);
 seg.setMethodType(pcl::SAC_RANSAC);
 seg.setMaxIterations(100);
 seg.setDistanceThreshold(0.02);

 size_t nr_points = cloud_filtered->size();
 while (cloud_filtered->size() > 0.3 * nr_points) {
 seg.setInputCloud(cloud_filtered);
 seg.segment(*inliers, *coefficients);
 if (inliers->indices.size() == 0) {
 std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
 break;
 }

 pcl::ExtractIndices<pcl::PointXYZ> extract;
 extract.setInputCloud(cloud_filtered);
 extract.setIndices(inliers);
 extract.setNegative(false);
 extract.filter(*cloud_plane);

 extract.setNegative(true);
 extract.filter(*cloud_f);
 *cloud_filtered = *cloud_f;
 }

 std::cout << "PointCloud after plane seg has: " << cloud_filtered->size() << " data points" << std::endl;

 pclViewer(cloud_filtered);

 auto tree = std::make_shared<pcl::search::KdTree<pcl::PointXYZ>>();
 tree->setInputCloud(cloud_filtered);

 std::vector<pcl::PointIndices> cluster_indices;
 pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
 ec.setClusterTolerance(0.02);
 ec.setMinClusterSize(100);
 ec.setMaxClusterSize(25000);
 ec.setSearchMethod(tree);
 ec.setInputCloud(cloud_filtered);
 ec.extract(cluster_indices);
 std::cout << "PointCloud after cluster has: " << cluster_indices.size() << " clusters" << std::endl;

 pclViewCluster(cloud_filtered, cluster_indices);

 return 0;
}

PointCloud before filter has: 460400 data points
PointCloud after filter has: 41049 data points
PointCloud after plane seg has: 8071 data points
PointCloud after cluster has: 5 clusters

区域生长分割算法

区域生长算法目的是合并在平滑性约束下足够接近的点，因此该算法的输出是一组聚类，每个聚类是一组被认为属于同一平滑表面的点。

算法步骤：

选取的点被加入到名为种子点（seeds）的点集中。
算法遍历每一个种子点，查找其邻域点。
1. 逐一检测每个邻域点：计算该点法向量与当前种子点法向量之间的夹角。若夹角小于阈值，则将该邻域点归入当前区域。
2. 随后再检测该邻域点的曲率值。若曲率小于阈值，则将该点加入种子点集。
3. 将当前种子点从种子点集中移除。

当种子点集合为空时，生长完成

#include <format>
#include <iostream>
#include <filesystem>

#include <pcl/visualization/cloud_viewer.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/normal_3d.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/filter_indices.h>
#include <pcl/segmentation/region_growing.h>

int main() {
 auto home = std::filesystem::path(getenv("HOME"));
 auto dataset = home / "Documents/Dataset/region_growing_tutorial.pcd";
 auto cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
 if (pcl::io::loadPCDFile<pcl::PointXYZ>(dataset.c_str(), *cloud) == -1) {
 std::cout << "Cloud reading failed." << std::endl;
 return (-1);
 }
 std::cout << "PointCloud before filter has: " << cloud->size() << " data points" << std::endl;

 auto tree = std::make_shared<pcl::search::KdTree<pcl::PointXYZ>>();
 auto normals = std::make_shared<pcl::PointCloud<pcl::Normal>>();
 pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;

 normal_estimator.setSearchMethod(tree);
 normal_estimator.setInputCloud(cloud);
 normal_estimator.setKSearch(50);
 normal_estimator.compute(*normals);

 pcl::IndicesPtr indices(new std::vector<int>);
 pcl::removeNaNFromPointCloud(*cloud, *indices);

 pclViewer(cloud);

 // 区域增长
 pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
 reg.setMinClusterSize(50);
 reg.setMaxClusterSize(1000000);
 reg.setSearchMethod(tree);
 reg.setNumberOfNeighbours(30);
 reg.setInputCloud(cloud);
 reg.setIndices(indices);
 reg.setInputNormals(normals);
 reg.setSmoothnessThreshold(3.0 / 180.0 * M_PI);
 reg.setCurvatureThreshold(1.0);

 std::vector<pcl::PointIndices> clusters;
 reg.extract(clusters);
 std::cout << "Number of clusters is equal to " << clusters.size() << std::endl;

 pclViewCluster(cloud, clusters);
 return 0;
}

1
2

PointCloud before filter has: 108104 data points
Number of clusters is equal to 115

基于颜色的区域生长分割算法

和区域生长算法的区别：

使用颜色而不是法线特征
会合并颜色差异较小的聚类

#include <format>
#include <iostream>
#include <filesystem>

#include <pcl/visualization/cloud_viewer.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/normal_3d.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/filter_indices.h>
#include <pcl/segmentation/region_growing_rgb.h>

int main() {
 auto home = std::filesystem::path(getenv("HOME"));
 auto dataset = home / "Documents/Dataset/region_growing_rgb_tutorial.pcd";

 auto cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
 if (pcl::io::loadPCDFile<pcl::PointXYZRGB>(dataset.c_str(), *cloud) == -1) {
 std::cout << "Cloud reading failed." << std::endl;
 return (-1);
 }
 std::cout << "PointCloud before filter has: " << cloud->size() << " data points" << std::endl;

 auto tree = std::make_shared<pcl::search::KdTree<pcl::PointXYZRGB>>();

 pcl::IndicesPtr indices(new std::vector<int>);
 pcl::removeNaNFromPointCloud(*cloud, *cloud, *indices);

 pclViewer(cloud);

 // 区域增长
 pcl::RegionGrowingRGB<pcl::PointXYZRGB> reg;
 reg.setInputCloud(cloud);
 reg.setSearchMethod(tree);
 reg.setDistanceThreshold(10);
 reg.setPointColorThreshold(6);
 reg.setRegionColorThreshold(5);
 reg.setMinClusterSize(600);

 std::vector<pcl::PointIndices> clusters;
 reg.extract(clusters);
 std::cout << "Number of clusters is equal to " << clusters.size() << std::endl;
 pclViewCluster(cloud, clusters);
 return 0;
}

1
2

PointCloud before filter has: 307200 data points
Number of clusters is equal to 25

基于最小割的分割算法

这个算法对点云进行二值分割。根据对象中心和半径将点云分割为前景（属于对象的点）和背景（不属于对象的点）

#include <format>
#include <iostream>
#include <filesystem>

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

#include <pcl/filters/filter_indices.h>
#include <pcl/segmentation/min_cut_segmentation.h>

int main() {
 auto home = std::filesystem::path(getenv("HOME"));
 auto dataset = home / "Documents/Dataset/min_cut_segmentation_tutorial.pcd";

 auto cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
 if (pcl::io::loadPCDFile<pcl::PointXYZ>(dataset.c_str(), *cloud) == -1) {
 std::cout << "Cloud reading failed." << std::endl;
 return (-1);
 }
 std::cout << "PointCloud before filter has: " << cloud->size() << " data points" << std::endl;

 pcl::IndicesPtr indices(new std::vector<int>);
 pcl::removeNaNFromPointCloud(*cloud, *cloud, *indices);

 pclViewer(cloud);

 // min cut 分割
 pcl::MinCutSegmentation<pcl::PointXYZ> seg;
 seg.setInputCloud(cloud);
 seg.setIndices(indices);

 pcl::PointCloud<pcl::PointXYZ>::Ptr foreground_points(new pcl::PointCloud<pcl::PointXYZ>());
 pcl::PointXYZ point;
 point.x = 68.97;
 point.y = -18.55;
 point.z = 0.57;
 foreground_points->points.push_back(point);
 seg.setForegroundPoints(foreground_points);

 seg.setSigma(0.25);
 seg.setRadius(3.0433856);
 seg.setNumberOfNeighbours(14);
 seg.setSourceWeight(0.8);


 std::vector<pcl::PointIndices> clusters;
 seg.extract(clusters);

 std::cout << "Maximum flow is " << seg.getMaxFlow() << std::endl;

 std::cout << "Number of clusters is equal to " << clusters.size() << std::endl;
 pclViewCluster(cloud, clusters);
 return 0;
}

1
2
3

PointCloud before filter has: 9311 data points
Maximum flow is 5970.37
Number of clusters is equal to 2

条件欧几里得聚类分割

支持自定义条件的欧几里德

#include <format>
#include <iostream>
#include <filesystem>

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

#include <pcl/filters/filter_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/segmentation/conditional_euclidean_clustering.h>

bool enforceNormalOrIntensitySimilarity(const pcl::PointXYZINormal& point_a,
 const pcl::PointXYZINormal& point_b,
 float /*squared_distance*/) {
 Eigen::Map<const Eigen::Vector3f> point_a_normal = point_a.getNormalVector3fMap(),
 point_b_normal = point_b.getNormalVector3fMap();
 if (std::abs(point_a.intensity - point_b.intensity) < 5.0f) return (true);
 if (std::abs(point_a_normal.dot(point_b_normal)) >
 std::cos(30.0f / 180.0f * static_cast<float>(M_PI)))
 return (true);
 return (false);
}

bool customRegionGrowing(const pcl::PointXYZINormal& point_a,
 const pcl::PointXYZINormal& point_b,
 float squared_distance) {
 Eigen::Map<const Eigen::Vector3f> point_a_normal = point_a.getNormalVector3fMap(),
 point_b_normal = point_b.getNormalVector3fMap();

 if (squared_distance < 10000) {
 if (std::abs(point_a.intensity - point_b.intensity) < 8.0f) return (true);
 if (std::abs(point_a_normal.dot(point_b_normal)) >
 std::cos(30.0f / 180.0f * static_cast<float>(M_PI)))
 return (true);
 } else {
 if (std::abs(point_a.intensity - point_b.intensity) < 3.0f) return (true);
 }
 return (false);
}

int main() {
 auto home = std::filesystem::path(getenv("HOME"));
 auto dataset = home / "Documents/Dataset/Statues_4.pcd";

 auto cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
 auto cloud_out = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
 if (pcl::io::loadPCDFile<pcl::PointXYZI>(dataset.c_str(), *cloud) == -1) {
 std::cout << "Cloud reading failed." << std::endl;
 return (-1);
 }
 std::cout << "PointCloud before filter has: " << cloud->size() << " data points" << std::endl;

 pcl::VoxelGrid<pcl::PointXYZI> vg;
 vg.setInputCloud(cloud);
 vg.setLeafSize(80.0, 80.0, 80.0);
 vg.setDownsampleAllData(true);
 vg.filter(*cloud_out);
 std::cout << "PointCloud after voxel filter has: " << cloud_out->size() << " data points"
 << std::endl;

 pclViewer(cloud_out);

 // 法向量估计
 auto cloud_with_normals = std::make_shared<pcl::PointCloud<pcl::PointXYZINormal>>();
 auto search_tree = std::make_shared<pcl::search::KdTree<pcl::PointXYZI>>();
 pcl::copyPointCloud(*cloud_out, *cloud_with_normals);
 pcl::NormalEstimation<pcl::PointXYZI, pcl::PointXYZINormal> ne;
 ne.setInputCloud(cloud_out);
 ne.setSearchMethod(search_tree);
 ne.setRadiusSearch(300.0);
 ne.compute(*cloud_with_normals);

 // 聚类设置
 auto clusters = std::make_shared<pcl::IndicesClusters>();
 auto small_clusters = std::make_shared<pcl::IndicesClusters>();
 auto large_clusters = std::make_shared<pcl::IndicesClusters>();
 pcl::ConditionalEuclideanClustering<pcl::PointXYZINormal> cec(true);
 cec.setInputCloud(cloud_with_normals);
 cec.setConditionFunction(&customRegionGrowing);
 cec.setClusterTolerance(500.0);
 cec.setMinClusterSize(cloud_with_normals->size() / 1000);
 cec.setMaxClusterSize(cloud_with_normals->size() / 5);
 cec.segment(*clusters);
 cec.getRemovedClusters(small_clusters, large_clusters);

 // pclViewCluster(cloud_out, *clusters);

 pclViewCluster(cloud_out, *small_clusters);

 pclViewCluster(cloud_out, *large_clusters);
 return 0;
}

1
2

PointCloud before filter has: 19553780 data points
PointCloud after voxel filter has: 202529 data points

配准

点云配准就是把多片散乱的点云，拼到同一个坐标系里，拼成一整块完整的点云。

配准 A ，B 点云，就是计算从 A 到 B 所做的旋转 R 和平移 t 变换。

配准算法流程：

提取关键点（KeyPoints）
使用特征描述关键点
找到相关联的特征，估计变换，有误差
不断迭代，直到误差达到阈值

ICP 配准

迭代最近点算法，通过最小化两片点云之间点的距离估计出变换

#include <iostream>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

int main() {
 auto cloud_in = genPointCloud<pcl::PointXYZ>(5);
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);

 *cloud_out = *cloud_in;

 for (auto& p : *cloud_out) {
 p.x += 0.7f;
 }

 std::cout << "Transformed " << cloud_out->size() << " data points" << std::endl;

 pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
 icp.setInputSource(cloud_in);
 icp.setInputTarget(cloud_out);
 pcl::PointCloud<pcl::PointXYZ> final;
 icp.align(final);

 std::cout << "ICP has " << (icp.hasConverged() ? "coverage" : "not coverage") << ", score: " << icp.getFitnessScore() << std::endl;
 std::cout << "trans formation: " << icp.getFinalTransformation() << std::endl;

 return 0;
}

Transformed 5 data points
ICP has coverage, score: 1.28697e-13
trans formation: 1 3.31085e-07 -1.86265e-07 0.7
 4.28409e-08 1 2.12342e-07 1.2964e-07
 -1.3411e-07 1.19209e-07 1 1.9297e-07
 0 0 0 1

增量配准

使用 ICP 算法增量配准多片点云

#include <iostream>
#include <filesystem>
#include <format>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp_nl.h>

#include <pcl/point_representation.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>

using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;

using PointT = pcl::PointXYZ;
using PointCloud = pcl::PointCloud<PointT>;
using PointNormalT = pcl::PointNormal;
using PointCloudWithNormals = pcl::PointCloud<PointNormalT>;

static pcl::visualization::PCLVisualizer* p;
static int vp_1, vp_2;
static bool next_step = false;


struct PCD {
 PointCloud::Ptr cloud;
 std::string name;
 PCD() : cloud(new PointCloud){};
};

class MyPointRepresentation : public pcl::PointRepresentation<PointNormalT> {
 using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;

public:
 MyPointRepresentation() {
 nr_dimensions_ = 4;
 }

 void copyToFloatArray(const PointNormalT& p, float* out) const override {
 out[0] = p.x;
 out[1] = p.y;
 out[2] = p.z;
 out[3] = p.curvature;
 }
};

void showCloudsLeft(const PointCloud::Ptr cloud_target, const PointCloud::Ptr cloud_source) {
 p->removePointCloud("vp1_target");
 p->removePointCloud("vp1_source");

 PointCloudColorHandlerCustom<PointT> tgt_h(cloud_target, 0, 255, 0);
 PointCloudColorHandlerCustom<PointT> src_h(cloud_source, 255, 0, 0);
 p->addPointCloud(cloud_target, tgt_h, "vp1_target", vp_1);
 p->addPointCloud(cloud_source, src_h, "vp1_source", vp_1);

 next_step = false;
 while (!next_step && !p->wasStopped())
 {
 p->spinOnce(100);
 }
}

void showCloudsRight(const PointCloudWithNormals::Ptr cloud_target,
 const PointCloudWithNormals::Ptr cloud_source) {
 p->removePointCloud("source");
 p->removePointCloud("target");

 PointCloudColorHandlerGenericField<PointNormalT> tgt_h(cloud_target, "curvature");
 if (!tgt_h.isCapable()) {
 std::cerr << "Can not create curvature color handler" << std::endl;
 }

 PointCloudColorHandlerGenericField<PointNormalT> src_h(cloud_source, "curvature");
 if (!src_h.isCapable()) {
 std::cerr << "Can not create curvature color handler" << std::endl;
 }
 p->addPointCloud(cloud_target, tgt_h, "target", vp_2);
 p->addPointCloud(cloud_source, src_h, "source", vp_2);

 p->spinOnce();
}

std::vector<PCD> loadData() {
 std::vector<PCD> ret;
 std::filesystem::path home(std::getenv("HOME"));
 std::vector<std::string> filenames = {
 std::filesystem::path(home / "Documents/Dataset/capture0001.pcd").string(),
 std::filesystem::path(home / "Documents/Dataset/capture0002.pcd").string(),
 std::filesystem::path(home / "Documents/Dataset/capture0003.pcd").string(),
 std::filesystem::path(home / "Documents/Dataset/capture0004.pcd").string(),
 std::filesystem::path(home / "Documents/Dataset/capture0005.pcd").string(),
 };

 for (size_t i = 0; i < filenames.size(); i++) {
 PCD m;
 m.name = filenames[i];
 pcl::io::loadPCDFile(filenames[i], *m.cloud);
 std::vector<int> indices;
 pcl::removeNaNFromPointCloud(*m.cloud, *m.cloud, indices);
 ret.push_back(m);
 }

 return ret;
}

void pairAlign(const PointCloud::Ptr cloud_src,
 const PointCloud::Ptr cloud_tgt,
 PointCloud::Ptr output,
 Eigen::Matrix4f& final_transform,
 bool downsample = false) {
 PointCloud::Ptr src(new PointCloud);
 PointCloud::Ptr tgt(new PointCloud);
 pcl::VoxelGrid<PointT> grid;

 // 下采样
 if (downsample) {
 grid.setLeafSize(0.05, 0.05, 0.05);
 grid.setInputCloud(cloud_src);
 grid.filter(*src);

 grid.setInputCloud(cloud_tgt);
 grid.filter(*tgt);
 } else {
 src = cloud_src;
 tgt = cloud_tgt;
 }

 // 估计法向量
 PointCloudWithNormals::Ptr points_with_normals_src(new PointCloudWithNormals);
 PointCloudWithNormals::Ptr points_with_normals_tgt(new PointCloudWithNormals);

 pcl::NormalEstimation<PointT, PointNormalT> norm_est;
 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
 norm_est.setSearchMethod(tree);
 norm_est.setKSearch(30);

 norm_est.setInputCloud(src);
 norm_est.compute(*points_with_normals_src);
 pcl::copyPointCloud(*src, *points_with_normals_src);

 norm_est.setInputCloud(tgt);
 norm_est.compute(*points_with_normals_tgt);
 pcl::copyPointCloud(*tgt, *points_with_normals_tgt);

 MyPointRepresentation point_repr;
 float alpha[4] = {1.0, 1.0, 1.0, 1.0};
 point_repr.setRescaleValues(alpha);

 // ICP
 pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
 reg.setTransformationEpsilon(1e-6);
 reg.setMaxCorrespondenceDistance(0.1);
 reg.setPointRepresentation(std::make_shared<const MyPointRepresentation>(point_repr));

 reg.setInputSource(points_with_normals_src);
 reg.setInputTarget(points_with_normals_tgt);

 Eigen::Matrix4f ti = Eigen::Matrix4f::Identity(), prev, target_to_source;
 PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
 reg.setMaximumIterations(2);
 for (int i = 0; i < 30; ++i) {
 std::cerr << "Iteration nr: " << i << std::endl;

 points_with_normals_src = reg_result;
 reg.setInputSource(points_with_normals_src);
 reg.align(*reg_result);

 ti = reg.getFinalTransformation() * ti;
 if (std::abs((reg.getLastIncrementalTransformation() - prev).sum()) <
 reg.getTransformationEpsilon()) {
 reg.setMaxCorrespondenceDistance(reg.getMaxCorrespondenceDistance() - 0.001);
 }
 prev = reg.getLastIncrementalTransformation();
 showCloudsRight(points_with_normals_tgt, points_with_normals_src);
 }

 target_to_source = ti.inverse();

 pcl::transformPointCloud(*cloud_tgt, *output, target_to_source);

 p->removePointCloud("source");
 p->removePointCloud("target");
 PointCloudColorHandlerCustom<PointT> cloud_tgt_h(output, 0, 255, 0);
 PointCloudColorHandlerCustom<PointT> cloud_src_h(cloud_src, 255, 0, 0);
 p->addPointCloud(output, cloud_tgt_h, "target", vp_2);
 p->addPointCloud(cloud_src, cloud_src_h, "source", vp_2);

 next_step = false;
 while (!next_step && !p->wasStopped())
 {
 p->spinOnce(100);
 }

 p->removePointCloud("source");
 p->removePointCloud("target");

 *output += *cloud_src;
 final_transform = target_to_source;
}


// 键盘回调函数
void keyboardCallback(const pcl::visualization::KeyboardEvent& event, void*)
{
 if (event.keyDown() && event.getKeyCode() == 'n')
 {
 next_step = true;
 }
}


int main(int argc, char* argv[]) {

 auto pcds = loadData();

 p = new pcl::visualization::PCLVisualizer(argc, argv, "example");
 p->createViewPort(0.0, 0, 0.5, 1.0, vp_1);
 p->createViewPort(0.5, 0, 1.0, 1.0, vp_2);
 p->setBackgroundColor(0.2, 0.2, 0.2, vp_1);
 p->setBackgroundColor(0.7, 0.7, 0.7, vp_2);

 p->registerKeyboardCallback(keyboardCallback);

 PointCloud::Ptr result(new PointCloud), source, target;
 Eigen::Matrix4f global_transform = Eigen::Matrix4f::Identity(), pair_transform;

 for (size_t i = 1; i < pcds.size(); i++) {
 source = pcds[i - 1].cloud;
 target = pcds[i].cloud;


 showCloudsLeft(source, target);

 PointCloud::Ptr temp(new PointCloud);

 pairAlign(source, target, temp, pair_transform, true);

 pcl::transformPointCloud(*temp, *result, global_transform);

 global_transform *= pair_transform;
 }

 p->spin();

 return 0;
}

NDT 算法

#include <iostream>
#include <filesystem>
#include <thread>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/ndt.h>

using namespace std::chrono_literals;

int main(int argc, char* argv[]) {
 std::filesystem::path home(std::getenv("HOME"));
 auto pcd1_file =
 std::filesystem::path(home / "Documents/Dataset/room_scan1.pcd");
 auto pcd2_file =
 std::filesystem::path(home / "Documents/Dataset/room_scan2.pcd");

 pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 if (pcl::io::loadPCDFile(pcd1_file.string(), *target_cloud) == -1) {
 return -1;
 }

 pclViewer(target_cloud);

 pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 if (pcl::io::loadPCDFile(pcd2_file.string(), *input_cloud) == -1) {
 return -1;
 }

 pclViewer(input_cloud);

 // 近似体素滤波，精度比体素滤波低，但是速度快
 pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
 approximate_voxel_filter.setLeafSize(0.2, 0.2, 0.2);
 approximate_voxel_filter.setInputCloud(input_cloud);
 approximate_voxel_filter.filter(*filtered_cloud);

 pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
 ndt.setTransformationEpsilon(0.01);
 ndt.setStepSize(0.1);
 ndt.setResolution(1.0);
 ndt.setMaximumIterations(35);

 ndt.setInputSource(filtered_cloud);
 ndt.setInputTarget(target_cloud);

 // 初始化估计
 Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ());
 Eigen::Translation3f init_translation(1.79387, 0.720047, 0);
 Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();

 pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 ndt.align(*output_cloud, init_guess);

 std::cout << "Normal Distributions Transform has " << (ndt.hasConverged ()?"converged":"not converged")
 << ", score: " << ndt.getFitnessScore () << std::endl;

 // 可视化
 pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("viewer"));
 viewer->setBackgroundColor(0.1, 0.1, 0.1);

 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target_cloud, 255, 0, 0);
 viewer->addPointCloud(target_cloud, target_color, "target cloud");

 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> output_color(output_cloud, 0, 255, 0);
 viewer->addPointCloud(output_cloud, output_color, "output cloud");

 viewer->addCoordinateSystem(1.0, "global");
 viewer->initCameraParameters();

 while (!viewer->wasStopped()) {
 viewer->spinOnce(100);
 std::this_thread::sleep_for(100ms);
 }

 return 0;
}

`1`	`Normal Distributions Transform has converged, score: 0.679887`

在场景中估计对象位姿

#include <filesystem>

#include <Eigen/Core>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std::chrono_literals;

using PointNT = pcl::PointNormal;
using PointCloudT = pcl::PointCloud<PointNT>;
using FeatureT = pcl::FPFHSignature33;
using FeatureEstimationT = pcl::FPFHEstimationOMP<PointNT, PointNT, FeatureT>;
using FeatureCloudT = pcl::PointCloud<FeatureT>;
using ColorHandlerT = pcl::visualization::PointCloudColorHandlerCustom<PointNT>;

int main(int argc, char* argv[]) {
 std::filesystem::path home(std::getenv("HOME"));
 auto object_file =
 std::filesystem::path(home / "Documents/Dataset/chef.pcd");
 auto scene_file =
 std::filesystem::path(home / "Documents/Dataset/rs1.pcd");

 PointCloudT::Ptr object(new PointCloudT);
 PointCloudT::Ptr object_aligned(new PointCloudT);
 PointCloudT::Ptr scene_before_downsampling(new PointCloudT);
 PointCloudT::Ptr scene(new PointCloudT);
 FeatureCloudT::Ptr object_features(new FeatureCloudT);
 FeatureCloudT::Ptr scene_features(new FeatureCloudT);

 if (pcl::io::loadPCDFile<PointNT>(object_file.string(), *object) == -1) {
 return -1;
 }

 if (pcl::io::loadPCDFile<PointNT>(scene_file.string(), *scene_before_downsampling) == -1) {
 return -1;
 }

 // 下采样
 pcl::VoxelGrid<PointNT> voxel_filter;
 const float leaf = 0.005f;
 voxel_filter.setLeafSize(leaf, leaf, leaf);
 voxel_filter.setInputCloud(object);
 voxel_filter.filter(*object);

 voxel_filter.setInputCloud(scene_before_downsampling);
 voxel_filter.filter(*scene);

 // 法向量估计
 pcl::NormalEstimationOMP<PointNT, PointNT> nest;
 nest.setRadiusSearch(0.005);
 nest.setInputCloud(scene);
 nest.setSearchSurface(scene_before_downsampling);
 nest.compute(*scene);

 // 特征估计
 FeatureEstimationT fest;
 fest.setRadiusSearch(0.025);
 fest.setInputCloud(object);
 fest.setInputNormals(object);
 fest.compute(*object_features);

 fest.setInputCloud(scene);
 fest.setInputNormals(scene);
 fest.compute(*scene_features);

 // 配准
 pcl::SampleConsensusPrerejective<PointNT, PointNT, FeatureT> align;
 align.setInputSource(object);
 align.setSourceFeatures(object_features);
 align.setInputTarget(scene);
 align.setTargetFeatures(scene_features);
 align.setMaximumIterations(50000);
 align.setNumberOfSamples(3);
 align.setCorrespondenceRandomness(5);
 align.setSimilarityThreshold(0.95f);
 align.setMaxCorrespondenceDistance(2.5f * leaf);
 align.setInlierFraction(0.25f);
 align.align(*object_aligned);

 if (align.hasConverged()) {
 printf ("\n");
 Eigen::Matrix4f transformation = align.getFinalTransformation ();
 pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
 pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
 pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
 pcl::console::print_info ("\n");
 pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
 pcl::console::print_info ("\n");
 pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());
 pcl::visualization::PCLVisualizer visu("Alignment");
 visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
 visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
 visu.spin ();
 }

 return 0;
}


 | 0.007 -0.993 -0.118 |
R = | -1.000 -0.009 0.020 |
 | -0.021 0.118 -0.993 |

t = < -0.130, 0.065, 0.078 >

Inliers: 1422/3432

Linux网络代理设置

Sat, 24 Jan 2026 23:19:00 +0800

首先需要有代理服务端，可以自建或购买已有服务，然后就是本文中相关的客户端设置，客户端配置为透明代理模式，这样系统上的软件就不用每个都配置代理了

代理配置

v2raya 启动后打开 http://127.0.0.1:2017/ ，输入管理员账户和密码登录，密码忘记可以在终端执行 sudo v2raya reset
添加订阅地址，更新后应该能够看到代理服务端
右上角点击设置，每个设置项如下
- 透明代理/系统代理：启用：分类规则与规则端口所选模式一致
- 透明代理/系统代理实现方式：redierct
- 规则端口的分流模式：RoutingA
- 防止 DNS 污染： DoH
- 特殊模式：关闭
- TCPFastOpen: 关闭
- 多路复用：关闭
设置完成后保存，然后选择一个服务器，点击左上角运行即可

RoutingA 规则配置

# 默认走直连
default: direct

# gfw 走代理
domain(geosite:gfw)->proxy

# 一些网站走代理，速度更快
domain(domain: osrfoundation.org)->proxy

第一段：默认走直连
第二段：在 gfwlist 中的网站走代理
第三段：一些网站走代理

后续可能添加更多规则

Linux罗技鼠标设置dpi

Thu, 15 Jan 2026 10:44:00 +0800

安装非官方驱动： https://github.com/PixlOne/logiops ，安装完后启动服务
1

sudo systemctl enable --now logid

执行 sudo logid 查看识别的鼠标名称

`1`	`[INFO] Device found: G502 HERO Gaming Mouse on /dev/hidraw4:255`

配置文件 /etc/logid.cfg

devices: (
{
 name: "G502 HERO Gaming Mouse";
 dpi: 1600;
}
);

这个驱动还有很多其它功能，比如 dpi 切换，功能键设置等，有需要可以看 Wiki

Linux激活键盘Fn功能

Thu, 15 Jan 2026 10:36:00 +0800

临时修改

`1`	`echo 2 > /sys/module/hid_apple/parameters/fnmode`

向 /sys/module/hid_apple/parameters/fnmode 写入不同数字激活不同功能：

0: 禁用功能键，按 Fn + F8 等同于 F8
1: 默认功能键，按 F8 触发功能键 (play/pause) ，按 Fn + F8 触发 F8 键
2: 默认非功能键，按 F8 触发 F8 键，按 Fn + F8 触发功能键 (play/pause)

永久修改

向 /etc/modprobe.d/hid_apple.conf 中加上下面的配置，没有这个文件就新建一个：

`1`	`options hid_apple fnmode=2`

然后更新内核

`1`	`sudo update-initramfs -u -k all`

重启后生效

`1`	`sudo reboot`

WSL2配合Windows运行Gazebo

Sun, 11 Jan 2026 12:18:00 +0800

虽然当前 WSL2 通过 WSLg 已支持直接运行 Gazebo，即使启用了 GPU 硬件加速，其图形性能仍显著受限，导致仿真过程非常卡顿。因此，更推荐的方式是将 Gazebo 运行在原生 Ubuntu 系统上。如果你不想配置双系统，也可以采用以下替代方案：在 Windows 原生环境中运行 Gazebo 仿真器，同时在 WSL2 中运行 ROS 2 节点控制 Gazebo。

Gazebo 新版本采用了类似 ROS 2 的分布式架构——其内部组件以独立节点形式运行，并通过 topic 或 service 进行通信。得益于此架构，只要网络配置正确，WSL2 中的 ROS2 节点便能发现并订阅/调用 Windows 上 Gazebo 发布的 topic 和 service，从而实现跨平台协同仿真与控制。

Windows 安装 Gazebo

Gazebo 官方安装教程：https://gazebosim.org/docs/harmonic/install_windows/

这里使用 pixi 来安装

安装 pixi ，可以下载二进制，或者用 scoop 安装
1

scoop install pixi
创建一个文件夹，作为 gazebo 的安装目录，这里放到 D:/Applications/Manual/gazebo

在文件夹下新建一个 pixi.toml 文件，里面写上下面的内容

[workspace]
channels = ["conda-forge"]
name = "gazebo"
platforms = ["win-64"]
version = "0.1.0"

[target.win-64.activation.env]
PYTHONUTF8="1"
GZ_PARTITION="xbot"

[dependencies]
libgz-cmake3 = "*"
libgz-common5 = "*"
libgz-fuel-tools9 = "*"
libgz-sim8 = "*"
libgz-gui8 = "*"
libgz-launch7 = "*"
libgz-math7 = "*"
libgz-msgs10 = "*"
libgz-physics7 = "*"
libgz-plugin2 = "*"
libgz-rendering8 = "*"
libgz-sensors8 = "*"
libgz-tools2 = "*"
libgz-transport13 = "*"
libgz-utils2 = "*"
libsdformat14 = "*"

env 下包括这个环境下启用的环境变量：

PYTHONUTF8 ：强制 Python 默认使用 UTF8 编码，必须添加，不然会安装失败
GZ_PARTITION ：指定 Gazebo 节点的分区名称，相同分区名称的节点才能互相发现，既在 WSL2 环境中也要指定同样的 GZ_PARTITION 值

在目录下运行 pixi install 开始安装
运行 pixi shell 以当前环境开启一个 shell
运行 gz sim -s 启动 Gazebo 服务
重新开一个 shell ，运行 gz sim -g 启动 Gazebo UI

快速启动 Gazebo

使用 pixi 安装的软件每次要进入对应目录，使用 pixi shell 或者 pixi run 才能运行 Gazebo ，下面制作一个脚本一键启动。

将下面的脚本保存在 export_env.ps1 中

$OutputScript = "run_gazebo.ps1"

$envVars = Get-ChildItem env: | Sort-Object Name

$scriptLines = @()
$scriptLines += "# Environment variables exported at $(Get-Date -Format 'yyyy-MM-dd HH:mm:ss')"
$scriptLines += "# Using ${env:...} syntax to support special characters in names."
$scriptLines += ""

foreach ($var in $envVars) {
 # 转义值中的双引号和反引号
 $escapedValue = $var.Value -replace '"', '`"' -replace '`', '``'

 # 使用 ${env:...} 语法包裹变量名（支持括号、空格等）
 $line = '${env:' + $var.Name + '} = "' + $escapedValue + '"'
 $scriptLines += $line
}

# 写入文件（UTF-8 无 BOM）
$scriptLines | Out-File -FilePath $OutputScript -Encoding UTF8

Write-Host "✅ 已成功导出到: $(Resolve-Path $OutputScript)" -ForegroundColor Green

在 powershell 中使用 pixi shell 进入 gazebo
进入 shell 后运行 export_envs.ps1 ，会生成 run_gazebo.ps1 ，里面设置了当前 shell 的所有环境变量

在 run_gazebo.ps1 脚本后面加上 gazebo 启动的命令

# 启动 gazebo

param(
 [Parameter(ValueFromRemainingArguments = $true)]
 [string[]]$ServerArgs
)

if ($null -eq $ServerArgs) {
 $ServerArgs = @()
}

$serverProc = $null
$guiProc = $null


try {
 # 构造完整参数列表
 $fullServerArgs = @("sim", "-s") + $ServerArgs
 $guiArgs = @("sim", "-g")

 $serverProc = Start-Process -FilePath "gz" -ArgumentList $fullServerArgs -PassThru -NoNewWindow -RedirectStandardOutput "$env:TEMP\gz_server.log" -RedirectStandardError "$env:TEMP\gz_server.err"

 $guiProc = Start-Process -FilePath "gz" -ArgumentList $guiArgs -PassThru -NoNewWindow -RedirectStandardOutput "$env:TEMP\gz_gui.log" -RedirectStandardError "$env:TEMP\gz_gui.err"

 # 持续等待（允许 Ctrl+C 中断）
 while ($true) {
 Start-Sleep -Milliseconds 500
 }
}
catch {
 # 捕获 Ctrl+C (ActionPreferenceStopException) 或其他异常
 if ($_.Exception -is [System.Management.Automation.ActionPreferenceStopException]) {
 Write-Host "`n🛑 收到中断信号 (Ctrl+C)，正在关闭进程..." -ForegroundColor Yellow
 } else {
 Write-Error "💥 发生错误: $($_.Exception.Message)"
 }
}
finally {
 # === 3. 清理子进程 ===
 foreach ($proc in @($serverProc, $guiProc)) {
 if ($proc -and -not $proc.HasExited) {
 try {
 Stop-Process -Id $proc.Id -Force -ErrorAction SilentlyContinue
 }
 catch {
 # 忽略清理错误
 }
 }
 }
}

然后我们就可以右键 run_gazebo.ps1 ，然后使用 powershell 运行来快速启动 Gazebo 了

WSL2 配置

网络配置

要想 WSL2 中能正常发现 Windows 上 Gazebo 的节点，WSL2 的网络不能为镜像网络，既下面的配置不能为 mirrored ，可以设置为 NAT

1
2

[wsl2]
networkingMode=nat

安装 Gazebo

按照官方文档安装即可：https://gazebosim.org/docs/harmonic/install_ubuntu/

Gazebo 配置

在终端设置环境变量 GZ_PARTITION ，值与 Windows 上一致

`1`	`export GZ_PARTITION="xbot"`

如果 Windows 启动了 Gazebo ，现在运行 gazebo topic -l 应该可以看到对应的话题如下：

/clock
/gazebo/resource_paths
/gui/camera/pose
/gui/currently_tracked
/gui/track
/stats
/world/default/clock
/world/default/dynamic_pose/info
/world/default/pose/info
/world/default/scene/deletion
/world/default/scene/info
/world/default/state
/world/default/stats
/world/default/light_config
/world/default/material_color

测试

在 Windows 上启动 Gazebo 中自带的 diff_drive.sdf 例子
1

run_gazebo.ps1 diff_drive.sdf

在 WSL2 用 gz topic -l 应该可以看到对应话题

/clock
/gazebo/resource_paths
/gui/camera/pose
/gui/currently_tracked
/gui/track
/model/vehicle_blue/odometry
/model/vehicle_blue/tf
/model/vehicle_green/odometry
/model/vehicle_green/tf
/stats
/world/diff_drive/clock
/world/diff_drive/dynamic_pose/info
/world/diff_drive/pose/info
/world/diff_drive/scene/deletion
/world/diff_drive/scene/info
/world/diff_drive/state
/world/diff_drive/stats
/model/vehicle_blue/cmd_vel
/model/vehicle_blue/enable
/model/vehicle_green/cmd_vel
/model/vehicle_green/enable
/world/diff_drive/light_config
/world/diff_drive/material_color

在 WSL2 上使用 gz topic 命令发布 /model/vehicle_blue/cmd_vel 话题就可以控制小车

1
2

gz topic -t /model/vehicle_blue/cmd_vel -m gz.msgs.Twist \
 -p 'linear: {x: 1.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.5}'

ROS2 Launch 启动

WSL2 可以直接运行 Windows 程序，因此可以在 Launch 文件中启动运行 Gazebo 的脚本

1
2

import subprocess
subprocess.Popen(["powershell.exe", "run_gazebo.ps1"])

注意：当前这种方式启动后 WSL2 终端输出格式会乱掉，并且要按两次 Ctrl-C 才能停止 Launch

ROS2机器人仿真

Tue, 06 Jan 2026 00:13:00 +0800

URDF 机器人建模

URDF 使用 xml 格式描述机器人模型

首先创建一个包来保存文件 ros2 pkg create robot_describes --destination-directory=src --build-type=ament_cmake --license Apache-2.0

基础

<?xml version="1.0"?>
<!-- 机器人名称 -->
<robot name="first_robot">
 <!-- 机器人身体部分 -->
 <link name="base_link">
 <!-- 部件的外观描述 -->
 <visual>
 <!-- 沿着自己几何中心的偏移和旋转 -->
 <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
 <!-- 几何形状 -->
 <geometry>
 <!-- 圆柱体 -->
 <cylinder radius="0.10" length="0.12"/>
 </geometry>
 <!-- 材质和颜色 -->
 <material name="white">
 <color rgba="1.0 1.0 1.0 0.5" />
 </material>
 </visual>
 </link>

 <!-- 机器人的 IMU ，惯性测量单元 -->
 <link name="imu_link">
 <!-- 部件的外观描述 -->
 <visual>
 <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
 <geometry>
 <!-- 正方体 -->
 <box size="0.02 0.02 0.02" />
 </geometry>
 <!-- 材质和颜色 -->
 <material name="black">
 <color rgba="0.0 0.0 0.0 0.5" />
 </material>
 </visual>
 </link>

 <!-- 机器人关节，用来组合机器人部件 -->
 <joint name="imu_joint" type="fixed">
 <!-- 父部件 -->
 <parent link="base_link"/>
 <!-- 子部件固定到父部件上 -->
 <child link="imu_link"/>
 <!-- 固定的位置 -->
 <origin xyz="0.0 0.0 0.03" rpy="0.0 0.0 0.0"/>
 </joint>

</robot>

使用 xacro 简化 urdf

xacro 工具可以将 xacro 描述文件转成 xml 文件，xacro 对比原生 xml 提供了一些方便的功能，比如宏

<?xml version="1.0"?>
<!-- 机器人名称 -->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="first_robot">
 <xacro:macro name="base_link" params="length radius">
 <!-- 机器人身体部分 -->
 <link name="base_link">
 <!-- 部件的外观描述 -->
 <visual>
 <!-- 沿着自己几何中心的偏移和旋转 -->
 <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
 <!-- 几何形状 -->
 <geometry>
 <!-- 圆柱体 -->
 <cylinder radius="${radius}" length="${length}"/>
 </geometry>
 <!-- 材质和颜色 -->
 <material name="white">
 <color rgba="1.0 1.0 1.0 0.5" />
 </material>
 </visual>
 </link>
 </xacro:macro>

 <xacro:macro name="imu_link" params="imu_name xyz">
 <!-- 机器人的 IMU ，惯性测量单元 -->
 <link name="${imu_name}_link">
 <!-- 部件的外观描述 -->
 <visual>
 <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
 <geometry>
 <!-- 正方体 -->
 <box size="0.02 0.02 0.02" />
 </geometry>
 <!-- 材质和颜色 -->
 <material name="black">
 <color rgba="0.0 0.0 0.0 0.5" />
 </material>
 </visual>
 </link>
 <!-- 机器人关节，用来组合机器人部件 -->
 <joint name="${imu_name}_joint" type="fixed">
 <!-- 父部件 -->
 <parent link="base_link"/>
 <!-- 子部件固定到父部件上 -->
 <child link="${imu_name}_link"/>
 <!-- 固定的位置 -->
 <origin xyz="${xyz}" rpy="0.0 0.0 0.0"/>
 </joint>
 </xacro:macro>

 <xacro:base_link length="0.12" radius="0.1"/>
 <xacro:imu_link imu_name="imu_up" xyz="0.0 0.0 0.03"/>
 <xacro:imu_link imu_name="imu_down" xyz="0.0 0.0 -0.03"/>

</robot>

通过 xacro first_robot.urdf 就可以将上面的文件内容生成机器人模型 xml

使用 rviz 可视化 urdf

安装 ros-jazzy-robot-state-publisher 和 ros-jazzy-joint-state-publisher 两个功能包，这两个功能包可以解析 urdf 文件，自动发布对应的 TF 关系
1

sudo apt install ros-jazzy-joint-state-publisher ros-jazzy-robot-state-publisher

写 launch 文件，同时加载各个节点

# -*- coding: utf-8 -*-
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, Command
from launch.actions import DeclareLaunchArgument

def generate_launch_description():
 share_dir = FindPackageShare("robot_describes")
 default_model_path = PathJoinSubstitution([share_dir, "urdf", "first_robot.urdf"])
 default_config_path =PathJoinSubstitution([share_dir, "config", "display_robot.rviz"])

 model_path = LaunchConfiguration("model", default=default_model_path)
 config_path = LaunchConfiguration("config", default=default_config_path)

 urdf = Command(["xacro ", model_path])

 robot_state_publisher = Node(
 package="robot_state_publisher",
 executable="robot_state_publisher",
 parameters=[{"robot_description": ParameterValue(urdf, value_type=str)}],
 output="screen"
 )

 joint_state_publisher = Node(
 package="joint_state_publisher",
 executable="joint_state_publisher"
 )

 rviz2 = Node(
 package="rviz2",
 executable="rviz2",
 arguments=["-d", config_path]
 )

 return LaunchDescription([
 DeclareLaunchArgument("model", default_value=default_model_path, description="模型路径"),
 DeclareLaunchArgument("config", default_value=default_config_path, description="配置路径"),
 robot_state_publisher,
 joint_state_publisher,
 rviz2])

display_robot.rviz 为 rviz2 工具的配置文件，在 rviz2 工具中配置后保存即可

使用 ros2 launch robot_describes display_robot.py 启动就可以在 rviz2 中看到对应的机器人模型

增加物理属性

碰撞属性

碰撞属性使用 collision 标签添加，和 visual 类似

<collision>
 <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
 <geometry>
 <!-- 正方体 -->
 <box size="0.02 0.02 0.02" />
 </geometry>
 <!-- 材质和颜色 -->
 <material name="black">
 <color rgba="0.0 0.0 0.0 0.5" />
 </material>
</collision>

质量和惯性

使用 inertial 添加质量和惯性，质量用 mass 表示，单位为 kg ，惯性用 inertia ，使用惯性矩阵

常见三种形状的质量和惯性

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
 <xacro:macro name="box_inertia" params="m w h d">
 <inertial>
 <mass value="${m}" />
 <inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}" />
 </inertial>
 </xacro:macro>

 <xacro:macro name="cylinder_inertia" params="m r h">
 <inertial>
 <mass value="${m}" />
 <inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy="0" ixz="0" iyy="${(m/12) * (3*r*r + h*h)}" iyz="0" izz="${(m/2) * (r*r)}" />
 </inertial>
 </xacro:macro>

 <xacro:macro name="sphere_inertia" params="m r">
 <inertial>
 <mass value="${m}" />
 <inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}" />
 </inertial>
 </xacro:macro>

</robot>

Gazebo 仿真

ROS2 启动 Gazebo 并加载模型

可以在 launch 文件中启动 gazebo ，同时将之前创建的机器人模型创建出来

启动 gazebo 仿真，加载世界模型

default_world_path = PathJoinSubstitution([share_dir, "world", "xbot.sdf"])
gz_launch_path = PathJoinSubstitution([FindPackageShare("ros_gz_sim"), 'launch', 'gz_sim.launch.py'])
gz_launch = IncludeLaunchDescription(
 PythonLaunchDescriptionSource(gz_launch_path),
 launch_arguments={
 'gz_args': [default_world_path],
 'on_exit_shutdown': 'True'
 }.items(),
)

在世界中生成机器人

同样启动 robot_state_publisher 解析 URDF 文件并发布，同时计算各个关节的 TF 关系

robot_state_publisher = Node(
 package="robot_state_publisher",
 executable="robot_state_publisher",
 parameters=[{"robot_description": ParameterValue(urdf, value_type=str)}],
 output="screen"
)

通过 ros_gz_sim 包创建机器人，机器人模型来源 robot_description

spawn_entity = Node(
 package="ros_gz_sim",
 executable="create",
 arguments=[
 "-name", "xbot",
 "-topic", "robot_description",
 ]
)

启动 bridge

birdge 的作用是将 gz 和 ros2 的话题之间做一个转换，比如将 gz 发布的 tf 转发到 ros2 下，支撑的转换类型：https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge

启动 bridge ，配置文件如下：

- ros_topic_name: "clock"
gz_topic_name: "/clock"
ros_type_name: "rosgraph_msgs/msg/Clock"
gz_type_name: "gz.msgs.Clock"
direction: "GZ_TO_ROS"
- gz_topic_name: "/cmd_vel"
ros_type_name: "geometry_msgs/msg/Twist"
gz_type_name: "gz.msgs.Twist"
- ros_topic_name: "tf"
gz_topic_name: "/tf"
ros_type_name: "tf2_msgs/msg/TFMessage"
gz_type_name: "gz.msgs.Pose_V"
direction: "GZ_TO_ROS"

启动代码：

1
2
3

from ros_gz_bridge.actions import RosGzBridge
bridge_config_path = PathJoinSubstitution([share_dir, "config", "bridge.yaml"])
bridge = RosGzBridge(bridge_name="xbot_bridge", config_file=bridge_config_path)

完整的 launch 代码：

# -*- coding: utf-8 -*-
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, Command
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ros_gz_bridge.actions import RosGzBridge

def generate_launch_description():
 share_dir = FindPackageShare("robot_describes")
 default_model_path = PathJoinSubstitution([share_dir, "urdf", "xbot", "xbot.urdf.xacro"])
 bridge_config_path = PathJoinSubstitution([share_dir, "config", "bridge.yaml"])
 default_world_path = PathJoinSubstitution([share_dir, "world", "xbot.sdf"])
 rviz_config_path =PathJoinSubstitution([share_dir, "config", "display_robot.rviz"])

 model_path = LaunchConfiguration("model", default=default_model_path)

 urdf = Command(["xacro ", model_path])

 robot_state_publisher = Node(
 package="robot_state_publisher",
 executable="robot_state_publisher",
 parameters=[{"robot_description": ParameterValue(urdf, value_type=str)}],
 output="screen"
 )

 gz_launch_path = PathJoinSubstitution([FindPackageShare("ros_gz_sim"), 'launch', 'gz_sim.launch.py'])
 gz_launch = IncludeLaunchDescription(
 PythonLaunchDescriptionSource(gz_launch_path),
 launch_arguments={
 'gz_args': [default_world_path],
 'on_exit_shutdown': 'True'
 }.items(),
 )

 bridge = RosGzBridge(bridge_name="xbot_bridge", config_file=bridge_config_path)

 spawn_entity = Node(
 package="ros_gz_sim",
 executable="create",
 arguments=[
 "-name", "xbot",
 "-topic", "robot_description"
 ]
 )

 rviz2 = Node(
 package="rviz2",
 executable="rviz2",
 arguments=["-d", rviz_config_path]
 )

 return LaunchDescription([
 DeclareLaunchArgument("model", default_value=default_model_path, description="模型路径"),
 robot_state_publisher,
 gz_launch,
 bridge,
 spawn_entity,
 rviz2
 ])

两轮差速传感器仿真

gazebo 通过插件形式提供各种传感器仿真，可以在 urdf 中使用 gazebo 扩展标签描述插件

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 <xacro:macro name="gazebo_control_plugin">
 <gazebo>
 <!-- 两轮差速插件 -->
 <plugin name='gz::sim::systems::DiffDrive' filename='gz-sim-diff-drive-system'>

 <!-- 左右轮子 -->
 <left_joint>left_wheel_joint</left_joint>
 <right_joint>right_wheel_joint</right_joint>

 <!-- 轮子之间的距离 -->
 <wheel_separation>0.2</wheel_separation>
 <!-- 轮子的半径 -->
 <wheel_radius>0.032</wheel_radius>

 <!-- 里程计 -->
 <frame_id>odom</frame_id>
 <!-- tf 转换， odom 转换到 base_footprint -->
 <child_frame_id>base_footprint</child_frame_id>

 <!-- 接受控制的 topic -->
 <topic>/cmd_vel</topic>
 <!-- 发布里程的 topic -->
 <odom_topic>/odometry</odom_topic>
 <!-- tf 转换的 topic -->
 <tf_topic>/tf</tf_topic>

 </plugin>

 <!-- 发布关节状态 -->
 <plugin name='gz::sim::systems::JointStatePublisher' filename='gz-sim-joint-state-publisher-system'>
 <topic>/joint_states</topic>
 <left_joint>left_wheel_joint</left_joint>
 <right_joint>right_wheel_joint</right_joint>
 </plugin>
 </gazebo>

 </xacro:macro>
</robot>

然后再 xbot 中引用

1
2

<xacro:include filename="$(find robot_describes)/urdf/xbot/plugins/gazebo_control_plugin.xacro"/>
<xacro:gazebo_control_plugin/>

上面的插件会在 gazebo 中发布 /odometry ， /joint_states 话题，因此需要在 bridge 中配置，将对应内容转换到 ros2 中

- ros_topic_name: "odometry"
gz_topic_name: "/odometry"
ros_type_name: "nav_msgs/msg/Odometry"
gz_type_name: "gz.msgs.Odometry"
direction: "GZ_TO_ROS"
- ros_topic_name: "joint_states"
gz_topic_name: "/joint_states"
ros_type_name: "sensor_msgs/msg/JointState"
gz_type_name: "gz.msgs.Model"
direction: "GZ_TO_ROS"

激光雷达传感器仿真

同样增加激光雷达插件描述

<gazebo>
 <plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
 <render_engine>ogre2</render_engine>
 </plugin>
</gazebo>
<gazebo reference="laser_link">
 <sensor name="gpu_lidar" type="gpu_lidar">
 <!-- 发布数据的话题名称 -->
 <topic>scan</topic>
 <!-- 发布频率 1 HZ -->
 <update_rate>1</update_rate>
 <!-- 发布坐标数据用的 frame_id -->
 <gz_frame_id>laser_link</gz_frame_id>
 <ray>
 <!-- 设置扫描范围 -->
 <scan>
 <horizontal>
 <!-- 一圈采样点 -->
 <samples>360</samples>
 <!-- 分辨率 -->
 <resolution>1.0</resolution>
 <!-- 最小角度 -->
 <min_angle>0.0</min_angle>
 <!-- 最大角度 -->
 <max_angle>6.28</max_angle>
 </horizontal>
 </scan>
 <!-- 扫描距离 -->
 <range>
 <!-- 最短距离 -->
 <min>0.12</min>
 <!-- 最大距离 -->
 <max>8.0</max>
 <!-- 分辨率 -->
 <resolution>0.015</resolution>
 </range>
 </ray>
 <!-- 总是扫描 -->
 <always_on>1</always_on>
 <!-- 可视化打开 -->
 <visualize>true</visualize>
 </sensor>

</gazebo>

在 bridge 中配置话题转换

- ros_topic_name: "lidar_scan"
gz_topic_name: "scan"
ros_type_name: "sensor_msgs/msg/LaserScan"
gz_type_name: "gz.msgs.LaserScan"
direction: "GZ_TO_ROS"

IMU 仿真

添加插件配置

<gazebo>
 <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu">
 </plugin>
</gazebo>

<gazebo reference="imu_link">
 <sensor name="imu_sensor" type="imu">
 <!-- 发布话题名称 -->
 <topic>imu</topic>
 <always_on>1</always_on>
 <!-- 更新频率 -->
 <update_rate>1</update_rate>
 <!-- 可视化 -->
 <visualize>true</visualize>
 <!-- 六轴噪声设置 -->
 <imu>
 <angular_velocity>
 <x>
 <noise type="gaussian">
 <mean>0.0</mean>
 <stddev>2e-4</stddev>
 <bias_mean>0.0000075</bias_mean>
 <bias_stddev>0.0000008</bias_stddev>
 </noise>
 </x>
 <y>
 <noise type="gaussian">
 <mean>0.0</mean>
 <stddev>2e-4</stddev>
 <bias_mean>0.0000075</bias_mean>
 <bias_stddev>0.0000008</bias_stddev>
 </noise>
 </y>
 <z>
 <noise type="gaussian">
 <mean>0.0</mean>
 <stddev>2e-4</stddev>
 <bias_mean>0.0000075</bias_mean>
 <bias_stddev>0.0000008</bias_stddev>
 </noise>
 </z>
 </angular_velocity>
 <linear_acceleration>
 <x>
 <noise type="gaussian">
 <mean>0.0</mean>
 <stddev>1.7e-2</stddev>
 <bias_mean>0.1</bias_mean>
 <bias_stddev>0.001</bias_stddev>
 </noise>
 </x>
 <y>
 <noise type="gaussian">
 <mean>0.0</mean>
 <stddev>1.7e-2</stddev>
 <bias_mean>0.1</bias_mean>
 <bias_stddev>0.001</bias_stddev>
 </noise>
 </y>
 <z>
 <noise type="gaussian">
 <mean>0.0</mean>
 <stddev>1.7e-2</stddev>
 <bias_mean>0.1</bias_mean>
 <bias_stddev>0.001</bias_stddev>
 </noise>
 </z>
 </linear_acceleration>
 </imu>
 </sensor>
</gazebo>

bridge 中增加 topic 转换

- ros_topic_name: "imu"
gz_topic_name: "imu"
ros_type_name: "sensor_msgs/msg/Imu"
gz_type_name: "gz.msgs.IMU"
direction: "GZ_TO_ROS"

深度相机仿真

<!-- 深度相机 -->
<gazebo reference="camera_link">
 <sensor name="grbd_camera" type="rgbd_camera">
 <topic>camera</topic>
 <update_rate>1</update_rate>
 <always_on>1</always_on>
 <visualize>true</visualize>
 <gz_frame_id>camera_link</gz_frame_id>
 <camera name="head">
 <optical_frame_id>camera_link</optical_frame_id>
 <!-- 水平视野角度 -->
 <horizontal_fov>1.3</horizontal_fov>
 <image>
 <!-- 图像宽度 -->
 <width>320</width>
 <!-- 图像高度 -->
 <height>240</height>
 </image>
 <clip>
 <!-- 近裁剪平面 -->
 <near>0.02</near>
 <!-- 远裁剪平面 -->
 <far>12</far>
 </clip>
 </camera>
 </sensor>
</gazebo>

bridge 中增加对应的转换

- ros_topic_name: "/camera/depth_image"
gz_topic_name: "/camera/depth_image"
ros_type_name: "sensor_msgs/msg/Image"
gz_type_name: "gz.msgs.Image"
direction: "GZ_TO_ROS"
- ros_topic_name: "/camera/image"
gz_topic_name: "/camera/image"
ros_type_name: "sensor_msgs/msg/Image"
gz_type_name: "gz.msgs.Image"
direction: "GZ_TO_ROS"
- ros_topic_name: "/camera/points"
gz_topic_name: "/camera/points"
ros_type_name: "sensor_msgs/msg/PointCloud2"
gz_type_name: "gz.msgs.PointCloudPacked"
direction: "GZ_TO_ROS"

ROS2坐标变换及TF2的使用

Mon, 29 Dec 2025 14:07:00 +0800

坐标变换的含义

假设有一个小车，小车上有一个雷达，右边是一堵墙，已知小车的坐标原点 base_link 与雷达坐标原点 lase_laser 横竖距离 10cm , 既雷达相对小车的坐标为 (10, 0, 10), 雷达探测到墙体的坐标为 （60, 0, 0） ，求墙体相对小车的坐标？

上面这个问题实际就是坐标变换，针对这个问题，我们可以手算出来为 (70, 0, 10) , 但是在实际的场景下需要一个工具能够快速算出对应的坐标。 ROS2 提供了 TF2 这个工具包来实现这个功能。

ROS2 TF2 命令行工具

发布 base_link 到 base_laser 的坐标

`1`	`ros2 run tf2_ros static_transform_publisher --x 10 --y 0 --z 10 --roll 0 --pitch 0 --yaw 0 --frame-id base_link --child-frame-id base_laser`

roll pitch yaw 为旋转角度，既 tf2 支持旋转的变换

发布墙体 wall_point 相对 base_laser 的坐标

`1`	`ros2 run tf2_ros static_transform_publisher --x 60 --y 0 --z 0 --roll 0 --pitch 0 --yaw 0 --frame-id base_laser --child-frame-id wall_point`

然后就可以查询 wall_point 相对 base_laser 的坐标
1

ros2 run tf2_ros tf2_echo base_link wall_point
查询到的结果和我们手算的一致：
1

Translation: [70.000, 0.000, 10.000]

工具的原理就是向 /tf_static 这个 topic 发布消息，可以查看详细信息 ros2 topic info /tf_static

1
2
3

Type: tf2_msgs/msg/TFMessage
Publisher count: 2
Subscription count: 0

ros2 interface show tf2_msgs/msg/TFMessage 查看 topic 信息

geometry_msgs/TransformStamped[] transforms
 #
 #
 std_msgs/Header header
 builtin_interfaces/Time stamp
 int32 sec
 uint32 nanosec
 string frame_id
 string child_frame_id
 Transform transform
 Vector3 translation
 float64 x
 float64 y
 float64 z
 Quaternion rotation
 float64 x 0
 float64 y 0
 float64 z 0
 float64 w 1

/tf_static 用来发布固定的坐标关系，既静态发布，比如雷达固定在小车上，相对小车的坐标不会改变
/tf 用来发布动态坐标关系，既动态发布，比如小车雷达一直在运动，相对墙体的坐标一直在变化

C++ 应用坐标变化

创建一个功能包

`1`	`ros2 pkg create cpp_tf_demo --destination-directory=src --build-type=ament_cmake --license Apache-2.0 --dependencies rclcpp tf2_ros geometry_msgs tf2_geometry_msgs`

发布静态 TF

通过静态 TF 发布 base_link 和 base_laser 之间的坐标关系

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/executors.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/static_transform_broadcaster.hpp"

class StaticTf : public rclcpp::Node {

public:
 StaticTf() : Node("StaticTf") {
 broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

 // publish
 geometry_msgs::msg::TransformStamped transform;
 transform.header.stamp = get_clock()->now(); // 时间戳
 transform.header.frame_id = "base_link"; // 父坐标系名称
 transform.child_frame_id = "base_laser"; // 子坐标系名称
 // 坐标 (10, 0, 10)
 transform.transform.translation.x = 10;
 transform.transform.translation.y = 0;
 transform.transform.translation.z = 10;

 // 旋转角度为 0
 tf2::Quaternion q;
 q.setRPY(0, 0, 0);
 transform.transform.rotation = tf2::toMsg(q);

 // 发布消息
 broadcaster_->sendTransform(transform);
 }

private:
 std::shared_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;
};

int main(int argc, char* argv[]) {

 rclcpp::init(argc, argv);

 rclcpp::spin(std::make_shared<StaticTf>());

 rclcpp::shutdown();

 return 0;
}

发布动态 TF

通过动态 TF 发布 base_laser 和 wall_point 之间的坐标关系， base_laser 每秒移动 10cm

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/executors.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/transform_broadcaster.hpp"

using namespace std::chrono_literals;

class DynamicTf : public rclcpp::Node {

public:
 DynamicTf() : Node("DynamicTf"), count_(0) {
 broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
 timer_ = create_wall_timer(1s, [this]{
 // publish
 geometry_msgs::msg::TransformStamped transform;
 transform.header.stamp = get_clock()->now(); // 时间戳
 transform.header.frame_id = "base_laser"; // 父坐标系名称
 transform.child_frame_id = "wall_point"; // 子坐标系名称
 transform.transform.translation.x = 60 + count_ * 10; // 每秒增加 10 cn
 transform.transform.translation.y = 0;
 transform.transform.translation.z = 0;

 // 旋转角度为 0
 tf2::Quaternion q;
 q.setRPY(0, 0, 0);
 transform.transform.rotation = tf2::toMsg(q);

 // 发布消息
 broadcaster_->sendTransform(transform);

 count_++;
 });

 }

private:
 int count_;
 std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
 rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char* argv[]) {

 rclcpp::init(argc, argv);

 rclcpp::spin(std::make_shared<DynamicTf>());

 rclcpp::shutdown();

 return 0;
}

定时查询指定坐标

现在可以查询 wall_point 相对 base_link 的坐标

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/executors.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.hpp"

using namespace std::chrono_literals;

class ListenTf : public rclcpp::Node {

public:
 ListenTf() : Node("ListenTf") {
 buffer_ = std::make_unique<tf2_ros::Buffer>(get_clock());
 listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_);

 timer_ = create_wall_timer(1s, std::bind(&ListenTf::listen, this));
 }

 void listen() {
 try {
 // 查询坐标关系
 const auto transform = buffer_->lookupTransform("base_link", "wall_point", tf2::TimePointZero);
 auto translation = transform.transform.translation;
 RCLCPP_INFO(this->get_logger(), "transform: (%f, %f, %f)" , translation.x, translation.y, translation.z);
 } catch (const tf2::TransformException& e) {
 RCLCPP_WARN(this->get_logger(), "Could not transform: %s" , e.what());
 }

 }

private:
 std::shared_ptr<tf2_ros::TransformListener> listener_;
 rclcpp::TimerBase::SharedPtr timer_;
 std::unique_ptr<tf2_ros::Buffer> buffer_;
};

int main(int argc, char* argv[]) {

 rclcpp::init(argc, argv);

 rclcpp::spin(std::make_shared<ListenTf>());

 rclcpp::shutdown();

 return 0;
}

通过 launch 机制同时启动三个 Node ，Listen 节点就会输出转换的坐标

[cpp_listen_tf-3] [INFO] [1767545900.414608372] [ListenTf]: transform: (80.000000, 0.000000, 10.000000)
[cpp_listen_tf-3] [INFO] [1767545901.464255733] [ListenTf]: transform: (90.000000, 0.000000, 10.000000)
[cpp_listen_tf-3] [INFO] [1767545902.464750606] [ListenTf]: transform: (100.000000, 0.000000, 10.000000)
[cpp_listen_tf-3] [INFO] [1767545903.429858553] [ListenTf]: transform: (110.000000, 0.000000, 10.000000)

ROS2 Launch 脚本的使用

Sun, 28 Dec 2025 22:18:00 +0800

一个实际的机器人一般包括 N 个节点，实际运行调试时每个节点开一个终端去启动效率太低，也不方便管理。 Launch 脚本就是用于启动多个节点，支持不同节点使用不同的配置及参数，方便在不同环境下启动配置不同的机器人程序。

Launch 启动文件支持 XML ， YAML ， PYTHON 格式

Python 版本 Launch

我们创建一个包用来保存 launch 文件

`1`	`ros2 pkg create launch_demo --destination-directory src --build-type ament_cmake --license Apache-2.0`

在包根目录下创建一个 launch 目录，里面创建一个 demo_launch.py 文件

# -*- coding: utf-8 -*-
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
 """函数名字不能修改
 """
 # 定义一个节点动作，表示启动这个节点
 topic_publish_node = Node(
 package="cpp_topic_publish_demo", # 包名
 executable="cpp_topic_publish_demo", # 可执行文件名
 output="screen" # 日志输出位置
 )

 topic_subscribe_node = Node(
 package="cpp_topic_subscribe_demo",
 executable="cpp_topic_subscribe_demo",
 output="screen"
 )

 # 返回启动描述，是一个 launch_ros.actions 类型的数组
 return LaunchDescription([topic_publish_node, topic_subscribe_node])

在 CMakeLists.txt 中将 launch 目录安装到 share 目录

1
2
3

install(DIRECTORY launch
 DESTINATION share/${PROJECT_NAME}
)

然后使用 ros2 launch launch_demo demo_launch.py 启动，就可以看到两个节点启动了，并且将日志输出到屏幕

YAML 格式 Launch 文件

%YAML 1.2
---
launch:
- node:
pkg: "cpp_topic_publish_demo"
exec: "cpp_topic_publish_demo"
output: "screen"
- node:
pkg: "cpp_topic_subscribe_demo"
exec: "cpp_topic_subscribe_demo"
output: "screen"

启动命令： ros2 launch launch_demo demo_launch.yaml

XML 格式 Launch 文件

<?xml version="1.0" encoding="UTF-8"?>
<launch>
 <node pkg="cpp_topic_publish_demo" exec="cpp_topic_publish_demo" output="screen"/>
 <node pkg="cpp_topic_subscribe_demo" exec="cpp_topic_subscribe_demo" output="screen"/>
</launch>

启动命令： ros2 launch launch_demo demo_launch.xml

Launch 传递节点参数

python 文件创建节点时可以使用 arguments , ros_arguments 或者 parameters 几种方式

int_param_node = Node(
 package="cpp_param_declare_demo",
 executable="cpp_param_declare_demo",
 output="screen",
 # arguments=["--ros-args", "-p", "int_param:=1234578"]
 # ros_arguments=["-p", "int_param:=222333"],

 parameters=[{"int_param": 9999999}]
)

xml 和 yaml 使用 args 或 ros_args 字段

python 文件从命令行动态传入参数可以声明一个参数动作，解析后传给节点

1
2

from launch import substitutions
from launch.actions import DeclareLaunchArgument


int_param_arg = DeclareLaunchArgument("int_param", default_value="777777")

int_param_node = Node(
 package="cpp_param_declare_demo",
 executable="cpp_param_declare_demo",
 output="screen",
 # arguments=["--ros-args", "-p", "int_param:=1234578"]
 # ros_arguments=["-p", "int_param:=222333"],
 parameters=[{"int_param": substitutions.LaunchConfiguration("int_param")}]
)

启动时就可以传入参数

`1`	`ros2 launch launch_demo demo_launch.py int_param:=555555555`

ROS2 参数

Sun, 28 Dec 2025 21:38:00 +0800

ROS2 节点可以声明多个参数，其它节点可以查看或修改参数，参数的设置和获取实际是通过服务实现的，只是 ROS2 提供了针对参数提供了更加方便的 API

C++ 节点声明参数

创建一个功能包

`1`	`ros2 pkg create cpp_param_declare_demo --destination-directory src --build-type ament_cmake --dependencies rclcpp --license Apache-2.0`

声明参数

#include "rclcpp/executors.hpp"

class ParamNode : public rclcpp::Node {
public:
 ParamNode() : Node("param_node") {
 // 声明参数
 declare_parameter("int_param", 10086);
 // 获取参数默认值
 get_parameter("int_param", int_param_);

 RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "int param default value is : %ld", int_param_);
 }

private:
 int64_t int_param_;
};

int main(int argc, char* argv[]) {
 rclcpp::init(argc, argv);

 auto node = std::make_shared<ParamNode>();
 rclcpp::spin(node);

 rclcpp::shutdown();
 return 0;
}

可以通过 ros2 param get param_node int_param 命令获取节点的参数

监听节点参数被修改的事件

通过节点的 add_on_set_parameters_callback API 添加参数改变的回调函数

auto paramCb = [this](const std::vector<rclcpp::Parameter>& parameters) -> auto {
 rcl_interfaces::msg::SetParametersResult result;
 result.successful = true;
 for (const auto& parameter : parameters) {
 int_param_ = parameter.as_int();
 RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "param changed to : %ld", int_param_);
 }
 return result;
};
handle_ = add_on_set_parameters_callback(paramCb);

使用 ros2 param set param_node int_param 11111 可以看到参数修改为指定的值，输出下面日志：

`1`	`[INFO] [1766931105.659879840] [rclcpp]: param changed to : 111111`

获取和设置其它节点的参数

节点自动提供了以下服务用于操作参数：

/param_node/describe_parameters: rcl_interfaces/srv/DescribeParameters
/param_node/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
/param_node/get_parameters: rcl_interfaces/srv/GetParameters
/param_node/get_type_description: type_description_interfaces/srv/GetTypeDescription
/param_node/list_parameters: rcl_interfaces/srv/ListParameters
/param_node/set_parameters: rcl_interfaces/srv/SetParameters
/param_node/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically

所以设置或获取其它节点可以用调用服务的方式，具体参考 [[Denote query for ‘20251228T140129’]]

启动时修改参数

使用 ros2 run 启动节点时可以设置参数的值

`1`	`ros2 run cpp_param_declare_demo cpp_param_declare_demo --ros-args -p int_param:=12345`

启动后可以看到参数为设置的值，而不是代码中的默认值

`1`	`[INFO] [1766934109.576643905] [rclcpp]: int param default value is : 12345`

ROS2 服务

Sun, 28 Dec 2025 14:01:00 +0800

ROS2 中的服务就类似于 RPC 调用，每个节点提供服务，其它节点可以调用服务并获取返回值

服务查看

查看节点提供的服务

`1`	`ros2 node info /turtlesim`

Service Servers:
 /clear: std_srvs/srv/Empty
 /kill: turtlesim/srv/Kill
 /reset: std_srvs/srv/Empty
 /spawn: turtlesim/srv/Spawn
 /turtle1/set_pen: turtlesim/srv/SetPen
 /turtle1/teleport_absolute: turtlesim/srv/TeleportAbsolute
 /turtle1/teleport_relative: turtlesim/srv/TeleportRelative
 /turtlesim/describe_parameters: rcl_interfaces/srv/DescribeParameters
 /turtlesim/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
 /turtlesim/get_parameters: rcl_interfaces/srv/GetParameters
 /turtlesim/get_type_description: type_description_interfaces/srv/GetTypeDescription
 /turtlesim/list_parameters: rcl_interfaces/srv/ListParameters
 /turtlesim/set_parameters: rcl_interfaces/srv/SetParameters
 /turtlesim/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically

服务和话题一样，包括服务名和服务参数定义，可以查看服务的详细定义
1

ros2 service info /spawn
1 2 3

Type: turtlesim/srv/Spawn Clients count: 0 Services count: 1

查看服务类型定义

`1`	`ros2 interface show turtlesim/srv/Spawn`

float32 x
float32 y
float32 theta
string name # Optional. A unique name will be created and returned if this is empty
---
string name

这个定义表示输入参数为 x, y, theta, name ，其中 name 是可选的，返回值为 name

ros2 命令行调用服务

`1`	`ros2 service call /spawn turtlesim/srv/Spawn "{x: 1.0, y: 1.0, theta: 0}"`

调用后的结果

waiting for service to become available...
requester: making request: turtlesim.srv.Spawn_Request(x=1.0, y=1.0, theta=0.0, name='')

response:
turtlesim.srv.Spawn_Response(name='turtle2')

定义服务描述文件

在 demo_interfaces 下的 srv 目录自定义一个服务描述文件，名称叫 AddInt.srv
1 2 3 4

int64 a int64 b --- int64 sum

CMakeLists.txt 中添加编译

rosidl_generate_interfaces(${PROJECT_NAME}
 "msg/Demo.msg"
 "srv/AddInt.srv"
 DEPENDENCIES std_msgs
)

使用 colcon build 编译

C++ 节点实现一个服务

创建一个功能包

`1`	`ros2 pkg create cpp_server_demo --destination-directory src --build-type ament_cmake --dependencies rclcpp demo_interfaces --license Apache-2.0`

写服务文件

#include "rclcpp/executors.hpp"
#include "demo_interfaces/srv/add_int.hpp"

void AddInt(demo_interfaces::srv::AddInt_Request::SharedPtr request,
 demo_interfaces::srv::AddInt_Response::SharedPtr response) {
 response->sum = request->a + request->b;
 RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld",
 request->a, request->b);
 RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);
}

int main(int argc, char* argv[]) {
 rclcpp::init(argc, argv);

 auto node = rclcpp::Node::make_shared("add_int_server");
 auto service = node->create_service<demo_interfaces::srv::AddInt>("add_int", AddInt);

 RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add ints.");

 rclcpp::spin(node);

 rclcpp::shutdown();
 return 0;
}

编译完成后运行就可以看到对应服务了

C++ 实现一个节点调用服务

创建一个功能包

1
2

ros2 pkg create cpp_client_demo --destination-directory src --build-type ament_cmake --dependencies rclcpp demo
_interfaces --license Apache-2.0

客户端代码

#include "rclcpp/executors.hpp"
#include "demo_interfaces/srv/add_int.hpp"

using namespace std::chrono_literals;

int main(int argc, char* argv[]) {
 rclcpp::init(argc, argv);

 auto node = rclcpp::Node::make_shared("add_client");
 auto client = node->create_client<demo_interfaces::srv::AddInt>("add_int");
 auto request = std::make_shared<demo_interfaces::srv::AddInt_Request>();
 request->a = 100;
 request->b = 200;

 while (!client->wait_for_service(1s)) {
 if (!rclcpp::ok()) {
 RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
 return 0;
 }
 RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
 }

 auto result = client->async_send_request(request);
 if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
 RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sum: %ld", result.get()->sum);
 } else {
 RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_two_ints");
 }
 rclcpp::shutdown();
 return 0;
}

ROS2 话题

Sat, 27 Dec 2025 15:47:00 +0800

ROS2 中节点通过发布和订阅话题来传递信息，一个节点可以作为发布者发布信息，也可以同时作为订阅者订阅其它节点发布的话题。

话题由主题和消息类型组成：

主题是一个字符串
消息类型是传输数据的格式

话题查看

启动小海龟节点
1

ros2 run turtlesim turtlesim_node

查看小海龟节点信息

`1`	`ros2 node info /turtlesim`

可以看到 node 的相关信息

/turtlesim
 Subscribers:
 /parameter_events: rcl_interfaces/msg/ParameterEvent
 /turtle1/cmd_vel: geometry_msgs/msg/Twist
 Publishers:
 /parameter_events: rcl_interfaces/msg/ParameterEvent
 /rosout: rcl_interfaces/msg/Log
 /turtle1/color_sensor: turtlesim/msg/Color
 /turtle1/pose: turtlesim/msg/Pose

Subscribers 列出节点订阅了哪些话题，前面为主题，后面为话题消息类型，Publishers 列出发布哪些话题

查看一个话题信息

`1`	`ros2 topic info /turtle1/cmd_vel`

1
2
3

Type: geometry_msgs/msg/Twist
Publisher count: 0
Subscription count: 1

显示出数据类型，发布者数量，被订阅的数量

查看消息类型的详细定义

`1`	`ros2 interface show geometry_msgs/msg/Twist`

# This expresses velocity in free space broken into its linear and angular parts.

Vector3 linear
 float64 x
 float64 y
 float64 z
Vector3 angular
 float64 x
 float64 y
 float64 z

可以看到消息类型为两个数组，第一个为线速度，第二个为角速度

使用 ros2 命令发布话题消息

`1`	`ros2 topic pub /turtle1/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 1.0, y: 0.0}, angular: {z: 1.0}}"`

C++ 发布话题

用 C++ 实现每秒发布一个字符串

创建功能包

`1`	`ros2 pkg create cpp_topic_publish_demo --destination-directory src --build-type ament_cmake --dependencies rclcpp std_msgs --license Apache-2.0`

新建节点，每秒发布字符串

#include "rclcpp/node.hpp"
#include "rclcpp/executors.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class TopicPublishDemo : public rclcpp::Node {
public:
 TopicPublishDemo(): Node("topic_publish_demo_node"), count_(0) {
 publisher_ = create_publisher<std_msgs::msg::String>("topic_string", 10);
 auto timer_callback = [this]() {
 auto msg = std_msgs::msg::String();
 msg.data = "Hello, Now is " + std::to_string(count_++);
 RCLCPP_INFO(get_logger(), "Publishing: %s", msg.data.c_str());
 publisher_->publish(msg);
 };
 timer_ = create_wall_timer(1s, timer_callback);

 }


private:
 rclcpp::TimerBase::SharedPtr timer_;
 rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
 size_t count_;
};


int main(int argc, char* argv[]) {
 rclcpp::init(argc, argv);

 rclcpp::spin(std::make_shared<TopicPublishDemo>());

 rclcpp::shutdown();
 return 0;
}

C++ 订阅话题

创建功能包

1
2

ros2 pkg create cpp_topic_subscribe_demo --destination-directory src --build-type ament_cmake --dependencies rc
lcpp std_msgs --license Apache-2.0

节点订阅代码

#include "rclcpp/node.hpp"
#include "rclcpp/executors.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class TopicSubscribeDemo : public rclcpp::Node {
public:
 TopicSubscribeDemo(): Node("topic_subscribe_demo_node") {
 auto topic_callback = [this](std_msgs::msg::String::UniquePtr msg) {
 RCLCPP_INFO(get_logger(), "I received: %s", msg->data.c_str());
 };
 subscriber_ = create_subscription<std_msgs::msg::String>("topic_string", 10, topic_callback);
 }

private:
 rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
};


int main(int argc, char* argv[]) {
 rclcpp::init(argc, argv);

 rclcpp::spin(std::make_shared<TopicSubscribeDemo>());

 rclcpp::shutdown();
 return 0;
}

自定义消息类型

创建一个功能包，用来定义消息类型

`1`	`ros2 pkg create --destination-directory=src --build-type=ament_cmake --license Apache-2.0 --dependencies std_msgs demo_interfaces`

在包根目录下新建一个 msg 目录，里面创建一个 Demo.msg 文件
1

std_msgs/String name

在 CMakeLists.txt 中添加以下代码

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
 "msg/Demo.msg"
 DEPENDENCIES std_msgs
)

在 package.xml 中添加以下代码

1
2
3

<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

使用 colcon build 编译

使用自定义消息

将前面的 publish 和 subscribes 改成使用自定义消息

在 package.xml 中添加依赖
1

<depend>demo_interfaces</depend>

在 CMakeLists.txt 中添加依赖

1
2

find_package(demo_interfaces)
ament_target_dependencies(${PROJECT_NAME} rclcpp std_msgs demo_interfaces)

将原来的 std_msgs::msg::String 改为 demo_interface::msg::Demo

#include "rclcpp/node.hpp"
#include "rclcpp/executors.hpp"
#include "demo_interfaces/msg/demo.hpp"

using namespace std::chrono_literals;

class TopicSubscribeDemo : public rclcpp::Node {
public:
 TopicSubscribeDemo(): Node("topic_subscribe_demo_node") {
 auto topic_callback = [this](demo_interfaces::msg::Demo::UniquePtr msg) {
 RCLCPP_INFO(get_logger(), "I received: %s", msg->name.data.c_str());
 };
 subscriber_ = create_subscription<demo_interfaces::msg::Demo>("topic_string", 10, topic_callback);
 }

private:
 rclcpp::Subscription<demo_interfaces::msg::Demo>::SharedPtr subscriber_;
};


int main(int argc, char* argv[]) {
 rclcpp::init(argc, argv);

 rclcpp::spin(std::make_shared<TopicSubscribeDemo>());

 rclcpp::shutdown();
 return 0;
}

ROS2 节点

Fri, 26 Dec 2025 19:41:00 +0800

ROS2 中的节点是一个通过 ROS2 和其它节点通信的实体。

Python 实现节点

# -*- coding: utf-8 -*-

import rclpy
from rclpy.node import Node


def main():
 # 环境初始化
 rclpy.init()

 # 创建一个新节点
 node = Node("python_node")

 # 调度节点运行
 rclpy.spin(node)

 # 环境去初始化
 rclpy.shutdown()


if __name__ == '__main__':
 main()

C++ 实现节点

#include "rclcpp/node.hpp"
#include "rclcpp/executors.hpp"

int main(int argc, char* argv[]) {
 rclcpp::init(argc, argv);

 auto node = std::make_shared<rclcpp::Node>("cpp_node");

 RCLCPP_INFO(node->get_logger(), "Hello, C++ Node");

 rclcpp::spin(node);

 rclcpp::shutdown();

 return 0;
}

节点包管理

一个机器人程序一般会有很多节点，ROS2 使用功能包管理节点，功能包的优点：

方便管理运行
功能节点复用

Python 节点包

创建 python 节点包

`1`	`ros2 pkg create python_pkg_demo --build-type ament_python --license Apache-2.0`

创建后包的目录结构：

- LICENSE
- package.xml
- python_pkg_demo
 - __init__.py
- resource
 - python_pkg_demo
- setup.cfg
- setup.py
- test
 - test_copyright.py
 - test_flake8.py
 - test_pep257.py

4 directories, 9 files

在 python_pkg_demo 目录下新建 python_node.py ，写入上面节点创建的代码

在 setup.py 里加上可执行入口点

entry_points={
 'console_scripts': [
 'python_node = python_pkg_demo.python_node:main'
 ],

在 package.xml 中声明依赖
1

<depend>rclpy</depend>
使用 colcon build 构建

运行

1
2

source install/setup.bash
ros2 run python_pkg_demo python_node

CPP 节点包

创建 cpp 包

`1`	`ros2 pkg create cpp_pkg_demo --build-type ament_cmake --license Apache-2.0`

创建后的文件目录：

- CMakeLists.txt
- include
 - cpp_pkg_demo
- LICENSE
- package.xml
- src

修改 CMakeLists.txt ，添加下面几行

find_package(rclcpp REQUIRED)

add_executable(cpp_node src/cpp_node)

ament_target_dependencies(cpp_node rclcpp)

install(TARGETS cpp_node DESTINATION lib/${PROJECT_NAME})

在 package.xml 添加依赖信息
1

<depend>rclcpp</depend>
colcon build 命令构建包

运行

1
2

source install/setup.bash
ros2 run cpp_pkg_demo cpp_node

工作空间

完整的机器人项目由许多不同的功能模块组成，ROS2 使用工作空间（Workspace）这个概率来组织不同的功能包

创建工作空间

一个工作空间实际上是一个目录，创建一个工作空间就是新建一个目录

创建一个 ros2-code-recipes 存放学习 ros2 的代码

`1`	`mkdir ros2-code-recipes`

创建包

功能包的代码一般放到工作空间的 src 目录下，可以将前面两个包放到 src 目录下

构建包

运行 colcon build 构建所有包

运行 colcon build --packages-select cpp_pkg_demo 构建指定的包

运行包

和单个包运行一样

1
2

source install/setup.bash
ros2 run python_pkg_demo python_node

ROS2 节点相关的命令

列出所有节点
1

ros2 node list
查看某一个节点的信息
1

ros2 node info /cpp_node

WSL2 安装 ROS2

Thu, 25 Dec 2025 23:29:00 +0800

WSL2 使用的系统是 Ubuntu 24.04 ，所以安装的 ROS2 版本对应为 jazzy

UTF8 环境设置

1
2
3

sudo apt update && sudo apt install locales
sudo locale-gen zh_CN zh_CN.UTF-8
sudo update-locale LC_ALL=zh_CN.UTF-8 LANG=zh_CN.UTF-8

激活软件仓库

1
2

sudo apt install software-properties-common
sudo add-apt-repository universe

使用国内源

# 安装证书
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg

echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://mirrors.ustc.edu.cn/ros2/ubuntu $(lsb_release -sc) main" | sudo tee /etc/apt/sources.list.d/ros2.sources > /dev/null

安装 ROS2 开发工具

`1`	`sudo apt update && sudo apt install ros-dev-tools`

安装 ROS2

`1`	`sudo apt update && sudo apt upgrade && sudo apt install ros-jazzy-desktop`

运行例子

发布者

1
2

source /opt/ros/jazzy/setup.bash
ros2 run demo_nodes_cpp talker

发布者打印的消息

1
2
3

[INFO] [1766678416.648218277] [talker]: Publishing: 'Hello World: 1'
[INFO] [1766678417.642466793] [talker]: Publishing: 'Hello World: 2'
[INFO] [1766678418.638927732] [talker]: Publishing: 'Hello World: 3'

订阅者

1
2

source /opt/ros/jazzy/setup.bash
ros2 run demo_nodes_py listener

订阅者打印的日志

1
2
3

[INFO] [1766678417.643847384] [listener]: I heard: [Hello World: 1]
[INFO] [1766678418.640908725] [listener]: I heard: [Hello World: 2]
[INFO] [1766678419.637684973] [listener]: I heard: [Hello World: 3]

Windows Terminal 标签

添加一个新的 Windows Terminal 标签，方便快速打开 ROS2 环境，命令行如下：

`1`	`C:\Windows\system32\wsl.exe --distribution-id {c2779a6e-4ba7-4f69-a981-bbdf9f4f6325} --cd ~ -e bash -c -l "source /opt/ros/jazzy/setup.bash && exec bash -l"`

Gazebo 安装

初学时，Gazebo 安装与 ROS 对应版本更加稳定，jazzy 推荐安装 Gazebo harmonic

sudo curl https://packages.osrfoundation.org/gazebo.gpg --output /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] https://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
sudo apt-get update
sudo apt-get install gz-harmonic

安装对应的 ROS 包

`1`	`sudo apt install ros-jazzy-ros-gz`

运行 gz sim 启动，设置 GALLIUM_DRIVER=d3d12 环境变量，让程序可以使用 GPU

SQL 基础

Fri, 12 Dec 2025 16:21:00 +0800

查询

查询步骤：

选择要查询的库 USE db
使用 SELECT 语句

基本语句：

SELECT 查询的列
FROM 查询的表
WHERE 筛选条件
ORDER BY 排序条件

1
2

USE sql_store;
SELECT customer_id, first_name FROM customers WHERE customer_id = 1 ORDER BY first_name;

customer_id	first_name
1	Babara

SELECT 子句

选择查询的列， * 表示查询所有列

1
2

USE sql_store
SELECT * FROM customers WHERE customer_id = 2

customer_id	first_name	last_name	birth_date	phone	address	city	state	points
2	Ines	Brushfield	1986-04-13	804-427-9456	14187 Commercial Trail	Hampton	VA	947

SELECT 可以对列进行算数运算

1
2

USE sql_store
SELECT points, points * 10 + 100 FROM customers WHERE customer_id = 3

points	points * 10 + 100
2967	29770

列可以使用别名，使用 AS 关键字，有空格的列名使用引号包裹

1
2

USE sql_store
SELECT points, points * 0.9 AS "points discount" FROM customers WHERE customer_id = 3

points	points discount
2967	2670.3

使用 DISTINCT 关键字可以删除列中的重复项

1
2

USE sql_store
SELECT DISTINCT points, points * 0.9 AS "points discount" FROM customers WHERE customer_id = 5

points	points discount
3675	3307.5

WHERE 子句

条件筛选

1
2

USE sql_store
SELECT * FROM customers WHERE points > 3000

customer_id	first_name	last_name	birth_date	phone	address	city	state	points
5	Clemmie	Betchley	1973-11-07	NULL	5 Spohn Circle	Arlington	TX	3675
6	Elka	Twiddell	1991-09-04	312-480-8498	7 Manley Drive	Chicago	IL	3073

组合条件，支持 AND , OR , NOT ，多个逻辑运算符时， AND 优先级更高，最好用括号显示表达优先级

1
2

USE sql_store
SELECT * FROM customers WHERE points > 3000 AND birth_date >= "1991-01-01"

customer_id	first_name	last_name	birth_date	phone	address	city	state	points
6	Elka	Twiddell	1991-09-04	312-480-8498	7 Manley Drive	Chicago	IL	3073

1
2

USE sql_store
SELECT * FROM order_items WHERE order_id = 6 AND (quantity * unit_price) > 30

order_id	product_id	quantity	unit_price
6	1	4	8.65

IN 运算符判断是否在集合中

1
2

USE sql_store
SELECT * FROM customers WHERE state in ("VA", "FL")

customer_id	first_name	last_name	birth_date	phone	address	city	state	points
2	Ines	Brushfield	1986-04-13	804-427-9456	14187 Commercial Trail	Hampton	VA	947
4	Ambur	Roseburgh	1974-04-14	407-231-8017	30 Arapahoe Terrace	Orlando	FL	457
8	Thacher	Naseby	1993-07-17	941-527-3977	538 Mosinee Center	Sarasota	FL	205

BETWEEN 运算符判断是否在闭区间中

1
2

USE sql_store
SELECT * FROM customers WHERE points BETWEEN 2000 AND 3000

customer_id	first_name	last_name	birth_date	phone	address	city	state	points
1	Babara	MacCaffrey	1986-03-28	781-932-9754	0 Sage Terrace	Waltham	MA	2273
3	Freddi	Boagey	1985-02-07	719-724-7869	251 Springs Junction	Colorado Springs	CO	2967

LIKE 运算符进行字符串模式匹配， % 匹配任意字符， _ 匹配单个任意字符

1
2

USE sql_store
SELECT * FROM customers WHERE last_name LIKE 'B%'

customer_id	first_name	last_name	birth_date	phone	address	city	state	points
2	Ines	Brushfield	1986-04-13	804-427-9456	14187 Commercial Trail	Hampton	VA	947
3	Freddi	Boagey	1985-02-07	719-724-7869	251 Springs Junction	Colorado Springs	CO	2967
5	Clemmie	Betchley	1973-11-07	NULL	5 Spohn Circle	Arlington	TX	3675

REGEXP 运算符，进行正则匹配，匹配相关可以看正则表达式

1
2

USE sql_store
SELECT * FROM customers WHERE last_name REGEXP "field"

customer_id	first_name	last_name	birth_date	phone	address	city	state	points
2	Ines	Brushfield	1986-04-13	804-427-9456	14187 Commercial Trail	Hampton	VA	947

IS NULL 运算符，判断是否为 NULL

1
2

USE sql_store
SELECT * FROM customers WHERE phone IS NULL

customer_id	first_name	last_name	birth_date	phone	address	city	state	points
5	Clemmie	Betchley	1973-11-07	NULL	5 Spohn Circle	Arlington	TX	3675

ORDER BY 子句

ORDER BY 子句指定一列或多列排序，每列后面可跟 DESC 表示降序

1
2

USE sql_store
SELECT * FROM customers ORDER BY state DESC, last_name

customer_id	first_name	last_name	birth_date	phone	address	city	state	points
2	Ines	Brushfield	1986-04-13	804-427-9456	14187 Commercial Trail	Hampton	VA	947
5	Clemmie	Betchley	1973-11-07	NULL	5 Spohn Circle	Arlington	TX	3675
7	Ilene	Dowson	1964-08-30	615-641-4759	50 Lillian Crossing	Nashville	TN	1672
1	Babara	MacCaffrey	1986-03-28	781-932-9754	0 Sage Terrace	Waltham	MA	2273
6	Elka	Twiddell	1991-09-04	312-480-8498	7 Manley Drive	Chicago	IL	3073
10	Levy	Mynett	1969-10-13	404-246-3370	68 Lawn Avenue	Atlanta	GA	796
4	Ambur	Roseburgh	1974-04-14	407-231-8017	30 Arapahoe Terrace	Orlando	FL	457
8	Thacher	Naseby	1993-07-17	941-527-3977	538 Mosinee Center	Sarasota	FL	205
3	Freddi	Boagey	1985-02-07	719-724-7869	251 Springs Junction	Colorado Springs	CO	2967
9	Romola	Rumgay	1992-05-23	559-181-3744	3520 Ohio Trail	Visalia	CA	1486

MySQL 支持 ORDER BY 使用不在 SELECT 里的列进行排序，支持别名列，列做算数运算

1
2

USE sql_store
SELECT *, unit_price * quantity as total_price FROM order_items WHERE order_id = 2 ORDER BY total_price DESC;

order_id	product_id	quantity	unit_price	total_price
2	1	2	9.10	18.20
2	4	4	1.66	6.64
2	6	2	2.94	5.88

LIMIT 子句

LIMIT 子句限制返回的行数

单个数字表示从第一行开始显示 n 行

1
2

USE sql_store
SELECT customer_id, first_name, last_name FROM customers LIMIT 3

customer_id	first_name	last_name
1	Babara	MacCaffrey
2	Ines	Brushfield
3	Freddi	Boagey

LIMIT 可以指定跳过多少行

1
2

USE sql_store
SELECT customer_id, first_name, last_name FROM customers LIMIT 6, 3

customer_id	first_name	last_name
7	Ilene	Dowson
8	Thacher	Naseby
9	Romola	Rumgay

连接

连接相关的原理参考数据库中的连接操作

两表内连接

JOIN 子句连接两个表， ON 表示连接条件

1
2

USE sql_store
SELECT order_id, first_name, last_name FROM orders JOIN customers ON orders.customer_id = customers.customer_id

order_id	first_name	last_name
4	Ines	Brushfield
7	Ines	Brushfield
5	Clemmie	Betchley
8	Clemmie	Betchley
1	Elka	Twiddell
10	Elka	Twiddell
2	Ilene	Dowson
3	Thacher	Naseby
6	Levy	Mynett
9	Levy	Mynett

给列加上数据库名称前缀就能跨数据库连接

1
2

USE sql_store
SELECT order_id, oi.product_id FROM order_items oi JOIN sql_inventory.products p ON oi.product_id = p.product_id LIMIT 2

order_id	product_id
2	1
6	1

自连接

表可以自连接，同一个表要取不同的别名

1
2

USE sql_hr
SELECT e.employee_id, e.reports_to, e.first_name , m.first_name as manager FROM employees e JOIN employees m ON e.reports_to = m.employee_id LIMIT 3

employee_id	reports_to	first_name	manager
33391	37270	D’arcy	Yovonnda
37851	37270	Sayer	Yovonnda
40448	37270	Mindy	Yovonnda

多表连接

使用多个 join 句子

USE sql_store;

SELECT order_id, o.customer_id, status, first_name, last_name
FROM orders o
JOIN customers c
 ON o.customer_id = c.customer_id
JOIN order_statuses os
 ON o.status = os.order_status_id
LIMIT 3

order_id	customer_id	status	first_name	last_name
1	6	1	Elka	Twiddell
2	7	2	Ilene	Dowson
3	8	1	Thacher	Naseby

复合连接条件

当表中有复合主键时，可以用复合连接条件确定具体的列

USE sql_store;

SELECT *
FROM order_items oi
JOIN order_item_notes oin
 ON oi.order_id = oin.order_id
 AND oi.product_id = oin.product_id

外连接

左外连接：除了满足连接条件的数据，还包括左表的所有数据右外连接：除了满足连接条件的数据，还包括右表的所有数据

USE sql_store;

SELECT
 c.customer_id,
 c.first_name,
 o.order_id
FROM customers c
LEFT JOIN orders o
 ON c.customer_id = o.customer_id
ORDER BY c.customer_id
LIMIT 3

customer_id	first_name	order_id
1	Babara	NULL
2	Ines	4
2	Ines	7

1 不满足连接条件，但是也在查询结果中

USE sql_store;

SELECT
 c.customer_id,
 c.first_name,
 o.order_id
FROM orders o
RIGHT JOIN customers c
 ON c.customer_id = o.customer_id
ORDER BY c.customer_id
LIMIT 3

customer_id	first_name	order_id
1	Babara	NULL
2	Ines	4
2	Ines	7

右外连接，换了查表顺序得到相同结果

外连接同样可以多表连接

USE sql_store;

SELECT
 c.customer_id,
 c.first_name,
 o.order_id,
 s.name AS shipper
FROM customers c
LEFT JOIN orders o
 ON c.customer_id = o.customer_id
LEFT JOIN shippers s
 ON o.shipper_id = s.shipper_id
ORDER BY c.customer_id
LIMIT 10

customer_id	first_name	order_id	shipper
1	Babara	NULL	NULL
2	Ines	4	NULL
2	Ines	7	Mraz, Renner and Nolan
3	Freddi	NULL	NULL
4	Ambur	NULL	NULL
5	Clemmie	5	Satterfield LLC
5	Clemmie	8	NULL
6	Elka	1	NULL
6	Elka	10	Schinner-Predovic
7	Ilene	2	Mraz, Renner and Nolan

外连接同样可以自连接

USE sql_hr;

SELECT
 e.employee_id,
 e.first_name,
 m.first_name as manager
FROM employees e
LEFT JOIN employees m
 ON e.reports_to = m.employee_id
LIMIT 3

employee_id	first_name	manager
33391	D’arcy	Yovonnda
37270	Yovonnda	NULL
37851	Sayer	Yovonnda

USING 子句

如果 JOIN 使用的连接条件列名相同，可以使用 USING 代替 ON

USE sql_store;

SELECT order_id, first_name, last_name
FROM orders
JOIN customers
 -- ON orders.customer_id = customers.customer_id
 USING (customer_id)
LIMIT 3

order_id	first_name	last_name
4	Ines	Brushfield
7	Ines	Brushfield
5	Clemmie	Betchley

自然连接

NATURE JOIN 不指定连接条件，由数据库自己决定

交叉连接

CROSS JOIN 两张表做笛卡尔积，适合列出所有的排列组合，比如有大中小的型号表和各种颜色的颜色表，查出型号和颜色的所有组合

联合

UNION 可以将多个 SELECT 查询结果合并到一张表

USE sql_store;

SELECT
 c.customer_id,
 c.first_name,
 c.points,
 "Bronze" as type
FROM customers c
WHERE c.points < 2000

UNION

SELECT
 c.customer_id,
 c.first_name,
 c.points,
 "Silver" as type
FROM customers c
WHERE c.points >= 2000 AND c.points < 3000

UNION

SELECT
 c.customer_id,
 c.first_name,
 c.points,
 "Gold" as type
FROM customers c
WHERE c.points >= 3000

ORDER BY first_name

customer_id	first_name	points	type
4	Ambur	457	Bronze
1	Babara	2273	Silver
5	Clemmie	3675	Gold
6	Elka	3073	Gold
3	Freddi	2967	Silver
7	Ilene	1672	Bronze
2	Ines	947	Bronze
10	Levy	796	Bronze
9	Romola	1486	Bronze
8	Thacher	205	Bronze

插入

插入单行

使用 INSERT INTO 语句插入单行

USE sql_store;

INSERT INTO customers(
 first_name,
 last_name,
 birth_date,
 address,
 city,
 state
)

VALUES (
 'xiao',
 'ming',
 '1990-01-01',
 'address',
 'Los',
 'CA'
)

插入多行

VALUES 后跟多个括号

USE sql_store;

INSERT INTO shippers
 (name)
VALUES
 ("name1"),
 ("name2"),
 ("name3")

多表插入

使用 LAST_INSERT_ID() 函数可以获取最后自增的 ID

USE sql_store;

INSERT INTO orders (customer_id, order_date, status)
VALUES (1, "2010-01-01", 1);

INSERT INTO order_items
VALUES
 (LAST_INSERT_ID(), 1, 1, 3.55),
 (LAST_INSERT_ID(), 2, 1, 5.55)

复制表

使用 CREATE TABLE name AS

USE sql_store;

CREATE TABLE orders_archive AS
SELECT * from orders

第二个 SELECT 称为子查询，也可以用在 INSERT 语句中，将查询到达数据插入 orders_archive 中

use sql_store;

INSERT INTO orders_archive
SELECT * FROM orders
WHERE order_date < '2019-01-01'

更新

使用 UPDATE 语句更新一行或多行数据

USE sql_invoicing;

UPDATE invoices
SET payment_total = 10, payment_date = '2020-02-02'
WHERE invoice_id = 1

多行数据时使用 WHERE 语句选出多行即可，比如 WHERE invoice_id IN (3, 4, 5)

同样可以用 SELECT 作为子查询条件

USE sql_invoicing;

UPDATE invoices
SET payment_total = 10, payment_date = '2020-02-02'
WHERE client_id IN (
SELECT client_id
FROM clients
WHERE name = 'Myworks'
)

删除

使用 DELETE FROM 语句删除表中的数据，用 WHERE 选择要删除的行

USE sql_invoicing;

DELETE FROM invoices
WHERE invoice_id = 1

同样可以使用子查询

USE sql_invoicing;

DELETE FROM invoices
WHERE client_id IN (
SELECT client_id
FROM clients
WHERE name = 'Myworks'
)

聚合函数

聚合函数可以对查询的数据进行计算并产生一个值，比如 MAX() MIN() AVG() SUM() COUNT() 等，只会计算非空的数据

USE sql_invoicing;

SELECT
 MIN(invoice_total) AS MIN,
 MAX(invoice_total) AS MAX,
 AVG(invoice_total) AS AVG,
 SUM(invoice_total) AS SUM,
 COUNT(invoice_total) AS COUNT
FROM invoices

MIN	MAX	AVG	SUM	COUNT
101.79	189.12	152.388235	2590.60	17

GROUP BY

GROUP BY 可以将数据安装指定列分组，分组后每一组进行聚合

USE sql_invoicing;

SELECT
 client_id,
 SUM(invoice_total) AS total
FROM invoices
GROUP BY client_id
ORDER BY total DESC

client_id	total
5	980.02
1	802.89
3	705.90
2	101.79

也可以多列分组，结果是每一列的组合

USE sql_invoicing;

SELECT
 p.date,
 pm.name,
 SUM(p.amount) as total_payments
FROM payments p
JOIN payment_methods pm
 ON p.payment_method = pm.payment_method_id
GROUP BY p.date, pm.name
ORDER BY p.date

date	name	total_payments
2019-01-03	Credit Card	74.55
2019-01-08	Cash	10.00
2019-01-08	Credit Card	32.77
2019-01-11	Credit Card	0.03
2019-01-15	Credit Card	148.41
2019-01-26	Credit Card	87.44
2019-02-12	Credit Card	8.18

HAVING 子句

HAVING 子句在 GROUP BY 分组后进行筛选

USE sql_invoicing

SELECT
 client_id,
 SUM(invoice_total) AS total
FROM invoices
GROUP BY client_id
HAVING total > 500
ORDER BY total DESC

client_id	total
5	980.02
1	802.89
3	705.90

WITH ROLLUP

增加一行，对分组的数据做一次总计，只有 Mysql 有

USE sql_invoicing

SELECT pm.name,
 SUM(p.amount) AS total_amount
FROM payments p
JOIN payment_methods pm
 ON p.payment_method = pm.payment_method_id
GROUP BY pm.name WITH ROLLUP
ORDER BY total_amount

name	total_amount
Cash	10.00
Credit Card	351.38
NULL	361.38

复杂查询

子查询返回值用于比较

USE sql_hr;
-- 超过平均薪水的员工
SELECT salary
FROM employees
WHERE salary > (
 SELECT AVG(salary)
 FROM employees
)
LIMIT 3

salary
98926
94860
110150

子查询返回一列用 IN 判断

USE sql_invoicing;

SELECT client_id
FROM clients
WHERE client_id NOT IN (
 SELECT DISTINCT client_id
 FROM invoices
)

client_id
4

子查询返回一列值用 ALL 判断

USE sql_invoicing;

SELECT invoice_id, invoice_total
FROM invoices
WHERE invoice_total > ALL (
 SELECT invoice_total
 FROM invoices
 WHERE client_id = 3
)

invoice_id	invoice_total
2	175.32
5	169.36
8	189.12
9	172.17
18	180.17

上面的查询可以使用 MAX() 改写

类似还有 ANY 和 SOME 关键字，比如 = ANY(1, 2, 3) 表示等于 1,2,3 中的任意一个

EXIST 运算符

EXIST 判断是否存在

USE sql_invoicing;

SELECT c.client_id, c.name
FROM clients c
WHERE EXISTS (
 SELECT client_id
 FROM invoices
 WHERE client_id = c.client_id
)

client_id	name
1	Vinte
2	Myworks
3	Yadel
5	Topiclounge

SELECT 中的子查询

子查询也可以用在 SELECT 列中

USE sql_invoicing;

SELECT
 client_id,
 c.name,
 (SELECT SUM(invoice_total) FROM invoices WHERE client_id = c.client_id) AS total,
 (SELECT AVG(invoice_total) FROM invoices) AS avger,
 (SELECT total - avger) AS diff
FROM clients c

client_id	name	total	avger	diff
1	Vinte	802.89	152.388235	650.501765
2	Myworks	101.79	152.388235	-50.598235
3	Yadel	705.90	152.388235	553.511765
4	Kwideo	NULL	152.388235	NULL
5	Topiclounge	980.02	152.388235	827.631765

FROM 中的子查询

子查询可以用在 FROM 中，当成一个虚拟的表

USE sql_invoicing;

SELECT client_id, name, diff
FROM (
SELECT
 client_id,
 c.name,
 (SELECT SUM(invoice_total) FROM invoices WHERE client_id = c.client_id) AS total,
 (SELECT AVG(invoice_total) FROM invoices) AS avger,
 (SELECT total - avger) AS diff
FROM clients c
) AS summary
WHERE diff IS NOT NULL

client_id	name	diff
1	Vinte	650.501765
2	Myworks	-50.598235
3	Yadel	553.511765
5	Topiclounge	827.631765

C++ 20协程的使用

Wed, 10 Dec 2025 22:55:00 +0800

协程概念参考协程的概念及实现，C++ 20 的协程为无栈协程

协程语法

定义协程

C++20 将以下三种函数当成协程

有 co_await 表达式
有 co_yield 表达式
有 co_return 表达式

协程约束：

参数不能为可变参数
不能有 return 表达式
不能为常量函数
不能为构造函数，析构函数和 main 函数

协程的执行

一个协程涉及以下对象：

Promise 类型对象，协程内部通过它来提交协程结果和异常，这个和 std::promise 没有任何关系
协程句柄，协程外部使用，用来恢复协程执行和销毁协程资源
协程的上下文，使用堆分配内存，包括：
- promise 对象
- 协程参数
- 协程挂起点的状态
- 协程局部变量

协程执行流程：

使用 operator new 分配协程上下文
拷贝所有函数参数到协程上下文，值传递的参数会被移动或复制，引用传递的参数保持引用（可能出现悬垂引用）
调用 Promise 对象的构造函数
调用 promise.get_return_object() ，保持返回值在局部变量中。调用的结果在协程第一次挂起时返回
调用 promise.initial_suspend() ，并对结果执行 co_await
当 co_await promise.initial_suspend() 恢复执行时，开始运行协程的函数体。
当协程运行到挂起点时：
- 之前获得的返回对象在必要时经过隐式转换为协程的返回类型后，返回给调用方或恢复方
当协程到达 co_return 时，执行下面操作：
- 针对 co_return; 或 co_return expr 而 expr 的类型为 void, 调用 promise.return_void()
- 针对 co_return expr 而 expr 类型不为 void 时，调用 promise.return_value(expr)
- 以创建相反的顺序销毁局部变量
- 调用 promise.final_suspend() 并对结果执行 co_await

协程执行出现异常的处理：

捕获所有异常，调用 promise.unhandled_exception()
调用 promise.final_suspend() 并对结果执行 co_await

协程上下文销毁动作（co_return ，异常或协程句柄被释放）：

调用 promise 对象的析构函数
调用函数参数的析构函数
调用 operator delete 释放内存
返回到协程被调用或被恢复的地方

协程上下文内存分配优化：

协程生命周期比调用者生命周期短
协程栈的大小在调用点已知

这种情况，协程上下文会放到调用者的内存空间

Promise 类型

编译器通过协程的返回类型确定协程所用到的 promise 类型

比如协程声明为 task<void> (int f) ，对应的 promise 类型为 std::coroutine_traits<task<void>, int>::promise_typ

co_await

co_await expr 挂起一个协程，将控制流返回给调用者， co_await 只能出现在函数体中（包括 lambda 函数）

co_await 处理流程：

将 expr 用下面规则转换为 awaitable ：
- 如果 expr 是由初始挂起点、最终挂起点或 yield 表达式产生的，则 awaitable 对象就是 expr 本身
- 否则如果当前协程的 promise 类型有 await_transform 成员函数，则 awaitable 对象是 promise.await_transform(expr)
- 否则 awaitable 对象就是 expr
然后通过以下方式获取 awaitable 对象，如果 expr 为纯右值，则 awaiter 为临时对象，否则 awaiter 为该值的引用
- 如果重载了 operator co_await ，则调用对应的重载函数：
  - 对于成员函数，调用 awaitable.operator co_await()
  - 对于非成员函数，调用 operator co_await(static_case<Awaitable&&>(awaitable))
- 否则如果没有找到重载函数，对象就是 awaitble 本身
- 最后如果重载存在歧义，则编译失败
调用 awaiter.await_ready() ，如果结果为 false ，协程会被挂起：
- 此时调用 awaiter.await_suspend(handle) ，handle 表示当前协程句柄。此函数职责在 executor 上调度或恢复协程，根据 await_suspend 返回类型处理：
  - 返回 void ，控制立即返回到调用者
  - 返回 bool 值， true 控制返回到调用者， false 则恢复当前协程
  - 返回另一个协程的协程句柄，则该句柄会被恢复执行（通过调用 handle.resume() ）
  - 抛出异常，当前协程恢复执行，异常重新抛出
最后无论协程是否会被挂起， awaiter.await_resume() 都会被调用，其返回值为整个 co_await expr 的结果

co_yield

co_yield 会返回挂起当前协程并返回一个值给调用者，等价于 await promise.yield_value()

使用例子

计算阶乘

#include <coroutine>
#include <cstdint>
#include <exception>
#include <iostream>

struct MyGenerator {

 struct promise_type;
 using handle_type = std::coroutine_handle<promise_type>;

 struct promise_type {
 uint64_t value_;
 std::exception_ptr exception_;

 MyGenerator get_return_object() {
 return MyGenerator(handle_type::from_promise(*this));
 }

 std::suspend_always initial_suspend() { return {}; }
 std::suspend_always final_suspend() noexcept { return {}; }
 void unhandled_exception(void) { exception_ = std::current_exception(); }

 std::suspend_always yield_value(uint64_t value) {
 value_ = value;
 return {};
 }

 void return_void() {}

 };

 handle_type handle_;

 MyGenerator(handle_type handle) : handle_(handle) {}
 ~MyGenerator() { handle_.destroy(); };

 uint64_t operator()() {
 handle_();
 return handle_.promise().value_;
 }
};


MyGenerator Factorial(uint64_t n) {
 uint64_t ans = 1;
 for (int i = 1; i <= n; i++) {
 ans *= i;
 co_yield ans;
 }
}

int main(int argc, char* argv[]) {

 auto fac = Factorial(3);
 for (int i = 1; i <= 3; i++) {
 std::cout << i << " factorial: " << fac() << std::endl;
 }
 return 0;
}

1 factorial: 1 2 factorial: 2 3 factorial: 6

异步任务

#include <coroutine>
#include <cstdint>
#include <exception>
#include <iostream>
#include <thread>
#include <chrono>

using namespace std::chrono_literals;

auto Timeout(std::jthread& t, int seconds) {
 struct Awaitable {
 std::jthread* t;
 int seconds;

 bool await_ready() {
 return false;
 }

 void await_suspend(std::coroutine_handle<> handle) {
 *t = std::jthread([handle, seconds = this->seconds]{
 std::cout << "thread sleep for " << seconds << "s" << std::endl;
 std::this_thread::sleep_for(std::chrono::seconds(seconds));
 std::cout << "thread sleep for " << seconds << "s finished" << std::endl;
 // 任务完成，唤醒协程，resume 非线程安全，且不能在 await_suspend 返回前调用
 handle.resume();
 });
 }
 void await_resume() {}
 };

 return Awaitable{&t, seconds};
}

struct Task {
 struct promise_type {
 Task get_return_object() {
 return {};
 }
 std::suspend_never initial_suspend() {
 return {};
 }
 std::suspend_never final_suspend() noexcept {
 return {};
 }
 void return_void() {}
 void unhandled_exception() {}
 };
};

Task AsyncSleep(std::jthread& t, int seconds) {
 std::cout << "Timeout start" << std::endl;
 co_await Timeout(t, seconds);
 std::cout << "Timeout end" << std::endl;
}

int main(int argc, char* argv[]) {

 std::jthread t;
 std::cout << "before async sleep" << std::endl;
 AsyncSleep(t, 2);
 std::cout << "after async sleep" << std::endl;

 return 0;
}

before async sleep Timeout start after async sleep thread sleep for 2s thread sleep for 2s finished Timeout end

协程的概念及实现

Mon, 08 Dec 2025 15:55:00 +0800

协程概念

协程是计算机程序的一类组件，允许执行被挂起和恢复。

协程与函数比较

函数只有一个入口点和出口点；协程有多个
函数可以调用其它函数，调用者等待被调用者结束后继续执行，生命周期遵循后进先出；协程的生命周期有他们的使用需要来决定
函数一次性返回全部结果；协程可以每次返回部分结果，这种协程称为生成器

协程有多个入口点，执行过程过

协程与线程比较

线程是抢占式多任务处理；协程是协作式多任务处理
线程能提供并行性，协程只能并发

协程与回调函数比较

回调函数异步时编码复杂，可读性差；协程以顺序方式组织异步代码，可读性更好

协程实现

函数的机器级表示里提到，函数调用时将调用点保存在栈里，函数返回从栈中取出调用点地址，直接跳转回调用点。同时函数的局部变量也保存在栈中。

协程分为有栈协程和无栈协程：

有栈协程通过类似函数，为每个协程分配栈空间保存上下文，切换时保存当前栈的完整状态，恢复时恢复另一个协程的栈状态。
无栈协程：所有协程在同一个线程栈上运行。挂起时，通过状态机记录状态，将局部变量等数据（延续）保存在堆上，并返回。恢复时，重新调用函数并从保存的状态开始执行.

	有栈协程	无栈协程
优点	任意函数挂起和恢复	开销小，不需要栈空间
缺点	需要为每个协程分配栈空间，消耗大	只能在语言定义的特定点挂起，比如 await 处

有栈协程

有栈协程可以实现为每个协程独立一个栈，或者所有协程共享一个栈，下面实现一个简单的有栈协程

定义协程对象，协程栈大小为 64 KB

typedef struct co co_t;

typedef void (*co_entry_t)();

#define CO_STACK_SIZE (64 * 1024) // 64KB

struct co_ctx {
 void* rsp;
 void* rbx;
 void* rbp;
 void* r12;
 void* r13;
 void* r14;
 void* r15;
};

struct co {
 co_entry_t entry;
 struct co_ctx ctx;
 char stack[CO_STACK_SIZE];
};

创建一个协程

co_t* co_create(co_entry_t entry) {
 co_t* co = malloc(sizeof(co_t));
 if (!co) return NULL;

 co->entry = entry;

 // 协程的栈顶
 char* stack_top = co->stack + CO_STACK_SIZE;
 stack_top = (char*)((uintptr_t)stack_top & ~0xF);

 // 将 entry 地址放到栈中
 stack_top -= sizeof(void*);
 *(void**)stack_top = (void*)entry;

 co->ctx.rsp = stack_top;
 return co;
}

内存视图

 -----| stack_bottom
|entry|
 -----| stack_top
| |
| |
| |
| |
|-----|
| |
|co_t |
| |
|-----|

协程上下文切换

`1`	`extern void co_ctx_swap(struct co_ctx* from, struct co_ctx* to);`

# co_ctx_swap.S
.text
.globl co_ctx_swap
.type co_ctx_swap, @function

co_ctx_swap:
 movq %rsp, (%rdi)
 movq %rbx, 8(%rdi)
 movq %rbp, 16(%rdi)
 movq %r12, 24(%rdi)
 movq %r13, 32(%rdi)
 movq %r14, 40(%rdi)
 movq %r15, 48(%rdi)

 movq 48(%rsi), %r15
 movq 40(%rsi), %r14
 movq 32(%rsi), %r13
 movq 24(%rsi), %r12
 movq 16(%rsi), %rbp
 movq 8(%rsi), %rbx
 movq (%rsi), %rsp

 ret

.size co_ctx_swap, .-co_ctx_swap

首先将当前上下文保存在第一个参数，然后将切换的上下文赋值给寄存器，最后 ret 指令将 entry 地址弹出并将 PC 寄存器指向 entry

协程运行

static co_t* cur_co = NULL;
static struct co_ctx main_ctx;

void co_resume(co_t* co) {
 if (!co) return;
 cur_co = co;
 co_ctx_swap(&main_ctx, &co->ctx);
}

当前只实现一个协程，因此简单的用 cur_co 记录

协程让出

1
2
3

void co_yield(void) {
 co_ctx_swap(&cur_co->ctx, &main_ctx);
}

协程销毁

void co_destroy(co_t* co)
{
 free(co);
}

测试程序


#include "coroutine.h"

#include <stdio.h>

void test(void)
{
 int a = 10;
 printf("test 1... a = %d\n", a);
 co_yield();

 a++;
 printf("test 2... a = %d\n", a);
 co_yield();

 a++;
 printf("test 3... a = %d\n", a);
 co_yield();
}

int main(int argc, char *argv[])
{

 co_t* co = co_create(test);

 co_resume(co);

 printf("main 1....\n");

 co_resume(co);

 printf("main 2 ...\n");

 co_resume(co);

 printf("main end\n");

 co_destroy(co);

 return 0;
}

运行结果

test 1... a = 10
main 1....
test 2... a = 11
main 2 ...
test 3... a = 12
main end

无栈协程

无栈协程使用状态机进行跳转，使用堆内存保存协程状态和局部变量

#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

typedef struct co co_t;
typedef void (*co_entry_t)(co_t* co);

struct co {
 co_entry_t entry;
 int state;
 int val;
};

co_t* co_create(co_entry_t entry) {
 co_t* co = malloc(sizeof(co_t));
 if (co == NULL) {
 return NULL;
 }
 co->entry = entry;
 co->state = 0;
 co->val = 0;
 return co;
}

void co_resume(co_t* co) {
 co->entry(co);
}

void co_destroy(co_t* co) {
 free(co);
}

void test(co_t* co) {
 switch (co->state) {
 case 0:
 co->val = 10;
 printf("test 1... a = %d\n", co->val);
 co->state = 1;
 return;
 case 1:
 co->val++;
 printf("test 2... a = %d\n", co->val);
 co->state = 2;
 return;
 case 2:
 co->val++;
 printf("test 3... a = %d\n", co->val);
 return;
 }
}

int main() {

 co_t* co = co_create(test);

 co_resume(co);

 printf("main 1....\n");

 co_resume(co);

 printf("main 2 ...\n");

 co_resume(co);

 printf("main end\n");

 co_destroy(co);

 return 0;
}

test 1... a = 10
main 1....
test 2... a = 11
main 2 ...
test 3... a = 12
main end

上面的 test 协程实际上由编译器处理

性能优化-函数性能度量及分析工具

Wed, 03 Dec 2025 05:38:00 +0800

问题

度量下面累加程序的性能

#include <vector>
#include <random>
#include <limits>

using namespace std;

int64_t Accumulate(vector<int64_t>& arr)
{
 int64_t sum = 0;
 for (int i = 0; i < arr.size(); i++) {
 sum += arr[i];
 }
 return sum;
}

vector<int64_t> RandGenerator(int n)
{
 random_device rd;
 mt19937 gen(rd());
 uniform_int_distribution<> distrib(numeric_limits<int32_t>::min(), numeric_limits<int32_t>::max());

 vector<int64_t> ans(n);
 for (int i = 0; i < ans.size(); i++) {
 ans[i] = distrib(gen);
 }
 return ans;
}

int main(int argc, char* argv[]) {

 auto arr = RandGenerator(1'000'000'000);

 auto sum = Accumulate(arr);


 return 0;
}

程序运行时间

最直观的度量方法就是程序运行的时间，可以使用 C++ 标准库 <chrono> 中的 high_resolution_clock 来测量。

int main(int argc, char *argv[]) {

 auto arr = RandGenerator(1'000'000'000);

 auto start = chrono::high_resolution_clock::now();
 auto sum = Accumulate(arr);
 auto end = chrono::high_resolution_clock::now();

 auto diff_ns =
 chrono::duration_cast<chrono::nanoseconds>(end - start).count();

 cout << format("sum: {}, diff: {} ns\n", sum, diff_ns) << endl;

 return 0;
}

Windows 上 msvc 为 steady_clock 的别名，具体实现封装了 QueryPerformanceCounter ¹。

Linux gcc 上为 system_clock 的别名，具体实现为 clock_gettime(CLOCK_REALTIME) ²

Intel VTune Profiler

Intel® VTune™ Profiler 是 Intel 官方推出的高性能、跨平台（Windows/Linux/macOS）的性能分析工具。

工具安装

下载地址：https://www.intel.com/content/www/us/en/developer/tools/oneapi/vtune-profiler-download.html ，直接下载离线安装包安装，安装完成后以管理员身份运行。

分析程序准备

编译程序时带上调试符号，MSVC 以 RelWithDebInfo 编译，GCC 带上 -g -O2 参数。

工程配置

以管理员身份打开 Intel Vtune Profiler ，新建一个 Project ，点击 Configure Analysis

Where 选择 Local Host ，What 选择 Launch Application ，右边 HOW 选择 Hotspots

热点分析

运行完成后 Profiler 输出分析结果：

Summary ：总结性的输出，主要包括 CPU 占用前几的函数
Bottom-up ：自定向上，包括每个函数的调用链
Caller/Callee ：调用者和被调用者的 CPU 时间
Top-down Tree ：自顶向下的函数调用消耗
Flame Graph ：火焰图

在分析结果上双击函数，可以打开函数的源代码和汇编代码

微架构分析

HOW 选择 Microarchitecture Exploration ，运行后收集到微架构相关的信息：

Summary ：微架构指标统计信息
Bottom-up ：各个函数关联的微架构指标统计信息
Event Count ：各个函数对应的事件数量

Perf

热点分析

使用 perf record 命令进行事件采样，默认的事件为 cpu cycle ，通过 -e 指定其他事件，加上 -g 参数生成函数调用栈，程序运行完成后生成 perf.data 文件

使用 perf report 命令查看生成的 perf.data 文件

使用 perf annotate 查看汇编及汇编指令占用的资源百分比

火焰图生成

下载火焰图生成工具 https://github.com/brendangregg/FlameGraph
使用 perf script 处理生成的 perf.data ，注意加 -g 参数才有调用栈
1

perf script -i perf.data > perf.unfold

使用 stackcollapse-perf.pl 处理 perf.unfold

`1`	`./stackcollapse-perf.pl perf.unfold > perf.folded`

使用 flamegraph.pl 生成火焰图

`1`	`./flamegraph.pl perf.folded > perf.svg`

事件统计

使用 perf stat 命令统计程序运行过程中产生的事件， -e 参数指定统计的事件列表

函数的机器级表示

Sat, 29 Nov 2025 12:58:00 +0800

概念

函数提高了一种封装代码的方式，它可以被多次调用，调用完成后会返回调用点，可选的它可以接收多个参数，返回一个值。

运行时栈

栈是后进先出的数据结构，系统在程序运行时会分配一块内存区域以栈的方式管理，这块内存区域就叫运行时栈或栈内存空间。

函数调用，参数传递，局部变量所需要的内存都使用栈空间。

在内存中，栈底为高地址，栈顶为底地址，即栈向低地址增长，压栈时栈顶地址减小，出栈时栈顶地址增大。

x86 使用 RSP 寄存器保存栈顶，arm64 使用 sp 寄存器保存栈顶

函数的调用与返回

void f(void)
{
}

void c(void)
{
 f();
}

函数调用只需要将程序计数器（PC）执行被调用函数代码的起始位置，函数返回时需要跳转到调用点下一条执行以继续执行当前函数。

因此函数调用分为两步：

保存当前位置到栈中
跳转到新函数执行

函数返回也是两步：

从栈中获取之前保存的调用点位置
跳转到调用点位置继续执行

下面汇编均使用 gcc 编译器，其他编译器可能不一样

x86 平台：

call 指令用来调用函数，它会将调用点地址即 call 指令下一条指令地址压入栈中，然后将 PC 指向调用函数的地址。

ret 指令从函数返回，它从栈中弹出地址，并将 PC 指向该地址

上面代码生成的 x86 汇编

f:
 pushq %rbp
 movq %rsp, %rbp
 nop
 popq %rbp
 ret
c:
 pushq %rbp
 movq %rsp, %rbp
 call f
 nop
 popq %rbp
 ret

arm64 平台：

bl 指令相当于 call 指令，它将下一条指令保存到 x30 寄存器，然后跳转

ret 指令从函数返回，地址为 x30 寄存器地址，如果嵌套调用，使用栈保存和恢复 x30 寄存器

生成的 arm64 汇编：

f:
 nop
 ret
c:
 stp x29, x30, [sp, -16]!
 mov x29, sp
 bl f
 nop
 ldp x29, x30, [sp], 16
 ret

局部变量

函数内部局部变量内存使用栈空间分配，函数返回时释放用到的栈空间

void f(void)
{
 long a = 10;
 char b = 20;
 long c = 30;
}

对应的 x86 汇编

f:
 pushq %rbp // 将 rbp 寄存器原来内容放入栈中 ，rsp 增长 8
 movq %rsp, %rbp // 将栈顶 rsp 的值赋值给 rbp ，现在 rbp 指向栈顶
 movq $10, -8(%rbp) // 栈增长 8 ，将 10 放入
 movb $20, -9(%rbp) // 虽然是 char ，但是栈增长依旧是 8
 movq $30, -24(%rbp)
 nop
 popq %rbp // 将 rbp 还原 ，rsp 减少
 ret

变长栈

当用 alloca 或者声明变长数组时，栈的大小要在运行时才能确定

void f(int n)
{
 int a = 10;
 int array[n];
 array[a] = 100;
}

这种情况，编译器会使用 rbp 作为栈的基地址，函数调用过程中保持不变，栈中局部变量利用 rbp 寻址

对应的汇编及解释：

f:
 // 前两条指令存储 rbp, 并将 rsp 保存到 rbp
 pushq %rbp
 movq %rsp, %rbp
 // 分配 48 字节的栈空间
 subq $48, %rsp
 // 变量 n 放到栈上
 movl %edi, -36(%rbp)
 movq %rsp, %rax
 movq %rax, %rcx
 // int a = 10
 movl $10, -4(%rbp)
 movl -36(%rbp), %eax
 // n 放到 rdx ，rdx 符号扩展
 movslq %eax, %rdx
 // n - 1 放到栈上
 subq $1, %rdx
 movq %rdx, -16(%rbp)

 cltq
 // ((n * 4 + 15) / 16) * 16) , 向上对齐到 16 字节
 leaq 0(,%rax,4), %rdx
 movl $16, %eax
 subq $1, %rax
 addq %rdx, %rax
 movl $16, %esi
 movl $0, %edx
 divq %rsi
 imulq $16, %rax, %rax
 // 分配栈空间
 subq %rax, %rsp
 movq %rsp, %rax
 // 栈地址对齐 4 字节 (n + 3) / 4 * 4
 addq $3, %rax
 shrq $2, %rax
 salq $2, %rax
 // 保存数组首地址到栈中
 movq %rax, -24(%rbp)
 // array[a] = 100
 movq -24(%rbp), %rax
 movl -4(%rbp), %edx
 movslq %edx, %rdx
 movl $100, (%rax,%rdx,4)
 // 恢复 rsp ，释放动态数组
 movq %rcx, %rsp
 nop
 // 等价于 movq %rbp ，%rsp; popq %rbp
 leave
 ret

参数传递

x86 小于 6 个参数，使用寄存器传递，大于 6 个参数的部分使用栈传递

void f(long a, long b, long c, long d, long e, long f, long g, long h);

void c(void)
{
 f(1, 2, 3, 4, 5, 6, 7, 8);
}

f 调用时第 7, 8 参数放到栈中，1, 2, 3, 4, 5, 6 放到寄存器中

参数数量	1	2	3	4	5	6
寄存器	rdi	rsi	rdx	rcx	r8	r9

7，8 两个参数 8 先入栈，7 后入栈

c:
 pushq %rbp
 movq %rsp, %rbp
 pushq $8
 pushq $7
 movl $6, %r9d
 movl $5, %r8d
 movl $4, %ecx
 movl $3, %edx
 movl $2, %esi
 movl $1, %edi
 call f
 addq $16, %rsp
 nop
 leave
 ret

结构体作为参数时，按照成员顺序分配给不同的寄存器传递

struct A {
 long a;
 long b;
};

void f(struct A a);

其中寄存器 rdi 传递 A.a ，寄存器 rsi 传递 A.b

返回值传递

int f(void)
{
 return 100;
}

x86 使用 rax 寄存器传递返回值

f:
 pushq %rbp
 movq %rsp, %rbp
 movl $100, %eax
 popq %rbp
 ret

asm 使用 w0 寄存器传递

1
2
3

f:
 mov w0, 100
 ret

字节序之大小端

Thu, 27 Nov 2025 14:42:00 +0800

内存以字节为单位保存信息，每个字节都有一个编号，这个编号就是内存地址。

一个 int a = 0x12345678 变量占用 4 个字节，而变量的地址为变量 a 在内存中的最低地址，比如 &a 为 0x100 时，说明 a 在内存中的位置为 0x100 0x101 0x102 0x103 。

如果系统为小端序，则将 a 的低位放到低地址，高位放到高地址:

1
2
3

0x100 0x101 0x102 0x103

0x78 0x56 0x34 0x12

如果系统为大端序，则将 a 的低位放到高地址，高位放到低地址:

1
2
3

0x100 0x101 0x102 0x103

0x12 0x34 0x56 0x78

按照上面的排列，可以写一个简单的程序判断系统为小端序还是大端序

#include <iostream>

using namespace std;

int main(int argc, char* argv[]) {

 int a = 0x12345678;

 char* b = (char*)&a;
 if (*b == 0x78) {
 cout << "System is little endian" << endl;
 }

 if (*b == 0x12) {
 cout << "System is big endian" << endl;
 }

 return 0;
}

`1`	`System is little endian`

枚举算法技巧

Tue, 25 Nov 2025 15:41:00 +0800

枚举算法即遍历已有的集合，判断哪些元素符合要求，最终求解问题。

例如 LeetCode 第一题，两数之和：

给定一个整数数组 nums 和一个整数目标值 target，请你在该数组中找出 和为目标值 target 的那 两个 整数，并返回它们的数组下标。

示例 1：

 输入：nums = [2,7,11,15], target = 9
 输出：[0,1]
 解释：因为 nums[0] + nums[1] == 9 ，返回 [0, 1]

暴力枚举

class Solution {
public:
 vector<int> twoSum(vector<int>& nums, int target) {
 for (int i = 0; i < nums.size() - 1; i++) {
 for (int j = i + 1; j < nums.size(); j++) {
 if (nums[i] + nums[j] == target) {
 return {i, j};
 }
 }
 }
 return { 0, 0 };
 }
};

时间复杂度：$O(N^2)$ ，空间复杂度：$O(1)$

保留额外信息优化

枚举算法优化技巧：枚举前面的元素时保存已知信息，枚举到后面时利用前面信息直接求解

本题求解 a + b = target 可以转换为 b = target - a ，遍历 a 时在表中查找前面是否出现过对应的 b ，如果出现直接返回，否则将 a 加入表中，供后面的遍历查询

class Solution {
public:
 vector<int> twoSum(vector<int>& nums, int target) {
 unordered_map<int, int> cache;
 for (int i = 0; i < nums.size(); i++) {
 int a = nums[i];
 int b = target - a;
 auto iter = cache.find(b);
 if (iter != cache.end()) {
 return {iter->second, i};
 } else {
 cache[a] = i;
 }
 }
 return { 0, 0 };
 }
};

时间复杂度：$O(n)$ ，空间复杂度 $O(n)$

类似例子：

设计一个算法，找出数组中两数之和为指定值的所有整数对。一个数只能属于一个数对。

示例 2：

输入：nums = [5,6,5,6], target = 11
输出：[[5,6],[5,6]]

同样 a + b = target ，但是存在多对，因此表格中要保存 b 出现的次数，当匹配一次时，次数减 1 ，减到 0 时从表中移除

class Solution {
public:
 vector<vector<int>> pairSums(vector<int>& nums, int target) {
 unordered_map<int, int> cache;
 vector<vector<int>> ans;

 for (auto num : nums) {
 int diff = target - num;
 auto iter = cache.find(diff);
 if (iter == cache.end()) {
 cache[num]++;
 } else {
 ans.emplace_back(vector<int>{iter->first, num});
 iter->second--;
 if (iter->second <= 0) {
 cache.erase(iter);
 }
 }
 }
 return ans;
 }
};

C++ 奇异的递归模板 CRTP

Wed, 15 Oct 2025 21:42:00 +0800

CRTP (The Curiously Recurring Template Pattern) 是 C++ 模板中的一种习惯用法：子类继承时将自己作为基类的模板参数。

一般形式：

template <typename T>
class Base {

};

class Derived: public Base<Derived> {

};

这样就能在基类访问子类的成员变量及函数。

#include <iostream>

template <typename T>
class Base {
public:
 void call() {
 // 通过强制类型转换转换成子类指针并调用子类方法和成员
 T* t = static_cast<T*>(this);
 t->impl();
 std::cout << "base : " << t->a << std::endl;
 }
};

class DerivedA : public Base<DerivedA> {
public:
 void impl() {
 std::cout << "derived A" << std::endl;
 }

int a = 10;
};

class DerivedB : public Base<DerivedB> {
public:
 void impl() {
 std::cout << "derived B" << std::endl;
 }
int a = 20;
};

int main(int argc, char* argv[]) {
 Base<DerivedA>* a = new DerivedA;
 Base<DerivedB>* b = new DerivedB;

 a->call();
 b->call();

 free(a);
 free(b);
 return 0;
}

derived A
base : 10
derived B
base : 20

约束：Base 中调用的方法和成员必须在子类中实现，否则会出现编译错误

通过这种方式，可以实现编译期多态，日常使用中最常见到使用这种机制的就是 enable_shared_from_this 这个类。

并查集的概念及实现

Sun, 10 Aug 2025 21:55:00 +0800

动态连通性问题

“连通” 是一种等价关系，两个对象 p 和 q 是相连通的，意味着它具有：

自反性：p 和 p 是相连的
对称性：如果 p 和 q 是相连的，那么 q 和 p 也是相连的
传递性：如果 p 和 q 相连且 q 和 r 相连，那么 p 和 r 也是相连的

等价关系将对象分为多个等价类，在这个，当且仅当两个对象相连通时才属于同一个等价类。

使用一个数据结构保存 n 个对象的连通关系，并用来判断一对新的对象是否相连，这样的问题就是动态连通性问题，该数据结构叫做 并查集 。

在网络中等价类叫做连通分量。

并查集的操作

init(): 初始化所有对象
find(p): 返回 p 所在的连通分量
union(p, q): 在 p 和 q 直接添加一条连接
connected(p, q): 如果 p 和 q 在同一连通分量则返回 true
count(): 连通分量的数量

普通并查集

并查集使用数组保存每个对象的关系，对象 p 作为数组的索引，值为连通分量的 id 。

init 时每个对象都不连通，即连通分量 id 都为自身，cnt 记录连通分量数量

void init(vector<int>& uf) {
 for (int i = 0; i < uf.size(); i++) {
 uf[i] = i;
 }
 cnt = uf.size()
}

find 时直接返回对应的连通分量

1
2
3

int find(vector<int>& uf, int p) {
 return uf[p];
}

union 时如果 p 和 q 的连通分量不同，将 q 的连通分量下所有对象的连通分量 id 改为 p 的连通分量 id

void un(vector<int>& uf, int p, int q) {
 int pId = find(p);
 int qId = find(q);
 if (pId == qId) {
 return;
 }
 for (int i = 0; i < uf.size(); i++) {
 if (uf[i] == qId) {
 uf[i] = pId;
 }
 }
 cnt--;
}

connected 只需判断 p 和 q 的连通分量 id 是否相同

1
2
3

bool connected(vector<int>& uf, int p, int q) {
 return find(p) == find(q);
}

count 时返回连通分量的计数

1
2
3

int count() {
 return cnt;
}

find-union 优化

对象 p 在数组中记录的值 uf[p] 指向同一连通分量的其它对象，根对象指向它自己。

因此 find 时需要一直查找到根对象

int find(vector<int>& uf, int p) {
 while (uf[p] != p) {
 p = uf[p];
 }
 return uf[p];
}

union 时只需要将 p 和 q 的根对象统一

void un(vector<int>& uf, int p, int q)
{
 int pRoot = find(p);
 int qRoot = find(q);
 if (pRoot == qRoot) {
 return;
 }
 uf[qRoot] = pRoot;
 cnt--;
}

这样处理后，这个 uf 的结构就是一个森林，每个连通分量就是森林里的一颗树

加权 union 算法

在 union 时将小的连通分量连接到大的连通分量上，减少树的深度，使用额外的数组记录连通分量节点数量，初始化时为 1

void un(vector<int>& uf, vector<int>& w, int p, int q)
{
 int pRoot = find(p);
 int qRoot = find(q);
 if (pRoot == qRoot) {
 return;
 }
 if (w[pRoot] > w[qRoot]) {
 uf[qRoot] = pRoot;
 w[pRoot] += w[qRoot];
 } else {
 uf[pRoot] = qRoot;
 w[qRoot] += w[pRoot];
 }
 cnt--;
}

路径压缩的 find 算法

即将所有节点连接到根节点上，在 find 时实现

int find(vector<int>& uf, int p)
{
 int pRoot = p;
 while (uf[pRoot] != pRoot) {
 pRoot = uf[pRoot];
 }
 // 路径压缩
 while (uf[p] != p) {
 p = uf[p];
 uf[p] = pRoot;
 }
 return p;
}

递归版本：

int find(vector<int>& uf, int p)
{
 if (uf[p] != p) {
 uf[p] = find(uf, uf[p]);
 }

 return uf[p];
}

实战

leetcode

1971: https://leetcode.cn/problems/find-if-path-exists-in-graph/description/

class Solution {
public:
 int ufFind(vector<int>& uf, int p) {
 int start = p;
 while (uf[p] != p) {
 p = uf[p];
 }
 while (uf[start] != p) {
 start = uf[start];
 uf[start] = p;
 }
 return p;
 }

 void ufUnion(vector<int>& uf, int p, int q) {
 int pRoot = ufFind(uf, p);
 int qRoot = ufFind(uf, q);
 if (pRoot == qRoot) {
 return;
 }
 uf[pRoot] = qRoot;
 }

 bool validPath(int n, vector<vector<int>>& edges, int source, int destination) {
 vector<int> uf(n);
 for (int i = 0; i < n; i++) {
 uf[i] = i;
 }
 for (const auto& edge: edges) {
 ufUnion(uf, edge[0], edge[1]);
 }
 return ufFind(uf, source) == ufFind(uf, destination);
 }
};

动态规划之背包问题

Sat, 08 Apr 2023 00:00:00 +0800

0-1 背包

给你一个可装载重量为 W 的背包和 N 个物品，每个物品有重量和价值两个属性。其中第 i 个物品的重量为 wt[i]，价值为 val[i]，现在让你用这个背包装物品，最多能装的价值是多少？

明确状态和选择

明确状态：背包的容量和可选择的物品
明确选择：对于一个物品，可以选择装进背包，或者选择不装进背包

定义 dp 数组

状态有两个，所以需要二维 dp 数组 dp[i][w]

dp[i][w] 含义：对于前 i 个物品，背包容量为 w 时能够获得的最大价值

我们需要的答案就是 dp[N][W]

当物品为 0 或者背包容量为 0 时获得的价值为 0 ，所以 base case 就是 dp[0][…] = dp[…][0] = 0

寻找状态转移方程

如何计算 dp[i][w] 呢？对于第 i 个物品：

如果背包的剩余容量以及装不下 i 了，即 w - w[i] < 0 , 此时总的价值等于 dp[i-1][w]
剩余容量能够装下 i
- 如果不把它放进背包，那总的价值等于 dp[i-1][w]
- 如果将它放进背包，那总的价值等于 value[i] + dp[i-1][w - w[i]] ，即当前物品价值加上前 i - 1 个物品，背包容量为 w - value[i] 时获得的最大价值。
- dp[i][w] = max(dp[i-1][w], value[i] + dp[i-1][w - value[i]])

套用框架写出代码

for 状态1 in 状态1的所有取值：
 for 状态2 in 状态2的所有取值：
 for ...
 dp[状态1][状态2][...] = 择优(选择1，选择2...)

#include <algorithm>
#include <iostream>
#include <vector>

using namespace std;

// capacity: 背包容量
// n: 种类数
// weight[i-1]: 第 i 种物品的重量，i 从 0 计算
// value[i-1]: 第 i 种物品的价值，i 从 0 计算
int knapsack_0_1(int capacity, int n, const vector<int>& weight, const vector<int>& value) {
 vector<vector<int>> dp(n + 1, vector<int>(capacity + 1, 0));

 for (int i = 1; i <= n; i++) {
 for (int j = 1; j <= capacity; j++) {
 if (j - weight[i - 1] < 0) {
 dp[i][j] = dp[i - 1][j];
 } else {
 dp[i][j] = max(dp[i - 1][j], value[i - 1] + dp[i - 1][j - weight[i - 1]]);
 }
 }
 }
 return dp[n][capacity];
}

int main(int argc, char* argv[]) {
 int capacity, n;
 cin >> n >> capacity;

 vector<int> weight(n, 0);
 vector<int> value(n, 0);

 for (int i = 0; i < n; i++) {
 cin >> weight[i] >> value[i];
 }

 int result = knapsack_0_1(capacity, n, weight, value);

 cout << result << endl;

 return 0;
}

优化 dp 数组空间

可以看到 dp[i][j] 计算时只依赖 dp[i-1][...j] ，即只用到了上一行，所以可以用一维数组存储上一行的信息。

由于 dp[i][j] 中的 j 依赖于 [...j] 前面的数据，所以我们要从后往前计算 j ，避免新的数据覆盖需要的数据

#include <algorithm>
#include <iostream>
#include <vector>

using namespace std;

// capacity: 背包容量
// n: 种类数
// weight[i-1]: 第 i 种物品的重量，i 从 0 计算
// value[i-1]: 第 i 种物品的价值，i 从 0 计算
int knapsack_0_1(int capacity, int n, const vector<int>& weight, const vector<int>& value) {
 vector<int> dp(capacity + 1);

 for (int i = 1; i <= n; i++) {
 for (int j = capacity; j >= weight[i - 1]; j--) {
 dp[j] = max(dp[j], value[i - 1] + dp[j - weight[i - 1]]);
 }
 }
 return dp[capacity];
}

int main(int argc, char* argv[]) {
 int capacity, n;
 cin >> n >> capacity;

 vector<int> weight(n, 0);
 vector<int> value(n, 0);

 for (int i = 0; i < n; i++) {
 cin >> weight[i] >> value[i];
 }

 int result = knapsack_0_1(capacity, n, weight, value);

 cout << result << endl;

 return 0;
}

例题链接

https://www.acwing.com/problem/content/2/

完全背包

完全背包和 0-1 背包类似，不同的点是每个物品的数量是无限的，可以选无限次，而不是一次。

明确状态和选择

明确状态：背包的容量和可选择的物品
明确选择：对于第 i 个物品，可以选择装 k 个进背包，或者选择不装进背包

定义 dp 数组

状态有两个，所以需要二维 dp 数组 dp[i][w]

dp[i][w] 含义：对于前 i 个物品（可以重复选择），背包容量为 w 时能够获得的最大价值

我们需要的答案就是 dp[N][W]

当物品为 0 或者背包容量为 0 时获得的价值为 0 ，所以 base case 就是 dp[0][…] = dp[…][0] = 0

寻找状态转移方程

如何计算 dp[i][w] 呢？对于第 i 个物品：

如果背包容量一个 i 都放不下，=dp[i][w] = dp[i-1][w]=
如果背包容量至少可以放一个 i ，虽然 i 的个数是无限的，但是实际能够放 i 的个数为 w/w[i] ：
- 第 i 个物品的可选择的个数就为 [0..w/w[i]]
- dp[i][w] = max(dp[i-1][w-0 * w[i]], ..., value[i]*w/w[i] +dp[i-1][w-w/w[i]*w[i]])

根据框架写出代码

输入格式第一行两个整数，N，V，用空格隔开，分别表示物品种数和背包容积。

接下来有 N 行，每行两个整数 vi,wi，用空格隔开，分别表示第 i 种物品的体积和价值。

#include <algorithm>
#include <iostream>
#include <vector>

using namespace std;

// capacity: 背包容量
// n: 种类数
// weight[i-1]: 第 i 种物品的重量，i 从 0 计算
// value[i-1]: 第 i 种物品的价值，i 从 0 计算
int knapsack_inf(int capacity, int n, const vector<int>& weight, const vector<int>& value) {

 vector<vector<int>> dp(n + 1, vector<int>(capacity + 1, 0));

 for (int i = 1; i <= n; i++) {
 for (int j = 1; j <= capacity; j++) {
 if (j - weight[i - 1] < 0) {
 dp[i][j] = dp[i - 1][j];
 } else {
 dp[i][j] = dp[i - 1][j]; // 不选择
 for (int k = 1; k * weight[i - 1] <= j; k++) { // 在 1.. k 中求最优
 dp[i][j] = max(dp[i][j], value[i - 1] * k + dp[i - 1][j - k * weight[i - 1]]);
 }
 }
 }
 }
 return dp[n][capacity];
}

int main(int argc, char* argv[]) {
 int capacity, n;
 cin >> n >> capacity;

 vector<int> weight(n, 0);
 vector<int> value(n, 0);

 for (int i = 0; i < n; i++) {
 cin >> weight[i] >> value[i];
 }

 int result = knapsack_inf(capacity, n, weight, value);

 cout << result << endl;

 return 0;
}

优化时间复杂度

1
2
3

for (int k = 1; k * weight[i - 1] <= j; k++) { // 在 1.. k 中求最优
 dp[i][j] = max(dp[i][j], value[i - 1] * k + dp[i - 1][j - k * weight[i - 1]]);
}

上面这段代码中存在重复计算，比如当 k 为 0， 1， 2 时，

dp[i][j] = max(dp[i-1][j], dp[i-1][j-w[i)] + v[i], dp[i-1][j-w[i]*2] + v[i]*2)

当 j = j-w[i] 时

dp[i][j-w[i]] = max(dp[i-1][j-w[i]], dp[i-1][j-w[i]*2] + v[i], dp[i-1][j-w[i]*3] + v[i]*2)

所以当 j 从前往后时， dp[i][j-w[i]] 已经由 dp[i-1][j-w[i]*2] 更新过，所以优化后的状态转移为：

dp[i][j] = max(dp[i-1][j], dp[i][j-w[i-1]])

优化后的代码：

#include <algorithm>
#include <iostream>
#include <vector>

using namespace std;

// capacity: 背包容量
// n: 种类数
// weight[i-1]: 第 i 种物品的重量，i 从 0 计算
// value[i-1]: 第 i 种物品的价值，i 从 0 计算
int knapsack_inf(int capacity, int n, const vector<int>& weight, const vector<int>& value) {

 vector<vector<int>> dp(n + 1, vector<int>(capacity + 1, 0));

 for (int i = 1; i <= n; i++) {
 for (int j = 1; j <= capacity; j++) {
 if (j - weight[i - 1] < 0) {
 dp[i][j] = dp[i - 1][j];
 } else {
 dp[i][j] = max(dp[i - 1][j], dp[i][j - weight[i - 1]] + value[i - 1]);
 }
 }
 }
 return dp[n][capacity];
}

int main(int argc, char* argv[]) {
 int capacity, n;
 cin >> n >> capacity;

 vector<int> weight(n, 0);
 vector<int> value(n, 0);

 for (int i = 0; i < n; i++) {
 cin >> weight[i] >> value[i];
 }

 int result = knapsack_inf(capacity, n, weight, value);

 cout << result << endl;

 return 0;
}

优化空间复杂度

类似的， dp[i][j] 的计算只依赖于 dp[i-1] 的数据，所以可以用一维数组滚动计算。

注意， dp[i][j] 的计算依赖于前面计算的 dp[i][j-w[i]] ，所以需要 j 需要从前往后计算

#include <algorithm>
#include <iostream>
#include <vector>

using namespace std;

// capacity: 背包容量
// n: 种类数
// weight[i-1]: 第 i 种物品的重量，i 从 0 计算
// value[i-1]: 第 i 种物品的价值，i 从 0 计算
int knapsack_inf(int capacity, int n, const vector<int>& weight, const vector<int>& value) {

 vector<int> dp(capacity + 1);

 for (int i = 1; i <= n; i++) {
 for (int j = weight[i - 1]; j <= capacity; j++) {
 dp[j] = max(dp[j], dp[j - weight[i - 1]] + value[i - 1]);
 }
 }
 return dp[capacity];
}

int main(int argc, char* argv[]) {
 int capacity, n;
 cin >> n >> capacity;

 vector<int> weight(n, 0);
 vector<int> value(n, 0);

 for (int i = 0; i < n; i++) {
 cin >> weight[i] >> value[i];
 }

 int result = knapsack_inf(capacity, n, weight, value);

 cout << result << endl;

 return 0;
}

例题

https://www.acwing.com/problem/content/3/

多重背包问题

和完全背包类似，但是第 i 种物品的个数限制为 nums[i] 个，而完全背包的物品个数不限。

所以我们可以直接套用完全背包的解决方法，需要注意的是每种物品最多只有 nums[i] 个

多重背包无法像完全背包一样进行时间优化，消除 k ，因为每个物品不是无限，不能放任意次数。

下面是空间优化后的版本

#include <algorithm>
#include <iostream>
#include <vector>

using namespace std;

// capacity: 背包容量
// n: 种类数
// weight[i-1]: 第 i 种物品的重量，i 从 0 计算
// value[i-1]: 第 i 种物品的价值，i 从 0 计算
// nums[i-1]: 第 i 种物品的数量
int knapsack_nums(int capacity, int n, const vector<int>& weight, const vector<int>& value,
 const vector<int>& nums) {

 vector<int> dp(capacity + 1, 0);
 for (int i = 1; i <= n; i++) {
 for (int j = capacity; j >= weight[i - 1]; j--) {
 for (int k = 1; k <= nums[i - 1] && (j - k * weight[i - 1]) >= 0; k++) {
 dp[j] = max(dp[j], value[i - 1] * k + dp[j - k * weight[i - 1]]);
 }
 }
 }
 return dp[capacity];
}
int main(int argc, char* argv[]) {
 int capacity, n;
 cin >> n >> capacity;

 vector<int> weight(n, 0);
 vector<int> value(n, 0);
 vector<int> nums(n, 0);

 for (int i = 0; i < n; i++) {
 cin >> weight[i] >> value[i] >> nums[i];
 }

 int result = knapsack_nums(capacity, n, weight, value, nums);

 cout << result << endl;

 return 0;
}

小幻Coding - 记录成长

机器人二维地图的各种表示及转换

占据栅格地图 (nav_msgs::OccupancyGrid)

多层栅格地图 (grid_map)

栅格访问方式

获取地图信息

OpenCV 图像表示

坐标变换

像素访问

ros2-control介绍及使用

介绍

自定义硬件及控制器

硬件实现

实现控制器

ros2_control 框架的使用

PCL 点云手记

数据结构

IO

文件

可视化

查找

KdTree

特征

积分图法向量估计

滤波

直通滤波(PassThrough filter)

体素滤波(VoxelGrid filter)

统计离群点移除滤波

条件滤波

半径离群点移除滤波

投影滤波

索引提取滤波

分割

平面模型分割

圆柱模型分割

欧几里得聚类分割

区域生长分割算法

基于颜色的区域生长分割算法

基于最小割的分割算法

条件欧几里得聚类分割

配准

ICP 配准

增量配准

NDT 算法

在场景中估计对象位姿

Linux网络代理设置

相关软件下载

代理配置

RoutingA 规则配置

Linux罗技鼠标设置dpi

Linux激活键盘Fn功能

临时修改

永久修改

WSL2配合Windows运行Gazebo

Windows 安装 Gazebo

快速启动 Gazebo

WSL2 配置

网络配置

安装 Gazebo

Gazebo 配置

测试

ROS2 Launch 启动

ROS2机器人仿真

URDF 机器人建模

基础

使用 xacro 简化 urdf

使用 rviz 可视化 urdf

增加物理属性

碰撞属性

质量和惯性

Gazebo 仿真

ROS2 启动 Gazebo 并加载模型

启动 gazebo 仿真，加载世界模型

在世界中生成机器人

启动 bridge

完整的 launch 代码：

两轮差速传感器仿真

激光雷达传感器仿真

IMU 仿真

深度相机仿真