C++ ON_NurbsSurface::Evaluate方法代码示例

void
Triangulation::convertSurface2Vertices (const ON_NurbsSurface &nurbs, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                                        std::vector<pcl::Vertices> &vertices, unsigned resolution)
{
  // copy knots
  if (nurbs.KnotCount (0) <= 1 || nurbs.KnotCount (1) <= 1)
  {
    printf ("[Triangulation::convertSurface2Vertices] Warning: ON knot vector empty.\n");
    return;
  }

  cloud->clear ();
  vertices.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;

  createVertices (cloud, float (x0), float (y0), 0.0f, float (w), float (h), resolution, resolution);
  createIndices (vertices, 0, resolution, resolution);

  for (auto &v : *cloud)
  {
    double point[9];
    nurbs.Evaluate (v.x, v.y, 1, 3, point);

    v.x = static_cast<float> (point[0]);
    v.y = static_cast<float> (point[1]);
    v.z = static_cast<float> (point[2]);
  }
}

void
visualizeCurve (ON_NurbsCurve &curve, ON_NurbsSurface &surface, pcl::visualization::PCLVisualizer &viewer)
{
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr curve_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);

  pcl::on_nurbs::Triangulation::convertCurve2PointCloud (curve, surface, curve_cloud, 4);
  for (std::size_t i = 0; i < curve_cloud->size () - 1; i++)
  {
    pcl::PointXYZRGB &p1 = curve_cloud->at (i);
    pcl::PointXYZRGB &p2 = curve_cloud->at (i + 1);
    std::ostringstream os;
    os << "line" << i;
    viewer.removeShape (os.str ());
    viewer.addLine<pcl::PointXYZRGB> (p1, p2, 1.0, 0.0, 0.0, os.str ());
  }

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr curve_cps (new pcl::PointCloud<pcl::PointXYZRGB>);
  for (int i = 0; i < curve.CVCount (); i++)
  {
    ON_3dPoint p1;
    curve.GetCV (i, p1);

    double pnt[3];
    surface.Evaluate (p1.x, p1.y, 0, 3, pnt);
    pcl::PointXYZRGB p2;
    p2.x = float (pnt[0]);
    p2.y = float (pnt[1]);
    p2.z = float (pnt[2]);

    p2.r = 255;
    p2.g = 0;
    p2.b = 0;

    curve_cps->push_back (p2);
  }
  viewer.removePointCloud ("cloud_cps");
  viewer.addPointCloud (curve_cps, "cloud_cps");
}

ON_2dPoint
FittingSurface::findClosestElementMidPoint (const ON_NurbsSurface &nurbs, const ON_3dPoint &pt)
{
  ON_2dPoint hint;
  ON_3dVector r;
  std::vector<double> elementsU = getElementVector (nurbs, 0);
  std::vector<double> elementsV = getElementVector (nurbs, 1);

  double d_shortest (DBL_MAX);
  for (unsigned i = 0; i < elementsU.size () - 1; i++)
  {
    for (unsigned j = 0; j < elementsV.size () - 1; j++)
    {
      double points[3];
      double d;

      double xi = elementsU[i] + 0.5 * (elementsU[i + 1] - elementsU[i]);
      double eta = elementsV[j] + 0.5 * (elementsV[j + 1] - elementsV[j]);

      nurbs.Evaluate (xi, eta, 0, 3, points);
      r[0] = points[0] - pt[0];
      r[1] = points[1] - pt[1];
      r[2] = points[2] - pt[2];

      d = ON_DotProduct(r,r);

      if ((i == 0 && j == 0) || d < d_shortest)
      {
	d_shortest = d;
	hint[0] = xi;
	hint[1] = eta;
      }
    }
  }

  return hint;
}

Vector2d
FittingSurface::findClosestElementMidPoint (const ON_NurbsSurface &nurbs, const Vector3d &pt)
{
  Vector2d hint;
  Vector3d r;
  std::vector<double> elementsU = getElementVector (nurbs, 0);
  std::vector<double> elementsV = getElementVector (nurbs, 1);

  double d_shortest (DBL_MAX);
  for (unsigned i = 0; i < elementsU.size () - 1; i++)
  {
    for (unsigned j = 0; j < elementsV.size () - 1; j++)
    {
      double points[3];
      double d;

      double xi = elementsU[i] + 0.5 * (elementsU[i + 1] - elementsU[i]);
      double eta = elementsV[j] + 0.5 * (elementsV[j + 1] - elementsV[j]);

      nurbs.Evaluate (xi, eta, 0, 3, points);
      r (0) = points[0] - pt (0);
      r (1) = points[1] - pt (1);
      r (2) = points[2] - pt (2);

      d = r.squaredNorm ();

      if ((i == 0 && j == 0) || d < d_shortest)
      {
        d_shortest = d;
        hint (0) = xi;
        hint (1) = eta;
      }
    }
  }

  return hint;
}

Vector2d
FittingCylinder::inverseMapping (const ON_NurbsSurface &nurbs, const Vector3d &pt, const Vector2d &hint, double &error,
                                 Vector3d &p, Vector3d &tu, Vector3d &tv, int maxSteps, double accuracy, bool quiet)
{

  double pointAndTangents[9];

  Vector2d current, delta;
  Matrix2d A;
  Vector2d b;
  Vector3d r;
  std::vector<double> elementsU = getElementVector (nurbs, 0);
  std::vector<double> elementsV = getElementVector (nurbs, 1);
  double minU = elementsU[0];
  double minV = elementsV[0];
  double maxU = elementsU[elementsU.size () - 1];
  double maxV = elementsV[elementsV.size () - 1];

  current = hint;

  for (int k = 0; k < maxSteps; k++)
  {

    nurbs.Evaluate (current (0), current (1), 1, 3, pointAndTangents);
    p (0) = pointAndTangents[0];
    p (1) = pointAndTangents[1];
    p (2) = pointAndTangents[2];
    tu (0) = pointAndTangents[3];
    tu (1) = pointAndTangents[4];
    tu (2) = pointAndTangents[5];
    tv (0) = pointAndTangents[6];
    tv (1) = pointAndTangents[7];
    tv (2) = pointAndTangents[8];

    r = p - pt;

    b (0) = -r.dot (tu);
    b (1) = -r.dot (tv);

    A (0, 0) = tu.dot (tu);
    A (0, 1) = tu.dot (tv);
    A (1, 0) = A (0, 1);
    A (1, 1) = tv.dot (tv);

    delta = A.ldlt ().solve (b);

    if (delta.norm () < accuracy)
    {

      error = r.norm ();
      return current;

    }
    else
    {
      current = current + delta;
      if (current (0) < minU)
        current (0) = minU;
      else if (current (0) > maxU)
        current (0) = maxU;

      if (current (1) < minV)
        current (1) = maxV - (minV - current (1));
      else if (current (1) > maxV)
        current (1) = minV + (current (1) - maxV);
    }

  }

  error = r.norm ();

  if (!quiet)
  {
    printf ("[FittingCylinder::inverseMapping] Warning: Method did not converge (%e %d)\n", accuracy, maxSteps);
    printf ("  %f %f ... %f %f\n", hint (0), hint (1), current (0), current (1));
  }

  return current;
}

ON_2dPoint
FittingSurface::inverseMappingBoundary (const ON_NurbsSurface &nurbs, const ON_3dPoint &pt, int side, double hint,
					double &error, ON_3dPoint &p, ON_3dVector &tu, ON_3dVector &tv, int maxSteps,
					double accuracy, bool quiet)
{

  double pointAndTangents[9];
  double current, delta;
  ON_3dVector r, t;
  ON_2dPoint params;

  current = hint;

  std::vector<double> elementsU = getElementVector (nurbs, 0);
  std::vector<double> elementsV = getElementVector (nurbs, 1);
  double minU = elementsU[0];
  double minV = elementsV[0];
  double maxU = elementsU[elementsU.size () - 1];
  double maxV = elementsV[elementsV.size () - 1];

  for (int k = 0; k < maxSteps; k++)
  {

    switch (side)
    {

      case WEST:

	params[0] = minU;
	params[1] = current;
	nurbs.Evaluate (minU, current, 1, 3, pointAndTangents);
	p[0] = pointAndTangents[0];
	p[1] = pointAndTangents[1];
	p[2] = pointAndTangents[2];
	tu[0] = pointAndTangents[3];
	tu[1] = pointAndTangents[4];
	tu[2] = pointAndTangents[5];
	tv[0] = pointAndTangents[6];
	tv[1] = pointAndTangents[7];
	tv[2] = pointAndTangents[8];

	t = tv; // use tv

	break;
      case SOUTH:

	params[0] = current;
	params[1] = maxV;
	nurbs.Evaluate (current, maxV, 1, 3, pointAndTangents);
	p[0] = pointAndTangents[0];
	p[1] = pointAndTangents[1];
	p[2] = pointAndTangents[2];
	tu[0] = pointAndTangents[3];
	tu[1] = pointAndTangents[4];
	tu[2] = pointAndTangents[5];
	tv[0] = pointAndTangents[6];
	tv[1] = pointAndTangents[7];
	tv[2] = pointAndTangents[8];

	t = tu; // use tu

	break;
      case EAST:

	params[0] = maxU;
	params[1] = current;
	nurbs.Evaluate (maxU, current, 1, 3, pointAndTangents);
	p[0] = pointAndTangents[0];
	p[1] = pointAndTangents[1];
	p[2] = pointAndTangents[2];
	tu[0] = pointAndTangents[3];
	tu[1] = pointAndTangents[4];
	tu[2] = pointAndTangents[5];
	tv[0] = pointAndTangents[6];
	tv[1] = pointAndTangents[7];
	tv[2] = pointAndTangents[8];

	t = tv; // use tv

	break;
      case NORTH:

	params[0] = current;
	params[1] = minV;
	nurbs.Evaluate (current, minV, 1, 3, pointAndTangents);
	p[0] = pointAndTangents[0];
	p[1] = pointAndTangents[1];
	p[2] = pointAndTangents[2];
	tu[0] = pointAndTangents[3];
	tu[1] = pointAndTangents[4];
	tu[2] = pointAndTangents[5];
	tv[0] = pointAndTangents[6];
	tv[1] = pointAndTangents[7];
	tv[2] = pointAndTangents[8];

	t = tu; // use tu

	break;
      default:
	throw std::runtime_error ("[FittingSurface::inverseMappingBoundary] ERROR: Specify a boundary!");
//.........这里部分代码省略.........

ON_2dPoint
FittingSurface::inverseMapping (const ON_NurbsSurface &nurbs, const ON_3dPoint &pt, const ON_2dPoint &hint, double &error,
				ON_3dPoint &p, ON_3dVector &tu, ON_3dVector &tv, int maxSteps, double accuracy, bool quiet)
{

  double pointAndTangents[9];

  ON_2dVector current, delta;
  ON_Matrix A(2,2);
  ON_2dVector b;
  ON_3dVector r;
  std::vector<double> elementsU = getElementVector (nurbs, 0);
  std::vector<double> elementsV = getElementVector (nurbs, 1);
  double minU = elementsU[0];
  double minV = elementsV[0];
  double maxU = elementsU[elementsU.size () - 1];
  double maxV = elementsV[elementsV.size () - 1];

  current = hint;

  for (int k = 0; k < maxSteps; k++)
  {

    nurbs.Evaluate (current[0], current[1], 1, 3, pointAndTangents);
    p[0] = pointAndTangents[0];
    p[1] = pointAndTangents[1];
    p[2] = pointAndTangents[2];
    tu[0] = pointAndTangents[3];
    tu[1] = pointAndTangents[4];
    tu[2] = pointAndTangents[5];
    tv[0] = pointAndTangents[6];
    tv[1] = pointAndTangents[7];
    tv[2] = pointAndTangents[8];

    r = p - pt;

    b[0] = -ON_DotProduct(r,tu);
    b[1] = -ON_DotProduct(r,tv);

    A[0][0] = ON_DotProduct(tu,tu);
    A[0][1] = ON_DotProduct(tu,tv);
    A[1][0] = A[0][1];
    A[1][1] = ON_DotProduct(tv,tv);

    Eigen::Vector2d eb(b[0],b[1]);
    Eigen::Vector2d edelta;
    Eigen::Matrix2d eA;

    eA (0, 0) = A[0][0];
    eA (0, 1) = A[0][1];
    eA (1, 0) = A[1][0];
    eA (1, 1) = A[1][1];

    edelta = eA.ldlt ().solve (eb);

    delta[0] = edelta (0);
    delta[1] = edelta (1);

    if (sqrt(ON_DotProduct(delta,delta)) < accuracy)
    {

      error = sqrt(ON_DotProduct(r,r));
      return current;

    }
    else
    {
      current = current + delta;

      if (current[0] < minU)
	current[0] = minU;
      else if (current[0] > maxU)
	current[0] = maxU;

      if (current[1] < minV)
	current[1] = minV;
      else if (current[1] > maxV)
	current[1] = maxV;

    }

  }

  error = sqrt(ON_DotProduct(r,r));
  if (!quiet)
  {
    printf ("[FittingSurface::inverseMapping] Warning: Method did not converge (%e %d)\n", accuracy, maxSteps);
    printf ("  %f %f ... %f %f\n", hint[0], hint[1], current[0], current[1]);
  }

  return current;

}

Vector2d
FittingSurface::inverseMappingBoundary (const ON_NurbsSurface &nurbs, const Vector3d &pt, int side, double hint,
                                        double &error, Vector3d &p, Vector3d &tu, Vector3d &tv, int maxSteps,
                                        double accuracy, bool quiet)
{

  double pointAndTangents[9];
  double current, delta;
  Vector3d r, t;
  Vector2d params;

  current = hint;

  std::vector<double> elementsU = getElementVector (nurbs, 0);
  std::vector<double> elementsV = getElementVector (nurbs, 1);
  double minU = elementsU[0];
  double minV = elementsV[0];
  double maxU = elementsU[elementsU.size () - 1];
  double maxV = elementsV[elementsV.size () - 1];

  for (int k = 0; k < maxSteps; k++)
  {

    switch (side)
    {

      case WEST:

        params (0) = minU;
        params (1) = current;
        nurbs.Evaluate (minU, current, 1, 3, pointAndTangents);
        p (0) = pointAndTangents[0];
        p (1) = pointAndTangents[1];
        p (2) = pointAndTangents[2];
        tu (0) = pointAndTangents[3];
        tu (1) = pointAndTangents[4];
        tu (2) = pointAndTangents[5];
        tv (0) = pointAndTangents[6];
        tv (1) = pointAndTangents[7];
        tv (2) = pointAndTangents[8];

        t = tv; // use tv

        break;
      case SOUTH:

        params (0) = current;
        params (1) = maxV;
        nurbs.Evaluate (current, maxV, 1, 3, pointAndTangents);
        p (0) = pointAndTangents[0];
        p (1) = pointAndTangents[1];
        p (2) = pointAndTangents[2];
        tu (0) = pointAndTangents[3];
        tu (1) = pointAndTangents[4];
        tu (2) = pointAndTangents[5];
        tv (0) = pointAndTangents[6];
        tv (1) = pointAndTangents[7];
        tv (2) = pointAndTangents[8];

        t = tu; // use tu

        break;
      case EAST:

        params (0) = maxU;
        params (1) = current;
        nurbs.Evaluate (maxU, current, 1, 3, pointAndTangents);
        p (0) = pointAndTangents[0];
        p (1) = pointAndTangents[1];
        p (2) = pointAndTangents[2];
        tu (0) = pointAndTangents[3];
        tu (1) = pointAndTangents[4];
        tu (2) = pointAndTangents[5];
        tv (0) = pointAndTangents[6];
        tv (1) = pointAndTangents[7];
        tv (2) = pointAndTangents[8];

        t = tv; // use tv

        break;
      case NORTH:

        params (0) = current;
        params (1) = minV;
        nurbs.Evaluate (current, minV, 1, 3, pointAndTangents);
        p (0) = pointAndTangents[0];
        p (1) = pointAndTangents[1];
        p (2) = pointAndTangents[2];
        tu (0) = pointAndTangents[3];
        tu (1) = pointAndTangents[4];
        tu (2) = pointAndTangents[5];
        tv (0) = pointAndTangents[6];
        tv (1) = pointAndTangents[7];
        tv (2) = pointAndTangents[8];

        t = tu; // use tu

        break;
      default:
        throw std::runtime_error ("[FittingSurface::inverseMappingBoundary] ERROR: Specify a boundary!");
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    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);
    }

    for (std::size_t i = 0; i < cloud->size (); i++)
    {
        pcl::PointXYZ &v = cloud->at (i);

        double point[3];
        nurbs.Evaluate (v.x, v.y, 0, 3, point);

        v.x = point[0];
        v.y = point[1];
        v.z = point[2];
    }

    for (std::size_t i = 0; i < start.size (); i++)
    {
        Eigen::Vector3d &p1 = start[i];
        Eigen::Vector3d &p2 = end[i];

        double point[3];
        nurbs.Evaluate (p1 (0), p1 (1), 0, 3, point);
        p1 (0) = point[0];
        p1 (1) = point[1];
        p1 (2) = point[2];

        nurbs.Evaluate (p2 (0), p2 (1), 0, 3, point);
        p2 (0) = point[0];
        p2 (1) = point[1];
        p2 (2) = point[2];
    }

    toROSMsg (*cloud, mesh.cloud);
}

//.........这里部分代码省略.........

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

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

  Eigen::Vector3d a0, a1;
  pcl::on_nurbs::NurbsTools::computeBoundingBox (curve, a0, a1);
  double rScale = 1.0 / pcl::on_nurbs::NurbsTools::computeRScale (a0, a1);

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

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

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

    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;
    pt_is_in[i] = (z (2) >= 0.0);
  }

  for (const auto &poly : polygons)
  {
    unsigned in (0);
    std::vector<uint32_t> out_idx_tmp;
    pcl::on_nurbs::vector_vec2d out_pc_tmp;

    for (const unsigned int &vi : poly.vertices)
    {
      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 = float (pc (0));
    v.y = float (pc (1));
  }

  for (auto &v : *cloud)
  {
    Eigen::Vector3d tu, tv;

    double point[3];
    nurbs.Evaluate (v.x, v.y, 0, 3, point);

    v.x = float (point[0]);
    v.y = float (point[1]);
    v.z = float (point[2]);
    //    tu[0] = point[3];
    //    tu[1] = point[4];
    //    tu[2] = point[5];
    //    tv[0] = point[6];
    //    tv[1] = point[7];
    //    tv[2] = point[8];

    // TODO: add normals to mesh
  }

  toPCLPointCloud2 (*cloud, mesh.cloud);
}