void
ClosingBoundary::sampleFromBoundary (ON_NurbsSurface *nurbs, vector_vec3d &point_list, vector_vec2d &param_list,
                                     unsigned samples)
{
  double ds = 1.0 / (samples - 1);

  double minU = nurbs->Knot (0, 0);
  double maxU = nurbs->Knot (0, nurbs->KnotCount (0) - 1);
  double minV = nurbs->Knot (1, 0);
  double maxV = nurbs->Knot (1, nurbs->KnotCount (1) - 1);

  Eigen::Vector2d params;
  Eigen::Vector3d point;

  double points[3];

  // WEST
  params (0) = minU;
  for (unsigned i = 0; i < samples; i++)
  {
    params (1) = minV + (maxV - minV) * ds * i;
    nurbs->Evaluate (params (0), params (1), 0, 3, points);
    point_list.push_back (Eigen::Vector3d (points[0], points[1], points[2]));
    param_list.push_back (params);
  }

  // EAST
  params (0) = maxU;
  for (unsigned i = 0; i < samples; i++)
  {
    params (1) = minV + (maxV - minV) * ds * i;
    nurbs->Evaluate (params (0), params (1), 0, 3, points);
    point_list.push_back (Eigen::Vector3d (points[0], points[1], points[2]));
    param_list.push_back (params);
  }

  // SOUTH
  params (1) = minV;
  for (unsigned i = 0; i < samples; i++)
  {
    params (0) = minU + (maxU - minU) * ds * i;
    nurbs->Evaluate (params (0), params (1), 0, 3, points);
    point_list.push_back (Eigen::Vector3d (points[0], points[1], points[2]));
    param_list.push_back (params);
  }

  // NORTH
  params (1) = maxV;
  for (unsigned i = 0; i < samples; i++)
  {
    params (0) = minU + (maxU - minU) * ds * i;
    nurbs->Evaluate (params (0), params (1), 0, 3, points);
    point_list.push_back (Eigen::Vector3d (points[0], points[1], points[2]));
    param_list.push_back (params);
  }
}

void
NurbsTools::pca (const vector_vec3d &data, ON_3dVector &mean, Eigen::Matrix3d &eigenvectors,
		 Eigen::Vector3d &eigenvalues)
{
  if (data.empty ())
  {
    printf ("[NurbsTools::pca] Error, data is empty\n");
    abort ();
  }

  mean = computeMean (data);

  unsigned s = data.size ();

  ON_Matrix Q(3, s);

  for (unsigned i = 0; i < s; i++) {
    Q[0][i] = data[i].x - mean.x;
    Q[1][i] = data[i].y - mean.y;
    Q[2][i] = data[i].z - mean.z;
  }

  ON_Matrix Qt = Q;
  Qt.Transpose();

  ON_Matrix oC;
  oC.Multiply(Q,Qt);

  Eigen::Matrix3d C(3,3);
  for (unsigned i = 0; i < 3; i++) {
      for (unsigned j = 0; j < 3; j++) {
	  C(i,j) = oC[i][j];
      }
  }

  Eigen::SelfAdjointEigenSolver < Eigen::Matrix3d > eigensolver (C);
  if (eigensolver.info () != Eigen::Success)
  {
    printf ("[NurbsTools::pca] Can not find eigenvalues.\n");
    abort ();
  }

  for (int i = 0; i < 3; ++i)
  {
    eigenvalues (i) = eigensolver.eigenvalues () (2 - i);
    if (i == 2)
      eigenvectors.col (2) = eigenvectors.col (0).cross (eigenvectors.col (1));
    else
      eigenvectors.col (i) = eigensolver.eigenvectors ().col (2 - i);
  }
}

void
NurbsTools::downsample_random (vector_vec3d &data, unsigned size)
{
  if (data.size () <= size && size > 0)
    return;

  unsigned s = data.size ();

  vector_vec3d data_tmp;

  for (unsigned i = 0; i < size; i++)
  {
    unsigned rnd = unsigned ((s - 1) * (double (rand ()) / RAND_MAX));
    data_tmp.push_back (data[rnd]);
  }

  data = data_tmp;
}

unsigned
NurbsTools::getClosestPoint (const Eigen::Vector3d &p, const vector_vec3d &data)
{
  if (data.empty ())
    throw std::runtime_error ("[NurbsTools::getClosestPoint(2d)] Data empty.\n");

  size_t idx = 0;
  double dist2 (DBL_MAX);
  for (size_t i = 0; i < data.size (); i++)
  {
    double d2 = (data[i] - p).squaredNorm ();
    if (d2 < dist2)
    {
      idx = i;
      dist2 = d2;
    }
  }
  return idx;
}

unsigned
NurbsTools::getClosestPoint (const ON_3dPoint &p, const vector_vec3d &data)
{
  if (data.empty ())
    throw std::runtime_error ("[NurbsTools::getClosestPoint(2d)] Data empty.\n");

  unsigned idx (0);
  double dist2 (DBL_MAX);
  for (unsigned i = 0; i < data.size (); i++)
  {
    ON_3dVector v = (data[i] - p);
    double d2 = ON_DotProduct(v, v);  // Was the squaredNorm in Eigen
    if (d2 < dist2)
    {
      idx = i;
      dist2 = d2;
    }
  }
  return idx;
}

ON_3dVector
NurbsTools::computeMean (const vector_vec3d &data)
{
  ON_3dVector u(0.0, 0.0, 0.0);

  unsigned s = data.size ();
  double ds = 1.0 / s;

  for (unsigned i = 0; i < s; i++)
    u += (data[i] * ds);

  return u;
}

Eigen::Vector3d
NurbsTools::computeMean (const vector_vec3d &data)
{
  Eigen::Vector3d u (0.0, 0.0, 0.0);

  unsigned s = unsigned (data.size ());
  double ds = 1.0 / s;

  for (unsigned i = 0; i < s; i++)
    u += (data[i] * ds);

  return u;
}

void
NurbsTools::pca (const vector_vec3d &data, Eigen::Vector3d &mean, Eigen::Matrix3d &eigenvectors,
                 Eigen::Vector3d &eigenvalues)
{
  if (data.empty ())
  {
    printf ("[NurbsTools::pca] Error, data is empty\n");
    abort ();
  }

  mean = computeMean (data);

  unsigned s = unsigned (data.size ());

  Eigen::MatrixXd Q (3, s);

  for (unsigned i = 0; i < s; i++)
    Q.col (i) << (data[i] - mean);

  Eigen::Matrix3d C = Q * Q.transpose ();

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver (C);
  if (eigensolver.info () != Success)
  {
    printf ("[NurbsTools::pca] Can not find eigenvalues.\n");
    abort ();
  }

  for (int i = 0; i < 3; ++i)
  {
    eigenvalues (i) = eigensolver.eigenvalues () (2 - i);
    if (i == 2)
      eigenvectors.col (2) = eigenvectors.col (0).cross (eigenvectors.col (1));
    else
      eigenvectors.col (i) = eigensolver.eigenvectors ().col (2 - i);
  }
}

ON_NurbsCurve
FittingCurve::initNurbsCurvePCA (int order, const vector_vec3d &data, int ncps, double rf)
{
  if (data.empty ())
    printf ("[FittingCurve::initNurbsCurvePCA] Warning, no boundary parameters available\n");

  Eigen::Vector3d mean;
  Eigen::Matrix3d eigenvectors;
  Eigen::Vector3d eigenvalues;

  unsigned s = unsigned (data.size ());

  NurbsTools::pca (data, mean, eigenvectors, eigenvalues);

  eigenvalues = eigenvalues / s; // seems that the eigenvalues are dependent on the number of points (???)

  double r = rf * sqrt (eigenvalues (0));

  if (ncps < 2 * order)
    ncps = 2 * order;

  ON_NurbsCurve nurbs = ON_NurbsCurve (3, false, order, ncps);
  nurbs.MakePeriodicUniformKnotVector (1.0 / (ncps - order + 1));

  double dcv = (2.0 * M_PI) / (ncps - order + 1);
  Eigen::Vector3d cv, cv_t;
  for (int j = 0; j < ncps; j++)
  {
    cv (0) = r * sin (dcv * j);
    cv (1) = r * cos (dcv * j);
    cv (2) = 0.0;
    cv_t = eigenvectors * cv + mean;
    nurbs.SetCV (j, ON_3dPoint (cv_t (0), cv_t (1), cv_t (2)));
  }

  return nurbs;
}

Eigen::Vector3d
NurbsTools::computeVariance (const Eigen::Vector3d &mean, const vector_vec3d &data)
{
  Eigen::Vector3d var (0.0, 0.0, 0.0);

  size_t s = data.size ();
  double ds = 1.0 / double (s);

  for (size_t i = 0; i < s; i++)
  {
    Eigen::Vector3d v = data[i] - mean;
    var += Eigen::Vector3d (v (0) * v (0) * ds, v (1) * v (1) * ds, v (2) * v (2) * ds);
  }

  return var;
}

unsigned
SequentialFitter::PCL2ON (pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pcl_cloud, const std::vector<int> &indices,
                          vector_vec3d &on_cloud)
{
  unsigned numPoints (0);

  for (unsigned i = 0; i < indices.size (); i++)
  {

    pcl::PointXYZRGB &pt = pcl_cloud->at (indices[i]);

    if (!pcl_isnan (pt.x) && !pcl_isnan (pt.y) && !pcl_isnan (pt.z))
    {
      on_cloud.push_back (Eigen::Vector3d (pt.x, pt.y, pt.z));
      numPoints++;
    }

  }

  return numPoints;
}

void
Triangulation::convertTrimmedSurface2PolygonMesh (const ON_NurbsSurface &nurbs, const ON_NurbsCurve &curve,
        PolygonMesh &mesh, unsigned resolution, vector_vec3d &start,
        vector_vec3d &end)
{
    // copy knots
    if (nurbs.KnotCount (0) <= 1 || nurbs.KnotCount (1) <= 1 || curve.KnotCount () <= 1)
    {
        printf ("[Triangulation::convertTrimmedSurface2PolygonMesh] Warning: ON knot vector empty.\n");
        return;
    }

    mesh.polygons.clear ();

    double x0 = nurbs.Knot (0, 0);
    double x1 = nurbs.Knot (0, nurbs.KnotCount (0) - 1);
    double w = x1 - x0;
    double y0 = nurbs.Knot (1, 0);
    double y1 = nurbs.Knot (1, nurbs.KnotCount (1) - 1);
    double h = y1 - y0;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    std::vector<pcl::Vertices> polygons;
    createVertices (cloud, x0, y0, 0.0, w, h, resolution, resolution);
    createIndices (polygons, 0, resolution, resolution);

    vector_vec2d points (cloud->size (), Eigen::Vector2d ());
    std::vector<double> params (cloud->size (), 0.0);
    std::vector<bool> pt_is_in (cloud->size (), false);

    std::vector<uint32_t> out_idx;
    pcl::on_nurbs::vector_vec2d out_pc;

    for (unsigned i = 0; i < cloud->size (); i++)
    {
        double err, param;
        Eigen::Vector2d pc, tc;
        pcl::PointXYZ &v = cloud->at (i);
        Eigen::Vector2d vp (v.x, v.y);

        if(curve.Order()==2)
            param = pcl::on_nurbs::FittingCurve2d::inverseMappingO2 (curve, vp, err, pc, tc);
        else
        {
            param = pcl::on_nurbs::FittingCurve2d::findClosestElementMidPoint(curve, vp);
            param = pcl::on_nurbs::FittingCurve2d::inverseMapping(curve, vp, param, err, pc, tc);
        }

        Eigen::Vector3d a (vp (0) - pc (0), vp (1) - pc (1), 0.0);
        Eigen::Vector3d b (tc (0), tc (1), 0.0);
        Eigen::Vector3d z = a.cross (b);

        points[i] = pc;
        params[i] = param;
        pt_is_in[i] = (z (2) >= 0.0);

        end.push_back (Eigen::Vector3d (pc (0), pc (1), 0.0));
        start.push_back (Eigen::Vector3d (pc (0) + tc (0) * 0.01, pc (1) + tc (1) * 0.01, 0.0));
    }

    for (unsigned i = 0; i < polygons.size (); i++)
    {
        unsigned in (0);
        pcl::Vertices &poly = polygons[i];

        std::vector<uint32_t> out_idx_tmp;
        pcl::on_nurbs::vector_vec2d out_pc_tmp;

        for (std::size_t j = 0; j < poly.vertices.size (); j++)
        {
            uint32_t &vi = poly.vertices[j];
            if (pt_is_in[vi])
                in++;
            else
            {
                out_idx_tmp.push_back (vi);
                out_pc_tmp.push_back (points[vi]);
            }
        }

        if (in > 0)
        {
            mesh.polygons.push_back (poly);
            if (in < poly.vertices.size ())
            {
                for (std::size_t j = 0; j < out_idx_tmp.size (); j++)
                {
                    out_idx.push_back (out_idx_tmp[j]);
                    out_pc.push_back (out_pc_tmp[j]);
                }
            }
        }
    }

    for (std::size_t i = 0; i < out_idx.size (); i++)
    {
        pcl::PointXYZ &v = cloud->at (out_idx[i]);
        Eigen::Vector2d &pc = out_pc[i];
        v.x = pc (0);
        v.y = pc (1);
//.........这里部分代码省略.........