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; }
double FittingCurve::findClosestElementMidPoint (const ON_NurbsCurve &nurbs, const Eigen::Vector3d &pt) { double hint (0.0); Eigen::Vector3d p, r; std::vector<double> elements = getElementVector (nurbs); double points[3]; double d_shortest (DBL_MAX); for (unsigned i = 0; i < elements.size () - 1; i++) { double xi = elements[i] + 0.5 * (elements[i + 1] - elements[i]); nurbs.Evaluate (xi, 0, 3, points); p (0) = points[0]; p (1) = points[1]; p (2) = points[2]; r = p - pt; double d = r.squaredNorm (); if (d < d_shortest) { d_shortest = d; hint = xi; } } return hint; }
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); } } }
double FittingCurve::inverseMapping (const ON_NurbsCurve &nurbs, const Eigen::Vector3d &pt, const double &hint, double &error, Eigen::Vector3d &p, Eigen::Vector3d &t, int maxSteps, double accuracy, bool quiet) { //int cp_red = (nurbs.m_order - 2); //int ncpj = int (nurbs.m_cv_count - 2 * cp_red); double pointAndTangents[6]; double current, delta; Eigen::Vector3d 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, 3, pointAndTangents); p (0) = pointAndTangents[0]; p (1) = pointAndTangents[1]; p (2) = pointAndTangents[2]; t (0) = pointAndTangents[3]; t (1) = pointAndTangents[4]; t (2) = pointAndTangents[5]; r = p - pt; // step width control int E = findElement (current, elements); double e = elements[E + 1] - elements[E]; delta = -(0.5 * e) * r.dot (t) / t.norm (); // A.ldlt().solve(b); if (std::fabs (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 ("[FittingCurve::inverseMapping] Warning: Method did not converge (%e %d).\n", accuracy, maxSteps); printf ("[FittingCurve::inverseMapping] hint: %f current: %f delta: %f error: %f\n", hint, current, delta, error); } return current; }
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; }