pointcloud_homework2

最新推荐文章于 2022-01-05 18:46:58 发布
最新推荐文章于 2022-01-05 18:46:58 发布
阅读量446 收藏
点赞数 1
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/dtuyg/article/details/105822568
版权

task

for one 𝑁 × 3 point cloud from KITTI
8-NN search for each point to the point cloud
Implement 3 NN algorithms

  • Numpy brute-force search
  • scipy.spatial.KDTree
  • own kd-tree in python

codes revised from（代码的借鉴来源） https://github.com/lijx10/NN-Trees.

solution

在google colab上完成并运行

catalogue

1.读取二进制数据文件

文件为KITTI数据集中的.bin二进制数据，需要读取raw data的激光雷达的三维坐标信息

import os 
import struct
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def read_velodyne_bin(path):
    '''
    :param path:
    :return: homography matrix of the point cloud, N*3
    '''
    pc_list = []
    with open(path, 'rb') as f:
        content = f.read()
        pc_iter = struct.iter_unpack('ffff', content)
        for idx, point in enumerate(pc_iter):
            pc_list.append([point[0], point[1], point[2]])
    return np.asarray(pc_list, dtype=np.float32)

path='/content/drive/My Drive/lesson2code/000000.bin'
origindata=read_velodyne_bin(path)

2.构造一种可记录并判断N个最近邻的容器的函数


import copy


class DistIndex:
    def __init__(self, distance, index):
        self.distance = distance
        self.index = index

    def __lt__(self, other):
        return self.distance < other.distance


class KNNResultSet:
    def __init__(self, capacity):
        self.capacity = capacity
        self.count = 0
        self.worst_dist = 1e10
        self.dist_index_list = []
        for i in range(capacity):
            self.dist_index_list.append(DistIndex(self.worst_dist, 0))

        self.comparison_counter = 0

    def size(self):
        return self.count

    def full(self):
        return self.count == self.capacity

    def worstDist(self):
        return self.worst_dist

    def add_point(self, dist, index):
        self.comparison_counter += 1
        if dist > self.worst_dist:
            return

        if self.count < self.capacity:
            self.count += 1

        i = self.count - 1
        while i > 0:
            if self.dist_index_list[i-1].distance > dist:
                self.dist_index_list[i] = copy.deepcopy(self.dist_index_list[i-1])
                i -= 1
            else:
                break

        self.dist_index_list[i].distance = dist
        self.dist_index_list[i].index = index
        self.worst_dist = self.dist_index_list[self.capacity-1].distance
        
    def __str__(self):
        output = ''
        for i, dist_index in enumerate(self.dist_index_list):
            output += '%d - %.2f\n' % (dist_index.index, dist_index.distance)
        output += 'In total %d comparison operations.' % self.comparison_counter
        return output

3.numpy暴力搜索法

import random
import math
import numpy as np
import time

def bruteSearch(db: np.ndarray,result_set:KNNResultSet, query: np.ndarray):
    
    diff = np.linalg.norm(np.expand_dims(query, 0) - db, axis=1)
    nn_idx = np.argsort(diff)
    nn_dist = diff[nn_idx]
    for idx, distindex in enumerate(result_set.dist_index_list):
        distindex.distance=nn_dist[idx]
        distindex.index=nn_idx[idx]
    return False
k=8
brute_time_sum=0
for ind ,p in enumerate(origindata):
    begin_t = time.time()
    result_set = KNNResultSet(capacity=k)
    bruteSearch(origindata,result_set,p)
    brute_time_sum += time.time() - begin_t
print('brute:',brute_time_sum)


使用colab里的GPU运行上述片断，耗时1348.81秒

4.scipy的kdtree模块

from scipy import spatial
import time

k=8

def scipyKdtreeSearch(tree:spatial.KDTree,result_set:KNNResultSet,point: np.ndarray):
    scipy_nn_dis,scipy_nn_idx=tree.query(point,result_set.capacity)
    for idx, distindex in enumerate(result_set.dist_index_list):
        distindex.distance=scipy_nn_dis[idx]
        distindex.index=scipy_nn_idx[idx]
    return False


construction_time_sum = 0
knn_time_sum = 0

begin_t = time.time()
tree = spatial.KDTree(origindata)
construction_time_sum += time.time() - begin_t

for ind ,p in enumerate(origindata):
    begin_t = time.time()
    result_set = KNNResultSet(capacity=k)
    scipyKdtreeSearch(tree,result_set,p)
    knn_time_sum += time.time() - begin_t
    
print('scipykdtreebuilding:',construction_time_sum)
print('scipykdtree:',knn_time_sum)
使用colab里的GPU运行上述片断，建树与查找共耗时114.28秒

5.kdtree的python实现1

此情况为切割位置没有点的kdtree实现

在这里插入图片描述


# kdtree的具体实现，包括构建和查找

import random
import math
import numpy as np
import time


# Node类，Node是tree的基本组成元素
class Node:
    def __init__(self, axis, value, left, right, point_indices):
        self.axis = axis
        self.value = value
        self.left = left
        self.right = right
        self.point_indices = point_indices

    def is_leaf(self):
        if self.value is None:
            return True
        else:
            return False

    def __str__(self):
        output = ''
        output += 'axis %d, ' % self.axis
        if self.value is None:
            output += 'split value: leaf, '
        else:
            output += 'split value: %.2f, ' % self.value
        output += 'point_indices: '
        output += str(self.point_indices.tolist())
        return output

# 功能：构建树之前需要对value进行排序，同时对一个的key的顺序也要跟着改变
# 输入：
#     key：键
#     value:值
# 输出：
#     key_sorted：排序后的键
#     value_sorted：排序后的值
def sort_key_by_vale(key, value):
    assert key.shape == value.shape
    assert len(key.shape) == 1
    sorted_idx = np.argsort(value)
    key_sorted = key[sorted_idx]
    value_sorted = value[sorted_idx]
    return key_sorted, value_sorted


def axis_round_robin(axis, dim):
    if axis == dim-1:
        return 0
    else:
        return axis + 1

# 功能：通过递归的方式构建树
# 输入：
#     root: 树的根节点
#     db: 点云数据
#     point_indices：排序后的键
#     axis: scalar
#     leaf_size: scalar
# 输出：
#     root: 即构建完成的树
def kdtree_recursive_build(root, db, point_indices, axis, leaf_size):
    if root is None:
        root = Node(axis, None, None, None, point_indices)

    # determine whether to split into left and right
    if len(point_indices) > leaf_size:
        # --- get the split position ---
        point_indices_sorted, _ = sort_key_by_vale(point_indices, db[point_indices, axis])  # M
        
        # 作业1
        # 屏蔽开始
        middle_left_idx = math.ceil(point_indices_sorted.shape[0] / 2) - 1
        middle_left_point_idx = point_indices_sorted[middle_left_idx]
        middle_left_point_value = db[middle_left_point_idx, axis]
        middle_right_idx = middle_left_idx + 1
        middle_right_point_idx = point_indices_sorted[middle_right_idx]
        middle_right_point_value = db[middle_right_point_idx, axis]
        root.value = (middle_left_point_value + middle_right_point_value) * 0.5


        root.left=kdtree_recursive_build(root.left,db,point_indices_sorted[0:middle_right_idx],axis_round_robin(axis, dim=db.shape[1]),leaf_size)
        root.right=kdtree_recursive_build(root.right,db,point_indices_sorted[middle_right_idx:],axis_round_robin(axis, dim=db.shape[1]),leaf_size)
        # 屏蔽结束
    return root


# 功能：翻转一个kd树
# 输入：
#     root：kd树
#     depth: 当前深度
#     max_depth：最大深度
def traverse_kdtree(root: Node, depth, max_depth):
    depth[0] += 1
    if max_depth[0] < depth[0]:
        max_depth[0] = depth[0]

    if root.is_leaf():
        print(root)
    else:
        traverse_kdtree(root.left, depth, max_depth)
        traverse_kdtree(root.right, depth, max_depth)

    depth[0] -= 1

# 功能：构建kd树（利用kdtree_recursive_build功能函数实现的对外接口）
# 输入：
#     db_np：原始数据
#     leaf_size：scale
# 输出：
#     root：构建完成的kd树
def kdtree_construction(db_np, leaf_size):
    N, dim = db_np.shape[0], db_np.shape[1]

    # build kd_tree recursively
    root = None
    root = kdtree_recursive_build(root,db_np,np.arange(N),axis=0,leaf_size=leaf_size)
    return root


# 功能：通过kd树实现knn搜索，即找出最近的k个近邻
# 输入：
#     root: kd树
#     db: 原始数据
#     result_set：搜索结果
#     query：索引信息
# 输出：
#     搜索失败则返回False
def kdtree_knn_search(root: Node, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray):
    if root is None:
        return False

    if root.is_leaf():
        # compare the contents of a leaf
        leaf_points = db[root.point_indices, :]
        diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1)
        for i in range(diff.shape[0]):
            result_set.add_point(diff[i], root.point_indices[i])
        return False

    # 作业2
    # 提示：仍通过递归的方式实现搜索
    # 屏蔽开始
    if query[root.axis]<=root.value:
        kdtree_knn_search(root.left,db,result_set,query)
        if math.fabs(query[root.axis]-root.value)<result_set.worstDist():
            kdtree_knn_search(root.right,db,result_set,query)
    else:
        kdtree_knn_search(root.right,db,result_set,query)
        if math.fabs(query[root.axis]-root.value)<result_set.worstDist():
            kdtree_knn_search(root.left,db,result_set,query)
    

    # 屏蔽结束

    return False

def main():
    # configuration
    leaf_size = 10
    k = 8

    db_np = origindata
    construction_time_sum = 0
    begin_t = time.time()
    root = kdtree_construction(db_np, leaf_size=leaf_size)
    construction_time_sum += time.time() - begin_t
    
    # depth = [0]
    # max_depth = [0]
    # traverse_kdtree(root, depth, max_depth)
    # print("tree max depth: %d" % max_depth[0])


    
    knn_time_sum = 0
    for ind ,p in enumerate(origindata):
        begin_t = time.time()
        result_set = KNNResultSet(capacity=k)
        kdtree_knn_search(root, db_np, result_set, p)
        knn_time_sum += time.time() - begin_t
        
    print('kdtreebuilding:',construction_time_sum)
    print('kdtree:',knn_time_sum)
    


if __name__ == '__main__':
    main()



使用colab的GPU建树与查找共耗时199.05秒

6.kdtree的python实现2

此情况为切割位置有点的kdtree实现
在这里插入图片描述


# kdtree的修改的具体实现，包括构建和查找

import random
import math
import numpy as np
import time


# Node类，Node是tree的基本组成元素
class Node:
    def __init__(self, axis, valueindex, left, right, point_indices):
        self.axis = axis
        self.valueindex = valueindex
        self.left = left
        self.right = right
        self.point_indices = point_indices

    def is_leaf(self):
        if len(self.point_indices)==0:
            return True
        else:
            return False

    def __str__(self):
        output = ''
        output += 'axis %d, ' % self.axis
        if self.valueindex is None:
            output += 'split value: leaf, '
        else:
            output +=( 'split valueindex: ' + str(self.valueindex))
        output += 'point_indices: '
        output += str(self.point_indices.tolist())
        return output

# 功能：构建树之前需要对value进行排序，同时对一个的key的顺序也要跟着改变
# 输入：
#     key：键
#     value:值
# 输出：
#     key_sorted：排序后的键
#     value_sorted：排序后的值
def sort_key_by_vale(key, value):
    assert key.shape == value.shape
    assert len(key.shape) == 1
    sorted_idx = np.argsort(value)
    key_sorted = key[sorted_idx]
    value_sorted = value[sorted_idx]
    return key_sorted, value_sorted


def axis_round_robin(axis, dim):
    if axis == dim-1:
        return 0
    else:
        return axis + 1

# 功能：通过递归的方式构建树
# 输入：
#     root: 树的根节点
#     db: 点云数据
#     point_indices：排序后的键
#     axis: scalar
#     leaf_size: scalar
# 输出：
#     root: 即构建完成的树
def kdtree_recursive_build(root, db, point_indices, axis, leaf_size):
    if root is None:
        root = Node(axis, None, None, None, point_indices)

    # determine whether to split into left and right
    if len(point_indices) > leaf_size:
        # --- get the split position ---
        point_indices_sorted, _ = sort_key_by_vale(point_indices, db[point_indices, axis])  # M
        middle_left_idx = math.ceil(point_indices_sorted.shape[0] / 2) - 1
        middle_left_point_idx = point_indices_sorted[middle_left_idx]
        middle_right_idx = middle_left_idx + 1
        
        root.valueindex=middle_left_point_idx
        
        root.left=kdtree_recursive_build(root.left,db,point_indices_sorted[0:middle_left_idx],axis_round_robin(axis, dim=db.shape[1]),leaf_size)
        root.right=kdtree_recursive_build(root.right,db,point_indices_sorted[middle_right_idx:],axis_round_robin(axis, dim=db.shape[1]),leaf_size)
        
        
    return root


# 功能：翻转一个kd树
# 输入：
#     root：kd树
#     depth: 当前深度
#     max_depth：最大深度
def traverse_kdtree(root: Node, depth, max_depth):
    depth[0] += 1
    if max_depth[0] < depth[0]:
        max_depth[0] = depth[0]

    if root.is_leaf():
        print(root)
    else:
        traverse_kdtree(root.left, depth, max_depth)
        traverse_kdtree(root.right, depth, max_depth)

    depth[0] -= 1

# 功能：构建kd树（利用kdtree_recursive_build功能函数实现的对外接口）
# 输入：
#     db_np：原始数据
#     leaf_size：scale
# 输出：
#     root：构建完成的kd树
def kdtree_construction(db_np, leaf_size):
    N, dim = db_np.shape[0], db_np.shape[1]

    # build kd_tree recursively
    root = None
    root = kdtree_recursive_build(root,db_np,np.arange(N),axis=0,leaf_size=leaf_size)
    return root


# 功能：通过kd树实现knn搜索，即找出最近的k个近邻
# 输入：
#     root: kd树
#     db: 原始数据
#     result_set：搜索结果
#     query：索引信息
# 输出：
#     搜索失败则返回False
def kdtree_knn_search(root: Node, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray):
    if root is None:
        return False
            
    diff=np.linalg.norm(query-db[root.valueindex])
    
    result_set.add_point(diff, root.valueindex)
    
    if root.is_leaf():
        return False

    
    if query[root.axis]<=db[root.valueindex][root.axis]:
        kdtree_knn_search(root.left,db,result_set,query)
        if math.fabs(query[root.axis]-db[root.valueindex][root.axis])<result_set.worstDist():
            kdtree_knn_search(root.right,db,result_set,query)
    else:
        kdtree_knn_search(root.right,db,result_set,query)
        if math.fabs(query[root.axis]-db[root.valueindex][root.axis])<result_set.worstDist():
            kdtree_knn_search(root.left,db,result_set,query)
    

    

    return False

def main():
    # configuration
    leaf_size = 10
    k = 8

    db_np = origindata
    construction_time_sum = 0
    begin_t = time.time()
    root = kdtree_construction(db_np, leaf_size=leaf_size)
    construction_time_sum += time.time() - begin_t
    
    # depth = [0]
    # max_depth = [0]
    # traverse_kdtree(root, depth, max_depth)
    # print("tree max depth: %d" % max_depth[0])

    
    
    
    knn_time_sum = 0
    for ind ,p in enumerate(origindata):
        begin_t = time.time()
        result_set = KNNResultSet(capacity=k)
        kdtree_knn_search(root, db_np, result_set, p)
        knn_time_sum += time.time() - begin_t
        
    print('kdtreebuilding:',construction_time_sum)
    print('kdtree:',knn_time_sum)
    


if __name__ == '__main__':
    main()
dtuyg
关注
点云算法-提取kitti路面点云
cangqiongxiaoye的博客
01-02 1657
点云算法-提取kitti路面点云
点云算法-python读取kitti数据并显示
cangqiongxiaoye的博客
12-29 549
点云算法-python读取kitti数据并显示
三维点云学习（2）五种算法比较
Sun的博客
02-20 911
三维点云学习（2）五种算法比较 代码参考来自 黎老师github 本次测试包含五种算法比较： octree print("octree --------------") #时间统计 constructiontimesum = 0 knntimesum = 0 radiustimesum = 0 brutetimesum = 0 # #construction begint = time.time() root = octree
三维点云学习（4）6-ransac 地面分割
Sun的博客
02-23 2164
三维点云学习（4）6-ransac 地面分割 ransac课堂笔记git大神参考代码 ransac代码主要参考如下知乎大佬的ransac的线性拟合ransac的线性拟合 使用ransac进行地面分割的原因： ransac模型的拟合的取决于物体的密度分布，在三维场景较为适合 ransac地面分割效果图 原点云图： 分割出的地面： eg:因为ransac是个人复现，所以效果不是很理想，ransac具有随机性，所以每次运行的结果不尽相同 红色为分割地面后的点云，蓝色为分割出的地面 ransac代码块：
点云课程学习——点云结构与查找（三）
张飞飞的博客
04-24 560
```python # 对数据集中的点云，批量执行构建树和查找，包括kdtree和octree，并评测其运行时间 import random import math import numpy as np import time import os import struct import octree as octree import kdtree as kdtree from result...
pointcloud_homework4
01-20
task Use KITTI 3D object detection dataset, do the followings. ...Step 2. Clustering over the remaining points. Visualize the clusters with random colors. • Any method you want solution
newsgroup_txt
weixin_44714173的博客
07-29 2万+
Why is the mirror of journey to the West useless to monkey king? |||0 First couplet: one night spring breeze to go, how to the second couplet? |||0 What do relatives say makes you most disgusted? |||0 The second couplet: three mountains and five mountains
电子邮件链接mailto_“ 3个链接”电子邮件的历史记录以及曾经发送的每个链接的存档...
cumi7754的博客
07-17 3万+
电子邮件链接mailtoby Quincy Larson 昆西·拉尔森(Quincy Larson) “ 3个链接”电子邮件的历史记录以及曾经发送的每个链接的存档 (The history of the “3 links” email and an archive of every link ever sent) It was March 2016. The freeCodeCamp comm...
c++编程思想-第2章 对象的创建与使用-2
Jake2549794390的博客
03-06 9115
#include #include #include #include using namespace std; int main() {     //cout     // 1.     string word;     ifstream in("HarryPotter.txt");     while (in >> word)//每次读取输入的一
视觉SLAM作业（四） 相机模型与非线性优化
GuoBen_的博客
06-21 1207
视觉SLAM作业（四） 相机模型与非线性优化 一 图像去畸变 现实生活中的图像总存在畸变。原则上来说，针孔透视相机应该将三维世界中的直线投影成直线，但是当我们使用广角和鱼眼镜头时，由于畸变的原因，直线在图像里看起来是扭曲的。本次作业，你将尝试如何对一张图像去畸变，得到畸变前的图像。 图1 是本次习题的测试图像（code/test.png），来自EuRoC 数据集[1]。可以明显看到实际的柱子、箱子的直线边缘在图像中被扭曲成了曲线。这就是由相机畸变造成的。根据我们在课上的介绍，畸变前后的坐标变换为： xdis
【KITTI数据集】Velodyne Point Cloud-激光雷达点云bin文件读取和显示-Open3D
Doraemon_Zzn的博客
05-12 5229
官方文档 0.安装 在anaconda环境下： conda install -c open3d-admin open3d Python环境下： pip install open3d-python 1.点云的读取和显示 import os import numpy as np import struct import open3d def read_bin_velodyne(path): pc_list=[] with open(path,'rb') as f: cont
Python算法：Brute-Force算法查找字符串子串位置
彭世瑜的博客
07-20 2776
Brute-Force算法， 简称为 BF算法，是一种简单朴素的模式匹配算法，常用于在一个主串 S 内查找一个子串 T 的出现位置。 它的核心思想与操作是： 对于给定的主串 S 与子串 P ，主串 S 的长度为 N，子串 T 的长度为 M ； 首先，将 S[1] 和 T[1] 进行比较； 若相等，则再比较 S[2] 和 T[2] ，一直到 T[M] 为止； 若 S[1] 和 T[1] 不等，则 ...
【转】Brute-Force匹配器基础
weixin_44190201的博客
03-14 907
作者：xxxss 链接：https://www.jianshu.com/p/ed57ee1056ab Brute-Force匹配器基础 Brute-Force匹配器很简单，它取第一个集合里一个特征的描述子并用第二个集合里所有其他的特征和他通过一些距离计算进行匹配。最近的返回。 对于BF匹配器，首先我们得用cv2.BFMatcher()创建BF匹配器对象.它取两个可选参数， 第一个是normType...
三维点云学习（4）7-ransac 地面分割+DBSCAN聚类
weixin_41281151的博客
07-26 4667
三维点云学习（4）7-ransac 地面分割+ DBSCAN聚类比较 回顾： 实现ransac地面分割 DBSCNA python 复现-1- 距离矩阵法 DBSCNA python 复现-2-kd-_tree加速 效果图： 自写dbscan聚类效果 因为数据点比较多，运行了10min左右 (cpu：AMD 3700x) 自写的聚类效果并不是很好 iters = 31.320905 生成的聚类个数：396 sklearn 库 自带的dbscan聚类效果 使用sklearn自带库 速度很快 可以
Python使用struct处理二进制(pack和unpack用法)
热门推荐
JackyOps
09-19 5万+
python有时需要处理二进制数据，例如 存取文件，socket操作时.这时候，可以使用python的struct模块来完成.可以用struct来处理c语言中的结构体. struct模块中最重要的三个函数是pack(), unpack(), calcsize # 按照给定的格式(fmt)，把数据封装成字符串(实际上是类似于c结构体的字节流) pack(fmt, v1, v2, ...
处理3D主要曲率方向方法
qq_23158477的博客
01-05 1096
pytorch3d.ops.estimate_pointcloud_local_coord_frames(pointclouds: Union[torch.Tensor, Pointclouds], neighborhood_size: int = 50, disambiguate_directions: bool = True)→ Tuple[torch.Tensor, torch.Tensor][source] Estimates the principal directions of curvat.
【转载】用python向txt文件中写数据时的追加和覆盖问题
越努力 越幸运
09-25 4577
原文链接：https://blog.csdn.net/weixin_39963132/article/details/85197397 将计算数据保存在txt中，每次写入都是把txt文件中原来存在的内容覆盖掉了，那么如何才能在原来的基础上继续往里面添加内容呢？ file = open(pathTxt, 'w', encoding='utf-8') ##原来 file = open(pa...
Python中struct.pack()和struct.unpack()用法详细说明
u012909001的博客
07-19 8639
转载：http://www.php.cn/python-tutorials-356984.html python中的struct主要是用来处理C结构数据的，读入时先转换为Python的字符串类型，然后再转换为Python的结构化类型，比如元组(tuple)啥的~。一般输入的渠道来源于文件或者网络的二进制流。 1.struct.pack()和struct.un
pointcloud_to_laserscan
最新发布
09-08
pointcloud_to_laserscan是一种ROS（Robot Operating System）中的节点，用于将点云数据转换为激光扫描数据。它通常用于机器人感知和导航系统，帮助机器人理解周围环境。 ### 回答2： pointcloud_to_laserscan是一...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值