雷达点云中脚步的提取（python）

一、雷达静态地图点的提取

目的是过滤掉雷达数据中的误测点（多数情况下为温漂造成的数据点丢失）

#雷达每一帧数据的样式 单位为m
Frame data : 0.356,0.37,0.36,0.35,0.35,0.36,0.362,0.364,0.372,0.37,0.374,0.36,0.356,0.368,0.366,0.372,0.376,0.372,0.378,0.372,0.368,0.358,0.368,0.38,0.376,0.374,0.384,0.376,0.384,0.378,0.38,0.374,0.388,0.382,0.396,0.396,0.396,0.404,0.404,0.404,0.404,0.4,0.404,0.406,0.404,0.406,0.402,0.418,0.414,0.434,0.438,0.436,0.45,0.434,0.448,0.46,0.456,0.47,0.482,0.488,0.48,0.472,0,0,0,0,0.706,0.732,0.734,0.724,0.736,0.734,0.728,0.742,0.748,0.756,0.742,0.758,0.764,0.788,0.8,0.812,0.818,0,0,0,0,1.972,1.97,1.95,1.968,1.95,0,0,0,0,0,0,0,0,0,0,0,0,0,2.108,2.124,2.126,2.124,2.128,2.126,2.098,2.11,2.102,0,0,0,0,0,1.87,1.866,0,0,0,0,0,0,0,0,0,3.308,0,0,0,0,0,0,0,0,0,0,0,0,2.032,0,0,0,0,5.99,5.972,5.992,5.978,5.968,5.998,6.002,5.998,6.008,6.024,5.992,6.022,0,0,0,0,6.05,6.096,6.076,6.094,6.058,6.046,6.048,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3.81,3.798,0,0,0,0,0,0,5.672,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.846,5.822,5.762,5.708,5.696,5.72,5.744,5.76,5.782,5.744,5.692,5.662,5.664,0,0,0,0,0,0,0,3.91,3.922,0,0,0,5.066,4.976,5.03,4.926,4.848,4.828,4.782,4.756,4.712,0,0,0,3.856,3.84,3.818,3.782,3.78,3.784,3.78,3.768,3.778,3.772,3.792,3.776,3.8,3.734,3.54,3.504,3.466,3.454,3.436,3.428,3.43,3.422,3.428,3.42,3.432,3.434,3.454,3.448,3.476,3.488,3.51,3.534,3.496,3.354,3.168,3.142,3.146,3.142,3.15,3.134,3.13,3.186,3.182,3.218,3.202,3.23,3.252,3.27,3.33,3.416,3.446,3.452,3.454,3.438,3.416,3.394,3.316,3.29,0,0,0,2.744,2.748,2.742,2.764,2.82,2.906,2.938,2.932,2.954,2.944,3.016,3.022,3.072,3.1,3.078,3.062,3.016,2.96,2.956,2.956,2.97,2.996,3.104,3.098,0,0,0,2.58,2.56,2.554,2.56,2.578,2.564,2.572,2.55,2.542,2.508,2.538,2.546,2.532,2.512,2.476,2.452,2.466,2.462,2.484,2.5,2.504,2.488,2.514,2.53,2.57,2.602,2.546,2.526,2.542,2.554,2.556,2.566,2.544,2.522,2.508,0,0,0,2.914,2.874,2.878,2.908,2.922,2.93,2.93,2.906,2.922,2.912,2.928,2.924,2.918,2.916,2.922,2.92,2.918,2.914,2.922,2.91,2.922,2.926,2.932,2.92,2.926,2.93,2.924,2.93,2.944,2.934,2.934,2.928,2.93,2.918,2.93,2.922,2.93,2.942,2.95,2.942,2.942,2.938,2.948,2.95,2.954,2.952,2.952,2.964,2.946,2.966,2.966,2.96,2.98,2.982,2.986,2.958,2.956,0,0,0,2.622,2.586,2.598,2.59,2.65,2.616,2.676,2.69,2.666,2.632,0,0,0,0,0,0,0,0,0,0,0,0,3.166,3.162,0,0,0,0,0,0,0,0,2.808,0,0,0,0,0,0,3.168,3.17,3.226,3.258,3.264,3.292,3.334,3.398,3.42,3.364,3.266,3.244,3.422,3.454,3.446,3.448,3.356,3.314,3.554,3.574,3.424,3.552,3.578,3.476,3.476,3.37,3.24,3.24,3.242,3.228,3.248,3.44,3.458,0,0,0,0,0,0,0,3.654,3.696,3.71,3.712,3.696,3.668,3.64,3.626,3.576,3.45,3.474,3.498,3.492,3.492,3.47,0,0,0,2.834,2.808,2.804,2.772,2.766,2.726,0,0,0,1.584,1.562,1.562,1.584,0,0,0,2.638,2.62,2.594,2.59,2.598,2.592,2.558,2.554,2.546,2.534,2.55,2.534,2.516,2.468,2.432,2.474,2.49,2.502,2.492,2.49,2.496,2.488,2.48,2.46,2.46,2.438,2.294,2.244,2.23,2.256,2.368,2.42,2.428,2.426,2.414,2.412,2.408,2.386,2.388,2.374,2.296,2.334,2.358,2.372,2.36,2.342,2.348,2.344,2.348,2.334,2.336,2.332,2.318,2.33,2.312,2.312,2.302,2.316,2.284,2.304,2.292,2.288,2.278,2.282,2.286,2.264,2.236,2.198,2.262,2.27,2.27,2.258,2.246,2.26,2.238,2.246,2.252,2.248,2.246,2.248,2.242,2.25,2.238,2.234,2.228,2.238,2.234,2.226,2.22,2.222,2.22,2.222,2.218,2.222,2.226,2.188,2.156,2.208,2.22,2.224,2.22,2.234,2.226,2.202,2.224,2.22,2.204,2.218,2.224,2.224,2.226,0,0,0,2.484,2.392,2.452,2.556,0,0,0,2.026,1.982,1.964,1.94,1.926,0,0,0,1.36,1.368,1.388,1.396,1.374,1.384,1.372,1.382,1.384,1.39,1.392,1.38,1.388,1.39,1.412,1.396,1.408,1.408,1.418,1.402,1.418,1.414,1.418,1.434,1.414,1.414,1.432,1.43,1.442,1.454,0,0,0,1.788,1.706,1.68,1.678,1.584,1.53,1.462,1.474,1.46,1.46,1.4,1.346,1.334,1.316,1.3,0,0,0,0,0,0.428,0.428,0.408,0.41,0.422,0.426,0,0,0,0,0,0,1.192,1.212,1.198,1.172,1.168,1.154,1.144,1.138,1.122,1.114,1.1,1.086,1.088,1.074,1.06,1.066,1.06,1.04,1.046,1.028,0,0,

def clear(filename):
	#保存雷达静态地图数据
    lidar_all = []
    #每帧768个点
    n = 0
    while n < 768:
        lidar_lists = []
        with open(filename, "r")as f:
            for line in f.readlines():
                s = line[13:]
                s_new = s.split(',')
                # 存100帧内对应点的所有数据
                lidar_lists.append(int(round(float(s_new[n]), 2) * 100))
        lidar_lists = np.array(lidar_lists)
        # 获取100帧内对应点出现最多的数据，认为是可信任的点
        lidar_all.append(np.argmax(np.bincount(lidar_lists)))
        n = n + 1
    return lidar_all

二、雷达数据转换为（x，y，z）写入TXT文件，为后续生成点云做准备

# s1为传入的地图数据，s2为要写的点云数据
def get_points(s1, s2):
    with open(s2, "w+")as f0:
    	# 首先进行静态地图点的提取
        lidar_all = clearData.clear(s1)
        angle = 0.0
        angle_increment = 0.006135923
         # 根据距离和角度，变换为二维坐标上的点
        for r in lidar_all:
            x = math.trunc(r * math.cos(angle + (-90.0 * 3.1416 / 180.0)))
            y = math.trunc(r * math.sin(angle + (-90.0 * 3.1416 / 180.0)))
            angle = angle + angle_increment
            # 手动添加z轴数据，并写入txt
            s = str(x) + " " + str(y) + " " + str(0) + "\n"
            f0.write(s)
        return 0

三、脚步点的提取

import numpy as np
from fuc.PeiZhun import get_points
import open3d as o3d

# 去除离群点
def remove_point(cloud):
    cl, ind = cloud.remove_radius_outlier(nb_points=10, radius=0.005)
    inlier_source = cloud.select_by_index(ind)
    return inlier_source


def fliter(mapTxt, footTxt, newFootTxt):
    # 地图点点云，脚步点点云，要写的点云文件
    n = 0
    f0 = open(newFootTxt, "w+")
    fMap = open(mapTxt, "r")
    fFoot = open(footTxt, "r")
    mapLine = fMap.readlines()
    footLine = fFoot.readlines()
    sum = len(footLine)
    while n < sum:
        # print(mapLine[n])
        mapPoint = mapLine[n].split(' ')
        footPoint = footLine[n].split(' ')
        mapX = float(mapPoint[0])
        mapY = float(mapPoint[1])
        footX = float(footPoint[0])
        footY = float(footPoint[1])
        if ((mapX - footX) ** 2 + (mapY - footY) ** 2) >= 100:
            # 判断为脚步点
            s = footPoint[0] + " " + footPoint[1] + " " + str(0) + "\n"
            f0.write(s)
        n = n + 1


if __name__ == "__main__":
    s1 = '../data/map.txt' #地图点
    s2 = '../data/foot3.txt'#包含脚步的地图点
    s3 = '../data/cloud1.txt'#要写入地图点的点云txt
    s4 = '../data/cloud2.txt'#要写入包含脚步点的点云txt
    s5 = '../data/cloud3.txt'#要写入只包含脚步点的txt
    get_points(s1, s3)
    get_points(s2, s4)
    fliter(s3, s4, s5)
    # 读取txt文件，生成点云对象
    mapCloud = o3d.io.read_point_cloud("../data/cloud1.txt", format='xyz')
    footCloud = o3d.io.read_point_cloud("../data/cloud3.txt", format='xyz')
    remove_point(footCloud)
    mapCloud.paint_uniform_color([0, 1, 0])  # 点云着色
    footCloud.paint_uniform_color([1, 0, 0])
    o3d.visualization.draw_geometries([mapCloud, footCloud],
                                      window_name="脚步分离",
                                      width=1024, height=768,
                                      left=50, top=50,
                                      mesh_show_back_face=True)

完成效果图