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); }