ON_NurbsCurve FittingCurve2d::initCPsNurbsCurve2D (int order, const vector_vec2d &cps) { int cp_red = order - 2; ON_NurbsCurve nurbs; if (cps.size () < 3 || cps.size () < (2 * cp_red + 1)) { printf ("[FittingCurve2d::initCPsNurbsCurve2D] Warning, number of control points too low.\n"); return nurbs; } int ncps = cps.size () + 2 * cp_red; // +2*cp_red for smoothness and +1 for closing nurbs = ON_NurbsCurve (2, false, order, ncps); nurbs.MakePeriodicUniformKnotVector (1.0 / (ncps - order + 1)); for (int j = 0; j < cps.size (); j++) nurbs.SetCV (cp_red + j, ON_3dPoint (cps[j] (0), cps[j] (1), 0.0)); // close nurbs nurbs.SetCV (cp_red + cps.size (), ON_3dPoint (cps[0] (0), cps[0] (1), 0.0)); // make smooth at closing point for (int j = 0; j < cp_red; j++) { ON_3dPoint cp; nurbs.GetCV (nurbs.CVCount () - 1 - cp_red + j, cp); nurbs.SetCV (j, cp); nurbs.GetCV (cp_red - j, cp); nurbs.SetCV (nurbs.CVCount () - 1 - j, cp); } return nurbs; }
ON_NurbsCurve FittingCurve2d::initCPsNurbsCurve2D (int order, const vector_vec2d &cps) { ON_NurbsCurve nurbs; if ((int)cps.size () < (2 * order)) { printf ("[FittingCurve2d::initCPsNurbsCurve2D] Warning, number of control points too low.\n"); return nurbs; } int cp_red = order - 2; int ncps = cps.size () + cp_red; nurbs = ON_NurbsCurve (2, false, order, ncps); nurbs.MakePeriodicUniformKnotVector (1.0 / (ncps - order + 1)); for (int j = 0; j < ncps; j++) nurbs.SetCV (j, ON_3dPoint (cps[j] (0), cps[j] (1), 0.0)); for (int j = 0; j < cp_red; j++) { ON_3dPoint cp; nurbs.GetCV (nurbs.m_cv_count - 1 - cp_red + j, cp); nurbs.SetCV (j, cp); nurbs.GetCV (cp_red - j, cp); nurbs.SetCV (nurbs.m_cv_count - 1 - j, cp); } return nurbs; }
void NurbsTools::computeBoundingBox (const ON_NurbsCurve &nurbs, Eigen::Vector3d &_min, Eigen::Vector3d &_max) { _min = Eigen::Vector3d (DBL_MAX, DBL_MAX, DBL_MAX); _max = Eigen::Vector3d (-DBL_MAX, -DBL_MAX, -DBL_MAX); for (int i = 0; i < nurbs.CVCount (); i++) { ON_3dPoint p; nurbs.GetCV (i, p); if (p.x < _min (0)) _min (0) = p.x; if (p.y < _min (1)) _min (1) = p.y; if (p.z < _min (2)) _min (2) = p.z; if (p.x > _max (0)) _max (0) = p.x; if (p.y > _max (1)) _max (1) = p.y; if (p.z > _max (2)) _max (2) = p.z; } }
void VisualizeCurve (ON_NurbsCurve &curve, double r, double g, double b, bool show_cps) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::on_nurbs::Triangulation::convertCurve2PointCloud (curve, cloud, 8); for (std::size_t i = 0; i < cloud->size () - 1; i++) { pcl::PointXYZRGB &p1 = cloud->at (i); pcl::PointXYZRGB &p2 = cloud->at (i + 1); std::ostringstream os; os << "line_" << r << "_" << g << "_" << b << "_" << i; viewer.addLine<pcl::PointXYZRGB> (p1, p2, r, g, b, os.str ()); } if (show_cps) { pcl::PointCloud<pcl::PointXYZ>::Ptr cps (new pcl::PointCloud<pcl::PointXYZ>); for (int i = 0; i < curve.CVCount (); i++) { ON_3dPoint cp; curve.GetCV (i, cp); pcl::PointXYZ p; p.x = float (cp.x); p.y = float (cp.y); p.z = float (cp.z); cps->push_back (p); } pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler (cps, 255 * r, 255 * g, 255 * b); viewer.addPointCloud<pcl::PointXYZ> (cps, handler, "cloud_cps"); } }
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_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); // } }