ros1-lite3-slam

硬件设备

绝影云深处 lite3

建图场所

学校工业中心 —-> 室内场景

slam

vlp16雷达

启动雷达节点脚本

slam算法的效果对比

因为机器狗的算力限制和一些个人能力问题，

对比了几个主流的算法，最终挑选了faster-lio作为建图算法

• 运行速度快
• 对算力资源要求不高
• 实时性强
• 易于部署

fast-lio2

faster-lio

point-lio

faster_lio

建图中

point cloud filtering

原本的点云

四足机器人使用faster-lio算法采集的点云

运动较为剧烈

操作为 :

使用 octomap_server 来转化的

脚本如下 pcd2grid.launch 文件

<launch>
    <!-- 静态 TF：map -> base_footprint（如果你用 base_footprint） -->
    <node pkg="tf" type="static_transform_publisher" name="static_tf_map_base" args="0 0 0 0 0 0 map base_footprint 100"/>

    <node pkg="octomap_server" type="my_octomap_node" name="my_octomap" output="screen">
        <param name="file_path" value="/home/ysc/Desktop/tools/pcdFiles/lite3.pcd"/>
        <param name="filter_leaf_size_m" value="0.05"/>
        <param name="frame_id" type="string" value="map"/>
        <param name="base_frame_id" value="base_footprint"/>
        
        <param name="pointcloud_min_z" value="-1.0"/>
        <param name="pointcloud_max_z" value="2.0"/>
        <param name="occupancy_min_z" value="-0.1"/>
        <param name="occupancy_max_z" value="2.0"/>

        <param name="filter_speckles" value="false"/>
        <param name="sensor_model/max_range" value="100.0"/>
        <param name="sensor_model/min_range" value="-1.0"/>
        <param name="resolution" value="0.05"/>
        <param name="compress_map" value="true"/>
        <param name="publish_free_space" value="false"/>
        <param name="latch" value="true"/>
    
        <remap from="/projected_map" to="/map" />
    </node>

    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find pcd2grid)/rviz/config.rviz" />
</launch>

改良

CSF

CSF算法，全称为Cloth Simulation Filtering，是一种基于欧几里得空间中最小生成树思想的聚类算法，它可以很好地对点云数据进行分割和分类。这个算法的核心思想是把点云数据转化成一张图，然后通过逐步连接最近邻点构建出多棵生成树，再将所有生成树合并成一张图，并利用深度优先搜索、双向广搜等手段对其上点进行分割。这样就可以将点云数据快速分割成多个类别，并且获取到每个点所属的类别。

​ 在CSF算法的基础上，可以使用其地面点滤波方法。具体方法是通过使用RANSAC算法估计地面模型的法向量，然后通过计算每个点到地面模型的距离来滤除非地面点，而最后剩下的点即为地面点。同时，需要在此基础上做一些修改来提高算法的效率：首先，将待处理的点云数据进行降采样，减少计算量；其次，使用KD树快速搜索每个点的最近邻点，加速点云数据的处理过程。

​ 此外，CSF算法也可以结合其他技术进行优化，例如在数据预处理阶段使用数据降维技术，减少数据量；在生成树构建阶段使用多线程并行计算，提高计算效率；在分割阶段使用机器学习算法，提高分割精度等。

​ 总之，CSF算法是一种基于图模型的聚类算法，适用于点云数据的分割和分类任务。通过与其他技术的结合，可以实现更高效、更精确的点云数据处理。

使用的库为： https://github.com/jianboqi/CSF

import open3d as o3d
import numpy as np
import CSF  # pip install cloth-simulation-filter

'''
    算法的仓库地址为 : https://github.com/jianboqi/CSF    
    CSF 对 numpy 的版本 要求是 < 2 ,即 1几版本的 numpy
    布料结算
'''


def run_csf_filter(txt_path, ground_txt="ground.txt", non_ground_txt="non_ground.txt"):
    csf = CSF.CSF()
    csf.readPointsFromFile(txt_path)

    # 设置参数（你可根据实际需求调节）
    csf.params.bSloopSmooth = False
    csf.params.cloth_resolution = 0.5
    csf.params.class_threshold = 0.5
    csf.params.interations = 500

    ground = CSF.VecInt()
    non_ground = CSF.VecInt()
    csf.do_filtering(ground, non_ground)

    csf.savePoints(ground, ground_txt)
    csf.savePoints(non_ground, non_ground_txt)
    print("[✓] CSF 地面分离完成")


def pcd_to_csf_txt(pcd_path, txt_path):
    pcd = o3d.io.read_point_cloud(pcd_path)
    points = np.asarray(pcd.points)
    np.savetxt(txt_path, points, fmt="%.6f")
    print(f"[✓] 点云已保存为 CSF 输入格式：{txt_path}")

def csf_txt_to_pcd(txt_path, pcd_path):
    points = np.loadtxt(txt_path)
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)
    o3d.io.write_point_cloud(pcd_path, pcd)
    print(f"[✓] 已保存为 PCD 格式：{pcd_path}")



def filter_lower_points(points_np, threshold=-0.1):
    """
    过滤掉 z 坐标低于阈值的点，默认阈值为 -0.1 米（地面以下10cm）
    """
    filtered_points_np = points_np[points_np[:, 2] >= threshold]
    print(f"过滤后点云共 {len(filtered_points_np)} 个点")
    return filtered_points_np

def downsample_pcd(points_np, voxel_size=0.05):
    """
    使用体素网格对点云下采样。
    参数:
        points_np: 原始点云的 numpy 数组，形状为 (N, 3)
        voxel_size: 下采样体素大小（单位：米）
    返回:
        下采样后的 numpy 数组
    """
    # 将 NumPy 数组转换为 PointCloud
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points_np)

    # 执行下采样
    downsampled_pcd = pcd.voxel_down_sample(voxel_size=voxel_size)

    # 转换为 NumPy 数组返回
    downsampled_points_np = np.asarray(downsampled_pcd.points)
    print(f"下采样后点云共 {len(downsampled_points_np)} 个点")
    return downsampled_points_np

def visualize_ground_non_ground(ground_points, non_ground_points):
    # 创建地面点云并上色（绿色）
    ground_pcd = o3d.geometry.PointCloud()
    ground_pcd.points = o3d.utility.Vector3dVector(ground_points)
    ground_pcd.paint_uniform_color([0, 1, 0])  # 绿色 RGB

    # 创建非地面点云并上色（红色）
    non_ground_pcd = o3d.geometry.PointCloud()
    non_ground_pcd.points = o3d.utility.Vector3dVector(non_ground_points)
    non_ground_pcd.paint_uniform_color([1, 0, 0])  # 红色 RGB

    # 可视化展示
    o3d.visualization.draw_geometries([ground_pcd, non_ground_pcd],
                                      window_name="CSF 地面去除效果",
                                      width=900, height=1500,
                                      point_show_normal=False)


def office_exp():
    csf = CSF.CSF()
    csf.readPointsFromFile('./pcd/temp_input.txt')

    csf.params.bSloopSmooth = False
    csf.params.cloth_resolution = 0.5

    ground = CSF.VecInt()  # a list to indicate the index of ground points after calculation
    non_ground = CSF.VecInt()  # a list to indicate the index of non-ground points after calculation
    csf.do_filtering(ground, non_ground)  # do actual filtering.
    csf.savePoints(ground, "ground.txt")


def main():
    # 1. 转换为 CSF 输入格式
    pcd_path = "./pcd/lite3.pcd"
    txt_input = "./pcd/temp_input.txt"
    pcd_to_csf_txt(pcd_path, txt_input)

    # 2. CSF 执行过滤
    run_csf_filter(txt_input, "ground.txt", "non_ground.txt")

    # 3. 输出转换回 PCD
    csf_txt_to_pcd("ground.txt", "./pcd/csf_ground.pcd")
    csf_txt_to_pcd("non_ground.txt", "./pcd/csf_non_ground.pcd")

if __name__ == '__main__':
    main()
    # office_exp()

过滤效果

csf 对于崎岖复杂的地形效果可能会更好，这边把桌腿也给过滤了，有点过滤过猛

• 速度偏慢【可能是py的原因】
• 效果还不错
• 环境配置比较复杂，对算力要求较高
• 有过滤过猛的现象，但是应该对于复杂地形十分地实用

转为栅格图

RANSAC

RANSAC 是一种“用少数点猜模型、用多数点投票验证”的鲁棒拟合方法，适合在含噪声的数据中找到最优几何结构（如地面、直线、平面等）。

代码如下

import open3d as o3d
import numpy as np
# import CSF  # pyCSF 库


'''
    ransac 不能高度过滤，过滤后产生的平面会变成墙体

'''


def read_pcd(file):
    pcd = o3d.io.read_point_cloud(file)
    points_np = np.asarray(pcd.points)
    print(f"已读取点云，共 {len(points_np)} 个点")
    return points_np


def fast_ground_filtering(points_np, distance_threshold=0.1, upright_threshold=0.95):
    """
    快速地面点提取，只提取接近水平的平面作为地面。

    参数：
        points_np: 输入点云 (Nx3)
        distance_threshold: 点到平面最大距离（RANSAC容忍）
        upright_threshold: 与 z 轴夹角的 cos 值下限，越接近 1 越接近水平

    返回：
        ground_points: 只包含地面的点
        non_ground_points: 其余所有点（含墙/物体等）
    """
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points_np)

    plane_model, inliers = pcd.segment_plane(distance_threshold=distance_threshold,
                                             ransac_n=3,
                                             num_iterations=5000)
    [a, b, c, d] = plane_model
    normal = np.array([a, b, c])
    normal = normal / np.linalg.norm(normal)
    cos_angle = abs(np.dot(normal, np.array([0, 0, 1])))  # 与z轴的夹角余弦值

    print(f"平面方程: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")
    print(f"法向量: {normal}，与z轴夹角cos值: {cos_angle:.3f}")

    if cos_angle < upright_threshold:
        print("[×] 拟合到的平面不是水平地面（可能是墙面），不提取")
        return np.empty((0, 3)), points_np

    ground_points = points_np[inliers]
    non_ground_points = np.delete(points_np, inliers, axis=0)

    print(f"[✓] 地面点提取成功：{len(ground_points)} 点，非地面点：{len(non_ground_points)} 点")
    return ground_points, non_ground_points

def filter_high_points(points_np, z_threshold=1.0):
    """只保留z坐标低于 z_threshold 的点（排除墙面）"""
    filtered = points_np[points_np[:, 2] <= z_threshold]
    print(f"高度过滤后剩余 {len(filtered)} 个点")
    return filtered


def visualize_ground_non_ground(ground_points, non_ground_points):
    # 创建地面点云并上色（绿色）
    ground_pcd = o3d.geometry.PointCloud()
    ground_pcd.points = o3d.utility.Vector3dVector(ground_points)
    ground_pcd.paint_uniform_color([0, 1, 0])  # 绿色 RGB

    # 创建非地面点云并上色（红色）
    non_ground_pcd = o3d.geometry.PointCloud()
    non_ground_pcd.points = o3d.utility.Vector3dVector(non_ground_points)
    non_ground_pcd.paint_uniform_color([1, 0, 0])  # 红色 RGB

    # 可视化展示
    o3d.visualization.draw_geometries([ground_pcd, non_ground_pcd],
                                      window_name="CSF 地面去除效果",
                                      width=1000, height=1500,
                                      point_show_normal=True)

def save_non_ground(non_ground_points_np,path):
    print(non_ground_points_np.shape)
    # 将 NumPy 点云数组转换为 PointCloud 对象
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(non_ground_points_np)

    # 保存为 PCD 或 PLY 文件
    o3d.io.write_point_cloud(path, pcd)
    print(f"非地面点已保存至：{path}")

def downsample_pcd(points_np, voxel_size=0.05):
    """
    使用体素网格对点云下采样。
    参数:
        points_np: 原始点云的 numpy 数组，形状为 (N, 3)
        voxel_size: 下采样体素大小（单位：米）
    返回:
        下采样后的 numpy 数组
    """
    # 将 NumPy 数组转换为 PointCloud
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points_np)

    # 执行下采样
    downsampled_pcd = pcd.voxel_down_sample(voxel_size=voxel_size)

    # 转换为 NumPy 数组返回
    downsampled_points_np = np.asarray(downsampled_pcd.points)
    print(f"下采样后点云共 {len(downsampled_points_np)} 个点")
    return downsampled_points_np

def filter_lower_points(points_np, threshold=-0.1):
    """
    过滤掉 z 坐标低于阈值的点，默认阈值为 -0.1 米（地面以下10cm）
    """
    filtered_points_np = points_np[points_np[:, 2] >= threshold]
    print(f"过滤后点云共 {len(filtered_points_np)} 个点")
    return filtered_points_np



def main():
    file = "./pcd/lite3.pcd"
    pcd_points = read_pcd(file)
    # 高度阈值会扰乱
    # pcd_points = filter_lower_points(pcd_points)
    pcd_points = downsample_pcd(pcd_points)
    ground_points, non_ground_points = fast_ground_filtering(pcd_points)
    visualize_ground_non_ground(ground_points, non_ground_points)
    save_non_ground(non_ground_points,"./ransac_non_ground.pcd")

if __name__ == "__main__":
    main()

效果如下

效果很不错，速度很快，比较平坦的地面点云直接过滤出来了

栅格地图

csf和 ransac 和 原本的对比

传统机器学习—>ML 【突发奇想】

我的点云没打标签，所以我优先想到的是无监督学习算法

SVM 恰好有一个无监督学习的版本

SVM 【One-Class SVM】

KMEANS

从原理的角度上，KMEANS就很不适合，但想试试有多不适合

给自己蠢到了，效果很乐

深度学习

什么年代了还在玩传统的机器学习?

来试试深度学习！

。。。。。。未完待续

pcd to gird map

启动 pcd2gird.sh 脚本

将pcd转化为2维的栅格图

文件比较大，需要一定的时间来转化

栅格地图

nav

导航直接 基于 move_base 的架构

启动导航脚本

由于栅格地图太占据资源，故不显示在rviz中

位置初始化，位置比较好会自动初始化，如果位置太差也可以手动初始化

A* 作为全局规划器，TEB作为局部规划器

TEB对于四足机器人平台还挺好用的

设置导航点使用 A* 规划算法和 TEB 规划器

A*

A*（A-Star）算法是一种启发式搜索算法，是图搜索中最经典、最常用的路径规划算法之一，常用于机器人导航、游戏AI、地图路径规划等场景。

A* 为每个节点 n 计算一个评估函数：

f(n)=g(n)+h(n)f(n) = g(n) + h(n)f(n)=g(n)+h(n)

符号	含义	解释
g(n)	起点到当前节点 n 的实际代价	走到当前位置花了多少代价（已知）
h(n)	当前节点到目标的启发式估计	还剩多少距离（估算值）
f(n)	总估价	当前路径的“总成本”预估

1. 将起点加入 open 列表（待探索）
2. 重复以下步骤，直到 open 列表为空或找到终点：
    a. 从 open 中选出 f(n) 最小的节点 n
    b. 若 n 是终点，结束，路径找到
    c. 将 n 加入 closed（已探索）
    d. 遍历 n 的所有邻居：
        - 若邻居在 closed，跳过
        - 若不在 open 或发现更短路径：
            - 更新 g(n), h(n), f(n)
            - 设置父节点（用于回溯路径）
            - 加入 open
3. 回溯路径：从终点依次沿父节点返回起点

ros中的 A*

move_base 默认使用 global_planner 中的 A* 算法进行全局路径规划；

costmap_2d 提供的地图栅格成本用于 g(n) 评估；

启发式函数通常为目标距离；

结果是 /move_base/NavfnROS 话题输出的一条可行路径。

。。。。。。未完待续

TEB

。。。。。。未完待续