void ConvexPolygon::calculateMatrixForm() {
//every two adjacent endpoints define a line -> inequality constraint
//first, resize A and b. x and b are column vectors
this->A = Eigen::MatrixXd (this->endpoints.size(),2);
this->b = Eigen::VectorXd (this->endpoints.size());
Eigen::Vector2d normal;
for(size_t i=1; i<endpoints.size(); i++) {
//to define the correct line direction, we also need a point on the inside of the constraint - the seed
normal(0) = endpoints[i-1](1) - endpoints[i](1);
normal(1) = endpoints[i](0) - endpoints[i-1](0);
if(normal.dot(seed) > 0) { //we want the outward pointing normal, so n.dot(s) < 0
normal = -normal;
}
normal.normalize();
b(i-1) = -endpoints[i].dot(normal); //endpoints[i];
A(i-1,0) = normal(0);
A(i-1,1) = normal(1);
}
normal(0) = endpoints.back()(1) - endpoints.front()(1);
normal(1) = endpoints.front()(0) - endpoints.back()(0);
if(normal.dot(seed) > 0) { //we want the outward pointing normal, so n.dot(s) < 0
normal = -normal;
}
normal.normalize();
b(endpoints.size()-1) = -endpoints.front().dot(normal); //endpoints[i];
A(endpoints.size()-1,0) = normal(0);
A(endpoints.size()-1,1) = normal(1);
}
unsigned
NurbsTools::getClosestPoint (const Eigen::Vector2d &p, const Eigen::Vector2d &dir, const vector_vec2d &data,
unsigned &idxcp)
{
if (data.empty ())
throw std::runtime_error ("[NurbsTools::getClosestPoint(2d)] Data empty.\n");
size_t idx = 0;
idxcp = 0;
double dist2 (0.0);
double dist2cp (DBL_MAX);
for (size_t i = 0; i < data.size (); i++)
{
Eigen::Vector2d v = (data[i] - p);
double d2 = v.squaredNorm ();
if (d2 < dist2cp)
{
idxcp = i;
dist2cp = d2;
}
if (d2 == 0.0)
return i;
v.normalize ();
double d1 = dir.dot (v);
if (d1 / d2 > dist2)
{
idx = i;
dist2 = d1 / d2;
}
}
return idx;
}
ON_NurbsSurface
SequentialFitter::grow (float max_dist, float max_angle, unsigned min_length, unsigned max_length)
{
unsigned num_bnd = unsigned (this->m_data.boundary_param.size ());
if (num_bnd == 0)
throw std::runtime_error ("[SequentialFitter::grow] No boundary given.");
if (unsigned (this->m_data.boundary.size ()) != num_bnd)
{
printf ("[SequentialFitter::grow] %lu %u\n", this->m_data.boundary.size (), num_bnd);
throw std::runtime_error ("[SequentialFitter::grow] size of boundary and boundary parameters do not match.");
}
if (this->m_boundary_indices->indices.size () != num_bnd)
{
printf ("[SequentialFitter::grow] %lu %u\n", this->m_boundary_indices->indices.size (), num_bnd);
throw std::runtime_error ("[SequentialFitter::grow] size of boundary indices and boundary parameters do not match.");
}
float angle = cosf (max_angle);
unsigned bnd_moved (0);
for (unsigned i = 0; i < num_bnd; i++)
{
Eigen::Vector3d r, tu, tv, n, bn;
double pointAndTangents[9];
double u = this->m_data.boundary_param[i] (0);
double v = this->m_data.boundary_param[i] (1);
// Evaluate point and tangents
m_nurbs.Evaluate (u, v, 1, 3, pointAndTangents);
r (0) = pointAndTangents[0];
r (1) = pointAndTangents[1];
r (2) = pointAndTangents[2];
tu (0) = pointAndTangents[3];
tu (1) = pointAndTangents[4];
tu (2) = pointAndTangents[5];
tv (0) = pointAndTangents[6];
tv (1) = pointAndTangents[7];
tv (2) = pointAndTangents[8];
n = tu.cross (tv);
n.normalize ();
// calculate boundary normal (pointing outward)
double eps = 0.00000001;
if (u < eps)
bn = n.cross (tv);
if (u > 1.0 - eps)
bn = -n.cross (tv);
if (v < eps)
bn = -n.cross (tu);
if (v > 1.0 - eps)
bn = n.cross (tu);
bn.normalize ();
Eigen::Vector3d e (r (0) + bn (0) * max_dist, r (1) + bn (1) * max_dist, r (2) + bn (2) * max_dist);
// Project into image plane
Eigen::Vector2d ri = this->project (r);
Eigen::Vector2d ei = this->project (e);
Eigen::Vector2d bni = ei - ri;
bni.normalize ();
// search for valid points along boundary normal in image space
float max_dist_sq = max_dist * max_dist;
bool valid = false;
pcl::PointXYZRGB point = m_cloud->at (this->m_boundary_indices->indices[i]);
for (unsigned j = min_length; j < max_length; j++)
{
int col = int (ri (0) + bni (0) * j);
int row = int (ri (1) + bni (1) * j);
if (row >= int (m_cloud->height) || row < 0)
{
j = max_length;
break;
}
if (col >= int (m_cloud->width) || col < 0)
{
j = max_length;
break;
}
unsigned idx = row * m_cloud->width + col;
pcl::PointXYZRGB &pt = m_cloud->at (idx);
if (!pcl_isnan (pt.x) && !pcl_isnan (pt.y) && !pcl_isnan (pt.z))
{
// distance requirement
Eigen::Vector3d d (pt.x - r (0), pt.y - r (1), pt.z - r (2));
if (d.dot (d) < max_dist_sq)
{
d.normalize ();
if (std::abs (d.dot (bn)) > (angle))
{
valid = true;
point = pt;
} // dot
} // max_dist
} // isnan
} // j
// if valid point found, add current boundary point to interior points and move boundary
if (valid)
{
this->m_data.interior.push_back (this->m_data.boundary[i]);
this->m_data.boundary[i] (0) = point.x;
this->m_data.boundary[i] (1) = point.y;
this->m_data.boundary[i] (2) = point.z;
bnd_moved++;
}
} // i
compute_interior (m_nurbs);
double int_err (0.0);
double div_err = 1.0 / double (m_data.interior_error.size ());
for (unsigned i = 0; i < m_data.interior_error.size (); i++)
{
int_err += (m_data.interior_error[i] * div_err);
}
printf ("[SequentialFitter::grow] average interior error: %e\n", int_err);
return m_nurbs;
}
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;
}
template<typename PointInT> pcl::nurbs::NurbsSurface
pcl::nurbs::NurbsFitter<PointInT>::grow (float max_dist, float max_angle, unsigned min_length, unsigned max_length)
{
unsigned num_bnd = this->nurbs_data_.boundary_param.size ();
if (num_bnd == 0)
throw std::runtime_error ("[NurbsFitter::grow] No boundary given.");
if ((unsigned)this->nurbs_data_.boundary.size () != num_bnd)
{
printf ("[NurbsFitter::grow] %u %u\n", (unsigned)this->nurbs_data_.boundary.size (), num_bnd);
throw std::runtime_error ("[NurbsFitter::grow] size of boundary and boundary parameters do not match.");
}
if (this->boundary_indices_->indices.size () != num_bnd)
{
printf ("[NurbsFitter::grow] %u %u\n", (unsigned)this->boundary_indices_->indices.size (), num_bnd);
throw std::runtime_error ("[NurbsFitter::grow] size of boundary indices and boundary parameters do not match.");
}
float angle = cos (max_angle);
unsigned bnd_moved (0);
for (unsigned i = 0; i < num_bnd; i++)
{
vec3 r (0.0, 0.0, 0.0), tu (0.0, 0.0, 0.0), tv (0.0, 0.0, 0.0), n (0.0, 0.0, 0.0), bn (0.0, 0.0, 0.0);
double u = this->nurbs_data_.boundary_param[i] (0);
double v = this->nurbs_data_.boundary_param[i] (1);
// Evaluate point and tangents
nurbs_surface_.evaluate (u, v, r, tu, tv);
n = tu.cross (tv);
n.normalize ();
// calculate boundary normal (pointing outward)
double eps = 0.00000001;
if (u < eps)
bn = n.cross (tv);
if (u > 1.0 - eps)
bn = -n.cross (tv);
if (v < eps)
bn = -n.cross (tu);
if (v > 1.0 - eps)
bn = n.cross (tu);
bn.normalize ();
Eigen::Vector3d e (r (0) + bn (0) * max_dist, r (1) + bn (1) * max_dist, r (2) + bn (2) * max_dist);
// Project into image plane
Eigen::Vector2d ri = this->project (r);
Eigen::Vector2d ei = this->project (e);
Eigen::Vector2d bni = ei - ri;
bni.normalize ();
// search for valid points along boundary normal in image space
float max_dist_sq = max_dist * max_dist;
bool valid = false;
PointInT point = cloud_->at (this->boundary_indices_->indices[i]);
for (unsigned j = min_length; j < max_length; j++)
{
int col = static_cast<int> (ri (0) + bni (0) * j);
int row = static_cast<int> (ri (1) + bni (1) * j);
if (row >= int (cloud_->height) || row < 0)
{
j = max_length;
break;
}
if (col >= int (cloud_->width) || col < 0)
{
j = max_length;
break;
}
unsigned idx = row * cloud_->width + col;
const PointInT &pt = cloud_->at (idx);
if (!pcl_isnan (pt.x) && !pcl_isnan (pt.y) && !pcl_isnan (pt.z))
{
// distance requirement
Eigen::Vector3d d (pt.x - r (0), pt.y - r (1), pt.z - r (2));
if (d.dot (d) < max_dist_sq)
{
d.normalize ();
if (std::abs (d.dot (bn)) > (angle))
{
valid = true;
point = pt;
} // dot
} // max_dist
} // isnan
} // j
// if valid point found, add current boundary point to interior points and move boundary
if (valid)
{
this->nurbs_data_.interior.push_back (this->nurbs_data_.boundary[i]);
this->nurbs_data_.boundary[i] (0) = point.x;
this->nurbs_data_.boundary[i] (1) = point.y;
this->nurbs_data_.boundary[i] (2) = point.z;
++bnd_moved;
}
} // i
computeInterior (nurbs_surface_);
double int_err (0.0);
double div_err = 1.0 / nurbs_data_.interior_error.size ();
for (unsigned i = 0; i < nurbs_data_.interior_error.size (); i++)
{
int_err += (nurbs_data_.interior_error[i] * div_err);
}
printf ("[NurbsFitter::grow] average interior error: %e\n", int_err);
return (nurbs_surface_);
}