#!/bin/env python
# -*- coding:utf-8 -*-
"""
# _desc:SLAM
# _author:james
# _create_time:2020-08-01
"""
import math
import struct
import sys
import matplotlib.pyplot as plot
import numpy as np
import redis
import xlrd
np.set_printoptions(threshold=sys.maxsize)
class Lidar:
"""
激光雷达参数
"""
def __init__(self):
self.angle_min = -2.351831 # 最小角度-弧度值
self.angle_max = 2.351831 # 最大角度
self.angle_increment = 0.004363 # 角度增量
self.npoints = 1079 # 每一帧的节点数
self.range_min = 0.023 # 每一个测距值的最小值
self.range_max = 60 # 每一个测距值的最大值
self.scan_time = 0.025 # 扫描时间
self.time_increment = 1.736112e-05 # 时间增量
self.angles = np.transpose([np.arange(self.angle_min, self.angle_max, self.angle_increment)])
class Map:
"""
地图
"""
class MapStruct():
def __init__(self, points, connections, keyscans):
self.points = points
self.connections = connections
self.keyscans = keyscans
def set_map(self, pose, scan):
"""
设置地图
:param pose:
:param scan:
:return:
"""
points = transform(pose, scan)
connections = []
keyscans = []
m = dict()
m["pose"] = pose
m["i_begin"] = 0
m["i_end"] = scan.shape[0] - 1 # 行数
m["loop_closed"] = True
m["loop_tried"] = False
keyscans.append(m)
return self.MapStruct(points, connections, keyscans)
class GridMap:
"""
栅格地图
"""
class GridMapStruct():
def __init__(self, occGrid, metricMap, pixelSize, topLeftCorner):
self.occGrid = occGrid
self.metricMap = metricMap
self.pixelSize = pixelSize
self.topLeftCorner = topLeftCorner
def create_grid(self, local_map, pixel_size):
"""
从点集创建占用栅格地图
:param local_map:
:param pixel_size:
:return:
"""
# 网格尺寸
min_xy = local_map.min(axis=0) - 3 * pixel_size # min(local_map)：返回x的最小值和y的最小值构成的向量(不一定是对应左下角,可能图里面没有左下角)
max_xy = local_map.max(axis=0) + 3 * pixel_size # 3*pixelSize：构成的栅格地图中占用栅格最边界离地图边界留有3个栅格单元的余量
s_grid = np.round((max_xy - min_xy) / pixel_size) + 1 # s_grid[0]为x轴向栅格数量,s_grid[1]为y轴向栅格数量
s_grid = s_grid.astype(np.int) # float转int
n = local_map.shape[0]
# hits为被占用的栅格的二维坐标 (第hits(1)块,第hits(2)块)
# 这一步使得到的栅格地图会较原始地图出现一个翻转(当点集里不存在左下角时会出现翻转)
hits = np.round((local_map - np.tile(min_xy, (n, 1))) / pixel_size) + 1
hits = hits.astype(np.int) # float转int
# 构造一个空的栅格地图
grid = np.full((s_grid[1], s_grid[0]), False, dtype=bool)
idx = np.transpose((hits[:, 0] - 1) * s_grid[1] + hits[:, 1] - 1).reshape(-1, 1)
temp_grid = grid.flatten("F") # 按列拉平一维数组
temp_grid[idx] = True
grid = temp_grid.reshape(s_grid[1], s_grid[0], order='F') # 重新组合
occGrid = grid.copy() # 深拷贝
from scipy import ndimage
# 获取grid中零元素所在的位置靠近非零元素位置的最短距离构成的矩阵
temp_array = ndimage.distance_transform_edt(1 - grid)
metricMap = np.minimum(temp_array, 10).copy()
topLeftCorner = min_xy # 栅格地图的x最小值和y最小值构成的向量的全局坐标
return self.GridMapStruct(occGrid, metricMap, pixel_size, topLeftCorner)
def pol2cart(rho, phi):
"""
从极坐标转换为笛卡尔坐标
:param rho: 距离
:param phi: 角度
:return:
"""
x = rho * np.cos(phi)
y = rho * np.sin(phi)
return x, y
def read_scan(lidar_data, idx, lidar, usableRange):
"""
读取扫描数据
:param lidar_data: 测距数据
:param idx: 扫描数据索引，行标
:param lidar: 激光参数
:param usableRange:可使用的范围
:return:
"""
angles = lidar.angles
ranges = np.transpose([lidar_data[idx, :]]) # 获取索引行扫描数据，数据行转列
maxRange = min(lidar.range_max, usableRange)
isBad = np.where((ranges[:, 0] < lidar.range_min) | (ranges[:, 0] > maxRange)) # 获取不符合条件的索引
angles = np.delete(angles, isBad) # 移除angles中不符合条件的行
ranges = np.delete(ranges, isBad) # 移除ranges中不符合条件的行
x, y = pol2cart(ranges, angles)
scan = np.c_[x, y]
return scan
def transform(pose, scan):
"""
局部坐标转化为全局坐标
:param scan: 某次扫描数据的局部笛卡尔坐标
:param pose: 当前位姿(x坐标tx y坐标ty 旋转角theta)
:return:
"""
tx = pose[0]
ty = pose[1]
theta = pose[2]
ct = math.cos(theta)
st = math.sin(theta)
a = [ct, -st]
b = [st, ct]
R = np.vstack((a, b))
tscan = np.dot(scan, R.T)
tscan[:, 0] = tscan[:, 0] + tx
tscan[:, 1] = tscan[:, 1] + ty
return tscan
def get_local_map(points, pose, scan, border_size):
"""
从全局地图中 提取当前扫描周围的局部地图 的全局坐标
:param points:
:param pose:
:param scan:
:param border_size:
:return:
"""
scan_w = transform(pose, scan) # 将当前扫描数据坐标scan转化为全局坐标scan_w
# 设置左上角和右下角
min_x = min(scan_w[:, 0] - border_size)
min_y = min(scan_w[:, 1] - border_size)
max_x = max(scan_w[:, 0] + border_size)
max_y = max(scan_w[:, 1] + border_size)
# 提取位于范围内的全局地图中的点
is_around = np.where(
(points[:, 0] > min_x) & (points[:, 0] < max_x) & (points[:, 1] > min_y) & (points[:, 1] < max_y))
local_map = points[is_around, :]
return local_map
def diff_pose(pose1, pose2):
"""
计算位姿差
:param pose1:
:param pose2:
:return:
"""
dp = pose2 - pose1
dp[2] = pose2[2] - pose1[2]
return dp
def fast_match(grid_map, scan, pose, search_resolution):
"""
根据当前位姿的栅格地图，优化预测的下一位姿，使下一位姿的栅格地图与当前位姿的栅格地图达到最大的重合度
:param grid_map: 局部栅格地图
:param scan: 构成grid_map的当前扫描点集的局部笛卡尔坐标
:param pose: 预测的下一位姿(预测得到的pose_guess)
:param search_resolution: 搜索的分辨率
:return: pose 优化过后的下一位姿,使下一位姿的栅格地图与当前位姿的栅格地图达到最大的重合度
best_hits pose对应的最佳重合度score,对应的当前位姿栅格地图的原始距离矩阵
"""
# <editor-fold desc="栅格地图信息">
metric_map = grid_map.metricMap # 栅格地图中0元素所在的位置靠近非零元素位置的最短栅格距离构成的矩阵
i_pixel = 1 / grid_map.pixelSize # 实际距离1m对应几个栅格单元边长 (栅格单元尺寸对应的实际距离的倒数)
min_x = grid_map.topLeftCorner[0] # 栅格地图中的最左端的横坐标(全局)
min_y = grid_map.topLeftCorner[1] # 栅格地图中的最下端的纵坐标(全局)
n_cols = metric_map.shape[1] # metric_map的列数
n_rows = metric_map.shape[0] # metric_map的行数
# </editor-fold>
# <editor-fold desc="爬山算法">
max_iter = 50 # 最大循环次数 50
max_depth = 3 # 提高分辨率次数的最大值 3
iter = 0 # 循环变量
depth = 0 # 分辨率提高次数变量
pixel_scan = scan * i_pixel # 将扫描数据实际坐标转为栅�
- 1
- 2
- 3
- 4
前往页