C++ Vector4f::cross3方法代码示例

template <typename PointT> void
pcl::SampleConsensusModelStick<PointT>::selectWithinDistance (
      const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
  // Needs a valid set of model coefficients
  if (!isModelValid (model_coefficients))
    return;

  float sqr_threshold = static_cast<float> (threshold * threshold);

  int nr_p = 0;
  inliers.resize (indices_->size ());
  error_sqr_dists_.resize (indices_->size ());

  // Obtain the line point and direction
  Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  Eigen::Vector4f line_dir = line_pt2 - line_pt1;
  //Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  //Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0);
  line_dir.normalize ();
  //float norm = line_dir.squaredNorm ();

  // Iterate through the 3d points and calculate the distances from them to the line
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the line
    // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
    Eigen::Vector4f dir = input_->points[(*indices_)[i]].getVector4fMap () - line_pt1;
    //float u = dir.dot (line_dir);

    // If the point falls outside of the segment, ignore it
    //if (u < 0.0f || u > 1.0f)
    //  continue;

    float sqr_distance = dir.cross3 (line_dir).squaredNorm ();
    if (sqr_distance < sqr_threshold)
    {
      // Returns the indices of the points whose squared distances are smaller than the threshold
      inliers[nr_p] = (*indices_)[i];
      error_sqr_dists_[nr_p] = static_cast<double> (sqr_distance);
      ++nr_p;
    }
  }
  inliers.resize (nr_p);
  error_sqr_dists_.resize (nr_p);
}

template <typename PointT> int
pcl::SampleConsensusModelStick<PointT>::countWithinDistance (
      const Eigen::VectorXf &model_coefficients, const double threshold) const
{
  // Needs a valid set of model coefficients
  if (!isModelValid (model_coefficients))
    return (0);

  float sqr_threshold = static_cast<float> (threshold * threshold);

  int nr_i = 0, nr_o = 0;

  // Obtain the line point and direction
  Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  Eigen::Vector4f line_dir = line_pt2 - line_pt1;
  line_dir.normalize ();

  //Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0);
  //Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);

  // Iterate through the 3d points and calculate the distances from them to the line
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the line
    // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
    Eigen::Vector4f dir = input_->points[(*indices_)[i]].getVector4fMap () - line_pt1;
    //float u = dir.dot (line_dir);

    // If the point falls outside of the segment, ignore it
    //if (u < 0.0f || u > 1.0f)
    //  continue;

    float sqr_distance = dir.cross3 (line_dir).squaredNorm ();
    // Use a larger threshold (4 times the radius) to get more points in
    if (sqr_distance < sqr_threshold)
      nr_i++;
    else if (sqr_distance < 4 * sqr_threshold)
      nr_o++;
  }

  return (nr_i - nr_o < 0 ? 0 : nr_i - nr_o);
}

template <typename PointT, typename PointNT, typename PointFeature> void
pcl::NormalBasedSignatureEstimation<PointT, PointNT, PointFeature>::computeFeature (FeatureCloud &output)
{
  // do a few checks before starting the computations

  PointFeature test_feature;
  (void)test_feature;
  if (N_prime_ * M_prime_ != sizeof (test_feature.values) / sizeof (float))
  {
    PCL_ERROR ("NormalBasedSignatureEstimation: not using the proper signature size: %u vs %u\n", N_prime_ * M_prime_, sizeof (test_feature.values) / sizeof (float));
    return;
  }

  std::vector<int> k_indices;
  std::vector<float> k_sqr_distances;

  tree_->setInputCloud (input_);
  output.points.resize (indices_->size ());

  for (size_t index_i = 0; index_i < indices_->size (); ++index_i)
  {
    size_t point_i = (*indices_)[index_i];
    Eigen::MatrixXf s_matrix (N_, M_);

    Eigen::Vector4f center_point = input_->points[point_i].getVector4fMap ();

    for (size_t k = 0; k < N_; ++k)
    {
      Eigen::VectorXf s_row (M_);

      for (size_t l = 0; l < M_; ++l)
      {
        Eigen::Vector4f normal = normals_->points[point_i].getNormalVector4fMap ();
        Eigen::Vector4f normal_u = Eigen::Vector4f::Zero ();
        Eigen::Vector4f normal_v = Eigen::Vector4f::Zero ();

        if (fabs (normal.x ()) > 0.0001f)
        {
          normal_u.x () = - normal.y () / normal.x ();
          normal_u.y () = 1.0f;
          normal_u.z () = 0.0f;
          normal_u.normalize ();

        }
        else if (fabs (normal.y ()) > 0.0001f)
        {
          normal_u.x () = 1.0f;
          normal_u.y () = - normal.x () / normal.y ();
          normal_u.z () = 0.0f;
          normal_u.normalize ();
        }
        else
        {
          normal_u.x () = 0.0f;
          normal_u.y () = 1.0f;
          normal_u.z () = - normal.y () / normal.z ();
        }
        normal_v = normal.cross3 (normal_u);

        Eigen::Vector4f zeta_point = 2.0f * static_cast<float> (l + 1) * scale_h_ / static_cast<float> (M_) * 
            (cosf (2.0f * static_cast<float> (M_PI) * static_cast<float> ((k + 1) / N_)) * normal_u + 
             sinf (2.0f * static_cast<float> (M_PI) * static_cast<float> ((k + 1) / N_)) * normal_v);

        // Compute normal by using the neighbors
        Eigen::Vector4f zeta_point_plus_center = zeta_point + center_point;
        PointT zeta_point_pcl;
        zeta_point_pcl.x = zeta_point_plus_center.x (); zeta_point_pcl.y = zeta_point_plus_center.y (); zeta_point_pcl.z = zeta_point_plus_center.z ();

        tree_->radiusSearch (zeta_point_pcl, search_radius_, k_indices, k_sqr_distances);

        // Do k nearest search if there are no neighbors nearby
        if (k_indices.size () == 0)
        {
          k_indices.resize (5);
          k_sqr_distances.resize (5);
          tree_->nearestKSearch (zeta_point_pcl, 5, k_indices, k_sqr_distances);
        }
        
        Eigen::Vector4f average_normal = Eigen::Vector4f::Zero ();

        float average_normalization_factor = 0.0f;

        // Normals weighted by 1/squared_distances
        for (size_t nn_i = 0; nn_i < k_indices.size (); ++nn_i)
        {
          if (k_sqr_distances[nn_i] < 1e-7f)
          {
            average_normal = normals_->points[k_indices[nn_i]].getNormalVector4fMap ();
            average_normalization_factor = 1.0f;
            break;
          }
          average_normal += normals_->points[k_indices[nn_i]].getNormalVector4fMap () / k_sqr_distances[nn_i];
          average_normalization_factor += 1.0f / k_sqr_distances[nn_i];
        }
        average_normal /= average_normalization_factor;
        float s = zeta_point.dot (average_normal) / zeta_point.norm ();
        s_row[l] = s;
      }

      // do DCT on the s_matrix row-wise
//.........这里部分代码省略.........

void
  pcl_ros::ConvexHull2D::input_indices_callback (const PointCloudConstPtr &cloud, 
                                                 const PointIndicesConstPtr &indices)
{
  // No subscribers, no work
  if (pub_output_.getNumSubscribers () <= 0 && pub_plane_.getNumSubscribers () <= 0)
    return;

  PointCloud output;

  // If cloud is given, check if it's valid
  if (!isValid (cloud))
  {
    NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
    // Publish an empty message
    output.header = cloud->header;
    pub_output_.publish (output.makeShared ());
    return;
  }
  // If indices are given, check if they are valid
  if (indices && !isValid (indices, "indices"))
  {
    NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
    // Publish an empty message
    output.header = cloud->header;
    pub_output_.publish (output.makeShared ());
    return;
  }

  /// DEBUG
  if (indices)
    NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
                   "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
                   "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
                   getName ().c_str (),
                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
                   indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
  else
    NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());

  // Reset the indices and surface pointers
  IndicesPtr indices_ptr;
  if (indices)
    indices_ptr.reset (new std::vector<int> (indices->indices));

  impl_.setInputCloud (cloud);
  impl_.setIndices (indices_ptr);

  // Estimate the feature
  impl_.reconstruct (output);

  // If more than 3 points are present, send a PolygonStamped hull too
  if (output.points.size () >= 3)
  {
    geometry_msgs::PolygonStamped poly;
    poly.header = output.header;
    poly.polygon.points.resize (output.points.size ());
    // Get three consecutive points (without copying)
    pcl::Vector4fMap O = output.points[1].getVector4fMap ();
    pcl::Vector4fMap B = output.points[0].getVector4fMap ();
    pcl::Vector4fMap A = output.points[2].getVector4fMap ();
    // Check the direction of points -- polygon must have CCW direction
    Eigen::Vector4f OA = A - O;
    Eigen::Vector4f OB = B - O;
    Eigen::Vector4f N = OA.cross3 (OB);
    double theta = N.dot (O);
    bool reversed = false;
    if (theta < (M_PI / 2.0))
      reversed = true;
    for (size_t i = 0; i < output.points.size (); ++i)
    {
      if (reversed)
      {
        size_t j = output.points.size () - i - 1;
        poly.polygon.points[i].x = output.points[j].x;
        poly.polygon.points[i].y = output.points[j].y;
        poly.polygon.points[i].z = output.points[j].z;
      }
      else
      {
        poly.polygon.points[i].x = output.points[i].x;
        poly.polygon.points[i].y = output.points[i].y;
        poly.polygon.points[i].z = output.points[i].z;
      }
    }
    pub_plane_.publish (boost::make_shared<const geometry_msgs::PolygonStamped> (poly));
  }
  // Publish a Boost shared ptr const data
  output.header = cloud->header;
  pub_output_.publish (output.makeShared ());
}

template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computePairFeatures (
      const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
      int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4)
{
  // Compute the Cartesian difference between the two points
  Eigen::Vector4f delta = cloud.points[q_idx].getVector4fMap () - cloud.points[p_idx].getVector4fMap ();
  delta[3] = 0;

  // Compute the Euclidean norm = || p_idx - q_idx ||
  float distance_sqr = delta.squaredNorm ();

  if (distance_sqr == 0)
  {
    ROS_ERROR ("Euclidean distance between points %d and %d is 0!", p_idx, q_idx);
    f1 = f2 = f3 = f4 = 0;
    return (false);
  }

  // Estimate f4 = || delta ||
  f4 = sqrt (distance_sqr);

  // Create a Darboux frame coordinate system u-v-w
  // u = n1; v = (p_idx - q_idx) x u / || (p_idx - q_idx) x u ||; w = u x v
  pcl::Vector4fMapConst u = normals.points[p_idx].getNormalVector4fMap ();

  // Estimate f3 = u * delta / || delta ||
  // delta[3] = 0 (line 59)
  f3 = u.dot (delta) / f4;

  // v = delta * u
  Eigen::Vector4f v = Eigen::Vector4f::Zero ();
  v = delta.cross3 (u);

  distance_sqr = v.squaredNorm ();
  if (distance_sqr == 0)
  {
    ROS_ERROR ("Norm of Delta x U is 0 for point %d and %d!", p_idx, q_idx);
    f1 = f2 = f3 = f4 = 0;
    return (false);
  }

  // Copy the q_idx normal
  Eigen::Vector4f nq (normals.points[q_idx].normal_x,
                      normals.points[q_idx].normal_y,
                      normals.points[q_idx].normal_z,
                      0);

  // Normalize the vector
  v /= sqrt (distance_sqr);

  // Compute delta (w) = u x v
  delta = u.cross3 (v);

  // Compute f2 = v * n2;
  // v[3] = 0 (line 82)
  f2 = v.dot (nq);

  // Compute f1 = arctan (w * n2, u * n2) i.e. angle of n2 in the x=u, y=w coordinate system
  // delta[3] = 0 (line 59), nq[3] = 0 (line 97)
  f1 = atan2f (delta.dot (nq), u.dot (nq));       // @todo: optimize this

  return (true);
}