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

vtkSmartPointer<vtkDataSet>
pcl::visualization::createLine (const Eigen::Vector4f &pt1, const Eigen::Vector4f &pt2)
{
  vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
  line->SetPoint1 (pt1.x (), pt1.y (), pt1.z ());
  line->SetPoint2 (pt2.x (), pt2.y (), pt2.z ());
  line->Update ();

  return (line->GetOutput ());
}

  void computeCovarianceMatrix(PointCloudRGB cloud, std::vector<int> indices, Eigen::Vector4f centroid, Eigen::Matrix3f &covariance_matrix)
  {
    // ROS_INFO("Inside computeCovarianceMatrix() ");
    // Initialize to 0
    covariance_matrix.setZero ();
    
    if (indices.empty ())
      return;
  
    // If the data is dense, we don't need to check for NaN
    if (cloud.is_dense)
      {
	// For each point in the cloud
	for (size_t i = 0; i < indices.size (); ++i)
	  {
	    //Eigen::Vector4f ptx = cloud.points[indices[i]].getVector4fMap ();
	    ///std::cout << "Index : "<< i <<" : "<<ptx<<std::endl;
	    Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid;
	    
	    covariance_matrix (0, 0) += pt.x () * pt.x ();
	    covariance_matrix (0, 1) += pt.x () * pt.y ();
	    covariance_matrix (0, 2) += pt.x () * pt.z ();
	    covariance_matrix (1, 1) += pt.y () * pt.y ();
	    covariance_matrix (1, 2) += pt.y () * pt.z ();	
	    covariance_matrix (2, 2) += pt.z () * pt.z ();
	  }

      }
    // NaN or Inf values could exist => check for them
    else
      {
	//std::cout<<"Cloud is not dense "<<std::endl;
	// For each point in the cloud
	for (size_t i = 0; i < indices.size (); ++i)
	  {
	    // Check if the point is invalid
	    if (!pcl_isfinite (cloud.points[indices[i]].x) || 
		!pcl_isfinite (cloud.points[indices[i]].y) || 
		!pcl_isfinite (cloud.points[indices[i]].z))
	      continue;
	    
	    // Eigen::Vector4f ptx = cloud.points[indices[i]].getVector4fMap ();
	    // std::cout << "Index : "<< i <<" : "<<ptx<<std::endl;
	    Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid;
	    
	    covariance_matrix (0, 0) += pt.x () * pt.x ();
	    covariance_matrix (0, 1) += pt.x () * pt.y ();
	    covariance_matrix (0, 2) += pt.x () * pt.z ();
	    covariance_matrix (1, 1) += pt.y () * pt.y ();
	    covariance_matrix (1, 2) += pt.y () * pt.z ();	
	    covariance_matrix (2, 2) += pt.z () * pt.z ();
	    covariance_matrix (1, 1) += pt.y () * pt.y ();
	  }
      }
    covariance_matrix (1, 0) = covariance_matrix (0, 1);
    covariance_matrix (2, 0) = covariance_matrix (0, 2);
    covariance_matrix (2, 1) = covariance_matrix (1, 2);
    
    covariance_matrix /= indices.size();
  }

template <typename PointNT> void
pcl::GridProjection<PointNT>::getProjectionWithPlaneFit (const Eigen::Vector4f &p,
                                                         std::vector<int> (&pt_union_indices),
                                                         Eigen::Vector4f &projection)
{
  // Compute the plane coefficients
  Eigen::Vector4f model_coefficients;
  /// @remark iterative weighted least squares or sac might give better results
  Eigen::Matrix3f covariance_matrix;
  Eigen::Vector4f xyz_centroid;

  computeMeanAndCovarianceMatrix (*data_, pt_union_indices, covariance_matrix, xyz_centroid);

  // Get the plane normal
  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);

  // The normalization is not necessary, since the eigenvectors from libeigen are already normalized
  model_coefficients[0] = eigen_vector [0];
  model_coefficients[1] = eigen_vector [1];
  model_coefficients[2] = eigen_vector [2];
  model_coefficients[3] = 0;
  // Hessian form (D = nc . p_plane (centroid here) + p)
  model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);

  // Projected point
  Eigen::Vector3f point (p.x (), p.y (), p.z ());     //= Eigen::Vector3f::MapAligned (&output.points[cp].x, 3);
  float distance = point.dot (model_coefficients.head <3> ()) + model_coefficients[3];
  point -= distance * model_coefficients.head < 3 > ();

  projection = Eigen::Vector4f (point[0], point[1], point[2], 0);
}

void Mass::update(float dt){
    if(pinned)
            return;
    Eigen::Vector4f guess;
    guess << position.x(),position.y(),position.z(),1;
    Eigen::Vector4f prev;
    prev << position.x(),position.y(),position.z(),1;
    Eigen::Vector4f vguess;
    vguess << velocity.x(),velocity.y(),velocity.z(),1;
    for(int i = 0; i<1; i++){
        f(&guess,&vguess);
        guess = guess+10*dt*vguess; 
    }
    temp = guess;
   // position = position + velocity * dt;
}

void Mass::f(Eigen::Vector4f *pos, Eigen::Vector4f *result){
    float stiff = 10.0f;
    Eigen::Vector4f a;
    a << 0,0,0,0;
    Eigen::Vector4f v;
    v << velocity.x(),velocity.y(),velocity.z(),0;
    for(int i = 0; i<links.size(); i++){
        Mass *link = links[i];
        float length = (*pos-link->position).norm();
        float stretch = length-2.0f/40;
        Eigen::Vector4f force;

        force = (stiff*(link->position-(*pos))*stretch);
        if(force.norm()>.2){
                force = force*(.2/force.norm());
        }
        a += force;
    }
    a.z() += .2f*GRAVITY*.0001f;
    for(int i = 0; i<objects.size(); i++){
            Eigen::Vector4f normal;
            if(objects[i]->intersects(&position,&normal))
            {
                v *= 0;
                a *= 0;
            }
    }
    v += a;
    *result = v;
}

/** \brief Constructor.  Sets the cloud we search over, and the maximum radius search we will guarantee to be correct
* \param cloud the incoming point cloud
* \param tol the maximum radius search we will guarantee to be correct
*/
SplitCloud2::SplitCloud2(pcl::PointCloud<pcl::PointXYZ> &cloud, double tol){
	max_tol=tol;
	Eigen::Vector4f vec;
	pcl::compute3DCentroid(cloud,vec);
	xdiv=vec.x(); ydiv=vec.y(); zdiv=vec.z();
	fillMInds(cloud); //fill out divs
	initClouds(cloud);
}

	YUMEALIGN16 Eigen::Vector4f window::translate(const Eigen::Vector4f& vertex) {
		Eigen::Vector4f outv = (perspective_matrix * (matrix * vertex));
		int halfw = width / 2;
		int halfh = height / 2;
		outv(0) = (halfw + halfw * (outv.x() / outv(3)));
		outv(1) = (halfh - halfh * (outv.y() / outv(3)));
		return outv;
	}

Eigen::Vector3f quaternionMutltiply(Eigen::Vector4f quat, Eigen::Vector3f vec){
     float num = quat.x() * 2.0f;
     float num2 = quat.y() * 2.0f;
     float num3 = quat.z() * 2.0f;
     float num4 = quat.x()* num;
     float num5 = quat.y() * num2;
     float num6 = quat.z() * num3;
     float num7 = quat.x() * num2;
     float num8 = quat.x() * num3;
     float num9 = quat.y ()* num3;
     float num10 = quat.w() * num;
     float num11 = quat.w ()* num2;
     float num12 = quat.w ()* num3;
     Eigen::Vector3f  result;
     result.x() = (1.0f - (num5 + num6)) * vec.x ()+ (num7 - num12) * vec.y() + (num8 + num11) * vec.z();
     result.y() = (num7 + num12) * vec.x() + (1.0f - (num4 + num6)) * vec.y() + (num9 - num10) * vec.z();
     result.z() = (num8 - num11) * vec.x() + (num9 + num10) * vec.y() + (1.0f - (num4 + num5)) * vec.z();
     return result;
 }

template <typename PointT> inline void
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 
                              const std::vector<int> &indices,
                              const Eigen::Vector4f &centroid, 
                              Eigen::Matrix3f &covariance_matrix)
{
  // Initialize to 0
  covariance_matrix.setZero ();

  if (indices.empty ())
    return;
  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    // For each point in the cloud
    for (size_t i = 0; i < indices.size (); ++i)
    {
      Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid;

      covariance_matrix (1, 1) += pt.y () * pt.y ();
      covariance_matrix (1, 2) += pt.y () * pt.z ();

      covariance_matrix (2, 2) += pt.z () * pt.z ();

      pt *= pt.x ();
      covariance_matrix (0, 0) += pt.x ();
      covariance_matrix (0, 1) += pt.y ();
      covariance_matrix (0, 2) += pt.z ();
    }
  }
  // NaN or Inf values could exist => check for them
  else
  {
    // For each point in the cloud
    for (size_t i = 0; i < indices.size (); ++i)
    {
      // Check if the point is invalid
      if (!pcl_isfinite (cloud.points[indices[i]].x) || 
          !pcl_isfinite (cloud.points[indices[i]].y) || 
          !pcl_isfinite (cloud.points[indices[i]].z))
        continue;

      Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid;

      covariance_matrix (1, 1) += pt.y () * pt.y ();
      covariance_matrix (1, 2) += pt.y () * pt.z ();

      covariance_matrix (2, 2) += pt.z () * pt.z ();

      pt *= pt.x ();
      covariance_matrix (0, 0) += pt.x ();
      covariance_matrix (0, 1) += pt.y ();
      covariance_matrix (0, 2) += pt.z ();
    }
  }
  covariance_matrix (1, 0) = covariance_matrix (0, 1);
  covariance_matrix (2, 0) = covariance_matrix (0, 2);
  covariance_matrix (2, 1) = covariance_matrix (1, 2);
}

	void window::process_scanline(int y, const Eigen::Vector4f& pa, const Eigen::Vector4f& pb, const Eigen::Vector4f& pc, const Eigen::Vector4f& pd) {
		if (y < 0 || y >= height) {
			return;
		}
		float gradient1 = pa.y() != pb.y() ? (y - pa.y()) / (pb.y() - pa.y()) : 1.0f;
		float gradient2 = pc.y() != pd.y() ? (y - pc.y()) / (pd.y() - pc.y()) : 1.0f;
		int sx = static_cast<int>(interpolate(pa.x(), pb.x(), gradient1));
		int ex = static_cast<int>(interpolate(pc.x(), pd.x(), gradient2));
		float z1 = interpolate(pa.z(), pb.z(), gradient1);
		float z2 = interpolate(pc.z(), pd.z(), gradient2);
		for (int x = sx; x < ex; ++x) {
			if (x < 0 || x >= width) {
				continue;
			}
			size_t index = (y * width) + x;
			float z = interpolate(z1, z2, (x - sx) / static_cast<float>(ex - sx));
			if (depth_buf[index] > z) {
				continue;
			}
			depth_buf[index] = z;
			buf[index] = YUMERGBA(r, g, b, a);
		}
	}

	// according to http://www.pcl-users.org/Finding-oriented-bounding-box-of-a-cloud-td4024616.html
	FitCube PCLTools::fitBox(PointCloud<PointXYZ>::Ptr cloud) {

		FitCube retCube;
		PCA<PointXYZ> pca;
		PointCloud<PointXYZ> proj;

		pca.setInputCloud(cloud);
		pca.project(*cloud, proj);

		PointXYZ proj_min;
		PointXYZ proj_max;
		getMinMax3D(proj, proj_min, proj_max);

		PointXYZ min;
		PointXYZ max;
		pca.reconstruct(proj_min, min);
		pca.reconstruct(proj_max, max);

		// Rotation of PCA
		Eigen::Matrix3f rot_mat = pca.getEigenVectors();

		// Translation of PCA
		Eigen::Vector3f cl_translation = pca.getMean().head(3);

		Eigen::Matrix3f affine_trans;

		// Reordering of principal components
		affine_trans.col(2) << (rot_mat.col(0).cross(rot_mat.col(1))).normalized();
		affine_trans.col(0) << rot_mat.col(0);
		affine_trans.col(1) << rot_mat.col(1);

		retCube.rotation = Eigen::Quaternionf(affine_trans);
		Eigen::Vector4f t = pca.getMean();

		retCube.translation = Eigen::Vector3f(t.x(), t.y(), t.z());

		retCube.width = fabs(proj_max.x - proj_min.x);
		retCube.height = fabs(proj_max.y - proj_min.y);
		retCube.depth = fabs(proj_max.z - proj_min.z);

		return retCube;

	}

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 cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // ... do data processing
  sensor_msgs::PointCloud2 output;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg (*input, *cloud);

  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (0.10); // 2cm
  ec.setMinClusterSize (20);
  ec.setMaxClusterSize (2500);
  ec.setSearchMethod (tree);
  ec.setInputCloud(cloud);
  ec.extract (cluster_indices);
  
  ROS_INFO("cluster_indices has %d data points.", (int) cluster_indices.size());
  ROS_INFO("cloud has %d data points.", (int) cloud->points.size());

  visualization_msgs::Marker marker;
  marker.header   = cloud->header;
  marker.id       = 1;
  marker.type     = visualization_msgs::Marker::CUBE_LIST;
  marker.action   = visualization_msgs::Marker::ADD;
  marker.color.r  = 1;
  marker.color.g  = 0;
  marker.color.b  = 0;
  marker.color.a  = 0.7;
  marker.scale.x  = 0.2;
  marker.scale.y  = 0.2;
  marker.scale.z  = 0.2;
  marker.lifetime = ros::Duration(60.0);
  Eigen::Vector4f minPoint;
  Eigen::Vector4f maxPoint;
//  pcl::getMinMax3D(*cloud, minPoint, maxPoint);

  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
      cloud_cluster->points.push_back (cloud->points[*pit]); 

    std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
    // Merge current clusters to whole point cloud
    *clustered_cloud += *cloud_cluster;
 //   for(size_t j = 0; j < cloud_cluster->points.size() - 1; j++)
 //   {
      /*
      geometry_msgs::Point pt1;
      pt1.x = cloud_cluster->points[j].x;
      pt1.y = cloud_cluster->points[j].y;
      pt1.z = cloud_cluster->points[j].z;
      geometry_msgs::Point pt2;
      pt2.x = cloud_cluster->points[j+1].x;
      pt2.y = cloud_cluster->points[j+1].y;
      pt2.z = cloud_cluster->points[j+1].z;

      marker.points.push_back(pt1);
      marker.points.push_back(pt2);
      */
      //Seg for top of prism
      geometry_msgs::Point pt3;      
      pt3.x = 0.0;
      pt3.y = 0.0;
      pt3.z = 0.0;
      std_msgs::ColorRGBA colors;
      colors.r = 0.0;
      colors.g = 0.0;
      colors.b = 0.0;
      for(size_t i=0; i<cloud_cluster->points.size(); i++)
      {
        pt3.x += cloud_cluster->points[i].x;
        pt3.y += cloud_cluster->points[i].y;
        pt3.z += cloud_cluster->points[i].z;
      }
      pt3.x /= cloud_cluster->points.size();
      pt3.y /= cloud_cluster->points.size();
      pt3.z /= cloud_cluster->points.size();
      pcl::getMinMax3D(*cloud_cluster, minPoint, maxPoint);
      marker.scale.y= maxPoint.y();
      //marker.scale.x= maxPoint.x();
      //marker.scale.z= maxPoint.z();
      marker.scale.x= maxPoint.x()-minPoint.x();
      colors.r = marker.scale.x;
//      colors.g = marker.scale.y;
      //marker.scale.z= maxPoint.z()-minPoint.z();
      //pt3.z = maxPoint.z();

      //geometry_msgs::Point pt4;
      //pt4.x = cloud_cluster->points[j+1].x;
      //pt4.y = cloud_cluster->points[j+1].y;
      //pt4.z = cloud_cluster->points[j+1].z;
      //pt4.z = maxPoint.z();

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

void VertexProcessor::parameter(vp::Parameter param, const Eigen::Vector4f& v)
{
    parameter(param, v.x(), v.y(), v.z(), v.w());
}

template <typename PointT> inline unsigned
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
                              const Eigen::Vector4f &centroid,
                              Eigen::Matrix3f &covariance_matrix)
{
  if (cloud.points.empty ())
    return (0);

  // Initialize to 0
  covariance_matrix.setZero ();

  unsigned point_count;
  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    point_count = static_cast<unsigned> (cloud.size ());
    // For each point in the cloud
    for (size_t i = 0; i < point_count; ++i)
    {
      Eigen::Vector4f pt = cloud [i].getVector4fMap () - centroid;

      covariance_matrix (1, 1) += pt.y () * pt.y ();
      covariance_matrix (1, 2) += pt.y () * pt.z ();

      covariance_matrix (2, 2) += pt.z () * pt.z ();

      pt *= pt.x ();
      covariance_matrix (0, 0) += pt.x ();
      covariance_matrix (0, 1) += pt.y ();
      covariance_matrix (0, 2) += pt.z ();
    }
  }
  // NaN or Inf values could exist => check for them
  else
  {
    point_count = 0;
    // For each point in the cloud
    for (size_t i = 0; i < cloud.size (); ++i)
    {
      // Check if the point is invalid
      if (!isFinite (cloud [i]))
        continue;

      Eigen::Vector4f pt = cloud [i].getVector4fMap () - centroid;

      covariance_matrix (1, 1) += pt.y () * pt.y ();
      covariance_matrix (1, 2) += pt.y () * pt.z ();

      covariance_matrix (2, 2) += pt.z () * pt.z ();

      pt *= pt.x ();
      covariance_matrix (0, 0) += pt.x ();
      covariance_matrix (0, 1) += pt.y ();
      covariance_matrix (0, 2) += pt.z ();
      ++point_count;
    }
  }
  covariance_matrix (1, 0) = covariance_matrix (0, 1);
  covariance_matrix (2, 0) = covariance_matrix (0, 2);
  covariance_matrix (2, 1) = covariance_matrix (1, 2);

  return (point_count);
}