void
ClosingBoundary::sampleFromBoundary (ON_NurbsSurface *nurbs, vector_vec3d &point_list, vector_vec2d ¶m_list,
unsigned samples)
{
double ds = 1.0 / (samples - 1);
double minU = nurbs->Knot (0, 0);
double maxU = nurbs->Knot (0, nurbs->KnotCount (0) - 1);
double minV = nurbs->Knot (1, 0);
double maxV = nurbs->Knot (1, nurbs->KnotCount (1) - 1);
Eigen::Vector2d params;
Eigen::Vector3d point;
double points[3];
// WEST
params (0) = minU;
for (unsigned i = 0; i < samples; i++)
{
params (1) = minV + (maxV - minV) * ds * i;
nurbs->Evaluate (params (0), params (1), 0, 3, points);
point_list.push_back (Eigen::Vector3d (points[0], points[1], points[2]));
param_list.push_back (params);
}
// EAST
params (0) = maxU;
for (unsigned i = 0; i < samples; i++)
{
params (1) = minV + (maxV - minV) * ds * i;
nurbs->Evaluate (params (0), params (1), 0, 3, points);
point_list.push_back (Eigen::Vector3d (points[0], points[1], points[2]));
param_list.push_back (params);
}
// SOUTH
params (1) = minV;
for (unsigned i = 0; i < samples; i++)
{
params (0) = minU + (maxU - minU) * ds * i;
nurbs->Evaluate (params (0), params (1), 0, 3, points);
point_list.push_back (Eigen::Vector3d (points[0], points[1], points[2]));
param_list.push_back (params);
}
// NORTH
params (1) = maxV;
for (unsigned i = 0; i < samples; i++)
{
params (0) = minU + (maxU - minU) * ds * i;
nurbs->Evaluate (params (0), params (1), 0, 3, points);
point_list.push_back (Eigen::Vector3d (points[0], points[1], points[2]));
param_list.push_back (params);
}
}
void
ClosingBoundary::sampleUniform (ON_NurbsSurface *nurbs, vector_vec3d &point_list, unsigned samples)
{
double ds = 1.0 / (samples - 1);
double minU = nurbs->Knot (0, 0);
double maxU = nurbs->Knot (0, nurbs->KnotCount (0) - 1);
double minV = nurbs->Knot (1, 0);
double maxV = nurbs->Knot (1, nurbs->KnotCount (1) - 1);
Eigen::Vector2d params;
Eigen::Vector3d point;
double points[3];
for (unsigned j = 0; j < samples; j++)
{
params (1) = minV + (maxV - minV) * ds * j;
for (unsigned i = 0; i < samples; i++)
{
params (0) = minU + (maxU - minU) * ds * i;
nurbs->Evaluate (params (0), params (1), 0, 3, points);
point_list.push_back (Eigen::Vector3d (points[0], points[1], points[2]));
}
}
}
void
NurbsTools::downsample_random (const vector_vec3d &data1, vector_vec3d &data2, unsigned size)
{
if (data1.size () <= size && size > 0)
{
data2 = data1;
return;
}
unsigned s = data1.size ();
data2.clear ();
for (unsigned i = 0; i < size; i++)
{
unsigned rnd = unsigned (s * (double (rand ()) / RAND_MAX));
data2.push_back (data1[rnd]);
}
}
unsigned
SequentialFitter::PCL2ON (pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pcl_cloud, const std::vector<int> &indices,
vector_vec3d &on_cloud)
{
unsigned numPoints (0);
for (unsigned i = 0; i < indices.size (); i++)
{
pcl::PointXYZRGB &pt = pcl_cloud->at (indices[i]);
if (!pcl_isnan (pt.x) && !pcl_isnan (pt.y) && !pcl_isnan (pt.z))
{
on_cloud.push_back (Eigen::Vector3d (pt.x, pt.y, pt.z));
numPoints++;
}
}
return numPoints;
}
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);
}