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