void
Triangulation::convertSurface2PolygonMesh (const ON_NurbsSurface &nurbs, PolygonMesh &mesh, unsigned resolution)
{
// copy knots
if (nurbs.m_knot_capacity[0] <= 1 || nurbs.m_knot_capacity[1] <= 1)
{
printf ("[Triangulation::convert] Warning: ON knot vector empty.\n");
return;
}
double x0 = nurbs.Knot (0, 0);
double x1 = nurbs.Knot (0, nurbs.m_knot_capacity[0] - 1);
double w = x1 - x0;
double y0 = nurbs.Knot (1, 0);
double y1 = nurbs.Knot (1, nurbs.m_knot_capacity[1] - 1);
double h = y1 - y0;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
createVertices (cloud, x0, y0, 0.0, w, h, resolution, resolution);
createIndices (mesh.polygons, 0, resolution, resolution);
for (unsigned i = 0; i < cloud->size (); i++)
{
pcl::PointXYZ &v = cloud->at (i);
double point[9];
nurbs.Evaluate (v.x, v.y, 1, 3, point);
v.x = point[0];
v.y = point[1];
v.z = point[2];
}
toROSMsg (*cloud, mesh.cloud);
}
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
Triangulation::convertSurface2PolygonMesh (const ON_NurbsSurface &nurbs, PolygonMesh &mesh, unsigned resolution)
{
// copy knots
if (nurbs.KnotCount (0) <= 1 || nurbs.KnotCount (1) <= 1)
{
printf ("[Triangulation::convertSurface2PolygonMesh] Warning: ON knot vector empty.\n");
return;
}
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>);
mesh.polygons.clear ();
createVertices (cloud, float (x0), float (y0), 0.0f, float (w), float (h), resolution, resolution);
createIndices (mesh.polygons, 0, resolution, resolution);
for (auto &v : *cloud)
{
double point[9];
nurbs.Evaluate (v.x, v.y, 1, 3, point);
v.x = float (point[0]);
v.y = float (point[1]);
v.z = float (point[2]);
}
toPCLPointCloud2 (*cloud, mesh.cloud);
}
void
Triangulation::convertCurve2PointCloud (const ON_NurbsCurve &curve, const ON_NurbsSurface &surf,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, unsigned resolution)
{
// copy knots
if (curve.m_knot_capacity <= 1)
{
printf ("[Triangulation::Convert] Warning: ON knot vector empty.\n");
return;
}
cloud->clear ();
if (resolution < 2)
resolution = 2;
int cp_red = curve.Order () - 2;
// for each element of the nurbs curve
for (int i = 1; i < curve.KnotCount () - 1 - cp_red; i++)
{
double dr = 1.0 / (resolution - 1);
double xi0 = curve.m_knot[i];
double xid = (curve.m_knot[i + 1] - xi0);
for (unsigned j = 0; j < resolution; j++)
{
pcl::PointXYZRGB pt;
double xi = (xi0 + j * dr * xid);
double p[3];
double pp[3];
curve.Evaluate (xi, 0, 2, pp);
surf.Evaluate (pp[0], pp[1], 0, 3, p);
pt.x = p[0];
pt.y = p[1];
pt.z = p[2];
pt.r = 255;
pt.g = 0;
pt.b = 0;
cloud->push_back (pt);
}
}
}
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;
}
void
Triangulation::convertSurface2Vertices (const ON_NurbsSurface &nurbs, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
std::vector<pcl::Vertices> &vertices, unsigned resolution)
{
// copy knots
if (nurbs.m_knot_capacity[0] <= 1 || nurbs.m_knot_capacity[1] <= 1)
{
printf ("[Triangulation::convert] Warning: ON knot vector empty.\n");
return;
}
cloud->clear ();
vertices.clear ();
double x0 = nurbs.Knot (0, 0);
double x1 = nurbs.Knot (0, nurbs.m_knot_capacity[0] - 1);
double w = x1 - x0;
double y0 = nurbs.Knot (1, 0);
double y1 = nurbs.Knot (1, nurbs.m_knot_capacity[1] - 1);
double h = y1 - y0;
createVertices (cloud, x0, y0, 0.0, w, h, resolution, resolution);
createIndices (vertices, 0, resolution, resolution);
for (unsigned i = 0; i < cloud->size (); i++)
{
pcl::PointXYZ &v = cloud->at (i);
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]);
}
}
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;
}
void
Triangulation::convertTrimmedSurface2PolygonMesh (const ON_NurbsSurface &nurbs, const ON_NurbsCurve &curve,
PolygonMesh &mesh, unsigned resolution)
{
// copy knots
if (nurbs.m_knot_capacity[0] <= 1 || nurbs.m_knot_capacity[1] <= 1)
{
printf ("[Triangulation::convert] Warning: ON knot vector empty.\n");
return;
}
mesh.polygons.clear ();
double x0 = nurbs.Knot (0, 0);
double x1 = nurbs.Knot (0, nurbs.m_knot_capacity[0] - 1);
double w = x1 - x0;
double y0 = nurbs.Knot (1, 0);
double y1 = nurbs.Knot (1, nurbs.m_knot_capacity[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);
std::vector<uint32_t> out_idx;
pcl::on_nurbs::vector_vec2d out_pc;
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++)
{
double err;
Eigen::Vector2d pc, tc;
uint32_t &vi = poly.vertices[j];
pcl::PointXYZ &v = cloud->at (vi);
Eigen::Vector2d vp (v.x, v.y);
double param = pcl::on_nurbs::FittingCurve2d::findClosestElementMidPoint (curve, vp);
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);
if (z (2) >= 0.0)
in++;
else
{
out_idx_tmp.push_back (vi);
out_pc_tmp.push_back (pc);
}
}
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[9];
nurbs.Evaluate (v.x, v.y, 1, 3, point);
v.x = point[0];
v.y = point[1];
v.z = point[2];
}
toROSMsg (*cloud, mesh.cloud);
}
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!");
}
r[0] = pointAndTangents[0] - pt[0];
r[1] = pointAndTangents[1] - pt[1];
r[2] = pointAndTangents[2] - pt[2];
delta = -0.5 * ON_DotProduct(r,t) / ON_DotProduct(t,t);
if (fabs (delta) < accuracy)
{
error = sqrt(ON_DotProduct(r,r));
return params;
}
else
{
current = current + delta;
bool stop = false;
switch (side)
{
case WEST:
case EAST:
if (current < minV)
{
params[1] = minV;
stop = true;
}
else if (current > maxV)
{
params[1] = maxV;
stop = true;
}
break;
case NORTH:
case SOUTH:
if (current < minU)
{
params[0] = minU;
stop = true;
}
else if (current > maxU)
{
params[0] = maxU;
stop = true;
}
break;
}
if (stop)
{
error = sqrt(ON_DotProduct(r,r));
return params;
}
}
}
error = sqrt(ON_DotProduct(r,r));
if (!quiet)
printf (
"[FittingSurface::inverseMappingBoundary] Warning: Method did not converge! (residual: %f, delta: %f, params: %f %f)\n",
error, delta, params[0], params[1]);
return params;
}
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!");
}
r (0) = pointAndTangents[0] - pt (0);
r (1) = pointAndTangents[1] - pt (1);
r (2) = pointAndTangents[2] - pt (2);
delta = -0.5 * r.dot (t) / t.dot (t);
if (fabs (delta) < accuracy)
{
error = r.norm ();
return params;
}
else
{
current = current + delta;
bool stop = false;
switch (side)
{
case WEST:
case EAST:
if (current < minV)
{
params (1) = minV;
stop = true;
}
else if (current > maxV)
{
params (1) = maxV;
stop = true;
}
break;
case NORTH:
case SOUTH:
if (current < minU)
{
params (0) = minU;
stop = true;
}
else if (current > maxU)
{
params (0) = maxU;
stop = true;
}
break;
}
if (stop)
{
error = r.norm ();
return params;
}
}
}
error = r.norm ();
if (!quiet)
printf (
"[FittingSurface::inverseMappingBoundary] Warning: Method did not converge! (residual: %f, delta: %f, params: %f %f)\n",
error, delta, params (0), params (1));
return params;
}
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);
}
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);
}
void
Triangulation::convertTrimmedSurface2PolygonMesh (const ON_NurbsSurface &nurbs, const ON_NurbsCurve &curve,
PolygonMesh &mesh, unsigned resolution)
{
// 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, 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);
}