C++ Vector2d::normalize方法代码示例

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))
          {
//.........这里部分代码省略.........

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 ();

//.........这里部分代码省略.........

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)
    {
//.........这里部分代码省略.........