taifyang的博客

本文展示了使用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。该方法可有效实现点云配准，并支持性能分析和参数

预置环境：Windows10+Git+CMake3.23.3+VS2019+Qt5.14.2。

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

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

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

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

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

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

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

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

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

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

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

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

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

这里除了第一组图片（color.bmp和ir.bmp），其他的color图片和ir图片不需要一一对应（在同一场景下拍摄），可以分开采集。根据情况，分别采集若干张彩色摄像头和红外摄像头（对于带有红外摄像头进行深度测量的深度摄像头，红外摄像头和深度摄像头其实是同一个）的标定板成像图片。注意包装盒的右侧、水壶的把手以及右侧和地面接近部分的点云在rgbd对齐前后可看出较大区别。求解深度相机到彩色相机的变换矩阵的原理：（参考。第二步：求解深度相机到彩色相机的变换矩阵。第一步：标定彩色相机和深度相机。

测试了100多张图的中心线提取，100w像素gpu的计算速度比cpu快1/3左右，400w像素gpu的计算速度是cpu的3倍左右，1600w像素gpu的计算速度是cpu的10~15倍。下面放上main函数，其他文件放在下载链接里面了。毕竟是知识付费的时代，要几个积分也不算多吧哈哈。最近在学习CUDA编程，利用周末撸了一个GPU版本的中心线提取算法（灰度重心法）。

数据集增强仿射变换平移变换import numpy as npimport random#文件名old_file=r"rabbit.txt"new_file=r"rabbit_change.txt"#平移参数x_offset=random.uniform(-10, 10)y_offset=random.uniform(-10, 10)z_offset=random.uniform(-10, 10)#变换矩阵transformation_matrix=np.array([

目录数据集增强仿射变换平移变换旋转变换尺度变换仿射变换添加噪声高斯噪声随机噪声下采样指定体素指定点数数据归一化python版本实现见：点云数据常用处理：python实现数据集增强仿射变换平移变换#include <iostream>#include <fstream>#include <time.h>int main(int argc, char* argv[]){ std::ifstream infile; infile.open("rabbit

滤波、分割等预处理过程省略。几何中心法 极值法 细化法 灰度重心法 法向质心法 Steger算法

根据激光三角测量法原理写的线结构光传感器的自动标定程序，之前借助matlab和自己写的几个程序完成的步骤比较复杂，由于cpp容易部署及应用到项目中以及我懒，所以画了一个下午加晚上整了这么一个程序。标定板使用的是棋盘格标定板，标定线结构光平面方程之前需要先进行相机标定，拍摄的图像如下图所示。需要按下面要求拍摄30张图像并存入./image文件夹：图1-9为在平面a上标定板贴合平面平移的图像；图9-13为在平面a上标定板店起一定高度的不同位姿图像；图14为在平面a上激光条图像；图15为在平面a上激

环境配置：ubutnu系统+opencv+pclcamera.shuvcdynctrl -d /dev/video0 -S 6:8 '(LE)0x50ff'uvcdynctrl -d /dev/video0 -S 6:15 '(LE)0x00f6'uvcdynctrl -d /dev/video0 -S 6:8 '(LE)0x2500'uvcdynctrl -d /dev/video0 -S 6:8 '(LE)0x5ffe'uvcdynctrl -d /dev/video0 -S 6:1

pcd转txt#include <iostream>#include <fstream>#include <pcl/io/pcd_io.h> int main(int argc, char *argv[]){ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::io::loadPCDFile("rabbit.pcd

假设线扫激光已经可以得到单帧点云。滑台运动方向标定：其中(xstart,ystart,zstart)是标定板初始位姿在相机坐标系下的坐标，(xend,ystart,zstart)是标定板最终位姿在相机坐标系下的坐标。再将（△x，△y，△z）单位化即可得到滑台的运动方向。在相机坐标系下拼接的坐标变换矩阵为其中l为滑台运动的位移。转台旋转轴线标定：方法有多种，这里给出其中一种较简单的方法。通过共面参照物标定的方法测出一个平面上的特征点在旋转过程中几个位置处的空间坐标,然后对分别对每个点做圆拟

本文是在上一篇文章乐视三合一体感摄像头Astra pro开发记录1（深度图、彩色图及点云简单显示）的基础上写的。在开学之前终于把界面的基础功能做完了，估计还有一些bug，也没时间继续调了。界面的主要功能还是Astra pro相机的彩色图、深度图以及点云显示。在之前的基础上，完善了一些比如图像和点云的保存、读取，相机的打开、关闭以及参数设置等功能。由于只是追求基本功能的实现，整个项目文件代码量算上注释也就一千行左右。依赖的第三方库有OpenCV（图像处理）、OpenNI（深度图）、PCL（点云处理）和VTK

在某鱼上淘的乐视三合一体感摄像头，捡漏价九十几块，买来玩玩。网上已经有一些关于此款摄像头的开发资料。官方的开发资料：[官网链接]（https://orbbec3d.com/index/download.html)按官方网站以及其他帖子，下载并安装相机的驱动和SDK，不难配置好相机。下面这篇文章使用opencv4.5.1配置成功，本人按此方法尝试过确实可行。

对激光三角测量原理进行数学建模。以相机光心Oc为原点建立相机坐标系Oc-XcYcZc。沿OcZc轴在距离光心Oc焦距f处建立等效像素坐标系Opix-uv。设轴OcZc交Opix-uv平面于Op，建立图像坐标系Op-xpyp。设点P在像素坐标系中的对应点为(u,v)，在图像坐标系中的对应点为 (xp,yp)。设x和y方向的像元尺寸分别为dx和dy，根据像素坐标系到图像坐标系的变换，有(1)当f=1mm时，上式可写成(2)设点在相机坐标系下对应点坐标为 ，则有 (3)设结构光平面方程为(4)