使用谷神星优化两组3D点之间的转换

Question

我想实现一个优化问题，以模拟具有比例，旋转和平移的3D相似度矩阵，该矩阵将一组3D点转换为另一组3D点（严格但具有比例）。如果Pg_i是来自集合1的点，并且如果Pl_i是来自集合2的点，则我要计算一个转换T（Pl_i）= sR（Pl_i）+ t，这将导致Pg_i并使成本函数最小化。

i上的总和| Pg_i-T（Pl_i）| ^ 2

我知道有很多库可以为此类问题提供最佳解决方案。但是我想在Ceres中实现此成本函数，因为我将其与另一个成本函数一起使用。

我已经使用sRT [7]作为ceres中的参数变量。 sRT [0，1，2]是角轴旋转，sRT [3,4,5]是平移，sRT [6]是刻度。我的解决方案收敛，但是即使我开始接近该解决方案，也无法接近正确的解决方案。我的成本函数有问题吗？假设没有编程错误，这是如何估计点集之间的比例，旋转和平移的？还是有更多受约束的方法？请评论我编码成本函数的方法。

struct F2 {

    F2(double Xg_, double Yg_, double Zg_,double Xl_, double Yl_, double Zl_):
                                        Xg(Xg_), Yg(Yg_),Zg(Zg_),
                                        Xl(Xl_), Yl(Yl_), Zl(Zl_){}

  template <typename T> bool operator()(const T* const sRT,
                            T* residual) const {
    // f2
        T ip[3];
        ip[0] = T(Xg);
        ip[1] = T(Yg);
        ip[2] = T(Zg);

        T op[3];

        //scale
        ip[0] *= sRT[6];
        ip[1] *= sRT[6];
        ip[2] *= sRT[6];

          //rotate
        ceres::AngleAxisRotatePoint(sRT, ip, op);

        //translate
        op[0] += sRT[3];
        op[1] += sRT[4];
        op[2] += sRT[5];

      residual[0] = (T)Xl -  op[0];
      residual[1] = (T)Yl -  op[1];
      residual[2] = (T)Zl -  op[2];

    return true;
  }

private:
    const double Xg, Yg, Zg;
    const double Xl, Yl, Zl;
};