double
FittingCurve2d::findClosestElementMidPoint (const ON_NurbsCurve &nurbs, const Eigen::Vector2d &pt, double hint)
{
// evaluate hint
double param = hint;
double points[2];
nurbs.Evaluate (param, 0, 2, points);
Eigen::Vector2d p (points[0], points[1]);
Eigen::Vector2d r = p - pt;
double d_shortest_hint = r.squaredNorm ();
double d_shortest_elem (DBL_MAX);
// evaluate elements
std::vector<double> elements = pcl::on_nurbs::FittingCurve2d::getElementVector (nurbs);
double seg = 1.0 / (nurbs.Order () - 1);
for (unsigned i = 0; i < elements.size () - 1; i++)
{
double &xi0 = elements[i];
double &xi1 = elements[i + 1];
double dxi = xi1 - xi0;
for (unsigned j = 0; j < nurbs.Order (); j++)
{
double xi = xi0 + (seg * j) * dxi;
nurbs.Evaluate (xi, 0, 2, points);
p (0) = points[0];
p (1) = points[1];
r = p - pt;
double d = r.squaredNorm ();
if (d < d_shortest_elem)
{
d_shortest_elem = d;
param = xi;
}
}
}
if(d_shortest_hint < d_shortest_elem)
return hint;
else
return param;
}
/**
* \return List of bezier spline segments which together represent this curve.
*/
QList<RSpline> RSpline::getBezierSegments() const {
// spline is a single bezier segment:
if (countControlPoints()==getDegree()+1) {
return QList<RSpline>() << *this;
}
updateInternal();
QList<RSpline> ret;
#ifndef R_NO_OPENNURBS
ON_NurbsCurve* dup = dynamic_cast<ON_NurbsCurve*>(curve.DuplicateCurve());
if (dup==NULL) {
return ret;
}
dup->MakePiecewiseBezier();
for (int i=0; i<=dup->CVCount() - dup->Order(); ++i) {
ON_BezierCurve bc;
if (!dup->ConvertSpanToBezier(i, bc)) {
continue;
}
QList<RVector> ctrlPts;
for (int cpi=0; cpi<bc.CVCount(); cpi++) {
ON_3dPoint onp;
bc.GetCV(cpi, onp);
ctrlPts.append(RVector(onp.x, onp.y, onp.z));
}
ret.append(RSpline(ctrlPts, degree));
}
delete dup;
#endif
return ret;
}
bool
Triangulation::isInside(const ON_NurbsCurve &curve, const pcl::PointXYZ &v)
{
Eigen::Vector2d vp (v.x, v.y);
Eigen::Vector3d a0, a1;
pcl::on_nurbs::NurbsTools::computeBoundingBox (curve, a0, a1);
double rScale = 1.0 / pcl::on_nurbs::NurbsTools::computeRScale (a0, a1);
Eigen::Vector2d pc, tc;
double err, param;
if (curve.Order () == 2)
param = pcl::on_nurbs::FittingCurve2dAPDM::inverseMappingO2 (curve, vp, err, pc, tc);
else
{
param = pcl::on_nurbs::FittingCurve2dAPDM::findClosestElementMidPoint (curve, vp);
param = 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);
return (z (2) >= 0.0);
}
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
Triangulation::convertCurve2PointCloud (const ON_NurbsCurve &nurbs, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
unsigned resolution)
{
// copy knots
if (nurbs.m_knot_capacity <= 1)
{
printf ("[Triangulation::convert] Warning: ON knot vector empty.\n");
return;
}
cloud->clear ();
if (resolution < 2)
resolution = 2;
int cp_red = nurbs.Order () - 2;
// for each element in the nurbs curve
for (int i = 1; i < nurbs.KnotCount () - 1 - cp_red; i++)
{
double dr = 1.0 / (resolution - 1);
double xi0 = nurbs.m_knot[i];
double xid = (nurbs.m_knot[i + 1] - xi0);
for (unsigned j = 0; j < resolution; j++)
{
double xi = (xi0 + j * dr * xid);
pcl::PointXYZRGB p;
double points[3];
nurbs.Evaluate (xi, 0, 3, points);
p.x = static_cast<float> (points[0]);
p.y = static_cast<float> (points[1]);
p.z = static_cast<float> (points[2]);
p.r = 255;
p.g = 0;
p.b = 0;
cloud->push_back (p);
}
}
}
void ON_GL( const ON_NurbsCurve& nurbs_curve,
GLUnurbsObj* nobj, // created with gluNewNurbsRenderer )
GLenum type, // = 0 (and type is automatically set)
int bPermitKnotScaling,
double* knot_scale,
double xform[][4]
)
{
ON_GL( nurbs_curve.Dimension(),
nurbs_curve.IsRational(),
nurbs_curve.Order(),
nurbs_curve.CVCount(),
nurbs_curve.Knot(),
nurbs_curve.m_cv_stride,
nurbs_curve.m_cv,
nobj,
type,
bPermitKnotScaling,
knot_scale,
xform
);
}
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);
}
double
FittingCurve2d::inverseMappingO2 (const ON_NurbsCurve &nurbs, const Eigen::Vector2d &pt, double &error,
Eigen::Vector2d &p, Eigen::Vector2d &t)
{
if (nurbs.Order () != 2)
printf ("[FittingCurve2d::inverseMappingO2] Error, order not 2 (polynomial degree 1)\n");
std::vector<double> elements = getElementVector (nurbs);
Eigen::Vector2d min_pt;
double min_param (DBL_MAX);
double min_dist (DBL_MAX);
error = DBL_MAX;
int is_corner (-1);
for (unsigned i = 0; i < elements.size () - 1; i++)
{
Eigen::Vector2d p1;
nurbs.Evaluate (elements[i], 0, 2, &p1 (0));
Eigen::Vector2d p2;
nurbs.Evaluate (elements[i + 1], 0, 2, &p2 (0));
Eigen::Vector2d d1 (p2 (0) - p1 (0), p2 (1) - p1 (1));
Eigen::Vector2d d2 (pt (0) - p1 (0), pt (1) - p1 (1));
double d1_norm = d1.norm ();
double d0_norm = d1.dot (d2) / d1_norm;
Eigen::Vector2d d0 = d1 * d0_norm / d1_norm;
Eigen::Vector2d p0 = p1 + d0;
if (d0_norm < 0.0)
{
double tmp_dist = (p1 - pt).norm ();
if (tmp_dist < min_dist)
{
min_dist = tmp_dist;
min_pt = p1;
min_param = elements[i];
is_corner = i;
}
}
else if (d0_norm >= d1_norm)
{
double tmp_dist = (p2 - pt).norm ();
if (tmp_dist < min_dist)
{
min_dist = tmp_dist;
min_pt = p2;
min_param = elements[i + 1];
is_corner = i + 1;
}
}
else
{ // p0 lies on line segment
double tmp_dist = (p0 - pt).norm ();
if (tmp_dist < min_dist)
{
min_dist = tmp_dist;
min_pt = p0;
min_param = elements[i] + (d0_norm / d1_norm) * (elements[i + 1] - elements[i]);
is_corner = -1;
}
}
}
if (is_corner >= 0)
{
double param1, param2;
if (is_corner == 0 || is_corner == elements.size () - 1)
{
double x0a = elements[0];
double x0b = elements[elements.size () - 1];
double xa = elements[1];
double xb = elements[elements.size () - 2];
param1 = x0a + 0.5 * (xa - x0a);
param2 = x0b + 0.5 * (xb - x0b);
}
else
{
double x0 = elements[is_corner];
double x1 = elements[is_corner - 1];
double x2 = elements[is_corner + 1];
param1 = x0 + 0.5 * (x1 - x0);
param2 = x0 + 0.5 * (x2 - x0);
}
double pt1[4];
nurbs.Evaluate (param1, 1, 2, pt1);
Eigen::Vector2d t1 (pt1[2], pt1[3]);
t1.normalize ();
double pt2[4];
nurbs.Evaluate (param2, 1, 2, pt2);
Eigen::Vector2d t2 (pt2[2], pt2[3]);
t2.normalize ();
t = 0.5 * (t1 + t2);
}
else
{
double point_tangent[4];
nurbs.Evaluate (min_param, 1, 2, point_tangent);
t (0) = point_tangent[2];
t (1) = point_tangent[3];
}
t.normalize ();
p = min_pt;
return min_param;
}
double
FittingCurve2d::inverseMapping (const ON_NurbsCurve &nurbs, const Eigen::Vector2d &pt, const double &hint,
double &error, Eigen::Vector2d &p, Eigen::Vector2d &t, double rScale, int maxSteps,
double accuracy, bool quiet)
{
if (nurbs.Order () == 2)
return inverseMappingO2 (nurbs, pt, error, p, t);
int cp_red = (nurbs.m_order - 2);
int ncpj = (nurbs.m_cv_count - 2 * cp_red);
double pointAndTangents[4];
double current, delta;
Eigen::Vector2d r;
std::vector<double> elements = getElementVector (nurbs);
double minU = elements[0];
double maxU = elements[elements.size () - 1];
current = hint;
for (int k = 0; k < maxSteps; k++)
{
nurbs.Evaluate (current, 1, 2, pointAndTangents);
p (0) = pointAndTangents[0];
p (1) = pointAndTangents[1];
t (0) = pointAndTangents[2];
t (1) = pointAndTangents[3];
r = p - pt;
// step width control
int E = findElement (current, elements);
double e = elements[E + 1] - elements[E];
delta = -(0.5 * e * rScale) * r.dot (t) / t.norm (); // A.ldlt().solve(b);
// e = 0.5 * std::abs<double> (e);
// if (delta > e)
// delta = e;
// if (delta < -e)
// delta = -e;
if (std::abs<double> (delta) < accuracy)
{
error = r.norm ();
return current;
}
else
{
current = current + delta;
if (current < minU)
current = maxU - (minU - current);
else if (current > maxU)
current = minU + (current - maxU);
}
}
error = r.norm ();
if (!quiet)
{
printf ("[FittingCurve2d::inverseMapping] Warning: Method did not converge (%e %d).\n", accuracy, maxSteps);
printf ("[FittingCurve2d::inverseMapping] hint: %f current: %f delta: %f error: %f\n", hint, current, delta, error);
}
return current;
}
ON_NurbsCurve
FittingCurve2d::removeCPsOnLine (const ON_NurbsCurve &nurbs, double min_curve_th)
{
int cp_red = nurbs.Order () - 2;
int ncp = nurbs.CVCount () - 2 * cp_red;
std::vector<ON_3dPoint> cps;
for (int j = 1; j < ncp + 1; j++)
{
ON_3dPoint cp0, cp1, cp2;
nurbs.GetCV ((j + 0) % ncp, cp0);
nurbs.GetCV ((j - 1) % ncp, cp1);
nurbs.GetCV ((j + 1) % ncp, cp2);
Eigen::Vector3d v1 (cp1.x - cp0.x, cp1.y - cp0.y, cp1.z - cp0.z);
Eigen::Vector3d v2 (cp2.x - cp0.x, cp2.y - cp0.y, cp2.z - cp0.z);
v1.normalize ();
v2.normalize ();
double d = v1.dot (v2);
if (d >= min_curve_th)
{
cps.push_back (cp0);
}
}
int order = nurbs.Order ();
ON_NurbsCurve nurbs_opt = ON_NurbsCurve (2, false, order, cps.size () + 2 * cp_red);
nurbs_opt.MakePeriodicUniformKnotVector (1.0 / (cps.size ()));
nurbs_opt.m_knot[cp_red] = 0.0;
nurbs_opt.m_knot[nurbs_opt.m_knot_capacity - cp_red - 1] = 1.0;
for (unsigned j = 0; j < cps.size (); j++)
nurbs_opt.SetCV (j + cp_red, cps[j]);
for (int j = 0; j < cp_red; j++)
{
ON_3dPoint cp;
nurbs_opt.GetCV (nurbs_opt.m_cv_count - 1 - cp_red + j, cp);
nurbs_opt.SetCV (j, cp);
nurbs_opt.GetCV (cp_red - j, cp);
nurbs_opt.SetCV (nurbs_opt.m_cv_count - 1 - j, cp);
}
return nurbs_opt;
// NurbsSolve solver;
// solver.assign(nrows, ncp, 2);
//
// for (int i = 0; i < ncp; i++) {
// ON_3dPoint cp;
// m_nurbs.GetCV(i + cp_red, cp);
// solver.x(i, 0, cp.x);
// solver.x(i, 1, cp.y);
// }
//
// // addCageRegularisation
// int row(0);
// {
// solver.f(row, 0, 0.0);
// solver.f(row, 1, 0.0);
// for (int j = 1; j < ncp + 1; j++) {
// solver.K(row, (j + 0) % ncp, -2.0);
// solver.K(row, (j - 1) % ncp, 1.0);
// solver.K(row, (j + 1) % ncp, 1.0);
// row++;
// }
// }
//
// Eigen::MatrixXd d = solver.diff();
//
// for (int i = 0; i < ncp; i++) {
// double dn = d.row(i).norm();
// printf("[FittingCurve2d::optimize] error: %f\n", dn);
// if (dn > max_curve_th)
// dbgWin.AddPoint3D(solver.x(i, 0), solver.x(i, 1), 0.0, 0, 0, 255, 10);
// if (dn < min_curve_th)
// dbgWin.AddPoint3D(solver.x(i, 0), solver.x(i, 1), 0.0, 255, 0, 0, 10);
// }
}
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);
}
