gtsam OrientedPlane3的使用细节

在得到一个平面方程ax+by+cz+d=0后，将系数(a,b,c,d)传入到OrientedPlane3中进行gtsam的平面构建。
但是发现，在构建的时候是这样的：

  /// Construct from four numbers of plane coeffcients (a, b, c, d)
  OrientedPlane3(double a, double b, double c, double d) {
    Point3 p(a, b, c);
    n_ = Unit3(p);
    d_ = d;
  }

其中n_是法向量，在调用Unit3的时候，会进行归一化，也就是n_已经不等于传入的原始数据了。但是d还是没有变。假设n_=(nx,ny,nz)，此时不能直接恢复出nxx+nyy+nz*z+d=0，这个一般式的方程。
如果要用一般式，需要一些转换，利用单位法向量，以及原点到平面的距离d来进行恢复这个时候的平面方程里的d：
单位方向量是垂直于平面的向量，也是从原点，指向某个点的向量，把这个向量乘上原来原点到它的距离d，就可以得到一个点p0，这个点落在了平面上，那么此时真正的D=-n.dot(p0)

以下插图来自：https://zhuanlan.zhihu.com/p/102514602

得到了这个真正的d，计算点到平面的距离就回到正常的一般式就可以计算了：

综上，在构建了OrientedPlane3以后，如果需要进行这些计算，就需要稍微做一些封装。
如自己利用这些提取的平面进行跟踪，涉及到关联的时候，需要做一些计算。