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

	static void getCentroid3D(const pcl::PointCloud<PointT> &pcl_cloud_input, Eigen::Vector4f &centroid)
	{
		// Initialize to 0
		centroid.setZero();
		if(pcl_cloud_input.points.empty())
			return;
		// For each point in the cloud
		int cp = 0;

		// If the data is dense, we don't need to check for NaN
		if(pcl_cloud_input.is_dense)
		{
			for(size_t i = 0; i < pcl_cloud_input.points.size(); ++i)
				centroid += pcl_cloud_input.points[i].getVector4fMap();
			centroid[3] = 0;
			centroid /= pcl_cloud_input.points.size();
		}
		// NaN or Inf values could exist => check for them
		else
		{
			for(size_t i = 0; i < pcl_cloud_input.points.size(); ++i)
			{
				// Check if the point is invalid
				if(!pcl_isfinite(pcl_cloud_input.points[i].x) || !pcl_isfinite(pcl_cloud_input.points[i].y) || !pcl_isfinite(pcl_cloud_input.points[i].z))
					continue;

				centroid += pcl_cloud_input.points[i].getVector4fMap();
				cp++;
			}
			centroid[3] = 0;
			centroid /= cp;
		}
	}

void AbsoluteEulerAngleDecoder::Decode(array_view<DirectX::Quaternion> rots, const VectorType & y)
{
	int n = rots.size();

	Eigen::Vector4f qs;
	XMVECTOR q;
	qs.setZero();
	for (int i = 0; i < n; i++)
	{
		qs.segment<3>(0) = y.segment<3>(i * 3).cast<float>();
		q = XMLoadFloat4A(qs.data());
		q = XMQuaternionRotationRollPitchYawFromVector(q); // revert the log map
		XMStoreA(rots[i], q);
	}
}

void AbsoluteEulerAngleDecoder::Encode(array_view<const DirectX::Quaternion> rots, VectorType & x)
{
	int n = rots.size();

	Eigen::Vector4f qs;
	XMVECTOR q;
	qs.setZero();
	x.resize(n * 3);
	for (int i = 0; i < n; i++)
	{
		q = XMLoad(rots[i]);
		q = XMQuaternionEulerAngleYawPitchRoll(q); // Decompsoe in to euler angle
		XMStoreFloat4(qs.data(), q);
		x.segment<3>(i * 3) = qs.head<3>();
	}
}

bool
pcl::lineWithLineIntersection (const Eigen::VectorXf &line_a, 
                               const Eigen::VectorXf &line_b, 
                               Eigen::Vector4f &point, double sqr_eps)
{
  Eigen::Vector4f p1, p2;
  lineToLineSegment (line_a, line_b, p1, p2);

  // If the segment size is smaller than a pre-given epsilon...
  double sqr_dist = (p1 - p2).squaredNorm ();
  if (sqr_dist < sqr_eps)
  {
    point = p1;
    return (true);
  }
  point.setZero ();
  return (false);
}

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

  // Initialize to 0
  centroid.setZero ();
  // For each point in the cloud
  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    for (size_t i = 0; i < cloud.points.size (); ++i)
      centroid += cloud.points[i].getVector4fMap ();
    centroid[3] = 0;
    centroid /= static_cast<float> (cloud.points.size ());

    return (static_cast<unsigned int> (cloud.points.size ()));
  }
  // NaN or Inf values could exist => check for them
  else
  {
    unsigned cp = 0;
    for (size_t i = 0; i < cloud.points.size (); ++i)
    {
      // Check if the point is invalid
      if (!isFinite (cloud [i]))
        continue;

      centroid += cloud[i].getVector4fMap ();
      ++cp;
    }
    centroid[3] = 0;
    centroid /= static_cast<float> (cp);

    return (cp);
  }
}

template <typename PointT> inline void
pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
                        Eigen::Vector4f &centroid)
{
  // Initialize to 0
  centroid.setZero ();
  if (indices.empty ()) 
    return;
  // For each point in the cloud
  int cp = 0;

  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    for (size_t i = 0; i < indices.size (); ++i)
      centroid += cloud.points[indices[i]].getVector4fMap ();
    centroid[3] = 0;
    centroid /= indices.size ();
  }
  // NaN or Inf values could exist => check for them
  else
  {
    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;

      centroid += cloud.points[indices[i]].getVector4fMap ();
      cp++;
    }
    centroid[3] = 0;
    centroid /= cp;
  }
}