void
GlobalOptimization::setCommonBoundary (const vector_vec3d &boundary, const vector_vec2i &nurbs_indices)
{
if (boundary.empty () || boundary.size () != nurbs_indices.size ())
{
printf ("[GlobalOptimization::setCommonBoundary] Error, common boundary empty or size does not match.\n");
return;
}
}
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
NurbsTools::pca (const vector_vec3d &data, ON_3dVector &mean, Eigen::Matrix3d &eigenvectors,
Eigen::Vector3d &eigenvalues)
{
if (data.empty ())
{
printf ("[NurbsTools::pca] Error, data is empty\n");
abort ();
}
mean = computeMean (data);
unsigned s = data.size ();
ON_Matrix Q(3, s);
for (unsigned i = 0; i < s; i++) {
Q[0][i] = data[i].x - mean.x;
Q[1][i] = data[i].y - mean.y;
Q[2][i] = data[i].z - mean.z;
}
ON_Matrix Qt = Q;
Qt.Transpose();
ON_Matrix oC;
oC.Multiply(Q,Qt);
Eigen::Matrix3d C(3,3);
for (unsigned i = 0; i < 3; i++) {
for (unsigned j = 0; j < 3; j++) {
C(i,j) = oC[i][j];
}
}
Eigen::SelfAdjointEigenSolver < Eigen::Matrix3d > eigensolver (C);
if (eigensolver.info () != Eigen::Success)
{
printf ("[NurbsTools::pca] Can not find eigenvalues.\n");
abort ();
}
for (int i = 0; i < 3; ++i)
{
eigenvalues (i) = eigensolver.eigenvalues () (2 - i);
if (i == 2)
eigenvectors.col (2) = eigenvectors.col (0).cross (eigenvectors.col (1));
else
eigenvectors.col (i) = eigensolver.eigenvectors ().col (2 - i);
}
}
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 (vector_vec3d &data, unsigned size)
{
if (data.size () <= size && size > 0)
return;
unsigned s = data.size ();
vector_vec3d data_tmp;
for (unsigned i = 0; i < size; i++)
{
unsigned rnd = unsigned ((s - 1) * (double (rand ()) / RAND_MAX));
data_tmp.push_back (data[rnd]);
}
data = data_tmp;
}
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
NurbsTools::getClosestPoint (const Eigen::Vector3d &p, const vector_vec3d &data)
{
if (data.empty ())
throw std::runtime_error ("[NurbsTools::getClosestPoint(2d)] Data empty.\n");
size_t idx = 0;
double dist2 (DBL_MAX);
for (size_t i = 0; i < data.size (); i++)
{
double d2 = (data[i] - p).squaredNorm ();
if (d2 < dist2)
{
idx = i;
dist2 = d2;
}
}
return idx;
}
unsigned
NurbsTools::getClosestPoint (const ON_3dPoint &p, const vector_vec3d &data)
{
if (data.empty ())
throw std::runtime_error ("[NurbsTools::getClosestPoint(2d)] Data empty.\n");
unsigned idx (0);
double dist2 (DBL_MAX);
for (unsigned i = 0; i < data.size (); i++)
{
ON_3dVector v = (data[i] - p);
double d2 = ON_DotProduct(v, v); // Was the squaredNorm in Eigen
if (d2 < dist2)
{
idx = i;
dist2 = d2;
}
}
return idx;
}
ON_3dVector
NurbsTools::computeMean (const vector_vec3d &data)
{
ON_3dVector u(0.0, 0.0, 0.0);
unsigned s = data.size ();
double ds = 1.0 / s;
for (unsigned i = 0; i < s; i++)
u += (data[i] * ds);
return u;
}
Eigen::Vector3d
NurbsTools::computeMean (const vector_vec3d &data)
{
Eigen::Vector3d u (0.0, 0.0, 0.0);
unsigned s = unsigned (data.size ());
double ds = 1.0 / s;
for (unsigned i = 0; i < s; i++)
u += (data[i] * ds);
return u;
}
void
NurbsTools::pca (const vector_vec3d &data, Eigen::Vector3d &mean, Eigen::Matrix3d &eigenvectors,
Eigen::Vector3d &eigenvalues)
{
if (data.empty ())
{
printf ("[NurbsTools::pca] Error, data is empty\n");
abort ();
}
mean = computeMean (data);
unsigned s = unsigned (data.size ());
Eigen::MatrixXd Q (3, s);
for (unsigned i = 0; i < s; i++)
Q.col (i) << (data[i] - mean);
Eigen::Matrix3d C = Q * Q.transpose ();
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver (C);
if (eigensolver.info () != Success)
{
printf ("[NurbsTools::pca] Can not find eigenvalues.\n");
abort ();
}
for (int i = 0; i < 3; ++i)
{
eigenvalues (i) = eigensolver.eigenvalues () (2 - i);
if (i == 2)
eigenvectors.col (2) = eigenvectors.col (0).cross (eigenvectors.col (1));
else
eigenvectors.col (i) = eigensolver.eigenvectors ().col (2 - i);
}
}
ON_NurbsCurve
FittingCurve::initNurbsCurvePCA (int order, const vector_vec3d &data, int ncps, double rf)
{
if (data.empty ())
printf ("[FittingCurve::initNurbsCurvePCA] Warning, no boundary parameters available\n");
Eigen::Vector3d mean;
Eigen::Matrix3d eigenvectors;
Eigen::Vector3d eigenvalues;
unsigned s = unsigned (data.size ());
NurbsTools::pca (data, mean, eigenvectors, eigenvalues);
eigenvalues = eigenvalues / s; // seems that the eigenvalues are dependent on the number of points (???)
double r = rf * sqrt (eigenvalues (0));
if (ncps < 2 * order)
ncps = 2 * order;
ON_NurbsCurve nurbs = ON_NurbsCurve (3, false, order, ncps);
nurbs.MakePeriodicUniformKnotVector (1.0 / (ncps - order + 1));
double dcv = (2.0 * M_PI) / (ncps - order + 1);
Eigen::Vector3d cv, cv_t;
for (int j = 0; j < ncps; j++)
{
cv (0) = r * sin (dcv * j);
cv (1) = r * cos (dcv * j);
cv (2) = 0.0;
cv_t = eigenvectors * cv + mean;
nurbs.SetCV (j, ON_3dPoint (cv_t (0), cv_t (1), cv_t (2)));
}
return nurbs;
}
Eigen::Vector3d
NurbsTools::computeVariance (const Eigen::Vector3d &mean, const vector_vec3d &data)
{
Eigen::Vector3d var (0.0, 0.0, 0.0);
size_t s = data.size ();
double ds = 1.0 / double (s);
for (size_t i = 0; i < s; i++)
{
Eigen::Vector3d v = data[i] - mean;
var += Eigen::Vector3d (v (0) * v (0) * ds, v (1) * v (1) * ds, v (2) * v (2) * ds);
}
return var;
}
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);
}
