2025-08-27
抓取之后要进行 Manipulation.
Prehensible Manipulation：先抓取，然后物体和手的运动就在一起了然后进行操作.
Non-prehensible Manipulation：把一个物体推/拖到一个什么地方，可以不用抓取/无法抓取.
4-DoF grasp：像抓娃娃机那样，只能 top-down 抓. 四个维度是 3D position 和 1D Orientation. 擅长抓取一些没有堆叠的，规则的物体.
6-DoF grasp：加上了 3D Orientation.
Parrel Gripper：传统的二指的 gripper，抓取点在二指的中心. 只有一个自由度，即开合的宽度.
Dex hand：灵巧手，本身可以有 21 个自由度.
3d vision / pose estimation
6D Pose：对于一个 object，定义其 pose 为从自身坐标系到 camera space 的 translation & rotation（和之前定义的一样）.
对于一个物体，可以直接标注其在自身参考系中的抓取方式，然后就可以直接通过 pose 来转化成 world 坐标系下的抓取.
那怎么预测出物体的 pose 呢？首先如果已知 camera 的内参，以及物体的信息，那么可以直接通过 RGB 推测位置.
如何知道 rotation？一个方法是通过 rotation regression，直接训一个网络去预测 rotation. 一个很好的问题是如何表示 rotation.
直接使用四元数会出现一个问题：四元数到 SO(3) 群的映射是不连续的，但是网络只能预测出来一个连续的东西，所以会导致很大的问题（因为要预测一个毫无道理的突变）. 实际上，一个能构成到 SO(3) 的连续映射的东西至少要是 5 维度的.
另一个方法是使用旋转矩阵的前两列，然后做 schmidt 正交化（第三列显然不用预测）. 但是者带来的一个问题是第一列和第二列是不一样的，第二列的平行于第一列的分量是不重要的.
一个相对来说最好的方法是直接预测 \(9\) 维的整个旋转矩阵，然后通过 SVD 变成旋转矩阵. 这个在上一篇已有提及. 就是 SVD 成 \(U\lambda V^T\) 之后然后将 \(U(\text{diag}\{1\ 1 \det(UV)\})V^T\) 作为得到的旋转矩阵. 这里有 det(UV) 主要是为了防止算出来 det 不对，也就是一个正负 1 的问题.
这是对于，假如旋转是在 SO3 中全局随机的情况，那么防止这个不连续性就很重要. 但如果知识一个在 identity 小领域附近的预测（比如下一秒的姿势，肯定不会变化太多），那么这种情况下四元数 / axis-angle 就是连续的，就可以使用了.
另一个方法是，不直接做 regression，而是对于每个 pixel 预测在三维空间的坐标，然后再去拟合一个 pose.
这个监督数据如何生成？一个很好的方法是，直接将物体的每个表面点的 \((x^m,y^m,z^m)\) 直接作为其颜色，然后不打光地直接渲染之后就得到了一个 pixel - 三维坐标的对应. 注意这个 \((x^m,y^m,z^m)\) 指的是物体在自身坐标系下的，而物体在 camera 坐标系下的 \((x^c,y^c,z^c)\) 可以通过深度检测进行 backprojection 算出来.
拟合的本质就是求解一个 \(X'=RX+t\).
Orthogonal Procrustes：给定差了一个正交矩阵的 \(M,N\)，求解一个正交矩阵 \(A\) 使得 \(||M^T-AN^T||_F\) 最小的. \(||X||_F=\sqrt{\sum_{i,j}X_{i,j}^2}\). 这个问题的解是：\(M^TN=UDV^T\)，则 \(A=VU^T\). 同样，由于旋转矩阵 det 为正，所以需要 \(V(\text{diag}\{1\ 1 \det(VU^T)\})U^T\).
这个平移 \(t\) 怎么办呢？一个方法是求出 \(X'\) 和 \(X\) 的几何中心 \(\bar x'\) 和 \(\bar x\)，那么 \(X'-\bar x'\) 以及 \(X-\bar x\) 就应该只差一个 \(R\) 了.
这个问题是对 outlier 非常敏感，所以需要 RANSAC 一下.
ICP：Iterative Closest Point，可以调整 \(R,t\) 使得更加贴合. 对 regression 方法产生的结果很必要.
我们令 \(Q\) 为图片拍出来的 depth point cloud，这是一个 partial point cloud. 令 \(P\) 为物体本身的 point cloud，\(P'=RP+t\)，于是我们就需要让 \(P'\) 和 \(Q\) 尽量贴合.
当然由于 \(Q\) 是 full point cloud 而 \(P'\) 是 partial 的所以需要先 RANSAC 一下，然后再开始流程.
ICP 的流程是，首先先把 data 减掉 \(\bar P\) 和 \(\bar Q\) 然后就只差一个旋转了. 然后我们对于 \(P\) 中每一个点 \(i\) 找到最近的 \(Q\) 的 correspondance. 令 \((P_{corr})_i=\text{argmin}_{q_j\in Q} ||q_j-p_i||\)，然后我们的目的就是最小化 \(||P_{corr}-RP||_F\). 那么这和 orthogonal procrustes 是同理的，直接对 \(P_{corr}P^T\) 做 SVD 即可
下面考虑解决一个问题：假如我们不知道目标物体的具体建模怎么办？一个工作是，我们可以通过一些已知建模的同 category 的物体，来完成对一整个 category 的物体的 pose 的计算.
对于同一类物体，我们可以指定一个角度，指定一个物体中心点，然后将其等比放缩使得对角线长度 \(=1\). 对于一个物体，称这样的一个坐标系为其 NOCS.
我们还是采用直接预测每个 pixel 在三维空间中的坐标的方法. 但是与之不同的是，我们这次预测的是在这个 category 的 NOCS 坐标系下的坐标. 也就是训练的时候，我们要先将每个有建模的物体的坐标转换成 NOCS，然后再做训练. 这样的一个好处是对于同类别的物体，在 NOCS 坐标系下就有很大的相似之处，而且也不用管具体的大小.
于是，我们可以直接通过 RGB 图片来预测其 NOCS，然后得到一个在 NOCS 坐标系下的点云. 然后我们再通过 depth camera 获得一个 depth point cloud，然后再做拟合. 注意这里的拟合需要加一个 scaling 的常数，即 \(P_{cam}=s\times R\times NOCS+t\).
求解这个也是简单的. 我们还是对两个点云做归一化，只不过这时候还需要除以一个标准差. 然后再 SVD 就好了.
数据一般都是自己造的.
真实数据和合成数据会有 sim2real gap. 这个可能很大！
一个减小的方法是 mixed reality generation，使用真实的背景，和合成的前景.
还有 domain randomization: 让真实世界成为合成数据的子集. 合成数据里可以出现帽子飘在大海上的情景，随便乱来一通的很大的数据集中，让 test 成为 train 的子集，虽说不优，但是可行.
NOCS 工作的一个很大的 sim2real gap 发生在 segmentation 上，因为合成数据的 boundary 太假了. 所以采取一个方法，就是在 segmentation 的监督上加上 coco 的数据
grasping
force closure：gripper 可以发力，使得增加一点摩擦力，然后物体的重力方向任意改变都不会移动
form closure：gripper 不用发力，抓取的姿势直接锁住了物体
一个成功的抓取 \(\subset\) force closure \(\subset\) form closure
自锁：一个物体在水平面上，以 \(\theta\) 为入射角的力去推它，那么显然考虑因为有摩擦力，可以推得 \(\theta<\arctan \mu\) 时候，无论力多大物体都不会动.
Friction Cone：考虑在三维的世界里，所有这样的推动方向形成了一个圆锥（即与法线夹角 \(<\arctan \mu\) 的所有方向），称为 Friction Cone（摩擦椎）.
Force Closure 的数学定义：考虑有若干个接触点，每个接触点都会带一个 friction cone. 每个 friction cone 可以用一个六棱锥去近似，即近似成由 \(6\) 个力 span 得到的空间. 每一个力是一个六维向量（因为在 \(x,y,z\) 维度上的力以及力矩，总共 \(6\) 个维度）. 我们将所有的力给写成一个 \(6\times n\) 的矩阵 \(F\)，那么我们就要求对于任意的 \(f\)，存在一个正 \(k\) 使得 \(Fk=f\). 为了满足这个，我们只需要存在正 \(k\) 使得 \(Fk=0\) 就行了. 当然这是在 \(k\) 可以无限大的情况下的.
这个作用是可以在 synthetic data 中标注一个 grasp 是否是好的. 标注同样可以用 physical stimulator，但是仿真模拟的一个局限在于只能往上或者斜着去提，有时候得往别的方向抓或者怎么样. 而如果能 force closure，那么无论未来怎么拽怎么提都没有问题. 当然对于 physical stimulator，一个好方法是设置不同的重力方向. 无论如何，这两者是相辅相成的.
GraspNet-1Billion 的抓取数据集的生成方式如下：
首先对于每个物体，在点云上用 FPS 采样一些抓取点以及角度（法线方向）. 抓取点的意思是 gripper 的中心位置. 然后用 Force Closure 去删掉不合法的抓取. 这样我们获取了每个物体的若干种抓取方式.
然后设置两百个场景，每个场景都有若干个物体，以及从若干个角度拍摄. 对于每个场景，手动标注物体的 pose（满足从任何角度看都重合则代表其正确）.
对于每个场景，一个物体抓取成功，还需要满足和其他物体没有碰撞，于是对物体的 mesh 做碰撞检测.
于是得到的数据大小为：场景数量 $$ 角度数量 \(\times\) 物体数量 $$ 每个物体抓取方式数量.
注意这是只能对已知三维点云训练. 由于桌子都是绿底的，所以没法对 RGB 进行泛化.
实际上，现在可以看到，所有的场景都可以虚拟搭建，物体也可以是任意的模型.
这个数据集的一个需要注意的地方：首先摩擦系数 \(\mu\) 越小，成功的抓取应该是 \(\mu\) 大的时候的真子集. 这个数据集从 \(\mu=0.8\) 到 \(0.1\) 都标注了那些成功抓取. 但是一个问题是，这个 1Billion 是针对 \(\mu=0.8\) 的，而对于 \(\mu=0.1\)，只有 5M 个数据.
grasp detection
指标：success rate，percent cleared，planning time
VGN：Volumetric Grasping Network. 输入一个 voxel grid（grid 中每个数写的是到表面的距离）（称之为 TSDF），然后对于每个 voxel，输出 quality（是否能抓），orientation（gripper 的旋转，用四元数表达），width（gripper 张开的 width）
为了获取 TSDF，就需要拍六张照片，形成 complete input，所以这个会比较耗费时间.
训练的网络就是一个 encoder-decoder. 对于 quality，监督就是，如果有 label 落进来，就是 1，否则就是 0.
需要做一点后处理：quality 需要做一个 gaussian smooth；然后如果一个位置的 TSDF 值比 finger depth 还大那么肯定是 false positive，要去掉；然后 NMS.
GS-Net
GS-Net 采用点云的表达.
首先一个 grasp 是一个 6 DoF 的：坐标有 3 个 DoF，可以采用表面的一个 point + 一个往里面的深度 depth 的方式；旋转有 3 个 DoF，可以采用，首先对于每个 point，在其表面的一个半球上，采 256 个点，作为方向，这是 2 DoF 的；然后还有 gripper 本身的一个 rotation，是 1 DoF 的.
这四个东西可以有一个先后顺序：首先找到 Point，然后再找到方向，然后再去看 depth 和 rotation. 对于给定的 point 和方向，我们称此为一个 view. 对于每个 view，有自己的 view quality；然后对于每个 point，根据自己的所有 view 的 quality 可以再得到一个自己的 point quality. 推理的时候就先选 point quality 好的，然后再去选方向，然后再选 view quality 好的，然后再去看每个 rotation 和 depth 是否可行.
预测 pointwise quality 的时候直接用一个 pointcloud encoder-decoder，L2 Loss 做监督学习. 同理，我们也可以得到 view quality. 然后得到 selected views 之后就要找 depth and rotation. 做这一步的时候使用一个叫做 cylinder group：所有的沿着这个 view 的 rotation 和 depth 会形成一个圆柱，然后将这个圆柱内的所有点的 feature 给拉过来，做一次 pointnet，预测每个 angle 和 depth 是否会成功.
这个网络泛化性的来源，在于最后一步其实只是看局部，然后做抉择，于是网络学习的是，对于一个局部的几何，是否好抓.
Grasp Net
当然，为什么不能将抓取理解成一个生成任务呢？
注意为什么不能是 regression：因为有很多种抓取方法，你 regression 出来的很有可能是一个 average 的 mode，which 可能不是任何一个有效的抓取. 那显然是错的.
一个对于灵巧手的抓取的模型是：首先先对点云计算每个位置的 graspness（即 quality），然后从 graspness 比较高的位置中去 FPS 出一些位置. 然后对于每个位置，取出一个领域的 feature，然后将这个 feature 喂给一个 diffusion model. Diffusion model 会生成一个长度为 21 的东西，代表着灵巧手的 21 个 DoF. 而由于 diffusion model 可以得到生成出来的东西的概率，也就是说可以基于这个去抉择使用哪个抓取.
这个网络的流程是：首先，用 sparse conv 去 predict graspness（这个点可以不可以抓） 和 objectness（即是背景还是前景）. 然后，先去用 diffusion model 生成抓取的位姿 \(T,R\)，然后再用 regresssion 预测 joint angle \(\theta\)
深度修复
一个特殊的东西：我们的 depth sensor 在处理高光/透明物体的时候会倒闭. 有一项工作用 learning base 的方法去做了一个关于高光/透明物体的深度复原.
肯定是使用合成数据. 这里的 domain randomization 有以下：物体的 layout，物体的材质，background，illumination（这个很重要也很容易被忘记），camera viewpoint.
manipulation
Affordance：可攻性，哪些地方可以去 manipulate
Where2act 用 learning 的方法去预测 affordance.
做法就是随便弄弄，然后把成功的方式记录下来，然后再用这些数据去训练.
Pipeline：使用 2d RGB image（通过 unet 得到 pixel 信息）以及 3d 点云（通过 PN++ 得到每个点的信息），然后去看每个点能做的 action 是什么样的（推的角度？方向？置信度？）
一个 follow-up：不仅预测 affordance，还预测轨迹（比如拉门的时候轨迹是弧线而不是直线猛拉）