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

inline double
pcl::getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2)
{
  // Compute the actual angle
  double rad = v1.dot (v2) / sqrt (v1.squaredNorm () * v2.squaredNorm ());
  if (rad < -1.0) rad = -1.0;
  if (rad >  1.0) rad = 1.0;
  return (acos (rad));
}

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

      // tf::Transform transform;
      // transform.setOrigin( tf::Vector3(point_center.x, point_center.y, point_center.z));
      // transform.setRotation( tf::Quaternion(0, 0, 0) );
      // transform_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
      //                                                           cloud_raw.header.frame_id, rot_table_frame_));

      // ---[ Get the objects on top of the table - from the raw cloud!
      pcl::PointIndices cloud_object_indices;
      prism_.setHeightLimits (cluster_min_height_, cluster_max_height_);
      prism_.setInputCloud (boost::make_shared<PointCloud> (cloud_raw));
      prism_.setInputPlanarHull (boost::make_shared<PointCloud>(cloud_hull));
      prism_.segment (cloud_object_indices);
      //ROS_INFO ("[%s] Number of object point indices: %d.", getName ().c_str (), (int)cloud_object_indices.indices.size ());

      pcl::PointCloud<Point> cloud_object;
      pcl::ExtractIndices<Point> extract_object_indices;
      //extract_object_indices.setInputCloud (cloud_all_minus_table_ptr);
      extract_object_indices.setInputCloud (boost::make_shared<PointCloud> (cloud_raw));
      extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
      extract_object_indices.filter (cloud_object);
      //ROS_INFO ("[%s ] Publishing number of object point candidates: %d.", getName ().c_str (), (int)cloud_objects.points.size ());

      std::vector<pcl::PointIndices> clusters;
      cluster_.setInputCloud (boost::make_shared<PointCloud>(cloud_object));
      cluster_.setClusterTolerance (object_cluster_tolerance_);
      cluster_.setMinClusterSize (object_cluster_min_size_);
      cluster_.setMaxClusterSize (object_cluster_max_size_);
      cluster_.setSearchMethod (clusters_tree_);
      cluster_.extract (clusters);

      // TODO take closest to ray
      for (size_t i = 0; i < clusters.size (); i++)
      {
        // compute center and see if it is close enough to ray
        int cluster_center = clusters[i].indices[clusters[i].indices.size () / 2];
        Eigen::Vector4f pt = Eigen::Vector4f (cloud_object.points[cluster_center].x, cloud_object.points[cluster_center].y, cloud_object.points[cluster_center].z, 0);
        Eigen::Vector4f c = line_dir_.cross3 (line_point_ - pt); c[3] = 0;
#ifndef TEST
        if (c.squaredNorm () / line_dir_squaredNorm_ > 0.25*0.25) // further then 10cm
          continue;
#endif
        std::cerr << "found cluster" << std::endl;
        // TODO make more optimal? + ERRORS/INCONSISTENCY IN PCL: second formula and in c computation!

        //tf::transformPoint();
        //Eigen::Matrix4f transform_matrix;
        //pcl_ros::transformAsMatrix (c2h_transform, transform_matrix);

        // broadcast transform
//        tf::Transform fixed_transform;
//        fixed_transform.setOrigin (tf::Vector3(0.0, 0.0, 0.1));
//        fixed_transform.setRotation (tf::Quaternion(0, 0, 0));
//        transform_broadcaster_.sendTransform(tf::StampedTransform(fixed_transform, ros::Time::now(), "right_hand", "object_frame"));

        // transform object into right_hand frame
        pcl::PointCloud<Point> cloud_object_clustered ;
        pcl::copyPointCloud (cloud_object, clusters[i], cloud_object_clustered);
//        Eigen::Matrix4f eigen_transform;
//        pcl_ros::transformAsMatrix (c2h_transform, eigen_transform);
//        pcl::transformPointCloud(cloud_object_clustered, output_cloud_, eigen_transform);
//        output_cloud_.header.frame_id = ""right_hand";
        sensor_msgs::PointCloud2 cluster;
        pcl::toROSMsg (cloud_object_clustered, cluster);
//        template <typename PointT> void transformPointCloud (const pcl::PointCloud <PointT> &cloud_in, pcl::PointCloud <PointT> &cloud_out, const tf::Transform &transform);
//        void transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out);
        pcl_ros::transformPointCloud("right_hand", (tf::Transform)c2h_transform, cluster, output_cloud_);
        output_cloud_.header.frame_id = "right_hand"; // TODO needed?
//        for (unsigned cp = 0; cp < output_cloud_.points.size (); cp++)
//        {
//          output_cloud_.points[i].x -= output_cloud_.points[0].x;
//          output_cloud_.points[i].y -= output_cloud_.points[0].y;
//          output_cloud_.points[i].z -= output_cloud_.points[0].z;
//        }
//        cloud_objects_pub_.publish (output_cloud_);
        object_pub_.publish (output_cloud_);
        ROS_INFO("Published object with %d points", (int)clusters[i].indices.size ());

        // TODO get a nicer rectangle around the object
        unsigned center_idx = cloud_object_indices.indices.at (cluster_center);
        unsigned row = center_idx / 640;
        unsigned col = center_idx - row * 640;
        ros::ServiceClient client = nh_.serviceClient<kinect_cleanup::FilterObject>("/filter_object");
        kinect_cleanup::FilterObject srv;
        srv.request.min_row = std::max (0, (int)row-50);
        srv.request.min_col = std::max (0, (int)col-40);
        srv.request.max_row = std::min (479, (int)row+50);
        srv.request.max_col = std::min (479, (int)col+40);
        srv.request.rgb = cloud_raw.points[640 * srv.request.max_row + srv.request.max_col].rgb;
        for (int i=0; i<4; i++)
          srv.request.plane_normal[i] = table_coeff.values[i];
        if (client.call(srv))
          ROS_INFO("Object filter service responded: %s", srv.response.error.c_str ());
        else
          ROS_ERROR("Failed to call object filter service!");

        // take only 1 cluster into account
        break;
      }
    }

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