点云规范化

需求: 使用传统方法规范化一个长方体形状的不完全点云

1. 分面

通过Open3D的平面提取算法依次提取平面, 同时还可以得到拟合平面的方程, 作为后续优化的初始值.

2. 平面规范化

对四个面创建误差方程, 注意要加上误差权重

点到面的距离最小

if (isPerpendicular) {
  // 垂直
  return (A1 * A2 + B1 * B2 + C1 * C2) * T(weight);
} else {
  // 平行
  return (normal1 * normal2 - abs(A1 * A2 + B1 * B2 + C1 * C2)) * T(weight);
}

面与面间的平行垂直误差最小

// a
residual[0] = spatialRelation(plane_a, plane_b, false);
residual[1] = spatialRelation(plane_a, plane_c);
residual[2] = spatialRelation(plane_a, plane_d);

// b
residual[3] = spatialRelation(plane_b, plane_c);
residual[4] = spatialRelation(plane_b, plane_d);

// c
residual[5] = spatialRelation(plane_c, plane_d);