taifyang的博客

本文介绍了如何导出DCP点云配准模型的ONNX格式。首先需要从GitHub下载开源代码，然后在model.py中注册torch.svd算子以支持ONNX导出。接着修改main.py测试脚本，在测试过程中调用torch.onnx.export方法将模型转换为ONNX格式。关键步骤包括：1）添加SVD算子的符号注册；2）在测试流程中插入模型导出代码；3）指定opset_version为13以保证兼容性。该方法可直接将训练好的PyTorch模型转换为通用的ONNX格式，便于后续部署应用。

本文对比了自实现ICP算法与PCL库ICP算法的性能差异。测试结果显示，在处理bunny点云数据时，自实现ICP耗时0.76ms，PCL耗时1.47ms；处理table_scene点云时，自实现耗时1.14ms，PCL耗时62.68ms。两种算法均能正确收敛，但自实现ICP在小规模数据上更快，而PCL在大规模数据上性能下降明显。两种算法输出的变换矩阵结果基本一致，验证了自实现ICP的正确性。完整工程已提供下载。

本文展示了使用Open3D库实现多尺度ICP配准的方法。通过源点云和目标点云("cloud_bin_0.pcd"和"cloud_bin_1.pcd")，设置不同体素大小、收敛准则和最大对应距离进行多尺度ICP配准。程序分别在CPU和CUDA设备上运行，记录运行时间和配准精度(fitness和inlier RMSE)。结果显示，迭代过程中fitness从64.02%提升至80.06%，RMSE从0.1435降至0.1089。该方法可有效实现点云配准，并支持性能分析和参数

本文介绍了对GraspNet代码库的修改过程，主要包括两部分：基础环境适配和ONNX模型导出。首先针对CUDA和PyTorch版本问题，修改了import语句和删除了冗余代码。其次为支持ONNX导出，对模型结构进行了调整，包括修改GraspNet类的前向传播逻辑、修正池化操作参数类型，以及重构pointnet2工具库。这些修改使代码能够兼容不同环境并支持模型导出，同时参考了相关部署指南确保正常运行。修改后的代码保留了原始功能，同时增加了模型转换的灵活性。

本文介绍了GRCNN机器人抓取项目的模型转换与推理过程。首先从GitHub下载项目后，将训练好的PyTorch模型转换为ONNX格式，并展示了模型结构图。然后分别使用ONNXRuntime和TensorRT进行推理：ONNXRuntime版本实现了数据预处理、模型加载、推理执行以及结果后处理；TensorRT版本则详细展示了引擎加载、显存分配和推理流程。两种方法都能成功预测抓取点位置、角度和宽度，输出结果一致。代码提供了完整的数据处理流程和推理实现，便于在机器人抓取任务中应用该模型。

参考：高翔《自动驾驶与机器人中的SLAM技术》

算法的流程基本上和原理能对得上，但是实现过程中发现该算法结果不太稳定。可能实现有些问题吧，希望有懂的大神指出来（逃~）

算法流程：（1）点云下采样（体素滤波）；（2）ransac算法分割拟合地面平面；（3）裁剪工作区域（指定空间中四个点，裁剪点云只保留在（2）中平面上的投影在四边形内部的点）；（4）再用ransac算法去除多余平面；（5）Euclidean聚类算法分割出目标物体的点云簇；（6）通过包围盒算法计算包围盒。由于物体是放在地面上，因此可以利用地面平面的法向量约束物体包围盒的朝向。具体做法如下：可视化结果：具体做法如下：可视化结果：

【代码】Apriltag码坐标系方向可视化。

【代码】只用Eigen库实现的ICP算法。

上面的程序是通过在平面上放置若干个（3个以上）Apriltag码，计算这些码中心坐标，再使用最小二乘法拟合在相机坐标系下的平面方程z = X[0]*x + X[1]*y + X[2]，该平面的法向量为(X[0], X[1], -1)。如果只有Apriltag码个数少于3则无法通过最小二乘法求解，平面的法向量可以通过单个码的M[:3, 2]确定，又平面经过M[:3,3:] 点，也可以求出方程。

发现该算法在点云初始位姿较好时可以精调优化位姿（怎么感觉和LZ之前了解的ndt算法一般用于粗配准不太一样？下面的代码写的比较粗糙，上图原理基本上都实现了。不过只实现了第一次迭代，后续迭代过程LZ看了PCL中的ndt.hpp源码，需要使用高斯牛顿法求最小值，其中牵涉到了梯度下降方向，步长等问题（可见ComputeStepLengthMt函数），在源代码里也提到了论文名称，原理比较复杂就没有继续深究了。这个算法实现下来感觉还是有些复杂的，主要是公式推导比较多，论文中的求解过程还是比较清晰的。

LZ之前的文章[ICP算法实现（C++）](https://blog.csdn.net/taifyang/article/details/113898308) 用C++实现了基础的ICP算法，由于该算法是一种迭代的优化算法，里面含有大量循环操作以及矩阵运算，可以通过使用多线程或者GPU硬件来进行加速，具体分别可以通过OpenMP和CUDA编程实现。

其中是源点，是对应的目的点，是处的单位法线向量。该式没有解析解，我们只能使用非线性最小二乘的方法求解，为了加快求解，有研究者发现当两个输入表面之间的相对方向较小时，可以使用近似的方法来有效地求解的线性最小二乘法近似非线性优化问题。使用点到平面（point-plane）误差度量的迭代最近点 (ICP) 算法已被证明比使用点到点（point-point）误差度量的算法收敛得更快。在 ICP 算法的每次迭代中，产生最小点到平面误差的相对位姿变化通常使用标准的非线性最小二乘法来解决。多次迭代后直到算法收敛。

【2】基于KDTree改进的ICP算法在点云配准中的应用研究。在此基础上修改，直接读入pcd点云，加入了迭代求解。【1】改进的ICP算法在点云配准中的应用。代码参考：slam14讲高博的代码。

采用C++实现ICP（Iterative Closest Point，迭代最近点）算法，借助了PCL库实现点云基本变换、KD-tree以及可视化部分，核心迭代部分没有调用PCL的api。代码在KD-tree搜索部分采用了openmp加速优化，在适当的数据集下运行速度能超过PCL自带的ICP算法配准api。ICP算法的原理和推导，网上有很多，在此不赘述。下面链接是我觉得讲的比较详细的，可以参考：三维点云配准 – ICP 算法原理及推导ICP算法公式推导和PCL源码解析另外，本文的代码实现过程还参考了：

【代码】Kinect-Fusion中的ICP算法。

世界坐标系原点为id=10的tag中心，物体坐标系各坐标轴朝向和世界坐标系相同，仅存在一个平移变换obj2world_T。下面的python脚本实现了通过Apriltag码（tag16h5）估计物体在相机坐标系下的位姿的功能。

在从事6D位姿估计方面的工作时，经常需要可视化各种坐标系的朝向，而使用C++编写可视化程序比较复杂，而通过python脚本可以快速完成。下面给出一个可视化pose的程序，作用是从./pose文件夹下读取保存4*4位姿矩阵的一组txt文件可视化位姿，并可视化Oxyz坐标系。介绍一个很好用的python可视化库：mayavi，其底层实现是vtk库。