SVD+QR分解求点云法向量

 利用到了QR分解求解方程组的方法

std::vectorcomputePlaneSVD(const PCLPointCloudPtr& input_pc)
{
  if (input_pc->points.size() == 0)
    return std::vector{ 0, 0, 0, 0 };

  Eigen::Vector3d center(0, 0, 0);
  int n = input_pc->points.size();
  double A, B, C, D;

  Eigen::MatrixXd mat_A = Eigen::MatrixXd::Zero(n, 3);
  center = Eigen::Vector3d (0, 0, 0);
  for (int i = 0; i < n; ++i)
  {
    center[0] += input_pc->points[i].x;
    center[1] += input_pc->points[i].y;
    center[2] += input_pc->points[i].z;
  }
  center[0] /= double(n);
  center[1] /= double(n);
  center[2] /= double(n);

  for (int i = 0; i < n; ++i)
  {
    mat_A(i, 0) = input_pc->points[i].x - center[0];
    mat_A(i, 1) = input_pc->points[i].y - center[1];
    mat_A(i, 2) = input_pc->points[i].z - center[2];
  }
  Eigen::JacobiSVD